#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 "fgc/Config.h"
#include "fgc/Paths.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" };

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[])
{
    // Resolve the config file from a search order (no hardcoded paths):
    //   --config (added in a later phase) -> $FGC_CONFIG -> ./config.ini
    //   -> <exe dir>/config.ini -> $XDG_CONFIG_HOME/fire_gimbal_control/config.ini
    auto config_path = fgc::paths::resolveConfigPath();
    if (!config_path) {
        std::cerr << "No config.ini found. Searched:\n";
        for (const auto& p : fgc::paths::configSearchPaths())
            std::cerr << "  - " << p << "\n";
        std::cerr << "Copy config/config.example.ini to one of these locations "
                     "or set $FGC_CONFIG." << std::endl;
        return 1;
    }

    fgc::AppConfig cfg;
    try {
        cfg = fgc::ConfigLoader::loadFromFile(*config_path);
    } catch (const std::exception& e) {
        std::cerr << "Config error (" << *config_path << "): " << e.what() << std::endl;
        return 1;
    }

    // Bridge typed config into the existing globals/locals.
    camera_id_vec = cfg.camera.ids;
    fwt_name          = cfg.general.tower_name;
    imagerate         = cfg.image_rate();
    motorctl_info_out = cfg.general.debug;
    server_ip         = cfg.network.broker_ip;
    std::string mqtt_user = cfg.network.mqtt_user;
    std::string mqtt_pw   = cfg.network.mqtt_pw;

    std::cout << "Loaded config: " << *config_path << "\n";
    std::cout << camera_id_vec.size() << " camera(s) configured.\n";
    std::cout << "tower_name: " << fwt_name << "\n";
    std::cout << "image_interval: " << cfg.general.image_interval << " s\n";
    std::cout << "debug: " << cfg.general.debug << "\n";
    std::cout << "broker: " << 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(cfg.serial.device, cfg.serial.baud);
    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);
    cam_handler.SetOutputDir(cfg.paths.output_dir);
    //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;
}