#include <exception>
#include <iostream>
#include "cxxopts.hpp"
#include <thread>
#include <string>
#include <atomic>
#include <VmbC/VmbC.h>
#include <boost/program_options.hpp>
#include "Parser.h"
#include "Camera.h"
#include "MQTT.h"
#include "timing.h"
#include "ini.h"

namespace po = boost::program_options;

std::atomic<bool> running(true);
Timer loop_timer;
bool motorctl_info_out=false;
bool init_gimbal = false;
bool start_gimbal = false;
bool start_demo = false;
bool trigger_after_stopping = false;
double imagerate = 0.1;
int ctl_code = 0;
std::string mqtt_hdg = "0";
Parser parse_cmd;
std::string fwt_name = "";
std::string server_ip = "";
mqtt_sub_data mqtt_in;
MQTTClient* mqtt_client = nullptr;
std::vector<std::string> camera_id_vec; //= { "192.168.11.101"  , "192.168.11.102", "192.168.11.103" }; //{ "DEV_1AB22C04F7A9" }; //{ "192.168.11.101"  , "192.168.11.102", "192.168.11.103" };

// Callback function for parsing
static int handler(void* user, const char* section, const char* name, const char* value) {
    auto* config = static_cast<std::map<std::string, std::string>*>(user);
    std::string key = std::string(section) + "." + std::string(name);
    (*config)[key] = value;
    return 1; // Return 0 to stop parsing on error
}

void readInput() {
    std::string input;
    while (running) {
        if (!std::getline(std::cin, input)) {
            // stdin geschlossen oder EOF (z.B. unter systemd)
            // Einfach Thread schlafen legen und nichts tun
            std::this_thread::sleep_for(std::chrono::milliseconds(500));
            continue;
        }
        if (input.empty()) continue;  // leere Zeilen ignorieren
        if (input == "exit") {
            running = false;
            break;
        }
        std::cout << "Received input: " << input << "\n";
        parse_cmd.parse_input(input);
    }
}

