#include <iostream>
#include <cstring>
#include <cstdlib>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <pthread.h>
#include <sys/ioctl.h>
#include <sys/time.h>
#include <sys/select.h>
#include <errno.h>
#include <signal.h>
#include <chrono>
#include <arpa/inet.h>
#include <string>
#include <vector>
#include <fstream>
#include <jsoncpp/json/json.h> 

#include "SimMsg.h"
#include "Star_sensorHS.h"

using namespace std;

/* ------------------ 全局变量 ------------------ */
// FastDDS硬件服务化相关
SimMsg* StarHS_part = nullptr;
string servername = "Star_sensor_Hardware_Service";
string topic_name_cmd = "Command";
string topic_name_tlm = "Telemetry";

// 星敏设备相关
Star_Device nano_stars[MAX_STAR_NUM_PER_TYPE] = {0};
Star_Device pico_stars[MAX_STAR_NUM_PER_TYPE] = {0};
uint8_t nano_star_count = 0;
uint8_t pico_star_count = 0;

// 全局UTC时间变量
static uint8_t UTC_TIME_SET[5] = {0};

// 电星模数据结构体
static CMD_ESD_S CMD_ESD_S_DATA = {0};

// 互斥锁
static pthread_mutex_t cache_mutex = PTHREAD_MUTEX_INITIALIZER;

/* ------------------ 解析json配置文件 ------------------ */
int parse_config_file(const char* filename, StarConfig star_configs[], int max_stars) {
    ifstream config_file(filename);
    if (!config_file.is_open()) {
        cerr << "[ERROR] Cannot open config file: " << filename << endl;
        return -1;
    }
    
    // 解析JSON
    Json::CharReaderBuilder reader_builder;
    Json::Value root;
    string errs;
    
    if (!Json::parseFromStream(reader_builder, config_file, &root, &errs)) {
        cerr << "[ERROR] Failed to parse JSON: " << errs << endl;
        return -1;
    }
    
    // 读取星敏配置
    int star_count = 0;
    
    if (root.isMember("star_sensors") && root["star_sensors"].isArray()) {
        const Json::Value& stars = root["star_sensors"];
        
        for (unsigned int i = 0; i < stars.size() && star_count < max_stars; i++) {
            const Json::Value& star = stars[i];
            
            // 检查是否启用
            if (star.isMember("enabled") && !star["enabled"].asBool()) {
                continue;  // 跳过禁用的星敏
            }
            
            // 读取配置参数
            StarConfig config;
            
            // 星敏类型
            string type_str = star.isMember("type") ? star["type"].asString() : "nano";
            if (type_str == "nano") {
                config.type = STAR_TYPE_NANO;
            } else if (type_str == "pico") {
                config.type = STAR_TYPE_PICO;
            } else {
                cerr << "[WARNING] Unknown star type: " << type_str << ", defaulting to nano" << endl;
                config.type = STAR_TYPE_NANO;
            }
            
            // 串口设备
            config.device = star.isMember("device") ? star["device"].asString() : "";
            if (config.device.empty()) {
                cerr << "[WARNING] Star sensor " << i << " has no device specified, skipping" << endl;
                continue;
            }
            
            // 波特率
            config.baudrate = star.isMember("baudrate") ? star["baudrate"].asInt() : 115200;
            
            // 星敏编号
            config.star_num = star.isMember("star_num") ? star["star_num"].asInt() : (star_count + 1);
            
            config.enabled = true;
            star_configs[star_count] = config;
            
            
            star_count++;
        }
    } else {
        cerr << "[ERROR] No 'star_sensors' array found in config file" << endl;
        return -1;
    }
    
    return star_count;
}

/* ------------------ 日志函数 ------------------ */
void StarHSWriteLog(const std::string &msg)
{
    std::cout << "[StarHS] " << msg << std::endl;
}

/* ------------------ FastDDS初始化 ------------------ */
void Fastdds_init(uint8_t domainid, string appname)
{

    vector<string> parameters;
    string expression = "dest = '"+ servername + "'";
    
    if (nullptr == StarHS_part)
    {
        StarHS_part = new SimMsg(domainid, 3000, appname, StarHSWriteLog);
        // 创建发布者和订阅者
        StarHS_part->create_pub(topic_name_tlm);
        
        // 注册回调函数
        StarHS_part->create_sub(topic_name_cmd, command_callback, expression, parameters);
        StarHS_part->create_sub(topic_name_tlm, telemetry_callback, expression, parameters);
        
    }
}
    
