#include <mavsdk/mavsdk.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};
    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 << "Has AutoPilot : " << system.has_autopilot() << std::endl; // debug

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