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

#include "global.h"
#include "camera.h"
#include "control.h"
#include "Timer.h"
#include "serial.h"
#include "encoder.h"
#include "video.h"

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

int main(void)
{
    // Video video("test.mp4", 30);

    // // 获取帧缓冲区设备信息
    // int fb = open("/dev/fb0", O_RDWR);
    // if (fb == -1)
    // {
    //     std::cerr << "无法打开帧缓冲区设备" << std::endl;
    //     return -1;
    // }
    // struct fb_var_screeninfo vinfo;
    // ioctl(fb, FBIOGET_VSCREENINFO, &vinfo);

    // // 设置屏幕参数
    // int screenWidth = 160;
    // int screenHeight = 128;

    // int newWidth, newHeight;

    // // 创建帧缓冲区
    // uint16_t *fb_buffer = new uint16_t[screenWidth * screenHeight];

    // int frameCount = 0;

    // cv::Mat frame, resizedFrame, fbImage(screenHeight, screenWidth, CV_8UC3, cv::Scalar(0, 0, 0)); // 帧缓冲区图像

    // int frame_count = 0;

    // video.timer.start();

    // while(1) {
    //     video.frameMutex.lock();
    //     if(newWidth == 0)
    //     {
    //         // 保持视频长宽比
    //         int videoWidth = video.frame.cols;
    //         int videoHeight = video.frame.rows;
    //         double aspectRatio = static_cast<double>(videoWidth) / videoHeight;
    //         // 根据屏幕大小计算缩放后的宽度和高度
    //         if (screenWidth / static_cast<double>(screenHeight) > aspectRatio)
    //         {
    //             // 屏幕更宽，以高度为基准
    //             newHeight = screenHeight;
    //             newWidth = static_cast<int>(newHeight * aspectRatio);
    //         }
    //         else
    //         {
    //             // 屏幕更高，以宽度为基准
    //             newWidth = screenWidth;
    //             newHeight = static_cast<int>(newWidth / aspectRatio);
    //         }
    //     }

    //     // 缩放视频到新尺寸
    //     cv::resize(video.frame, resizedFrame, cv::Size(newWidth, newHeight));
    //     video.frameMutex.unlock();

    //     // 将缩放后的图像居中放置在帧缓冲区图像中，填充黑色边框
    //     fbImage.setTo(cv::Scalar(0, 0, 0)); // 清空缓冲区（填充黑色）
    //     cv::Rect roi((screenWidth - newWidth) / 2, (screenHeight - newHeight) / 2, newWidth, newHeight);
    //     resizedFrame.copyTo(fbImage(roi));

    //     // 将帧缓冲区图像转换为RGB565格式
    //     convertMatToRGB565(fbImage, fb_buffer, screenWidth, screenHeight);

    //     // 写入帧缓冲区
    //     lseek(fb, 0, SEEK_SET);
    //     write(fb, fb_buffer, screenWidth * screenHeight * 2);
    //     std::this_thread::sleep_for(std::chrono::milliseconds(30));
    // }
    std::signal(SIGINT, signalHandler);

    double dest_fps = readDoubleFromFile(destfps_file);
    int dest_frame_duration = CameraInit(0, dest_fps, 320, 240);
    printf("%d\n", dest_frame_duration);
    if (dest_frame_duration != -1)
    {
        streamCaptureRunning = true;
        std::thread camworker = std::thread(streamCapture);
        std::cout << "Stream Capture Service started!\n";
        ControlInit();
        std::cout << "Control Initialized!\n";

        Timer CameraTimer(dest_frame_duration, std::bind(CameraHandler));
        Timer MortorTimer(8, std::bind(ControlMain));
        CameraTimer.start();
        std::cout << "Camera Service started!\n";
        MortorTimer.start();
        std::cout << "Control Service started!\n";

        // 主循环，直到用户输入 Ctrl+C
        while (running.load())
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(500));
            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);
            // vofa_image(1, 160*120, 160, 120, Format_Grayscale8, (char*)IMG);
        }
        std::cout << "Stopping!\n";
        std::this_thread::sleep_for(std::chrono::milliseconds(500));

        CameraTimer.stop();
        std::cout << "Camera Timer stopped!\n";
        MortorTimer.stop();
        std::cout << "Control Timer stopped!\n";

        ControlExit();
        std::cout << "Control Service stopped!\n";

        streamCaptureRunning = false;
        if (camworker.joinable())
        {
            camworker.join();
        }

        cameraDeInit();
        std::cout << "Camera Service stopped!\n";
    }
    return 0;
}