/* ------------------ 命令回调函数 ------------------ */
void command_callback(string src, string dest, string type,
                            string reserve1, string reserve2,
                            vector<uint8_t>& data) {
    cout << "[INFO] Command received from " << src << " to " << dest << endl;
    
    // 解析命令数据
    if (data.size() >= 4) {
        uint8_t star_type = data[0];
        uint8_t star_num = data[1];
        uint8_t cmd = data[2];
        uint8_t param = data[3];
        
        cout << "[INFO] Executing: star_type=" << (star_type == 0 ? "NANO" : "PICO")
                  << ", num=" << (int)star_num << ", cmd=" << (int)cmd << ", param=" << (int)param << endl;
        
        // 加锁保护串口访问
        pthread_mutex_lock(&cache_mutex);
        
        // 执行命令
        int result = send_star_cmd((star_type_t)star_type, star_num, (enum SENSOR_Cmder)cmd, param);
        
        pthread_mutex_unlock(&cache_mutex);
        
        if (result != SAT_ERR_OK) {
            cerr << "[ERROR] Send command failed: " << result << endl;
        } else {
            cout << "[OK] Command executed successfully" << endl;
        }
    } else {
        cerr << "[ERROR] Invalid command data size: " << data.size() << endl;
    }
}

/* ------------------ 遥测回调函数 ------------------ */
void telemetry_callback(string src, string dest, string type,
                              string reserve1, string reserve2,
                              vector<uint8_t>& data) {
    
    // 解析请求：前2字节为star_type和star_num
    if (data.size() >= 2) {
        star_type_t star_type = (star_type_t)data[0];
        uint8_t star_num = data[1];
        enum SENSOR_Cmder cmd = (star_type == STAR_TYPE_NANO) ? CMD_TLM_1 : CMD_TLM_2;
        
        cout << "[INFO] Reading telemetry: star_type=" 
             << (star_type == STAR_TYPE_NANO ? "NANO" : "PICO")
             << ", num=" << (int)star_num << endl;
        
        // 加锁保护串口访问
        pthread_mutex_lock(&cache_mutex);
        
        // 读取数据
        Star_sensorHS_Frame response_frame;
        int result = read_single_star_data(star_type, star_num, cmd, &response_frame);
        
        pthread_mutex_unlock(&cache_mutex);
        
        if (result == SAT_ERR_OK) {
            // 发布遥测响应
            Star_sensorHS_telemetry_Pub((uint8_t *)&response_frame, src, sizeof(Star_sensorHS_Frame));
            cout << "[OK] Telemetry data sent to " << src << endl;
        } else {
            cerr << "[ERROR] Failed to read star data: " << result << endl;
            
            // 即使读取失败，也发送错误状态
            memset(&response_frame, 0, sizeof(Star_sensorHS_Frame));
            response_frame.on_off_status = 0; // 标记为错误状态
            Star_sensorHS_telemetry_Pub((uint8_t *)&response_frame, src, sizeof(Star_sensorHS_Frame));
        }
    } else {
        cerr << "[ERROR] Invalid telemetry request data size: " << data.size() << endl;
    }
}

/* ------------------ 遥测发布 ------------------ */
void Star_sensorHS_telemetry_Pub(uint8_t* data, const string& dest, uint16_t len) {
    StarHS_part->publish(topic_name_tlm, servername, dest, "telemetry", data, len);
}


/* ------------------ 星敏设备初始化 ------------------ */
int star_devices_init(void) {

    pthread_mutex_init(&cache_mutex, NULL);
    // 初始化纳型星敏
    char *nano_devs = getenv("STAR_NANO_DEVS");
    if (nano_devs) {
        char *token = strtok(nano_devs, ",");
        while (token && nano_star_count < MAX_STAR_NUM_PER_TYPE) {
            char dev[32];
            int baud;
            if (sscanf(token, "%[^:]:%d", dev, &baud) == 2) {
                if (star_uart_init(STAR_TYPE_NANO, nano_star_count + 1, dev, baud) == SAT_ERR_OK) {
                    nano_star_count++;
                }
            }
            token = strtok(NULL, ",");
        }
    } else {
        // 默认配置
        if (star_uart_init(STAR_TYPE_NANO, 1, "/dev/ttyS0", 115200) == SAT_ERR_OK) {
            nano_star_count = 1;
        }
    }

    // 初始化皮型星敏
    char *pico_devs = getenv("STAR_PICO_DEVS");
    if (pico_devs) {
        char *token = strtok(pico_devs, ",");
        while (token && pico_star_count < MAX_STAR_NUM_PER_TYPE) {
            char dev[32];
            int baud;
            if (sscanf(token, "%[^:]:%d", dev, &baud) == 2) {
                if (star_uart_init(STAR_TYPE_PICO, pico_star_count + 1, dev, baud) == SAT_ERR_OK) {
                    pico_star_count++;
                }
            }
            token = strtok(NULL, ",");
        }
    } else {
        // 默认配置
        if (star_uart_init(STAR_TYPE_PICO, 1, "/dev/ttyS1", 115200) == SAT_ERR_OK) {
            pico_star_count = 1;
        }
    }

    printf("[INFO] Initialized %d nano stars, %d pico stars\n", nano_star_count, pico_star_count);
    return (nano_star_count + pico_star_count) > 0 ? 0 : -1;
}



