#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;

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

    while (running.load())
    {
        auto t_start = std::chrono::steady_clock::now();
        CameraHandler();
        target_speed = g_cfg.speed;

        auto elapsed = std::chrono::steady_clock::now() - t_start;
        auto remain = std::chrono::milliseconds(frame_delay) - elapsed;
        if (remain > std::chrono::milliseconds(1))
            std::this_thread::sleep_for(remain);
    }

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