#include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/info/info.h>
#include <mavsdk/plugins/param/param.h>
#include <mavsdk/plugins/action/action.h>
#include <mavsdk/plugins/telemetry/telemetry.h>
#include <mavsdk/plugins/mavlink_passthrough/mavlink_passthrough.h>
#include <cstdint>
#include <iostream>
#include <fstream>
#include <sstream>
#include <iomanip>
#include <future>
#include <thread>
#include <chrono>
#include <vector>
#include <mutex>
#include <map>
#include <memory>


#include "globals.h"

using namespace mavsdk;
using std::chrono::seconds;
using std::chrono::milliseconds;
using std::this_thread::sleep_for;

/*
 * Important multi-threaded functions, 
 * handling system initialization, receiving messages and sending messages.
 */
void systemHandler(System& system) {
    // Initialization
    auto telemetry = Telemetry{system};
    auto action = Action{system};
    auto param = Param{system};
    int sysid = static_cast<int>(system.get_system_id());
    std::cout << "(systemHandler.cpp:systemHandler) System " << sysid << " get in Thread. (debug)" << std::endl; // debug

    // Store telemetry information
    std::map<std::string,std::string> telemetryInfo; 

    // 這些變數監控 handler 的行為
    gHandlerMask[sysid]["cutoff"] = 0;
    gHandlerMask[sysid]["is_connected"] = 1;
    bool reset = false;

    //
    int _counter;



    //
    std::cout << "Dev Test Start" << std::endl;

    // Get the info plugin for this drone
    auto atvData = system.get_autopilot_version_data();
    std::cout << "Autopilot Version Data:" << std::endl;
    std::cout << "  Vendor ID: " << atvData.vendor_id << std::endl;
    std::cout << "  Product ID: " << atvData.product_id << std::endl;
    std::cout << "  Capabilities: " << atvData.capabilities << std::endl;
    std::cout << "  Flight SW Version: " << atvData.flight_sw_version << std::endl;
    std::cout << "  Middleware SW Version: " << atvData.middleware_sw_version << std::endl;
    std::cout << "  OS SW Version: " << atvData.os_sw_version << std::endl;
    std::cout << "  Board Version: " << atvData.board_version << std::endl;

    std::array<uint8_t, 18> uid = atvData.uid2;
    std::cout << "UID: ";
    for (const auto& byte : uid) {
        std::cout << std::hex << std::setw(2) << std::setfill('0') << static_cast<int>(byte) << " ";
    }
    std::cout << std::endl;

    const Telemetry::FlightMode mode_result = telemetry.flight_mode();
    std::cerr << "Vehicle Mode: " << mode_result << '\n';

    // auto mavlink_passthrough = MavlinkPassthrough{system};
    // mavlink_command_long_t command;
    // command.target_system = sysid;
    // command.target_component = MAV_COMP_ID_AUTOPILOT1;
    // command.command = MAV_CMD_REQUEST_MESSAGE;
    // command.param1 = 1;
    // mavlink_message_t message;
    // mavlink_msg_command_long_encode(mavlink_passthrough.get_our_sysid(),
    //                                 mavlink_passthrough.get_our_compid(),
    //                                 &message,
    //                                 &command);
    // auto result_ackFromSendMessage = mavlink_passthrough.send_message(message);

    // // 演示一下 如何抓到 parameter
    // auto get_result_i = param.get_param_int("VISO_TYPE"); // This function is blocking.
    // std::cout << "Para Read: " << get_result_i.first << " : " << get_result_i.second << std::endl;

    // std::pair<Param::Result, float> get_result_f = {Param::Result::Unknown , 0};
    // get_result_f = param.get_param_float("RNGFND_FILT"); // This function is blocking.
    // std::cout << "Para Read: " << get_result_f.first << " : " << get_result_f.second << std::endl;

    // auto get_result = param.get_all_params(); // This function is blocking.
    // auto int_p = get_result.int_params;
    // for(const auto& item : int_p) {
    //     std::cout << item << std::endl;
    // }

    // // 抓一下 health
    // auto health = telemetry.health();
    // std::cout << "GYRO: " << (health.is_gyrometer_calibration_ok ? "OK" : "NG") << std::endl;
    // std::cout << "Accelerometer: " << (health.is_accelerometer_calibration_ok ? "OK" : "NG") << std::endl;
    // std::cout << "Magnetometer: " << (health.is_magnetometer_calibration_ok ? "OK" : "NG") << std::endl;
    // std::cout << "Local position NED: " << (health.is_local_position_ok ? "OK" : "NG") << std::endl;
    // std::cout << "Global position (Int): " << (health.is_global_position_ok ? "OK" : "NG") << std::endl;
    // std::cout << "Home position: " << (health.is_home_position_ok ? "OK" : "NG") << std::endl;

    
    // // 如果對接另一邊也是 mavsdk 這幾個 Parameter 要 provide，這邊讀看看
    // get_result_i = {Param::Result::Unknown , 0};
    // get_result_i = param.get_param_int("SYS_HITL"); // This function is blocking.
    // std::cout << "Para Read: " << get_result_i.first << " : " << get_result_i.second << std::endl;
    // get_result_i = {Param::Result::Unknown , 0};
    // get_result_i = param.get_param_int("CAL_GYRO0_ID"); // This function is blocking.
    // std::cout << "Para Read: " << get_result_i.first << " : " << get_result_i.second << std::endl;
    // get_result_i = {Param::Result::Unknown , 0};
    // get_result_i = param.get_param_int("CAL_ACC0_ID"); // This function is blocking.
    // std::cout << "Para Read: " << get_result_i.first << " : " << get_result_i.second << std::endl;
    // get_result_i = {Param::Result::Unknown , 0};
    // get_result_i = param.get_param_int("CAL_MAG0_ID"); // This function is blocking.
    // std::cout << "Para Read: " << get_result_i.first << " : " << get_result_i.second << std::endl;

    // // 看看 has_autopilot() 的值
    // std::cout << "Has AutoPilot : " << system.has_autopilot() << std::endl; // debug


    // std::cout << "Dev Test End" << std::endl;

    // Put all the subscriber init in this section
    if (system.has_autopilot()) {

        auto set_rate_result = telemetry.set_rate_position(1.0);
        // if (set_rate_result != mavsdk::Telemetry::Result::Success) {
        //     std::cout << "(systemHandler.cpp:systemHandler) Setting position rate failed:" << set_rate_result << std::endl;
        // }
        telemetry.subscribe_position([&system, &telemetryInfo](Telemetry::Position position) {
            // 轉換數字到字串用的暫存變數
            std::ostringstream num_str_ss;

            // std::cout << "System ID: " << sysid << " Altitude: " << position.relative_altitude_m << " m\n"; // debug
            // Store telemetry information
            num_str_ss << std::fixed << std::setprecision(4) << position.relative_altitude_m;
            telemetryInfo["drone_alt"] = num_str_ss.str();
            num_str_ss.str("");

            num_str_ss << std::fixed << std::setprecision(8) << position.latitude_deg;
            telemetryInfo["vehicle_lat"] = num_str_ss.str();
            num_str_ss.str("");

            num_str_ss << std::fixed << std::setprecision(8) << position.longitude_deg;
            telemetryInfo["vehicle_lon"] = num_str_ss.str();
            num_str_ss.str("");
        });

        telemetry.subscribe_raw_gps([&system, &telemetryInfo](Telemetry::RawGps rawGps) {
            // 轉換數字到字串用的暫存變數
            std::ostringstream num_str_ss;

            // Store telemetry information
            num_str_ss << std::fixed << std::setprecision(4) << rawGps.velocity_m_s;
            telemetryInfo["vehicle_speed"] = num_str_ss.str();
            num_str_ss.str("");
        });

        // set_rate_result = telemetry.set_rate_attitude_euler(1.0);  // only in v2.0
        // if (set_rate_result != mavsdk::Telemetry::Result::Success) {
        //     std::cout << "(systemHandler.cpp:systemHandler) Setting euler rate failed:" << set_rate_result << std::endl;
        // }
        telemetry.subscribe_attitude_euler([&system, &telemetryInfo](Telemetry::EulerAngle eulerAngle) {
            // 轉換數字到字串用的暫存變數
            std::ostringstream num_str_ss;

            // Store telemetry information
            num_str_ss << std::fixed << std::setprecision(4) << eulerAngle.roll_deg;
            telemetryInfo["drone_roll"] = num_str_ss.str();
            num_str_ss.str("");

            num_str_ss << std::fixed << std::setprecision(4) << eulerAngle.pitch_deg;
            telemetryInfo["drone_pitch"] = num_str_ss.str();
            num_str_ss.str("");

            num_str_ss << std::fixed << std::setprecision(4) << eulerAngle.yaw_deg;
            telemetryInfo["drone_yaw"] = num_str_ss.str();
            num_str_ss.str("");        
        });

        telemetry.subscribe_heading([&system, &telemetryInfo](Telemetry::Heading heading) {
            // 轉換數字到字串用的暫存變數
            std::ostringstream num_str_ss;

            // Store telemetry information
            num_str_ss << std::fixed << std::setprecision(4) << heading.heading_deg;
            telemetryInfo["vehicle_head"] = num_str_ss.str();
            num_str_ss.str("");
        });

        set_rate_result = telemetry.set_rate_battery(1.0);
        if (set_rate_result != mavsdk::Telemetry::Result::Success) {
            std::cout << "(systemHandler.cpp:systemHandler) Setting battery rate failed:" << set_rate_result << std::endl;
        }
        telemetry.subscribe_battery([&system, &telemetryInfo](Telemetry::Battery battery) {
            // 轉換數字到字串用的暫存變數
            std::ostringstream num_str_ss;

            // Store telemetry information
            num_str_ss << std::fixed << std::setprecision(4) << battery.remaining_percent;
            telemetryInfo["vehicle_bat_perc"] = num_str_ss.str();
            num_str_ss.str("");
            num_str_ss << std::fixed << std::setprecision(4) << battery.voltage_v;
            telemetryInfo["vehicle_bat_volt"] = num_str_ss.str();
            num_str_ss.str("");
        });

        set_rate_result = telemetry.set_rate_in_air(1.0);
        if (set_rate_result != mavsdk::Telemetry::Result::Success) {
            std::cout << "(systemHandler.cpp:systemHandler) Setting in_air rate failed:" << set_rate_result << std::endl;
        }
        telemetry.subscribe_in_air([&system, &telemetryInfo](bool in_air) {
            if(in_air){
                telemetryInfo["drone_air"] = "1";
            }else{
                telemetryInfo["drone_air"] = "0";
            }
        });

        telemetry.subscribe_armed([&system, &telemetryInfo](bool is_armed) {
            if(is_armed){
                telemetryInfo["drone_arm"] = "1";
            }else{
                telemetryInfo["drone_arm"] = "0";
            }
        });
    }

    // Wait Until telemetryInfo get something. and if nothing get from System for 10 sec will cut off this connection.
    _counter = 0;
    while(telemetryInfo.empty()){
        sleep_for(seconds(1));
        if(_counter++ > 10) {
            reset = true;
            std::cout << "(systemHandler.cpp:systemHandler) System " << sysid << " waiting too long without any coming info." << std::endl;
            break;
        }        
    }
    
    // Loop
    while(system.is_connected() & !reset) {
        
        // Send Telemetry Data 
        gTeleMtx.lock();
        gTelemetryInfo[sysid] = telemetryInfo;
        gTeleMtx.unlock();

        // Handler Command 
        reset = (gHandlerMask[sysid]["cutoff"] != 0);

        // Vehicle Command

        // for purpose
        sleep_for(milliseconds(100));

    }



    // Thread Terminate
    std::cout << "(systemHandler.cpp:systemHandler) Thread Out " << sysid << std::endl; //debug

    // Destroy
    gHandlerMask[sysid]["is_connected"] = 0;
}