#include <iostream>
#include <opencv2/opencv.hpp>
#include <csignal>
#include <atomic>
#include <thread>
#include <chrono>
#include <sched.h>
#include <sys/resource.h>

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

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

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

    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;

    double lidar_gain_val  = readDoubleFromFile(lidar_avoid_gain_file);
    int    lidar_range_val = (int)readDoubleFromFile(lidar_avoid_range_file);
    int    lidar_hold_val  = (int)readDoubleFromFile(lidar_hold_frames_file);
    int    lidar_thresh_val = (int)readDoubleFromFile(lidar_thresh_file);
    if (lidar_gain_val > 0)   g_cfg.lidar_avoid_gain  = lidar_gain_val;
    if (lidar_range_val > 0)  g_cfg.lidar_avoid_range = lidar_range_val;
    if (lidar_hold_val > 0)   g_cfg.lidar_hold_frames = lidar_hold_val;
    if (lidar_thresh_val > 0) g_cfg.lidar_thresh      = lidar_thresh_val;

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

    setpriority(PRIO_PROCESS, 0, -10);

    while (running.load()) {
        CameraHandler();
        target_speed = g_cfg.speed;
        sched_yield();
    }

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