/* ------------------ 串口初始化 ------------------ */
int star_uart_init(star_type_t type, uint8_t num, const char *dev, int baudrate) {
    if (type >= STAR_TYPE_MAX || num == 0 || num > MAX_STAR_NUM_PER_TYPE) {
        fprintf(stderr, "[ERROR] Invalid star type=%d or num=%d\n", type, num);
        return SAT_ERR_DATA;
    }
    
    uint8_t idx = num - 1;
    Star_Device *star = (type == STAR_TYPE_NANO) ? &nano_stars[idx] : &pico_stars[idx];

    // 如果已经打开，先关闭
    if (star->fd > 0) {
        close(star->fd);
        star->fd = -1;
    }

    // 打开串口
    star->fd = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK);
    if (star->fd < 0) {
        perror("[ERROR] Open star serial failed");
        return SAT_ERR_SERIAL;
    }

    // 配置串口参数
    struct termios options;
    if (tcgetattr(star->fd, &options) < 0) {
        perror("[ERROR] Get serial attributes failed");
        close(star->fd);
        star->fd = -1;
        return SAT_ERR_SERIAL;
    }
    
    // 设置波特率
    speed_t baud = B115200;
    switch (baudrate) {
        case 9600: baud = B9600; break;
        case 19200: baud = B19200; break;
        case 38400: baud = B38400; break;
        case 57600: baud = B57600; break;
        case 115200: baud = B115200; break;
        case 230400: baud = B230400; break;
        case 460800: baud = B460800; break;
        case 921600: baud = B921600; break;
        default: baud = B115200;
    }
    cfsetispeed(&options, baud);
    cfsetospeed(&options, baud);
    
    // 8位数据位，无奇偶校验，1位停止位
    options.c_cflag &= ~PARENB;
    options.c_cflag &= ~CSTOPB;
    options.c_cflag &= ~CSIZE;
    options.c_cflag |= CS8;
    
    // 启用接收
    options.c_cflag |= CREAD | CLOCAL;
    
    // 禁用软件流控
    options.c_iflag &= ~(IXON | IXOFF | IXANY);
    
    // 原始模式
    options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
    options.c_oflag &= ~OPOST;
    
    // 设置超时
    options.c_cc[VMIN] = 0;
    options.c_cc[VTIME] = 10; // 1秒超时
    
    if (tcsetattr(star->fd, TCSANOW, &options) < 0) {
        perror("[ERROR] Set serial attributes failed");
        close(star->fd);
        star->fd = -1;
        return SAT_ERR_SERIAL;
    }

    // 刷新缓冲区
    tcflush(star->fd, TCIOFLUSH);

    // 初始化设备信息
    star->dev = dev;
    star->baudrate = baudrate;
    star->on_off_status = 1;
    
    printf("[OK] %s STAR%d UART init: dev=%s, baud=%d, fd=%d\n",
           type == STAR_TYPE_NANO ? "NANO" : "PICO", num, dev, baudrate, star->fd);
    
    return SAT_ERR_OK;
}

/* ------------------ 工具函数 ------------------ */
// 计算校验和
int checksum_verify(const uint8_t *data, int len) {
    if (len <= 1 || data == NULL) return SAT_ERR_DATA;
    uint8_t checksum = 0;
    for (int i = 0; i < len - 1; ++i) checksum += data[i];
    if (checksum == data[len - 1]) return SAT_ERR_OK;
    fprintf(stderr, "[ERROR] Checksum error: calculated=0x%02X, received=0x%02X\n", checksum, data[len - 1]);
    return SAT_ERR_CHECKSUM;
}

