#include <iostream>
#include <csignal>
#include <cstdlib>
#include <string>

#include "FlywheelHS.h"

using namespace std;

// 全局变量用于信号处理
static bool running = true;

// 信号处理函数
void signal_handler(int signal) {
    running = false;
    cout << "[INFO] Received signal " << signal << ", shutting down..." << endl;
}

int main(int argc, char* argv[]) {
    // 设置信号处理
    signal(SIGINT, signal_handler);
    signal(SIGTERM, signal_handler);
    
    // 默认配置文件
    string config_file = "FlywheelHS.json";
    if (argc > 1) {
        config_file = argv[1];
    }
    
    cout << "[INFO] Starting Flywheel Hardware Service" << endl;
    cout << "[INFO] Loading configuration from: " << config_file << endl;
    
    // 解析配置文件
    const int MAX_CONFIGS = MAX_FLYWHEELS;
    FlywheelConfig flywheel_configs[MAX_CONFIGS];
    
    int config_count = parse_config_file(config_file.c_str(), flywheel_configs, MAX_CONFIGS);
    if (config_count <= 0) {
        cerr << "[ERROR] No valid flywheels configured" << endl;
        return -1;
    }
    
    cout << "[INFO] Found " << config_count << " flywheels in configuration" << endl;
    
    // 初始化飞轮设备
    flywheel_count = 0;
    for (int i = 0; i < config_count; i++) {
        FlywheelConfig config = flywheel_configs[i];
        
        // 验证ID范围
        if (config.id >= MAX_FLYWHEELS) {
            cerr << "[ERROR] Invalid flywheel ID: " << (int)config.id 
                 << ", must be less than " << MAX_FLYWHEELS << endl;
            continue;
        }
        
        // 调用初始化函数
        int result = flywheel_uart_init(config.id, config.device.c_str(), 
                                        config.baudrate, config.hardware_id);
        
        if (result == SAT_ERR_OK) {
            cout << "[OK] Flywheel " << (int)config.id << " initialized successfully" << endl;
            flywheel_count++;
            
            // 初始化缓存
            flywheels[config.id].id = config.id;
            flywheels[config.id].hardware_id = config.hardware_id;
            flywheels[config.id].is_data_valid = 0;
            
            // 初始化指令缓存
            cmd_cache[config.id].has_cmd = 0;
        } else {
            cerr << "[ERROR] Failed to initialize flywheel " << (int)config.id 
                 << ": " << flywheel_strerror(result) << endl;
        }
    }
    
    if (flywheel_count == 0) {
        cerr << "[ERROR] No flywheels initialized successfully" << endl;
        return -1;
    }
    
    // 初始化FastDDS
    Fastdds_init(0, "Flywheel_Hardware_Service");
    
    
    // 主循环
    while (running) {
        // 检查并接收串口数据（非阻塞）
        int frames = check_and_receive_serial_data();

        usleep(10000);  // 10ms
    }
    
    // 清理
    FlywheelHS_cleanup();
    
    return 0;
}