#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;

std::map<Telemetry::FlightMode, std::string> flightModeMap = {
    {Telemetry::FlightMode::Unknown, "Unknown"},
    {Telemetry::FlightMode::Takeoff, "Takeoff"},
    {Telemetry::FlightMode::Ready, "Ready"},
    {Telemetry::FlightMode::Hold, "Hold"},
    {Telemetry::FlightMode::ReturnToLaunch, "Return To Launch"},
    {Telemetry::FlightMode::Land, "Land"},
    {Telemetry::FlightMode::Offboard, "Offboard"},
    {Telemetry::FlightMode::FollowMe, "FollowMe"},
    {Telemetry::FlightMode::Manual, "Manual"},
    {Telemetry::FlightMode::Altctl, "Altctl"},
    {Telemetry::FlightMode::Posctl, "Posctl"},
    {Telemetry::FlightMode::Acro, "Acro"},
    {Telemetry::FlightMode::Stabilized, "Stabilized"},
    {Telemetry::FlightMode::Rattitude, "Rattitude"}
};


#define VEHICLE_CONTROL_ACTIVE


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

    // Initialization
    auto mavlink_passthrough = MavlinkPassthrough{system};
    auto telemetry = Telemetry{system};
    auto action = Action{system};
    auto param = Param{system};


    // Store telemetry information that will pass to main thread.
    std::map<std::string,std::string> telemetryInfo;

    // 控制指令 已經被主迴圈轉換過一次
    std::map<int, std::array<double, 2>> vehicleCommand;

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

    // 程序執行時會用到的變數
    int _counter;
    mavlink_message_t mav_message;
    mavlink_set_position_target_global_int_t set_position_target_global_int;
    MavlinkPassthrough::CommandLong mav_command_long;
    MavlinkPassthrough::Result result_mavpass;
    Action::Result result_action;

    //
    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';


    std::cout << "  Set Mode Test " << std::endl;
    sleep_for(milliseconds(1500));


    // sleep_for(milliseconds(1500));
    // action.set_takeoff_altitude(10);
    // std::cout << "ARM...\n";
    // action.arm();
    // sleep_for(seconds(1));

    // std::cout << "Taking off...\n";
    // action.takeoff();
    // sleep_for(seconds(10));

    // std::cout << "GoTo...\n";
    // // auto result_ackFromAction = action.goto_location(24.119289235158828, 120.67216137689361,30,180);

    // 測試GOTO
    // mavlink_message_t message;
    // MavlinkPassthrough::CommandLong command;
    // command.target_sysid = sysid;
    // command.target_compid = MAV_COMP_ID_AUTOPILOT1;  // 這行指 component id = 1
    // command.command = MAV_CMD_DO_REPOSITION;
    // command.param1 = 5; // Speed
    // command.param2 = 0; // IF change to GUILDED mode ?
    // command.param3 = 0; // Radius
    // command.param4 = 0; // Yaw
    // command.param5 = 24.119289235158828; // Latitude
    // command.param6 = 120.67216137689361; // Longitude
    // command.param5 = 24.11928923; // Latitude
    // command.param6 = 120.67216137; // Longitude
    // command.param7 = 25; // Altitude
    // mavlink_msg_command_long_encode(mavlink_passthrough.get_our_sysid(),
    //                                 mavlink_passthrough.get_our_compid(),
    //                                 &message,
    //                                 &command);
    // mavlink_passthrough.send_command_long(command);
    // auto result_ackFromSendMessage = mavlink_passthrough.send_message(message);
    // std::cout << "  ACK REL: " << result_ackFromSendMessage << std::endl;

    // // 演示一下 如何抓到 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";
            }
        });

        telemetry.subscribe_flight_mode([&system, &telemetryInfo](Telemetry::FlightMode fightMode) {
            telemetryInfo["vehicle_mode"] = flightModeMap[fightMode];
        });
    }

    // 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();

        // 取得來自主迴圈的指令 並清空
        vehicleCommand = gVehicleCommand[sysid];
        gComMtx.lock();
        gVehicleCommand[sysid].clear();
        gComMtx.unlock();

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

        // Vehicle Command
        for ( auto& oneVehicleCommand : vehicleCommand) {
            switch (oneVehicleCommand.first) {
                // Here deal with the function of Change Mode 
                case 1 : 
                    std::cout << "(systemHandler.cpp:systemHandler) System " << sysid << " Get Command Change Mode , " \
                            << "Para1 : " << std::to_string(oneVehicleCommand.second[0]) \
                            << ", Para2 : " << std::to_string(oneVehicleCommand.second[1]) << std::endl; // debug
                    #ifdef VEHICLE_CONTROL_ACTIVE
                    mav_command_long.target_sysid = sysid;
                    mav_command_long.target_compid = MAV_COMP_ID_AUTOPILOT1;  // 這行指 component id = 1
                    mav_command_long.command = 176; // MAV_CMD_DO_SET_MODE
                    mav_command_long.param1 = 1; // 固定是 1
                    mav_command_long.param2 = static_cast<int>(oneVehicleCommand.second[0]); // mode
                    
                    result_mavpass = mavlink_passthrough.send_command_long(mav_command_long); // 送出指令
                    std::cout << "   ACK REL: " << result_mavpass << std::endl; // dev
                    #endif
                    break;

                // Here deal with the function of Action Arm
                case 11 : 
                    std::cout << "(systemHandler.cpp:systemHandler) System " << sysid << " Get Command Arm, " \
                            << "Para1 : " << std::to_string(oneVehicleCommand.second[0]) \
                            << ", Para2 : " << std::to_string(oneVehicleCommand.second[1]) << std::endl; // debug
                    #ifdef VEHICLE_CONTROL_ACTIVE
                    result_action = action.arm();
                    std::cout << "   ACK REL: " << result_action << std::endl; // dev
                    #endif
                    break;

                // Here deal with the function of Action Takeoff
                case 12 : 
                    std::cout << "(systemHandler.cpp:systemHandler) System " << sysid << " Get Command Takeoff, " \
                            << "Para1 : " << std::to_string(oneVehicleCommand.second[0]) \
                            << ", Para2 : " << std::to_string(oneVehicleCommand.second[1]) << std::endl; // debug
                    #ifdef VEHICLE_CONTROL_ACTIVE
                    result_action = action.takeoff();
                    std::cout << "   ACK REL: " << result_action << std::endl; // dev
                    #endif
                    break;

                // Here deal with the function of change height
                case 13 : 
                    std::cout << "(systemHandler.cpp:systemHandler) System " << sysid << " Get Command Change Height, " \
                            << "Para1 : " << std::to_string(oneVehicleCommand.second[0]) \
                            << ", Para2 : " << std::to_string(oneVehicleCommand.second[1]) << std::endl; // debug
                    #ifdef VEHICLE_CONTROL_ACTIVE
                    #endif                    
                    break;

                // Here deal with the function of GOTO
                case 21 : 
                    std::cout << "(systemHandler.cpp:systemHandler) System " << sysid << " Get Command GoTo, " \
                            << "Para1 : " << std::to_string(oneVehicleCommand.second[0]) \
                            << ", Para2 : " << std::to_string(oneVehicleCommand.second[1]) << std::endl; // debug
                    #ifdef VEHICLE_CONTROL_ACTIVE
                    set_position_target_global_int.time_boot_ms = 0;
                    set_position_target_global_int.target_system = sysid;
                    set_position_target_global_int.target_component = MAV_COMP_ID_AUTOPILOT1;
                    set_position_target_global_int.coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; // 對於 home 點指定相對高度
                    set_position_target_global_int.type_mask = 0b110111111000; // Only latitude, longitude and altitude
                    set_position_target_global_int.lat_int = oneVehicleCommand.second[0] * 1E7; // Latitude in degrees * 1E7
                    set_position_target_global_int.lon_int = oneVehicleCommand.second[1] * 1E7; // Longitude in degrees * 1E7
                    set_position_target_global_int.alt = 30; // Altitude in meters

                    mavlink_msg_set_position_target_global_int_pack(
                        mavlink_passthrough.get_our_sysid(),
                        mavlink_passthrough.get_our_compid(),
                        &mav_message,
                        set_position_target_global_int.time_boot_ms,
                        set_position_target_global_int.target_system,
                        set_position_target_global_int.target_component,
                        set_position_target_global_int.coordinate_frame,
                        set_position_target_global_int.type_mask,
                        set_position_target_global_int.lat_int,
                        set_position_target_global_int.lon_int,
                        set_position_target_global_int.alt,
                        0, 0, 0, 0, 0, 0, 0, 0);  // vx, vy, vz, afx, afy, afz, yaw, yaw_rate are all 0
                    result_mavpass = mavlink_passthrough.send_message(mav_message);
                    std::cout << "   ACK REL: " << result_mavpass << std::endl; // dev
                    #endif                    
                    break;
            }
        }


        // 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;
}