#include <atomic>
#include <chrono>
#include <iostream>
#include <string>
#include <thread>

#include <boost/program_options.hpp>
#include <VmbC/VmbC.h>

#include "Camera.h"
#include "MQTT.h"
#include "Parser.h"
#include "timing.h"

namespace po = boost::program_options;

/* ------------------------------------------------------------------ */
/*  Application State                                                 */
/* ------------------------------------------------------------------ */

struct AppState {
    std::atomic<bool> running{true};

    bool motorctl_info_out{false};
    bool init_gimbal{false};
    bool start_gimbal{false};
    bool trigger_after_stopping{false};

    double imagerate{0.1};
    int ctl_code{0};
    std::string mqtt_hdg{"0"};

    std::string fwt_name{"Dev"};
    Parser parse_cmd;
    Timer loop_timer;
};

static AppState g_state;

/* ------------------------------------------------------------------ */
/*  Helpers                                                           */
/* ------------------------------------------------------------------ */

static void readInput() {
    std::string input;
    while (g_state.running) {
        std::getline(std::cin, input);
        if (input == "exit") {
            g_state.running = false;
            break;
        }
        std::cout << "Received input: " << input << "\n";
        g_state.parse_cmd.parse_input(input);
    }
}

static void parseCommandLine(int argc, char* argv[], bool& init, bool& start) {
    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");

    po::variables_map vm;
    try {
        po::store(po::parse_command_line(argc, argv, desc), vm);
        po::notify(vm);

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

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

        if (vm.count("start")) {
            start = vm["start"].as<bool>();
            std::cout << "starting gimbal: " << start << "\n";
        } else {
            std::cout << "Not starting Gimbal automatically.\n";
        }
    } catch (const std::exception& e) {
        std::cerr << "ARGS Error: " << e.what() << "\n";
        std::exit(1);
    } catch (...) {
        std::cerr << "Unknown ARGS error!\n";
        std::exit(1);
    }
}

static void initGimbal(SerialPort& serial) {
    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));
    serial.get_controller_info();
    serial.sendCommand("y");
    std::this_thread::sleep_for(std::chrono::milliseconds(500));
    serial.sendCommand("ud80");
    std::cout << "Gimbal init finished." << std::endl;
}

static void printMotorInfo(const motor_info& info) {
    std::cout << "_________________________________\n"
              << "Encoder act: " << info.Xenc << "\n"
              << "HDG: " << info.hdg << "\n"
              << "Encoder err: " << info.Xerr << "\n"
              << "sgt_stat/sgt_val/not_moving: " << info.sgt_stat
              << " / " << info.sgt_val << " / " << info.is_moving << "\n"
              << "Driver Status: " << info.control_status << "\n"
              << "Deviation Warning: " << info.deviation_warn << "\n"
              << "temp/humid/fan_pwm(0-255): " << info.temp
              << " / " << info.humid << " / " << info.fan_pwm << "\n";
}

static void processCommands(SerialPort& serial, VimbaHandler& cam) {
    parser_data cmd_data = g_state.parse_cmd.get_parser_data();
    CMD_eval eval;
    InputCommands cmds = eval.eval(g_state.parse_cmd);

    if (cmds == no_cmd) return;

    switch (cmds) {
        case startcamera:
            std::cout << "starting Camera" << std::endl;
            cam.Start();
            break;
        case stopcamera:
            std::cout << "stopping Camera" << std::endl;
            cam.Stop();
            break;
        case setcamera:
            cam.evaluateCommand(cmd_data.option, cmd_data.command_val);
            break;
        case setmotorcontrol:
            std::cout << "send command: " << cmd_data.option << std::endl;
            serial.sendCommand(cmd_data.option);
            break;
        case setimagerate:
            g_state.imagerate = cmd_data.command_val;
            std::cout << "imagerate is: " << g_state.imagerate << std::endl;
            break;
        case setdebug:
            g_state.motorctl_info_out = !g_state.motorctl_info_out;
            std::cout << "debug is: " << g_state.motorctl_info_out << std::endl;
            break;
        default:
            break;
    }
}

