//
// Simple example to demonstrate how takeoff and land using MAVSDK.
//

#include <chrono>
#include <cstdint>
#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 <iostream>
#include <fstream>
#include <sstream>
#include <future>
#include <memory>
#include <thread>
#include <vector>
#include <map>

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

struct MavInitParameter {
    std::string connectPort;
    std::string connectPort2;
    int B;
    float C;
};

struct systemHandlerInfo {
    int systemID;
    std::thread systemThread;


};


// 我無法解決跨執行緒的 promise 變數生命週期問題
// 用最智障的全域變數 (砍掉 promise 結構簡單好多)
std::map<int,std::map<std::string,float>> gTelemetryInfo;


int initializeParameters(const std::string& filePath, MavInitParameter& initSetting) {
    std::ifstream configFile(filePath);
    if (!configFile) {
        return 1;
    }
    
    std::vector<std::string> parameters; // Use a vector to store the parameters
    std::string line;
    while (std::getline(configFile, line)) {
        if (line == "EOC") {
            break; // Stop reading when "EOC" is encountered
        }
        // Split the line into key and value
        std::size_t delimiterPos = line.find('=');
        if (delimiterPos != std::string::npos) {
            std::string key = line.substr(0, delimiterPos);
            std::string value = line.substr(delimiterPos + 1);

            // Check if the key matches any field in MavInitParameter
            if (key == "connectPort") {
                initSetting.connectPort = value;
            } else if (key == "connectPort2") {
                initSetting.connectPort2 = value;
            } else if (key == "B") {
                initSetting.B = std::stoi(value);
            } else if (key == "C") {
                initSetting.C = std::stof(value);
            }
        }
        parameters.push_back(line); // Add the parameter to the vector
    }
    configFile.close();
    std::cout << "Parameters read successfully!" << std::endl;
    // Print all the parameters
    for (const auto& parameter : parameters) {
        std::cout << "Line: " << parameter << std::endl;
    }
    return 0;
}

