#include "CanHS.h"
#include <iostream>
#include <string>
#include <cstring>
#include <chrono>
#include <thread>
#include <signal.h>
#include <stdlib.h>

static volatile bool g_running = true;

/* 信号处理函数 */
static void signal_handler(int signum) {
    std::cout << "[INFO] Interrupt signal (" << signum << ") received." << std::endl;
    g_running = false;
}

/* 打印统计数据 */
static void print_stats(void) {
    const CanStats* stats = can_service_get_stats();
    if (!stats) return;
    
    std::cout << "[STATS] TelemetryReq: " << (int)stats->rec_normal_telemetry 
              << ", TelemetryCtrl: " << (int)stats->rec_telemetry_control << std::endl;
}

/* 获取CAN设备路径 */
static std::string get_can_device(int argc, char* argv[]) {
    /* 1. 命令行参数优先 */
    if (argc > 1) {
        return argv[1];
    }
    
    /* 2. 环境变量次之 */
    const char* env_can = getenv("CAN_DEV");
    if (env_can) {
        return env_can;
    }
    
    /* 3. 使用默认值 */
    return "/dev/canfd0";
}

int main(int argc, char* argv[]) {
    std::cout << "[INFO] CAN Hardware Service Starting..." << std::endl;
    
    /* 注册信号处理 */
    signal(SIGINT, signal_handler);
    signal(SIGTERM, signal_handler);
    
    /* 获取CAN设备路径 */
    std::string can_device = get_can_device(argc, argv);
    std::cout << "[CONFIG] Using CAN device: " << can_device << std::endl;
    
    /* 初始化CAN服务 */
    if (!can_service_init(can_device.c_str())) {
        std::cerr << "[ERROR] Failed to initialize CAN service" << std::endl;
        return -1;
    }
    
    /* 初始化FastDDS */
    CanHS_init(0, "Can_Hardware_Service");
    
    /* 启动CAN服务 */
    can_service_start();
    
    std::cout << "[INFO] CAN Service started successfully" << std::endl;
    std::cout << "[INFO] Press Ctrl+C to stop" << std::endl;
    
    /* 主循环 */
    int counter = 0;
    while (g_running) {
        std::this_thread::sleep_for(std::chrono::seconds(1));
        
        if (++counter % 30 == 0) {  // 每30秒打印一次
            std::cout << "[INFO] Service running..." << std::endl;
            print_stats();
        }
    }
    
    /* 清理资源 */
    std::cout << "[INFO] Shutting down CAN Service..." << std::endl;
    can_service_stop();
    can_service_destroy();
    
    std::cout << "[INFO] CAN Service terminated successfully" << std::endl;
    return 0;
}