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

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

    int tick = 0;
    while (running.load())
    {
        if (CameraHandler() < 0) {
            std::this_thread::sleep_for(std::chrono::milliseconds(5));
            continue;
        }

        if (++tick % 15 == 0)
        {
            target_speed = readDoubleFromFile(speed_file);
            mortor_kp   = readDoubleFromFile(mortor_kp_file);
            mortor_ki   = readDoubleFromFile(mortor_ki_file);
            mortor_kd   = readDoubleFromFile(mortor_kd_file);
            kp          = readDoubleFromFile(kp_file);
            ki          = readDoubleFromFile(ki_file);
            kd          = readDoubleFromFile(kd_file);
        }
    }

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