/* ------------------ 发送星敏命令 ------------------ */
int send_star_cmd(star_type_t type, uint8_t num, enum SENSOR_Cmder cmd, uint8_t param) {
    if (type >= STAR_TYPE_MAX || num == 0 || num > MAX_STAR_NUM_PER_TYPE) return SAT_ERR_DATA;
    
    uint8_t idx = num - 1;
    Star_Device *star = (type == STAR_TYPE_NANO) ? &nano_stars[idx] : &pico_stars[idx];
    if (star->fd < 0) {
        fprintf(stderr, "[ERROR] Serial not open for %s STAR%d\n",
                type == STAR_TYPE_NANO ? "NANO" : "PICO", num);
        return SAT_ERR_SERIAL;
    }

    uint8_t send_cmd[260] = {0};
    int cmd_len = 5;
    uint8_t i = 0;

    switch (cmd) {
        case CMD_MCT_OST:   // 光信号激励
            send_cmd[0] = 0X74; 
            send_cmd[1] = 0XA0; 
            send_cmd[2] = 0X00; 
            send_cmd[3] = 0X00; 
            send_cmd[4] = 0X14;
            break;

        case CMD_MCT_ESI:   // 电信号激励
            send_cmd[0] = 0X74; 
            send_cmd[1] = 0XA0; 
            send_cmd[2] = 0X00; 
            send_cmd[3] = 0X03; 
            send_cmd[4] = 0X17;
            break;

        case CMD_SAT:   // UTC时间（40bit，量纲ms）
            send_cmd[0] = 0X74; 
            send_cmd[1] = 0XA3;
            memcpy(send_cmd + 2, UTC_TIME_SET, 5);  // 从全局变量读取时间
            send_cmd[7] = 0;
            for (i = 0; i < 7; i++) send_cmd[7] += send_cmd[i];
            cmd_len = 8;
            break;

        case CMD_WM_NORM:   // 正常工作流程
            send_cmd[0] = 0X74; 
            send_cmd[1] = 0XA4; 
            send_cmd[2] = 0X01; 
            send_cmd[3] = 0X01; 
            send_cmd[4] = 0X1A;
            break;

        case CMD_WM_ALSKY:  // 仅工作于全天识别模式
            send_cmd[0] = 0X74; 
            send_cmd[1] = 0XA4; 
            send_cmd[2] = 0X02; 
            send_cmd[3] = 0X02; 
            send_cmd[4] = 0X1C;
            break;

        case CMD_WM_TST:    // 测试模式，用于标定
            send_cmd[0] = 0X74; 
            send_cmd[1] = 0XA4; 
            send_cmd[2] = 0X04; 
            send_cmd[3] = 0X04; 
            send_cmd[4] = 0X20;
            break;

        case CMD_WM_DLD:    // 星图下传
            send_cmd[0] = 0X74; 
            send_cmd[1] = 0XA4; 
            send_cmd[2] = 0X08; 
            send_cmd[3] = 0X08; 
            send_cmd[4] = 0X28;
            break;

        case CMD_WM_IDLE:   // 待机模式
            send_cmd[0] = 0X74; 
            send_cmd[1] = 0XA4; 
            send_cmd[2] = 0XF0; 
            send_cmd[3] = 0XF0; 
            send_cmd[4] = 0XF8;
            break;

        case CMD_WM_HK:    // 至启动模式进行在轨维护
            send_cmd[0] = 0X74; 
            send_cmd[1] = 0XA4; 
            send_cmd[2] = 0X0A; 
            send_cmd[3] = 0X0A; 
            send_cmd[4] = 0X2C;
            break;

        case CMD_WM_RST:    // 软件复位重启
            send_cmd[0] = 0X74; 
            send_cmd[1] = 0XA4; 
            send_cmd[2] = 0XFF; 
            send_cmd[3] = 0XFF; 
            send_cmd[4] = 0X16;
            break;

        case CMD_SEN_EXPO:  // 曝光时间设置
            send_cmd[0] = 0X74; 
            send_cmd[1] = 0XA9; 
            send_cmd[2] = 0X60; 
            send_cmd[3] = param;
            send_cmd[4] = 0;
            for (i = 0; i < 4; i++) send_cmd[4] += send_cmd[i];
            break;

        case CMD_SEN_AGAIN: // 模拟增益设置
            send_cmd[0] = 0X74; 
            send_cmd[1] = 0XA9; 
            send_cmd[2] = 0X30; 
            send_cmd[3] = param;
            send_cmd[4] = 0;
            for (i = 0; i < 4; i++) send_cmd[4] += send_cmd[i];
            break;

        case CMD_SEN_DGAIN: // 数字增益设置
            send_cmd[0] = 0X74; 
            send_cmd[1] = 0XA9; 
            send_cmd[2] = 0X90; 
            send_cmd[3] = param;
            send_cmd[4] = 0;
            for (i = 0; i < 4; i++) send_cmd[4] += send_cmd[i];
            break;

        case CMD_PAR_ADE:   // 设置星对角距匹配容差
            send_cmd[0] = 0X74; 
            send_cmd[1] = 0XAC; 
            send_cmd[2] = 0X03; 
            send_cmd[3] = param;
            send_cmd[4] = 0;
            for (i = 0; i < 4; i++) send_cmd[4] += send_cmd[i];
            break;

        case CMD_PAR_SSE:   // 设置单星匹配容差
            send_cmd[0] = 0X74; 
            send_cmd[1] = 0XAC; 
            send_cmd[2] = 0X05; 
            send_cmd[3] = param;
            send_cmd[4] = 0;
            for (i = 0; i < 4; i++) send_cmd[4] += send_cmd[i];
            break;

        case CMD_PAR_TKR:   // 星跟踪半径/二次迭代容差
            send_cmd[0] = 0X74; 
            send_cmd[1] = 0XAC; 
            send_cmd[2] = 0X06; 
            send_cmd[3] = param;
            send_cmd[4] = 0;
            for (i = 0; i < 4; i++) send_cmd[4] += send_cmd[i];
            break;

        case CMD_PAR_ADN:   // 设置定姿星数
            send_cmd[0] = 0X74; 
            send_cmd[1] = 0XAC; 
            send_cmd[2] = 0X09; 
            send_cmd[3] = param;
            send_cmd[4] = 0;
            for (i = 0; i < 4; i++) send_cmd[4] += send_cmd[i];
            break;

        case CMD_PAR_OFST:  // 波门星点提取阈值偏移量
            send_cmd[0] = 0X74; 
            send_cmd[1] = 0XAC; 
            send_cmd[2] = 0X0A; 
            send_cmd[3] = param;
            send_cmd[4] = 0;
            for (i = 0; i < 4; i++) send_cmd[4] += send_cmd[i];
            break;

        case CMD_PAR_OFST_IMG_0:    // 星图子块灰度均值0~5
            send_cmd[0] = 0X74; 
            send_cmd[1] = 0XAC; 
            send_cmd[2] = 0X0F; 
            send_cmd[3] = param;    // 默认为5
            send_cmd[4] = 0;
            for (i = 0; i < 4; i++) send_cmd[4] += send_cmd[i];
            break;

        case CMD_PAR_OFST_IMG_1:    // 星图子块灰度均值6~100
            send_cmd[0] = 0X74; 
            send_cmd[1] = 0XAC; 
            send_cmd[2] = 0X1F; 
            send_cmd[3] = param;    // 默认为10
            send_cmd[4] = 0;
            for (i = 0; i < 4; i++) send_cmd[4] += send_cmd[i];
            break;

        case CMD_PAR_OFST_IMG_2:    // 星图子块灰度均值101~150
            send_cmd[0] = 0X74; 
            send_cmd[1] = 0XAC; 
            send_cmd[2] = 0X2F; 
            send_cmd[3] = param;    // 默认为12
            send_cmd[4] = 0;
            for (i = 0; i < 4; i++) send_cmd[4] += send_cmd[i];
            break;

        case CMD_PAR_OFST_IMG_3:    // 星图子块灰度均值151~200
            send_cmd[0] = 0X74; 
            send_cmd[1] = 0XAC; 
            send_cmd[2] = 0X3F; 
            send_cmd[3] = param;    // 默认为15
            send_cmd[4] = 0;
            for (i = 0; i < 4; i++) send_cmd[4] += send_cmd[i];
            break;

        case CMD_PAR_OFST_IMG_4:    // 星图子块灰度均值201~255
            send_cmd[0] = 0X74; 
            send_cmd[1] = 0XAC; 
            send_cmd[2] = 0X4F; 
            send_cmd[3] = param;    // 默认为20
            send_cmd[4] = 0;
            for (i = 0; i < 4; i++) send_cmd[4] += send_cmd[i];
            break;

        case CMD_PAR_WIN:   // 波门图像大小
            send_cmd[0] = 0X74; send_cmd[1] = 0XAC; send_cmd[2] = 0X0E; send_cmd[3] = param;
            send_cmd[4] = 0;
            for (i = 0; i < 4; i++) send_cmd[4] += send_cmd[i];
            break;

        case CMD_PAR_RST:   // 软件参数恢复默认值
            send_cmd[0] = 0X74; send_cmd[1] = 0XAC; send_cmd[2] = 0X0B; send_cmd[3] = param;
            send_cmd[4] = 0;
            for (i = 0; i < 4; i++) send_cmd[4] += send_cmd[i];
            break;

        case CMD_ESD:
        {
            // 后续需修改
            CMD_ESD_S_DATA.FRAME_HEAD = htons(0x7456);
            CMD_ESD_S_DATA.FRAME_ID = htons(0x0001);
            memcpy(CMD_ESD_S_DATA.UTC, UTC_TIME_SET, 5);
            CMD_ESD_S_DATA.STAR_NUM_S[0] = 1;
            memset(&CMD_ESD_S_DATA.SS_SCP_X, 0, sizeof(CMD_ESD_S) - 8);
            
            uint8_t crc = 0;
            uint8_t *data_ptr = (uint8_t *)&CMD_ESD_S_DATA;
            for (uint32_t i = 0; i < sizeof(CMD_ESD_S) - 1; i++) {  // 排除最后1字节SUM_CRC
                crc += data_ptr[i];
            }
            CMD_ESD_S_DATA.SUM_CRC = crc;

            if (write(star->fd, &CMD_ESD_S_DATA, sizeof(CMD_ESD_S)) != sizeof(CMD_ESD_S)) {
                perror("[ERROR] Send ESD cmd failed");
                star->send_cmd_cnt++;
                return SAT_ERR_SEND_CMD;
            }
            star->send_cmd_cnt = 0;
            return SAT_ERR_OK;
        }

        case CMD_MCT_FLT_ON:    // 姿态滤波开
            send_cmd[0] = 0X74; 
            send_cmd[1] = 0XA0; 
            send_cmd[2] = 0X00; 
            send_cmd[3] = 0x05; 
            send_cmd[4] = 0x19;
            break;

        case CMD_MCT_FLT_OFF:   // 姿态滤波关
            send_cmd[0] = 0X74; 
            send_cmd[1] = 0XA0; 
            send_cmd[2] = 0X00; 
            send_cmd[3] = 0x07; 
            send_cmd[4] = 0x1B;
            break;

        case CMD_MCT_PPS_ON:    // PPS校时使能开
            send_cmd[0] = 0X74; 
            send_cmd[1] = 0XA0; 
            send_cmd[2] = 0X00; 
            send_cmd[3] = 0x0A; 
            send_cmd[4] = 0x1E;
            break;

        case CMD_MCT_PPS_OFF:   // PPS使能关
            send_cmd[0] = 0X74; 
            send_cmd[1] = 0XA0; 
            send_cmd[2] = 0X00; 
            send_cmd[3] = 0x0C; 
            send_cmd[4] = 0x20;
            break;

        case CMD_MCT_PPS_CNT_RST:   // PPS计数清零
            send_cmd[0] = 0X74; 
            send_cmd[1] = 0XA0; 
            send_cmd[2] = 0X00; 
            send_cmd[3] = 0x0E; 
            send_cmd[4] = 0x22;
            break;

        case CMD_TLM_1: // 读取纳型遥测数据包1（64字节）
            if (type != STAR_TYPE_NANO) return SAT_ERR_DATA;
            send_cmd[0] = 0X74; 
            send_cmd[1] = 0XA1; 
            send_cmd[2] = 0X00; 
            send_cmd[3] = 0x00; 
            send_cmd[4] = 0x15;
            break;

        case CMD_TLM_2: // 读取皮型遥测数据包2（256字节）
            if (type != STAR_TYPE_PICO) return SAT_ERR_DATA;
            send_cmd[0] = 0X74; 
            send_cmd[1] = 0XA1; 
            send_cmd[2] = 0X00; 
            send_cmd[3] = 0x05; 
            send_cmd[4] = 0x1A;
            break;

        default:
            fprintf(stderr, "[ERROR] Unsupported cmd=%d for %s STAR%d\n", cmd,
                    type == STAR_TYPE_NANO ? "NANO" : "PICO", num);
            return SAT_ERR_DATA;
    }

    if (write(star->fd, send_cmd, cmd_len) != cmd_len) {
        perror("[ERROR] Send star cmd failed");
        star->send_cmd_cnt++;
        return SAT_ERR_SEND_CMD;
    }

    star->send_cmd_cnt = 0;
    return SAT_ERR_OK;
}

