#include <iostream>
#include <fstream>
#include <opencv2/opencv.hpp>
#include <string>
#include <csignal>
#include <atomic>
#include <chrono>
#include <thread>

#include "global.h"
#include "camera.h"
#include "control.h"

std::atomic<bool> running(true);
void signalHandler(int signal)
{
    running.store(false);
}

int main(void)
{
    std::signal(SIGINT, signalHandler);

    double dest_fps = readDoubleFromFile(destfps_file);
    if (dest_fps <= 0) dest_fps = 30.0;

    cfg_load_all();

    double avoid_gain_val  = readDoubleFromFile(cone_avoid_gain_file);
    int    avoid_range_val = (int)readDoubleFromFile(cone_avoid_range_file);
    int    hold_frames_val = (int)readDoubleFromFile(cone_hold_frames_file);
    if (avoid_gain_val > 0)        g_cfg.cone_avoid_gain  = avoid_gain_val;
    if (avoid_range_val > 0)       g_cfg.cone_avoid_range = avoid_range_val;
    if (hold_frames_val > 0)       g_cfg.cone_hold_frames  = hold_frames_val;

    if (CameraInit(0, dest_fps, 320, 240) < 0) {
        std::cerr << "CameraInit failed" << std::endl;
        return -1;
    }
    ControlInit();
    std::cout << "All services started" << std::endl;

    int tick = 0;
    while (running.load())
    {
        CameraHandler();

        if (++tick % 7 == 0) {
            g_cfg.debug = readFlag(debug_file);
        }

        if (g_cfg.debug) {
            cfg_load_all();
        }

        target_speed = g_cfg.speed;
    }

    std::cout << "Stopping..." << std::endl;
    std::this_thread::sleep_for(std::chrono::milliseconds(500));
    ControlExit();
    cameraDeInit();
    std::cout << "Stopped." << std::endl;
    return 0;
}