#include <iostream>
#include <cstdlib>
#include <unistd.h>
#include <signal.h>
#include <pthread.h>

#include "Star_sensorHS.h"

/* ------------------ 全局变量 ------------------ */
extern Star_Device nano_stars[MAX_STAR_NUM_PER_TYPE];
extern Star_Device pico_stars[MAX_STAR_NUM_PER_TYPE];
extern uint8_t nano_star_count;
extern uint8_t pico_star_count;

/* ------------------ 信号处理 ------------------ */
static volatile bool running = true;

static void signal_handler(int sig) {
    running = false;
    std::cout << "[INFO] Received signal " << sig << ", shutting down..." << std::endl;
}

/* ------------------ 打印使用帮助 ------------------ */
static void print_usage(const char* program_name) {
    std::cout << "Usage: " << program_name << " [OPTIONS]" << std::endl;
    std::cout << "Options:" << std::endl;
    std::cout << "  -h, --help           Show this help message" << std::endl;
    std::cout << "  -v, --version        Show version information" << std::endl;
    std::cout << "  -i, --interval MS    Set publish interval in milliseconds (default: 500)" << std::endl;
    std::cout << std::endl;
    std::cout << "Environment variables:" << std::endl;
    std::cout << "  STAR_NANO_DEVS       Comma-separated list of nano star devices (e.g., /dev/ttyS0:115200,/dev/ttyS1:115200)" << std::endl;
    std::cout << "  STAR_PICO_DEVS       Comma-separated list of pico star devices" << std::endl;
}

/* ------------------ 主函数 ------------------ */
int main(int argc, char *argv[]) {
    // 解析命令行参数
    int publish_interval_ms = 500;
    
    for (int i = 1; i < argc; i++) {
        std::string arg = argv[i];
        if (arg == "-h" || arg == "--help") {
            print_usage(argv[0]);
            return 0;
        } else if (arg == "-v" || arg == "--version") {
            std::cout << "Star Sensor Hardware Service v1.0.0" << std::endl;
            return 0;
        } else if (arg == "-i" || arg == "--interval") {
            if (i + 1 < argc) {
                publish_interval_ms = std::atoi(argv[++i]);
                if (publish_interval_ms <= 0) {
                    std::cerr << "[ERROR] Invalid interval value: " << publish_interval_ms << std::endl;
                    return -1;
                }
            } else {
                std::cerr << "[ERROR] Missing interval value" << std::endl;
                print_usage(argv[0]);
                return -1;
            }
        } else {
            std::cerr << "[ERROR] Unknown option: " << arg << std::endl;
            print_usage(argv[0]);
            return -1;
        }
    }
    
    
    // 设置信号处理
    signal(SIGINT, signal_handler);
    signal(SIGTERM, signal_handler);
    signal(SIGPIPE, SIG_IGN);  // 忽略管道中断信号
    
    // 初始化星敏设备
    if (star_devices_init() != 0) {
        std::cerr << "[ERROR] No star sensors initialized" << std::endl;
        return -1;
    }
    std::cout << "[OK] Initialized " << (int)nano_star_count << " nano stars, "
              << (int)pico_star_count << " pico stars" << std::endl;
    
    // 初始化FastDDS硬件服务化
    if (Star_sensorHS_init(0, "Star_sensor_Hardware_Service") != 0) {
        std::cerr << "[ERROR] Failed to initialize FastDDS hardware service" << std::endl;
        return -1;
    }
    std::cout << "[OK] FastDDS hardware service initialized" << std::endl;
    
    // 创建发布线程
    pthread_t publish_tid;
    if (pthread_create(&publish_tid, NULL, star_publish_thread, NULL) != 0) {
        perror("[ERROR] Failed to create publish thread");
        return -1;
    }

    while (running) {
        sleep(1);
    }
    
    // 等待线程退出
    pthread_join(publish_tid, NULL);
    
    // 关闭所有串口
    for (int i = 0; i < MAX_STAR_NUM_PER_TYPE; i++) {
        if (nano_stars[i].fd > 0) {
            close(nano_stars[i].fd);
            nano_stars[i].fd = -1;
            std::cout << "[INFO] Closed nano star " << (i+1) << " UART" << std::endl;
        }
        if (pico_stars[i].fd > 0) {
            close(pico_stars[i].fd);
            pico_stars[i].fd = -1;
            std::cout << "[INFO] Closed pico star " << (i+1) << " UART" << std::endl;
        }
    }
    
    // 清理FastDDS资源
    Star_sensorHS_cleanup();
    return 0;
}