/* ------------------ 读取单个星敏数据 ------------------ */
int read_single_star_data(star_type_t type, uint8_t num, enum SENSOR_Cmder cmd, Star_sensorHS_Frame *frame) {
    if (type >= STAR_TYPE_MAX || num == 0 || num > MAX_STAR_NUM_PER_TYPE || !frame) {
        return SAT_ERR_DATA;
    }
    
    uint8_t idx = num - 1;
    Star_Device *star = (type == STAR_TYPE_NANO) ? &nano_stars[idx] : &pico_stars[idx];
    if (star->fd < 0) {
        fprintf(stderr, "[ERROR] Serial not open for %s STAR%d\n",
                type == STAR_TYPE_NANO ? "NANO" : "PICO", num);
        return SAT_ERR_SERIAL;
    }

    int expected_bin_len = 0;
    if (cmd == CMD_TLM_1 && type == STAR_TYPE_NANO) {
        expected_bin_len = 64;
    } else if (cmd == CMD_TLM_2 && type == STAR_TYPE_PICO) {
        expected_bin_len = 256;
    } else {
        fprintf(stderr, "[ERROR] Unknown frame length for cmd=%d, type=%d\n", cmd, type);
        return SAT_ERR_DATA;
    }

    const int MAX_RETRY = 10;
    uint8_t buf[MAX_BUF_SIZE] = {0};

    for (int retry = 0; retry < MAX_RETRY; ++retry) {
        // 清空串口缓冲区
        tcflush(star->fd, TCIOFLUSH);
        memset(buf, 0, sizeof(buf));
        int total_read = 0;

        // 发送命令
        int send_ret = send_star_cmd(type, num, cmd, 0);
        if (send_ret != SAT_ERR_OK) {
            fprintf(stderr, "[ERROR] Retry %d: send cmd failed\n", retry);
            // 增加命令发送错误计数
            star->send_cmd_cnt++;
            continue;
        }

        // 设置超时
        struct timeval start_time, current_time;
        gettimeofday(&start_time, NULL);
        const long TOTAL_TIMEOUT_MS = 1000;

        // 读取数据
        while (total_read < expected_bin_len && total_read < MAX_BUF_SIZE) {
            gettimeofday(&current_time, NULL);
            long elapsed_ms = (current_time.tv_sec - start_time.tv_sec) * 1000 +
                              (current_time.tv_usec - start_time.tv_usec) / 1000;
            if (elapsed_ms > TOTAL_TIMEOUT_MS) {
                fprintf(stderr, "[ERROR] Timeout: read %d/%d bytes\n", total_read, expected_bin_len);
                star->send_cmd_cnt++;
                break;
            }

            // 使用select等待数据
            struct timeval timeout = {0, 100000};
            fd_set readfds;
            FD_ZERO(&readfds);
            FD_SET(star->fd, &readfds);
            
            int ret = select(star->fd + 1, &readfds, NULL, NULL, &timeout);
            if (ret < 0) {
                perror("[ERROR] Select failed");
                return SAT_ERR_SELECT;
            } else if (ret == 0) {
                continue;  // 超时，继续等待
            }

            // 读取数据
            if (FD_ISSET(star->fd, &readfds)) {
                int bytes = read(star->fd, buf + total_read, expected_bin_len - total_read);
                if (bytes > 0) {
                    total_read += bytes;
                } else if (bytes == 0) {
                    usleep(10000);  // 没有数据，等待10ms
                } else {
                    perror("[ERROR] Read star data failed");
                    return SAT_ERR_SERIAL;
                }
            }
        }

        // 检查是否超时过多
        if (star->send_cmd_cnt >= 10) {
            fprintf(stderr, "[ERROR] Too many timeouts, resetting %s STAR%d UART\n",
                    type == STAR_TYPE_NANO ? "NANO" : "PICO", num);
            star_uart_init(type, num, star->dev, star->baudrate);
            star->uart_reset_cnt++;
            star->send_cmd_cnt = 0;
            continue;
        }

        // 检查数据长度
        if (total_read < expected_bin_len) {
            fprintf(stderr, "[ERROR] Frame too short: %d/%d bytes\n", total_read, expected_bin_len);
            star->length_err_cnt++;
            update_fault_count(star);
            continue;
        }

        // 检查帧头
        int header_valid = 0;
        if (type == STAR_TYPE_NANO && cmd == CMD_TLM_1) {
            header_valid = (buf[0] == 0x8A && buf[1] == 0xA1);
        } else if (type == STAR_TYPE_PICO && cmd == CMD_TLM_2) {
            header_valid = (buf[0] == 0xEB && buf[1] == 0x50 && buf[2] == 0x22);
        }
        if (!header_valid) {
            fprintf(stderr, "[ERROR] Invalid frame header for %s STAR%d\n",
                    type == STAR_TYPE_NANO ? "NANO" : "PICO", num);
            star->header_err_cnt++;
            update_fault_count(star);
            continue;
        }

        // 检查校验和
        if (checksum_verify(buf, expected_bin_len) != SAT_ERR_OK) {
            star->check_err_cnt++;
            update_fault_count(star);
            continue;
        }

        // 解析数据
        int parse_ret = SAT_ERR_DATA;
        if (type == STAR_TYPE_NANO) {
            parse_ret = parse_nano_quaternion(buf, frame);
        } else if (type == STAR_TYPE_PICO) {
            parse_ret = parse_pico_quaternion(buf, frame);
        }
        
        if (parse_ret == SAT_ERR_OK) {
            // 填充设备状态
            frame->on_off_status = star->on_off_status;
            
            // 填充错误计数信息
            frame->header_err_cnt = star->header_err_cnt;
            frame->check_err_cnt = star->check_err_cnt;
            frame->length_err_cnt = star->length_err_cnt;
            frame->uart_reset_cnt = star->uart_reset_cnt;
            frame->fault_cnt = star->fault_cnt;
            
            return SAT_ERR_OK;
        } else {
            fprintf(stderr, "[ERROR] Parse data failed for %s STAR%d\n",
                    type == STAR_TYPE_NANO ? "NANO" : "PICO", num);
            star->check_err_cnt++;  // 解析失败也算校验错误
            update_fault_count(star);
            continue;
        }
    }

    fprintf(stderr, "[ERROR] Read failed after %d retries for %s STAR%d\n",
            MAX_RETRY, type == STAR_TYPE_NANO ? "NANO" : "PICO", num);
    
    // 串口复位
    star_uart_init(type, num, star->dev, star->baudrate);
    star->uart_reset_cnt++;
    update_fault_count(star);
    
    // 返回错误数据
    memset(frame, 0, sizeof(Star_sensorHS_Frame));
    frame->on_off_status = star->on_off_status;
    frame->header_err_cnt = star->header_err_cnt;
    frame->check_err_cnt = star->check_err_cnt;
    frame->length_err_cnt = star->length_err_cnt;
    frame->uart_reset_cnt = star->uart_reset_cnt;
    frame->fault_cnt = star->fault_cnt;
    
    return SAT_ERR_UNKNOWN;
}

