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

void printJson(const std::string& title, const lluav::json& j, size_t maxLen = 2000) {
    std::cout << "\n--- " << title << " ---\n";
    std::string s = j.dump(2);
    std::cout << (s.size() > maxLen ? s.substr(0, maxLen) + "\n... [truncated]\n" : s) << "\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] : "";

    try {
        // auto_reconnect=true, heartbeat_interval=5s by default
        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");

        client.connect();
        std::cout << "Connected to " << client.url() << "\n";

        // Register a periodic task: get lidar pose every 3 seconds
        client.add_periodic_task("pose_poll", 3, "lidarpose");
        std::cout << "Registered periodic task: pose_poll every 3s\n";

        // Version
        printJson("Version", client.get_version());

        // Machine ID
        printJson("Machine ID", client.get_machine_id());

        // System info
        printJson("System Info", client.get_sysinfo());

        // State
        printJson("Drone State", client.get_state());

        // Preflight check
        try {
            printJson("Preflight Check", client.preflight_check());
        } catch (const lluav::LLUAVCommandError& e) {
            std::cerr << "Preflight check error: " << e.what() << "\n";
        }

        // Lidar pose
        try {
            printJson("Lidar Pose", client.get_lidar_pose());
        } catch (const lluav::LLUAVCommandError& e) {
            std::cerr << "Lidar pose error: " << e.what() << "\n";
        }

        // Wait a bit to see periodic tasks in action
        std::cout << "\n--- Waiting 7 seconds for periodic tasks ---\n";
        std::this_thread::sleep_for(std::chrono::seconds(7));

        client.remove_periodic_task("pose_poll");
        std::cout << "Removed periodic task: pose_poll\n";

        std::cout << "\nDone.\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;
}
