#include <iostream>
#include <thread>
#include <chrono>
#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] : "";

    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 Flight Mission Demo (C++)\n";
        std::cout << "========================================\n";

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

        // 2. Preflight check
        std::cout << "\n[2/6] Running preflight check...\n";
        try {
            auto preflight = client.preflight_check();
            int ret = preflight.value("preflight_check", -1);
            std::string msg = preflight.value("preflight_check_msg", "Unknown");
            std::cout << "      Result: " << msg << " (code=" << ret << ")\n";
            if (ret != 0) {
                std::cout << "      WARNING: Preflight check failed!\n";
            }
        } catch (const std::exception& e) {
            std::cout << "      Preflight check error: " << e.what() << "\n";
        }

        // 3. Initial state
        std::cout << "\n[3/6] Getting initial state...\n";
        auto state = client.get_state();
        auto st = state.value("state", lluav::json::object());
        std::cout << "      Armed: " << st.value("armed", "?")
                  << "  CtrlState: " << st.value("ctrl_state", "?") << "\n";

        // 4. Takeoff
        std::cout << "\n[4/6] Commanding TAKEOFF...\n";
        auto resp = client.takeoff();
        std::cout << "      Takeoff acknowledged: ret=" << resp.value("ret", -1) << "\n";
        std::cout << "      Waiting 5s for climb...\n";
        std::this_thread::sleep_for(std::chrono::seconds(5));

        // 5. Set setpoint NED
        std::cout << "\n[5/6] Setting setpoint NED: x=5, y=0, z=-5, yaw=0\n";
        resp = client.set_sp_ned(5.0f, 0.0f, -5.0f, 0.0f);
        std::cout << "      Setpoint acknowledged: ret=" << resp.value("ret", -1) << "\n";
        std::cout << "      Waiting 5s...\n";
        std::this_thread::sleep_for(std::chrono::seconds(5));

        // 6. RTL
        std::cout << "\n[6/6] Commanding RTL...\n";
        resp = client.rtl();
        std::cout << "      RTL acknowledged: ret=" << resp.value("ret", -1) << "\n";

        std::cout << "\n========================================\n";
        std::cout << "Mission 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;
}