/* ------------------ 数据解析纳型星敏 ------------------ */
int parse_nano_quaternion(const uint8_t *buf, Star_sensorHS_Frame *frame) {
    if (!buf || !frame) return SAT_ERR_DATA;
    
    int32_t raw_q0 = (int32_t)((buf[4] << 24) | (buf[5] << 16) | (buf[6] << 8) | buf[7]);
    int32_t raw_q1 = (int32_t)((buf[8] << 24) | (buf[9] << 16) | (buf[10] << 8) | buf[11]);
    int32_t raw_q2 = (int32_t)((buf[12] << 24) | (buf[13] << 16) | (buf[14] << 8) | buf[15]);
    int32_t raw_q3 = (int32_t)((buf[16] << 24) | (buf[17] << 16) | (buf[18] << 8) | buf[19]);
    
    frame->q0 = (float)raw_q0 / 2147483647.5f;
    frame->q1 = (float)raw_q1 / 2147483647.5f;
    frame->q2 = (float)raw_q2 / 2147483647.5f;
    frame->q3 = (float)raw_q3 / 2147483647.5f;

    int16_t raw_omega_x = (int16_t)((buf[45] << 8) | buf[46]);
    int16_t raw_omega_y = (int16_t)((buf[47] << 8) | buf[48]);
    int16_t raw_omega_z = (int16_t)((buf[49] << 8) | buf[50]);
    
    frame->wx = (float)raw_omega_x * 2e-13f;
    frame->wy = (float)raw_omega_y * 2e-13f;
    frame->wz = (float)raw_omega_z * 2e-13f;

    frame->bitpara_state.ext = buf[20];
    frame->star_num_d = buf[35];

    return SAT_ERR_OK;
}