static void handleCameraTrigger(
    SerialPort& serial,
    const motor_info& act_info,
    VimbaHandler& cam)
{
    if (!cam.cam_started) return;

    const double interval_ms = 1000.0 / g_state.imagerate;

    if (g_state.ctl_code == 0) {
        if (g_state.loop_timer.ElapsedMillis() > interval_ms && act_info.is_moving == 1) {
            g_state.loop_timer.Reset();
            g_state.trigger_after_stopping = true;
            serial.sendCommand("p");
        }
        if (g_state.trigger_after_stopping && g_state.loop_timer.ElapsedMillis() > 100 && act_info.is_moving == 1) {
            if (cam.TriggerCamera()) {
                std::cout << "trigger camera" << std::endl;
                g_state.trigger_after_stopping = false;
            }
        }
    } else if (g_state.ctl_code == 1) {
        if (g_state.loop_timer.ElapsedMillis() > interval_ms && act_info.is_moving == 1) {
            g_state.loop_timer.Reset();
            g_state.trigger_after_stopping = true;
            serial.sendCommand("kd" + g_state.mqtt_hdg);
            std::cout << "ctl " << g_state.ctl_code << std::endl;
            std::cout << "hdg " << g_state.mqtt_hdg << std::endl;
        }
        if (g_state.trigger_after_stopping && g_state.loop_timer.ElapsedMillis() > 100 && act_info.is_moving == 1) {
            if (cam.TriggerCamera()) {
                std::cout << "trigger camera" << std::endl;
                g_state.trigger_after_stopping = false;
            }
        }
    }
}

/* ------------------------------------------------------------------ */
/*  Main                                                              */
/* ------------------------------------------------------------------ */

int main(int argc, char* argv[]) {
    /* ---- Parse CLI arguments ---- */
    bool init = false, start = false;
    parseCommandLine(argc, argv, init, start);
    g_state.init_gimbal  = init;
    g_state.start_gimbal = start;

    /* ---- Serial communication ---- */
    SerialPort serial("/dev/ttyACM0", 115200);
    std::thread io_thread;
    try {
        serial.startReading();
        io_thread = std::thread([&serial]() { serial.run(); });
    } catch (const std::exception& e) {
        std::cerr << "Serial Exception: " << e.what() << std::endl;
    }

    /* ---- MQTT connection ---- */
    MQTTClient mqtt_client("172.16.42.1", g_state.fwt_name, g_state.fwt_name);
    const std::string mqtt_STATUS = "GGS/FWT/" + g_state.fwt_name + "/StatusCode";

    mqtt_client.connect_client();
    while (!mqtt_client.connected) {
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
    }
    mqtt_client.publish(mqtt_STATUS, "0");

    /* ---- Input thread ---- */
    std::thread inputThread(readInput);

    /* ---- Camera handler ---- */
    motor_info act_info;
    VimbaHandler cam_handler(NULL, &mqtt_client, &act_info);

    /* ---- Gimbal initialization ---- */
    if (g_state.init_gimbal) {
        initGimbal(serial);
    }

    /* ---- Main loop ---- */
    while (g_state.running) {
        /* Commands from stdin */
        processCommands(serial, cam_handler);

        /* Auto-start camera if requested */
        if (g_state.start_gimbal) {
            std::cout << "starting Camera" << std::endl;
            cam_handler.Start();
            g_state.start_gimbal = false;
        }

        /* Motor controller info */
        act_info = serial.get_controller_info();
        if (g_state.motorctl_info_out) {
            printMotorInfo(act_info);
        }

        /* MQTT inbound messages */
        mqtt_sub_data mqtt_in = mqtt_client.callback.get_sub_data();
        if (mqtt_in.ctl_avail) {
            g_state.ctl_code = mqtt_in.control_code;
            mqtt_client.publish(mqtt_STATUS, std::to_string(g_state.ctl_code));
        }
        if (mqtt_in.hdg_avail) {
            g_state.mqtt_hdg = mqtt_in.target_heading;
        }

        /* Gimbal control and camera trigger */
        handleCameraTrigger(serial, act_info, cam_handler);

        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }

    /* ---- Cleanup ---- */
    serial.stop();
    io_thread.join();
    inputThread.join();

    return 0;
}