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

    // 程序執行時會用到的變數
    mavlink_message_t mav_message;
    mavlink_command_int_t mav_command_int;
    MavlinkPassthrough::CommandLong mav_command_long;
    MavlinkPassthrough::Result result_mavpass;
    Action::Result result_action;

    int _counter;
    double _target_lon;
    double _target_lat;

    // std::cout << "Dev Test Start" << 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
                    _target_lat = std::stof(telemetryInfo["vehicle_lat"]);
                    _target_lon = std::stof(telemetryInfo["vehicle_lon"]);
                    #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
                    if (telemetryInfo["drone_air"] == "0") {
                        action.set_takeoff_altitude(static_cast<int>(oneVehicleCommand.second[0])); // mavsdk show error but still effect well (?)
                    } else {
                        mav_command_int.target_system = sysid;                      // Target system ID
                        mav_command_int.target_component = MAV_COMP_ID_AUTOPILOT1;  // Target component ID
                        mav_command_int.command = MAV_CMD_DO_REPOSITION;            // Command ID
                        mav_command_int.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;      // Coordinate system
                        mav_command_int.x = _target_lat * 1E7;                      // Target latitude
                        mav_command_int.y = _target_lon * 1E7;                      // Target longitude
                        mav_command_int.z = static_cast<float>(oneVehicleCommand.second[0]);  // Target altitude
                        mavlink_msg_command_int_encode(mavlink_passthrough.get_our_sysid(),
                                                        mavlink_passthrough.get_our_compid(),
                                                        &mav_message,
                                                        &mav_command_int);
                        result_mavpass = mavlink_passthrough.send_message(mav_message);
                        std::cout << "   ACK REL: " << result_mavpass << std::endl; // dev
                    }
                    #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
                    mav_command_int.target_system = sysid;                      // Target system ID
                    mav_command_int.target_component = MAV_COMP_ID_AUTOPILOT1;  // Target component ID
                    mav_command_int.command = MAV_CMD_DO_REPOSITION;            // Command ID
                    mav_command_int.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;      // Coordinate system
                    mav_command_int.x = oneVehicleCommand.second[0] * 1E7;      // Target latitude
                    mav_command_int.y = oneVehicleCommand.second[1] * 1E7;      // Target longitude
                    mav_command_int.z = std::stof(telemetryInfo["drone_alt"]);  // Target altitude
                    mavlink_msg_command_int_encode(mavlink_passthrough.get_our_sysid(),
                                                    mavlink_passthrough.get_our_compid(),
                                                    &mav_message,
                                                    &mav_command_int);
                    result_mavpass = mavlink_passthrough.send_message(mav_message);
                    _target_lon = oneVehicleCommand.second[1];
                    _target_lat = oneVehicleCommand.second[0];
                    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;
}