#include <iostream>
#include "lluav_sdk/client.hpp"

int main(int argc, char** argv) {
    std::string host = (argc > 1) ? argv[1] : "localhost";
    int port = (argc > 2) ? std::stoi(argv[2]) : 53001;
    std::string password = (argc > 3) ? argv[3] : "";
    std::string map_name = (argc > 4) ? argv[4] : "";

    try {
        lluav::LLUAVClient client(host, port, password,
            std::chrono::milliseconds(10000), true,
            std::chrono::milliseconds(1000),
            std::chrono::milliseconds(30000), 3,
            std::chrono::milliseconds(5000), "state");

        std::cout << "========================================\n";
        std::cout << "LLUAV Camera & Map Demo (C++)\n";
        std::cout << "========================================\n";

        std::cout << "\n[1/6] Connecting...\n";
        client.connect();
        std::cout << "      Connected.\n";

        // List maps
        std::cout << "\n[2/6] Listing saved maps...\n";
        try {
            auto maps = client.list_maps();
            auto arr = maps.value("maps", lluav::json::array());
            std::cout << "      Found " << arr.size() << " map(s):\n";
            for (const auto& m : arr) {
                std::cout << "        - " << m.get<std::string>() << "\n";
            }
        } catch (const std::exception& e) {
            std::cout << "      Error: " << e.what() << "\n";
        }

        // Capture
        std::cout << "\n[3/6] Capturing photo...\n";
        try {
            auto resp = client.capture();
            std::cout << "      Capture OK: ret=" << resp.value("ret", -1) << "\n";
        } catch (const std::exception& e) {
            std::cout << "      Error: " << e.what() << "\n";
        }

        // SIYI center
        std::cout << "\n[4/6] Centering SIYI gimbal...\n";
        try {
            auto resp = client.siyi_center();
            std::cout << "      Center OK: ret=" << resp.value("ret", -1) << "\n";
        } catch (const std::exception& e) {
            std::cout << "      Error: " << e.what() << "\n";
        }

        // Toggle record
        std::cout << "\n[5/6] Toggling video recording...\n";
        try {
            auto resp = client.record_toggle();
            std::cout << "      Record toggle OK: ret=" << resp.value("ret", -1) << "\n";
        } catch (const std::exception& e) {
            std::cout << "      Error: " << e.what() << "\n";
        }

        // Save map
        if (!map_name.empty()) {
            std::cout << "\n[6/6] Saving map to '" << map_name << "'...\n";
            try {
                auto resp = client.save_map(map_name);
                std::cout << "      Save OK: ret=" << resp.value("ret", -1) << "\n";
                if (resp.contains("path")) {
                    std::cout << "      Path: " << resp["path"].get<std::string>() << "\n";
                }
            } catch (const std::exception& e) {
                std::cout << "      Error: " << e.what() << "\n";
            }
        } else {
            std::cout << "\n[6/6] Skipping map save (pass map name as 4th arg)\n";
        }

        std::cout << "\n========================================\n";
        std::cout << "Demo completed.\n";
        std::cout << "========================================\n";

        client.disconnect();
    } catch (const lluav::LLUAVCommandError& e) {
        std::cerr << "Command failed: " << e.what() << " (ret=" << e.ret_code() << ")\n";
        std::cerr << "Response: " << e.response().dump(2) << "\n";
        return 1;
    } catch (const std::exception& e) {
        std::cerr << "Error: " << e.what() << "\n";
        return 1;
    }
    return 0;
}
