#include "types.hpp"
#include "hal/framebuffer.hpp"
#include "vision/camera.hpp"
#include "scheduler.hpp"
#include "control/motor.hpp"
#include "control/servo.hpp"
#include "debug/config.hpp"
#include "debug/draw.hpp"

#include <csignal>
#include <atomic>
#include <cstdio>
#include <thread>

static std::atomic<bool> g_running{true};
static FrameBuffer g_fb;

static void signal_handler(int)
{
    g_running.store(false);
}

int main()
{
    std::signal(SIGINT,  signal_handler);
    std::signal(SIGTERM, signal_handler);
    setvbuf(stdout, NULL, _IONBF, 0);   // stdout 无缓冲, 日志实时落地

    printf("========================================\n");
    printf("  SmartCar Framework v0.1\n");
    printf("  龙芯 2K0300 智能车开发框架\n");
    printf("========================================\n\n");

    // 1. 显示屏
    printf("[INIT] 初始化显示屏...\n");
    if (g_fb.init(FB_DEVICE))
    {
        printf("[INIT] 显示屏: %dx%d\n", g_fb.width(), g_fb.height());
        dbg_draw_init(g_fb.buffer(), g_fb.width(), g_fb.height());
    }
    else
        printf("[INIT] 显示屏初始化失败, 继续无屏运行\n");

    // 2. 摄像头
    printf("[INIT] 初始化摄像头...\n");
    if (camera_init())
    {
        camera_stream_start();
        printf("[INIT] 摄像头: %dx%d MJPG\n", CAMERA_WIDTH, CAMERA_HEIGHT);
    }
    else
    {
        printf("[INIT] 摄像头初始化失败\n");
        return -1;
    }

    // 3. 调度器 (内部初始化电机/舵机/编码器/IMU)
    printf("[INIT] 初始化调度器...\n");
    if (!scheduler_init(&g_fb))
    {
        printf("[INIT] 调度器初始化失败\n");
        return -1;
    }

    // 4. 启动调度器
    scheduler_start();

    // 5. 主循环 — 低频监控
    printf("\n[READY] 系统运行中, Ctrl+C 停止\n");
    while (g_running.load())
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(500));

        // 打印状态
        static int last_print = 0;
        if (g_print_flag.exchange(0))
        {
            printf("[STATUS] tick=%d 车辆运行中\n", last_print++);
        }
    }

    printf("\n[EXIT] 正在安全停止...\n");
    scheduler_stop();
    camera_deinit();
    g_fb.release();
    printf("[EXIT] 系统已安全退出\n");

    return 0;
}