#include <iostream>
#include <iomanip>
#include <thread>
#include <chrono>
#include <atomic>
#include <csignal>
#include "lluav_sdk/client.hpp"

std::atomic<bool> g_running{true};

void signal_handler(int) {
    g_running = false;
}

void printPose(const lluav::json& j) {
    auto lp = j.value("lidar_pose", lluav::json::object());
    std::cout << "[POSE]  "
              << "X=" << lp.value("x", 0.0f) << " "
              << "Y=" << lp.value("y", 0.0f) << " "
              << "Z=" << lp.value("z", 0.0f) << " | "
              << "YAW=" << lp.value("yaw", 0.0f) << " "
              << "PITCH=" << lp.value("pitch", 0.0f) << " "
              << "ROLL=" << lp.value("roll", 0.0f) << "\n";
}

void printHud(const lluav::json& j) {
    auto pos = j.value("uav_pos_enu", lluav::json::object());
    auto extra = j.value("extra", lluav::json::object());
    std::cout << "[HUD]   "
              << "ENU(" << pos.value("x", 0.0f) << ","
              << pos.value("y", 0.0f) << ","
              << pos.value("z", 0.0f) << ") "
              << "Vol=" << extra.value("volume_est", "N/A") << "\n";
}

void printStateBrief(const lluav::json& j) {
    auto st = j.value("state", lluav::json::object());
    std::cout << "[STATE] armed=" << st.value("armed", "?")
              << " ctrl=" << st.value("ctrl_state", "?")
              << " batt=" << st.value("battery_v", "?") << "V\n";
}

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::signal(SIGINT, signal_handler);

    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 << "Connecting to " << client.url() << " ...\n";
        client.connect();
        std::cout << "Connected. Press Ctrl+C to stop.\n\n";

        // Periodic task: poll state every 2 seconds via queue
        client.add_periodic_task("state_poll", 2, "state");

        while (g_running) {
            std::this_thread::sleep_for(std::chrono::seconds(2));
            if (!g_running) break;

            try {
                auto pose = client.get_lidar_pose();
                printPose(pose);
            } catch (const std::exception& e) {
                std::cout << "[POSE]  Error: " << e.what() << "\n";
            }

            try {
                auto hud = client.get_hud1();
                printHud(hud);
            } catch (const std::exception& e) {
                std::cout << "[HUD]   Error: " << e.what() << "\n";
            }

            try {
                auto state = client.get_state();
                printStateBrief(state);
            } catch (const std::exception& e) {
                std::cout << "[STATE] Error: " << e.what() << "\n";
            }
            std::cout << std::endl;
        }

        std::cout << "\nStopping monitor...\n";
        client.disconnect();
        std::cout << "Disconnected.\n";
    } catch (const std::exception& e) {
        std::cerr << "Error: " << e.what() << "\n";
        return 1;
    }
    return 0;
}