/* ------------------ 数据解析皮型星敏 ------------------ */
int parse_pico_quaternion(const uint8_t *buf, Star_sensorHS_Frame *frame) {
    if (!buf || !frame) return SAT_ERR_DATA;
    
    int32_t raw_q1 = (int32_t)((buf[7] << 24) | (buf[6] << 16) | (buf[5] << 8) | buf[4]);
    int32_t raw_q2 = (int32_t)((buf[11] << 24) | (buf[10] << 16) | (buf[9] << 8) | buf[8]);
    int32_t raw_q3 = (int32_t)((buf[15] << 24) | (buf[14] << 16) | (buf[13] << 8) | buf[12]);
    int32_t raw_q0 = (int32_t)((buf[19] << 24) | (buf[18] << 16) | (buf[17] << 8) | buf[16]);
    
    frame->q0 = (float)raw_q0 / 2147483647.0f;
    frame->q1 = (float)raw_q1 / 2147483647.0f;
    frame->q2 = (float)raw_q2 / 2147483647.0f;
    frame->q3 = (float)raw_q3 / 2147483647.0f;

    frame->star_num_d = (buf[34] >> 1) & 0x7F;

    frame->bitpara_state.str_state1_data1 = buf[31] & 0x0F;
    frame->bitpara_state.str_state1_data2 = (buf[31] >> 4) & 0x03;
    frame->bitpara_state.str_state1_data3 = (buf[32] >> 6) & 0x03;

    frame->wx = 0.0f;
    frame->wy = 0.0f;
    frame->wz = 0.0f;

    return SAT_ERR_OK;
}

/* ------------------ 硬件服务化清理 ------------------ */
void Star_sensorHS_cleanup(void) {
    cout << "[INFO] Star_sensorHS cleaning up..." << endl;
    
    // 清理FastDDS组件
    if (StarHS_part != nullptr) {
        delete StarHS_part;
        StarHS_part = nullptr;
    }
    
    // 关闭所有串口
    for (uint8_t 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;
        }
        if (pico_stars[i].fd > 0) {
            close(pico_stars[i].fd);
            pico_stars[i].fd = -1;
        }
    }
    
    // 销毁互斥锁
    pthread_mutex_destroy(&cache_mutex);
    
}