int main(int argc, char* argv[])
{
    //ini handling with ini.h
    std::map<std::string, std::string> config;
    // Parse the INI file
    if (ini_parse("/home/ggs/projects/Fire_Gimbal_Control/bin/x64/Release/config.ini", handler, &config) < 0) {
        std::cerr << "Can't load config.ini file!" << std::endl;
        return 1;
    }
    // Access configuration values
    std::string tower_name = config["General.tower_name"];
    int rate = std::stoi(config["General.image_interval"]);
    int debug = std::stoi(config["General.debug"]);
    std::string zkms_server_ip = config["Network.zkms_server_ip"];
    std::string mqtt_user = config["Network.mqtt_user"];
    std::string mqtt_pw = config["Network.mqtt_pw"];
    std::string cam_id1 = config["Camera.id_Cam1"];
    std::string cam_id2 = config["Camera.id_Cam2"];
    std::string cam_id3 = config["Camera.id_Cam3"];
    std::string cam_id4 = config["Camera.id_Cam4"];
    if (cam_id1 != "") 
        camera_id_vec.push_back(cam_id1);
    if (cam_id2 != "")
        camera_id_vec.push_back(cam_id2);
    if (cam_id3 != "")
        camera_id_vec.push_back(cam_id3);
    if (cam_id4 != "")
        camera_id_vec.push_back(cam_id4);
    std::cout << camera_id_vec.size() <<  " camera(s) should be connected according the config file." << "\n";
    
    //set globals
    fwt_name = tower_name;
    imagerate = 1/(double)rate;
    motorctl_info_out = (bool)debug;
    server_ip = zkms_server_ip;
    // Print the parsed values
    std::cout << "tower_name: " << tower_name << std::endl;
    std::cout << "rate: " << rate << std::endl;
    std::cout << "debug: " << debug << std::endl;
    std::cout << "zkms_server_ip: " << zkms_server_ip << std::endl;


    //args 
    // Declare the supported options
    po::options_description desc("Allowed options");
    desc.add_options()
        ("help,h", "produce help message")
        ("init,i", po::value<bool>(), "find endstops")
        ("start,s", po::value<bool>(), "start gimbal system automatically")
        ("demo,d", po::value<bool>(), "demo mode (simulation)");
    po::variables_map vm;
    try {
        // Parse command-line options
        po::store(po::parse_command_line(argc, argv, desc), vm);
        po::notify(vm);

        if (vm.count("help")) {
            std::cout << desc << "\n";
            return 0;
        }

        if (vm.count("init")) {
            std::cout << "find endstops " << vm["init"].as<bool>() << ".\n";
            init_gimbal = true;
        }
        else {
            std::cout << "init was not set.\n"; init_gimbal = false;
        }

        if (vm.count("start")) {
            std::cout << "starting gimbal is: " << vm["start"].as<bool>() << ".\n";
            start_gimbal = true;
        }
        else {
            std::cout << "Not starting Gimbal automatically.\n";  start_gimbal = false;
        }

        if (vm.count("demo")) {
            std::cout << "demomode is: " << vm["demo"].as<bool>() << ".\n";
            start_demo = true;
        }
        else {
            std::cout << "demomode is not set.\n"; start_demo = false;
        }
    }
    catch (std::exception& e) {
        std::cerr << "ARGS Error: " << e.what() << "\n";
        return 1;
    }
    catch (...) {
        std::cerr << "Unknown ARGS error!" << "\n";
        return 1;
    }
    //main vars
    SerialPort serial("/dev/ttyACM0", 115200);
    std::thread io_thread;

    const std::string mqtt_STATUS = "GGS/FWT/" + fwt_name + "/StatusCode";
    mqtt_client =new MQTTClient(server_ip, fwt_name, fwt_name, mqtt_user, mqtt_pw);
    mqtt_client->connect_client();
    std::this_thread::sleep_for(std::chrono::milliseconds(500));
    if (!mqtt_client->connected) {
        std::cerr << "MQTT not connected, exit!" << "\n";
        return 1;
    }
    mqtt_client->publish(mqtt_STATUS, "0");
    //cmd parser
    /*
    try {
        cxxopts::Options options("Firewatch Gimbal Software", "Base Gimbal Software C++");

        options.add_options()
            ("h,help", "Print usage")
            ("d,debug", "Enable debugging")
            ("f,file", "File name", cxxopts::value<std::string>())
            ("i,integer", "An integer", cxxopts::value<int>()->default_value("10"))
            ;

        auto result = options.parse(argc, argv);
        if (result.count("help")) {
            std::cout << options.help() << std::endl;
            return 0;
        }
        if (result.count("debug")) {
            std::cout << "Debugging enabled" << std::endl;
        }
        if (result.count("file")) {
            std::cout << "File: " << result["file"].as<std::string>() << std::endl;
        }
        std::cout << "Integer: " << result["integer"].as<int>() << std::endl;
    }
    catch (const cxxopts::exceptions::exception& e) {
        std::cerr << "Error parsing options: " << e.what() << std::endl;
        return 1;
    }
    */

    //serial communication
    try {
        // Start reading from the serial port
        serial.startReading();
        // Run Boost.Asio's I/O service in a separate thread
        io_thread=std::thread([&serial]() {
            serial.run();
            });
        // Send commands in the main thread or from another thread
    }
    catch (const std::exception& e) {
        std::cerr << "Serial Exception: " << e.what() << std::endl;
    }

    //start cin thread
    std::thread inputThread(readInput);
    const std::chrono::milliseconds interval(100);
    motor_info act_info;
    //Camera Obj
    VimbaHandler cam_handler(camera_id_vec, mqtt_client, &act_info, start_demo);
    cam_handler.SetTowerName(fwt_name);
    //init Gimbal
    if (init_gimbal) {
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
        serial.sendCommand("r");
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
        serial.sendCommand("e");
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
        serial.sendCommand("q");
        std::this_thread::sleep_for(std::chrono::milliseconds(60000));
        act_info = serial.get_controller_info();
        //while (act_info.is_moving==0) {
        //    std::this_thread::sleep_for(std::chrono::milliseconds(500));
        //    act_info = serial.get_controller_info();
        //    if (act_info.is_moving == 1) {
        //        std::this_thread::sleep_for(std::chrono::milliseconds(2000));
        //        act_info = serial.get_controller_info();
        //    }
        //}
        serial.sendCommand("y");
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
        serial.sendCommand("ud56"); // set number of imtervals per turn
        std::cout << "Gimbal init finished." << std::endl;
    }   
    //loop
    while (running) {
        //check parser and evaluate commands
        parser_data cmd_data = parse_cmd.get_parser_data();
        CMD_eval eval;
        InputCommands cmds;

        cmds=eval.eval(parse_cmd);
        if (cmds != no_cmd) {
            if (cmds == startcamera) {
                std::cout << "starting Camera" << std::endl;
                cam_handler.Start();
            }
            else if (cmds == stopcamera) {
                std::cout << "stopping Camera" << std::endl;
                cam_handler.Stop();
            }
            else if (cmds == setcamera) {
                cam_handler.evaluateCommand(cmd_data.option, cmd_data.command_val);
            }
            else if (cmds == setmotorcontrol) {
                std::cout << "send command: " << cmd_data.option << std::endl;
                serial.sendCommand(cmd_data.option);
            }
            else if (cmds == setimagerate) {
                imagerate = cmd_data.command_val;
                std::cout << "imagerate is: " << imagerate << std::endl;
            }
            else if (cmds == setdebug) {
                motorctl_info_out = !motorctl_info_out;
                std::cout << "debug is: " << motorctl_info_out << std::endl;
            }
        }if (start_gimbal) {
            std::cout << "starting Camera" << std::endl;
            cam_handler.Start();
            start_gimbal = false;
        }
        act_info= serial.get_controller_info();
        if(motorctl_info_out)
            std::cout << "_________________________________\nEncoder act: "<< act_info.Xenc << "\nHDG: " << act_info.hdg << "\nEncoder err: " << act_info.Xerr << "\nsgt_stat/sgt_val/not_moving: " << act_info.sgt_stat <<" / "<<act_info.sgt_val << " / " << act_info.is_moving << "\nDriver Status: " << act_info.control_status << "\nDeviation Warning: " << act_info.deviation_warn << "\ntemp/humid/fan_pwm(0-255): " << act_info.temp << " / " << act_info.humid << " / " << act_info.fan_pwm << std::endl;
        
        //mqtt
        mqtt_in = mqtt_client->callback.get_sub_data();
        if (mqtt_in.ctl_avail) {
            ctl_code = mqtt_in.control_code;
            mqtt_client->publish(mqtt_STATUS, std::to_string(ctl_code));
        }
        if (mqtt_in.hdg_avail) {
            mqtt_hdg = mqtt_in.target_heading;
        }
        //gimbal control
        if (ctl_code == 0) {
            if (loop_timer.ElapsedMillis() > (1000 / imagerate) && cam_handler.cam_started) {
                if (act_info.is_moving == 1) {
                    loop_timer.Reset();
                    trigger_after_stopping = true;
                    serial.sendCommand("p");
                    //if (cam_handler.TriggerCamera()) {
                    //}
                    //wait for stop and trigger
                }
            }
            if (trigger_after_stopping && loop_timer.ElapsedMillis() > 100) {
                if (act_info.is_moving == 1) {
                    if (cam_handler.TriggerCamera()) {
                        std::cout << "Temperature:" << act_info.temp << ", Humid:" << act_info.humid << ", FAN RPM:" << act_info.fan_pwm << std::endl;
                        trigger_after_stopping = false;
                    }
                }
            }
        }
        else if (ctl_code == 1) {

            if (loop_timer.ElapsedMillis() > (1000 / imagerate)&& cam_handler.cam_started) {
                if (act_info.is_moving == 1) {
                    loop_timer.Reset();
                    trigger_after_stopping = true;
                    serial.sendCommand("kd"+mqtt_hdg);
                    std::cout << "ctl" << ctl_code <<  std::endl;
                    std::cout << "hdg" << mqtt_hdg << std::endl;
                    //if (cam_handler.TriggerCamera()) {
                    //}
                    //wait for stop and trigger
                }
            }
            if (trigger_after_stopping && loop_timer.ElapsedMillis() > 100) {
                if (act_info.is_moving == 1) {
                    if (cam_handler.TriggerCamera()) {
                        trigger_after_stopping = false;
                    }
                }
            }
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
    //stop serial thread
    serial.stop();
    io_thread.join();
    //stop cin thread
    inputThread.join();
    // Clean up
    delete mqtt_client;
    mqtt_client = nullptr;
    return 0;
}