/*
 * 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};
    std::cout << "System " << static_cast<int>(system.get_system_id()) << " get in Thread. (debug)" << std::endl; // debug

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

    // Put all the subscriber init in this section
    telemetry.subscribe_position([&system, &telemetryInfo](Telemetry::Position position) {
        std::cout << "System ID: " << static_cast<int>(system.get_system_id()) << " Altitude: " << position.relative_altitude_m << " m\n"; // debug

        // Store telemetry information
        telemetryInfo["altitude"] = position.relative_altitude_m;
        gTelemetryInfo[static_cast<int>(system.get_system_id())] = telemetryInfo;
    });

    // Loop


    // Destroy


    sleep_for(seconds(5));
    // Thread Terminate
    std::cout << "Thread Out " << static_cast<int>(system.get_system_id()) << std::endl;
}


// class systemHandler {

//     private:
//         Telemetry telemetry;
//         Action action;


//     public:
//         std::map<std::string,float> telemetryInfo;
//         std::map<std::string,float> actionCommand;

//         // Constructor
//         systemHandler(System& system) {
//             // Initialization
//             telemetry = Telemetry{system};
//             action = Action{system};
//             std::cout << "System " << static_cast<int>(system.get_system_id()) << " captured. (debug)" << std::endl; // debug


//             telemetry.subscribe_position([&system, &telemetryInfo](Telemetry::Position position) {
//                 std::cout << "System ID: " << static_cast<int>(system.get_system_id()) << " Altitude: " << position.relative_altitude_m << " m\n"; // debug

//                 // Store telemetry information
//                 telemetryInfo["altitude"] = position.relative_altitude_m;
//             });

//         }


//         // Loop 
//         void operator()() {
            
//             std::cout << "Thread is running" << std::endl;
//         }
// }



int main(int argc, char** argv)
{

    // if (argc != 2) {
    //     std::cout << "Config File Needed! ex. config.txt" << std::endl;
    //     return 1;
    // }

    // Program Initial Read Setting from config file
    struct MavInitParameter initSetting;
    // auto i = initializeParameters(argv[1], initSetting);
    auto i = initializeParameters("config.txt", initSetting);
    if (i == 1){
        std::cerr << "Failed to open config file: " << argv[1] << std::endl;
    }

    // Start MAVSDK Server and connect to it.
    Mavsdk mavsdk;
    ConnectionResult connection_result = mavsdk.add_any_connection(initSetting.connectPort);
    if (connection_result != ConnectionResult::Success) {
        std::cerr << "Connection failed: " << connection_result << '\n';
        return 1;
    }

    // For store Discover systems in a list
    std::vector<systemHandlerInfo> systemHandlerInfos;

    // Subscribe to new system discovery
    mavsdk.subscribe_on_new_system([&mavsdk, &systemHandlerInfos]() {
        // Get the last subscribed system 
        auto comingSystems = mavsdk.systems();
        std::shared_ptr<System> sys = comingSystems.back();
        // Avoid Duplicate System ID (Component ID is careless, maybe determind by UUID will be better?)
        for(int i = 0;i < systemHandlerInfos.size();i++) {
            if(sys->get_system_id() == systemHandlerInfos[i].systemID){
                comingSystems.pop_back();
                if(comingSystems.size() != 0){
                    sys = comingSystems.back();
                } else {
                    std::cout << "Duplicate System ID Collision : " << static_cast<int>(sys->get_system_id()) << std::endl;
                    return;
                }
            }
        }
        // make std::shared_ptr<MAVSDK::System> back to MAVSDK::System
        System& new_system = *sys;
        std::cout << "system detect : " << static_cast<int>(new_system.get_system_id()) << "(Debug)"<< std::endl; // debug

        // Let handler progrem deal with System  
        std::thread systemHandleThread([&new_system]() {systemHandler(new_system);});

        systemHandlerInfos.push_back(systemHandlerInfo{
            static_cast<int>(new_system.get_system_id()),
            std::move(systemHandleThread),

        });

    });

    // MySQL connection is here


    // Main Loop is here
    //   -- 將這個服務目前的狀態 更新到 mysql 中
    //   -- 從 systemHandlerInfos 中 輪巡所有 system 檢查並處理是否有需要丟到 mysql 的資料
    //   -- 從 mysql 抓到指令 丟到 systemHandlerInfo


    sleep_for(seconds(3));

    std::cout << "mark A" << gTelemetryInfo[1]["altitude"] << std::endl;

    // make all thread join
    for( auto& info : systemHandlerInfos){
        info.systemThread.join();
    }

// ==============================================================================

    // mavsdk.subscribe_on_new_system([&mavsdk]() {
    //     auto system = mavsdk.systems().back();


    //     if (system->has_autopilot()) {
    //         std::cout << "Discovered autopilot\n";
    //     }
    // });



    

    // if (connection_result != ConnectionResult::Success) {
    //     std::cerr << "Connection failed: " << connection_result << '\n';
    //     return 1;
    // }

    // auto system = get_system(mavsdk);
    // if (!system) {
    //     return 1;
    // }



    // for (auto system : mavsdk.systems()) {
    //     std::cout << "(mavsdk.systems) get id : " << static_cast<int>(system->get_system_id()) << std::endl;
    //     for (auto systemHandler : systemHandlers) {
    //         std::cout << systemHandler.systemID << " Mark B" << std::endl;
    //         if (systemHandler.systemID == static_cast<int>(system->get_system_id())) {
    //             std::cout << "Mark C" << std::endl;
    //             // telemetry = Telemetry{system};
    //             const auto set_rate_result = telemetry.set_rate_position(1);
    //             if (set_rate_result != Telemetry::Result::Success) {
    //                 std::cerr << "Setting rate failed: " << set_rate_result << '\n';
    //                 return 1;
    //             }
    //             telemetry.subscribe_position([systemHandler](Telemetry::Position position) {
    //                 std::cout << "System ID: " << systemHandler.systemID << " Altitude: " << position.relative_altitude_m << " m\n";
    //             });

    //             // // Check until vehicle is ready to arm
    //             // while (telemetry.health_all_ok() != true) {
    //             //     std::cout << "System ID: " << systemHandler.systemID << " Vehicle is getting ready to arm\n";
    //             //     sleep_for(seconds(1));
    //             // }

    //             // // Arm vehicle
    //             // std::cout << "System ID: " << systemHandler.systemID << " Arming...\n";
    //             // const Action::Result arm_result = action.arm();

    //             // if (arm_result != Action::Result::Success) {
    //             //     std::cerr << "System ID: " << systemHandler.systemID << " Arming failed: " << arm_result << '\n';
    //             //     return 1;
    //             // }
    //             // telemetrys.push_back(telemetry);
    //             sleep_for(seconds(5));
    //         } else {
    //             std::cout << " Mark D " << std::endl;
    //         }
    //     }
    // }
    



    // std::cout << typeid().name() << std::endl;


    // return 1;

    // Instantiate plugins.
    // auto telemetry = Telemetry{systemsList[0]};
    // auto telemetry1 = Telemetry{systemsList[1]};

    // // Instantiate plugins.
    // auto telemetry = Telemetry{system};
    // auto action = Action{system};

    // // Instantiate mavlink_passthrough plugin.
    // auto mavlink_passthrough = MavlinkPassthrough{system};

    // auto x = static_cast<int>(system->get_system_id());
    // std::cout << "System ID : " << x << '\n';

    // System::AutopilotVersion y = system->get_autopilot_version_data();
    // std::cout << "MAV_PROTOCOL_CAPABILITY : " << y.capabilities << '\n';
    // std::cout << "Flight Stack Version : " << y.flight_sw_version << '\n';

    // // Check vehicle mode
    // const Telemetry::FlightMode mode_result = telemetry.flight_mode();
    // std::cerr << "Vehicle Mode: " << mode_result << '\n';
    // // return 1; // debug

    // // Set the flight mode to "AUTO". use SET_MODE (#11) in MAVLink Messages.
    // mavlink_message_t message;


    // // mavlink_msg_set_mode_pack(mavlink_passthrough.get_our_sysid(),
    // //                           mavlink_passthrough.get_our_compid(),
    // //                           &message,
    // //                           system->get_system_id(),
    // //                           MAV_COMP_ID_AUTOPILOT1,
    // //                           4);

    // mavlink_command_long_t command;
    // command.target_system = system->get_system_id();
    // command.target_component = MAV_COMP_ID_AUTOPILOT1;
    // command.command = MAV_CMD_DO_SET_MODE;
    // command.param1 = MAV_MODE_AUTO_ARMED;
    // mavlink_msg_command_long_encode(mavlink_passthrough.get_our_sysid(),
    //                                 mavlink_passthrough.get_our_compid(),
    //                                 &message,
    //                                 &command);

    // mavlink_passthrough.send_message(message);


    // return 1; // debug

    // We want to listen to the altitude of the drone at 1 Hz.
    // const auto set_rate_result = telemetry0.set_rate_position(1);
    // if (set_rate_result != Telemetry::Result::Success) {
    //     std::cerr << "Setting rate failed: " << set_rate_result << '\n';
    //     return 1;
    // }
    // const auto set_rate_result = telemetry1.set_rate_position(1);
    // if (set_rate_result != Telemetry::Result::Success) {
    //     std::cerr << "Setting rate failed: " << set_rate_result << '\n';
    //     return 1;
    // }

    

    // Set up callback to monitor altitude while the vehicle is in flight
    // auto x = systemsList[0]->get_system_id();
    // telemetry.subscribe_position([x](Telemetry::Position position) {
    //     std::cout << "System ID: " << x << "Altitude: " << position.relative_altitude_m << " m\n";
    // });

    // std::cout << "Mark C" << std::endl;

    // for(int i = 0; i < 5; ++i) {
    //     sleep_for(seconds(5));

    //     std::cout << "Number of discovered systems: " << systemsList.size() << std::endl;

    //     for (auto system : mavsdk.systems()) {
    //         std::cout << "Found system with system ID: " << static_cast<int>(system->get_system_id())
    //           << ", connected: " << (system->is_connected() ? "yes" : "no") << '\n';
    //     }

    //     std::cout << "link check " << i << " time. \n";
    // }

    // // Check until vehicle is ready to arm
    // while (telemetry.health_all_ok() != true) {
    //     std::cout << "Vehicle is getting ready to arm\n";
    //     sleep_for(seconds(1));
    // }

    // // Arm vehicle
    // std::cout << "Arming...\n";
    // const Action::Result arm_result = action.arm();

    // if (arm_result != Action::Result::Success) {
    //     std::cerr << "Arming failed: " << arm_result << '\n';
    //     return 1;
    // }

    // return 1; // debug



    // // Take off
    // std::cout << "Taking off...\n";
    // const Action::Result takeoff_result = action.takeoff();
    // if (takeoff_result != Action::Result::Success) {
    //     std::cerr << "Takeoff failed: " << takeoff_result << '\n';
    //     return 1;
    // }

    // Let it hover for a bit before landing again.
    sleep_for(seconds(5));

    // std::cout << "Landing...\n";
    // const Action::Result land_result = action.land();
    // if (land_result != Action::Result::Success) {
    //     std::cerr << "Land failed: " << land_result << '\n';
    //     return 1;
    // }

    // // Check if vehicle is still in air
    // while (telemetry.in_air()) {
    //     std::cout << "Vehicle is landing...\n";
    //     sleep_for(seconds(1));
    // }
    // std::cout << "Landed!\n";

    // // We are relying on auto-disarming but let's keep watching the telemetry for a bit longer.
    // sleep_for(seconds(3));
    // std::cout << "Finished...\n";

    return 0;
}






















// ===================================================
// 1.

    // std::cout << "Mark A" << std::endl;
    // sleep_for(seconds(3));

    // System& s0 = *mavsdk.systems()[0];
    // System& s1 = *mavsdk.systems()[1];
    // std::cout << static_cast<int>(s0.get_system_id()) << std::endl;


    // auto t0 = Telemetry{s0};
    // auto t1 = Telemetry{s1};
    // t0.subscribe_position([](Telemetry::Position position) {
    //     std::cout << "System ID: " << "0" << " Altitude: " << position.relative_altitude_m << " m\n";
    // });

    // t1.subscribe_position([](Telemetry::Position position) {
    //     std::cout << "System ID: " << "1" << " Altitude: " << position.relative_altitude_m << " m\n";
    // });