/*******************************************************************************
 * 星载硬件仿真系统 
 * 
 * 功能：监听串口，解析上层应用指令，调用现有仿真函数生成数据并返回
 * 
 * Created on: 2023-3-9
 ******************************************************************************/

#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <cstring>
#include <errno.h>
#include <poll.h>
#include <thread>
#include <iostream>
#include <vector>
#include <map>
#include <memory>
#include <atomic>
#include <mutex>
#include <csignal>
#include <chrono>

// 包含现有的硬件仿真头文件
#include "sensor_gnss.h"
#include "sensor_tx2.h"
#include "sensor_optical.h"
#include "sensor_mems.h"
#include "sensor_gyro.h"
#include "sensor_wheel.h"
#include "sensor_star.h"
#include "sensor_thrust.h"
#include "orbit_info.h"
#include "GNCFunction.h"
#include "liblog.h"

using namespace std;

// ==================================== 全局仿真数据结构 ====================================
// 轨道参数
static double TarRVECI[6] = {23621367.9190000, -34592141.8670000, 3657159.91500000, 
                      2548.34575500000, 1736.43994900000, -5.86735000000000};
static double ChaRVECI[6] = {23580586.0537251, -34621080.4670552, 3658341.81666531,
                      2550.44489430168, 1733.51288095298, -5.46126675290179};
static double AttiVelECI[3] = {0.0, 0.0, 0.0};
static double EulerVVLH[3] = {0.0, 0.0, 0.0};
static double MtxVVLH2Body[3][3] = {{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}};
static double MtxECI2VVLH[3][3] = {{0.8266, 0.5628, -0.0018}, 
                            {0.0474, -0.0729, -0.9962},
                            {-0.5608, 0.8234, -0.087}};
static double QuatECI[4] = {1.0, 0.0, 0.0, 0.0};
static double mjd = 60776.00480324;
static double Epoch2017 = 261130015.0;

// 光电/TX2相关参数
static int TargetFLAG = 0; // 1:左帆板, 0:本体
static double R_Plane[2] = {0.0, 0.0};
static double LOS_Remote[9] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};

// 硬件状态
struct HardwareStatus {
    atomic<uint8_t> gnss_onoff{1};
    atomic<uint8_t> star_onoff{1};
    atomic<uint8_t> gyro_onoff{1};
    atomic<uint8_t> mems_onoff{1};
    atomic<uint8_t> wheel_onoff{1};
    atomic<uint8_t> optical_onoff{1};
    atomic<uint8_t> carried_status{0}; // 0:运载模式, 2:独立模式
    
    // 动量轮
    double WheelTorqueBody[3] = {0.0, 0.0, 0.0};
    double WheelAM[3] = {0.0, 0.0, 0.0};
    double CmdWheel[3] = {0.0, 0.0, 0.0};
    
    // 推进
    double ThrustTorque[3] = {0.0, 0.0, 0.0};
    double SatMtxI[3][3] = {{10.0, 0.0, 0.0}, {0.0, 10.0, 0.0}, {0.0, 0.0, 10.0}};
    double ThrustForce[3] = {0.0, 0.0, 0.0};
    double CmdJetPWM[12] = {0.0};
    double JetPWMPlan[12] = {0.0};
    double SatMass = 100.0; // kg
    
    // 载荷
    atomic<int> LaunchFlag1{0};
    atomic<int> LaunchFlag2{0};
    atomic<int> LaunchAlreadyFlag1{0};
    atomic<int> LaunchAlreadyFlag2{0};
};

// 数据帧缓冲区
static uint8_t gnss_frame[GPS_Hk_FRAME_LENGTH];
static uint8_t xgd_frame[OPTIC_FRAME_LENGTH];
static uint8_t tx2_frame[VISION_Info_Length];
static uint8_t wheel_frame[WHEEL_FRAME_LENGTH];
static uint8_t gyro_frame[GYRO_FRAME_LENGTH];
static uint8_t mems_frame[MEMS_FRAME_LENGTH];
static uint8_t star_frame[CAK_TLM_1_Info_Length];
static uint8_t photo_frame[249];
static uint8_t orbit_frame[89];

// 全局硬件状态
static HardwareStatus hw_status;

// 全局串口管理器
static map<string, unique_ptr<class SerialPort>> serial_ports;

// ==================================== 串口管理器类 ====================================
class SerialPort {
private:
    int fd = -1;
    string device_type;  // 设备类型，如 "GNSS", "GYRO"等
    string device_path;
    atomic<bool> running{false};
    thread read_thread;
    mutex write_mutex;
    
    // 协议解析缓冲区
    uint8_t buffer[1024];
    uint16_t buffer_index = 0;

public:
    SerialPort(const string& type, const string& path) 
        : device_type(type), device_path(path) {}
    
    ~SerialPort() {
        stop();
        close();
    }
    
    bool open(int baudrate = 115200) {
        // 打开串口设备
        fd = ::open(device_path.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
        if (fd < 0) {
            spdlog::error("无法打开串口 {}: {}", device_path, strerror(errno));
            return false;
        }
        
        // 配置串口参数
        struct termios options;
        if (tcgetattr(fd, &options) != 0) {
            spdlog::error("获取串口属性失败: {}", strerror(errno));
            ::close(fd);
            fd = -1;
            return false;
        }
        
        // 设置波特率
        speed_t baud;
        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:
                spdlog::error("不支持的波特率: {}", baudrate);
                ::close(fd);
                fd = -1;
                return false;
        }
        cfsetispeed(&options, baud);
        cfsetospeed(&options, baud);
        
        // 8位数据，无校验，1位停止位
        options.c_cflag &= ~CSIZE;
        options.c_cflag |= CS8;
        options.c_cflag &= ~PARENB;
        options.c_cflag &= ~CSTOPB;
        options.c_cflag &= ~CRTSCTS;  // 无硬件流控制
        
        // 原始模式
        options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
        options.c_oflag &= ~OPOST;
        
        // 设置读取超时
        options.c_cc[VMIN] = 0;
        options.c_cc[VTIME] = 10;  // 1秒超时
        
        if (tcsetattr(fd, TCSANOW, &options) != 0) {
            spdlog::error("设置串口属性失败: {}", strerror(errno));
            ::close(fd);
            fd = -1;
            return false;
        }
        
        // 清除缓冲区
        tcflush(fd, TCIOFLUSH);
        
        spdlog::info("串口 {} ({}) 已打开，波特率: {}", device_type, device_path, baudrate);
        return true;
    }
    
    void close() {
        if (fd >= 0) {
            ::close(fd);
            fd = -1;
            spdlog::info("串口 {} 已关闭", device_type);
        }
    }
    
    void start() {
        if (fd < 0 || running) return;
        
        running = true;
        read_thread = thread(&SerialPort::read_loop, this);
        spdlog::info("串口 {} 读取线程已启动", device_type);
    }
    
    void stop() {
        running = false;
        if (read_thread.joinable()) {
            read_thread.join();
        }
        spdlog::info("串口 {} 读取线程已停止", device_type);
    }
    
    ssize_t write(const uint8_t* data, size_t len) {
        lock_guard<mutex> lock(write_mutex);
        
        if (fd < 0) {
            spdlog::error("串口 {} 未打开，无法写入", device_type);
            return -1;
        }
        
        ssize_t written = ::write(fd, data, len);
        if (written < 0) {
            spdlog::error("串口 {} 写入失败: {}", device_type, strerror(errno));
        } else if (written != (ssize_t)len) {
            spdlog::warn("串口 {} 部分写入: {}/{} 字节", device_type, written, len);
        } else {
            spdlog::debug("串口 {} 写入 {} 字节成功", device_type, len);
        }
        
        return written;
    }
    
    const string& get_type() const { return device_type; }
    const string& get_path() const { return device_path; }
    bool is_running() const { return running; }

private:
    void read_loop() {
        uint8_t read_buffer[1024];
        
        spdlog::debug("串口 {} 读取线程开始", device_type);
        
        while (running) {
            // 使用poll检查是否有数据可读
            struct pollfd pfd;
            pfd.fd = fd;
            pfd.events = POLLIN;
            
            int ret = poll(&pfd, 1, 100);  // 100ms超时
            if (ret < 0) {
                spdlog::error("串口 {} poll失败: {}", device_type, strerror(errno));
                break;
            } else if (ret == 0) {
                continue;  // 超时，继续循环
            }
            
            if (pfd.revents & POLLIN) {
                ssize_t n = read(fd, read_buffer, sizeof(read_buffer));
                if (n > 0) {
                    process_serial_data(read_buffer, n);
                } else if (n < 0 && errno != EAGAIN) {
                    spdlog::error("读取串口 {} 失败: {}", device_type, strerror(errno));
                    break;
                }
            }
        }
        
        spdlog::debug("串口 {} 读取线程结束", device_type);
    }
    
    void process_serial_data(const uint8_t* data, size_t len) {
        spdlog::debug("串口 {} 接收到 {} 字节数据", device_type, len);
        
        // 逐个字节处理数据
        for (size_t i = 0; i < len; i++) {
            uint8_t value = data[i];
            
            // 根据设备类型调用相应的处理函数
            if (device_type == "GNSS") {
                process_gnss_data(value);
            } else if (device_type == "GYRO") {
                process_gyro_data(value);
            } else if (device_type == "MEMS") {
                process_mems_data(value);
            } else if (device_type == "STAR1") {
                process_star_1_data(value);
            } else if (device_type == "STAR2") {
                process_star_2_data(value);
            } else if (device_type == "WHEEL1") {
                process_wheel_1_data(value);
            } else if (device_type == "WHEEL2") {
                process_wheel_2_data(value);
            } else if (device_type == "WHEEL3") {
                process_wheel_3_data(value);
            } else if (device_type == "OPTICAL") {
                process_optical_data(value);
            } else if (device_type == "VISION") {
                process_vision_data(value);
            } else {
                spdlog::warn("未知设备类型: {}", device_type);
            }
        }
    }
    
    // ==================================== 协议解析函数 ====================================
    void process_gnss_data(uint8_t data) {
        static uint8_t rec_step = 1;
        static uint16_t rec_count = 0;
        
        switch(rec_step) {
            case 1:
                if(data == 0xEB) {
                    buffer[rec_count++] = data;
                    rec_step = 2;
                } else {
                    rec_step = 1;
                    rec_count = 0;
                }
                break;
            case 2:
                if(data == 0x90) {
                    buffer[rec_count++] = data;
                    rec_step = 3;
                } else {
                    spdlog::warn("GNSS帧头错误: 0xEB 0x{:02X}", data);
                    rec_step = 1;
                    rec_count = 0;
                }
                break;
            case 3:
                if(data == 0x09) {
                    buffer[rec_count++] = data;
                    rec_step = 4;
                } else {
                    buffer[rec_count++] = data;
                }
                break;
            case 4:
                if(data == 0xD7) {
                    buffer[rec_count++] = data;
                    spdlog::info("GNSS数据接收完成, 长度: {}", rec_count);
                    // 处理GNSS指令并生成响应
                    handle_gnss_command();
                } else {
                    spdlog::warn("GNSS帧尾错误: 0x{:02X}", data);
                }
                rec_step = 1;
                rec_count = 0;
                break;
            default:
                rec_step = 1;
                rec_count = 0;
                break;
        }
    }
    
    void process_gyro_data(uint8_t data) {
        static uint8_t rec_step = 1;
        static uint16_t rec_count = 0;
        
        switch(rec_step) {
            case 1:
                if(data == 0x55) {
                    buffer[rec_count++] = data;
                    rec_step = 2;
                } else {
                    rec_step = 1;
                    rec_count = 0;
                }
                break;
            case 2:
                if(data == 0x68) {
                    buffer[rec_count++] = data;
                    spdlog::info("GYRO数据接收完成, 长度: {}", rec_count);
                    // 处理GYRO指令并生成响应
                    handle_gyro_command();
                } else {
                    spdlog::warn("GYRO帧头错误: 0x55 0x{:02X}", data);
                }
                rec_step = 1;
                rec_count = 0;
                break;
            default:
                rec_step = 1;
                rec_count = 0;
                break;
        }
    }
    
    void process_mems_data(uint8_t data) {
        static uint8_t rec_step = 1;
        static uint16_t rec_count = 0;
        
        switch(rec_step) {
            case 1:
                if(data == 0x55) {
                    buffer[rec_count++] = data;
                    rec_step = 2;
                } else {
                    rec_step = 1;
                    rec_count = 0;
                }
                break;
            case 2:
                if(data == 0x68) {
                    buffer[rec_count++] = data;
                    spdlog::info("MEMS数据接收完成, 长度: {}", rec_count);
                    // 处理MEMS指令并生成响应
                    handle_mems_command();
                } else {
                    spdlog::warn("MEMS帧头错误: 0x55 0x{:02X}", data);
                }
                rec_step = 1;
                rec_count = 0;
                break;
            default:
                rec_step = 1;
                rec_count = 0;
                break;
        }
    }
    
    void process_star_1_data(uint8_t data) {
        static uint8_t rec_step = 1;
        static uint16_t rec_count = 0;
        
        switch(rec_step) {
            case 1:
                if(data == 0x74) {
                    buffer[rec_count++] = data;
                    rec_step = 2;
                } else {
                    rec_step = 1;
                    rec_count = 0;
                }
                break;
            case 2:
                if(data == 0xA1) {
                    buffer[rec_count++] = data;
                    rec_step = 3;
                } else {
                    spdlog::warn("STAR1帧头错误: 0x74 0x{:02X}", data);
                    rec_step = 1;
                    rec_count = 0;
                }
                break;
            case 3:
                buffer[rec_count++] = data;
                if(rec_count == 5) {
                    if(buffer[4] == 0x15) {
                        spdlog::info("STAR-1数据接收完成, 长度: {}", rec_count);
                        // 处理STAR-1指令并生成响应
                        handle_star_1_command();
                    } else {
                        spdlog::warn("STAR-1帧尾错误: 0x{:02X}", buffer[4]);
                    }
                    rec_step = 1;
                    rec_count = 0;
                }
                break;
            default:
                rec_step = 1;
                rec_count = 0;
                break;
        }
    }
    
    void process_star_2_data(uint8_t data) {
        static uint8_t rec_step = 1;
        static uint16_t rec_count = 0;
        
        switch(rec_step) {
            case 1:
                if(data == 0x74) {
                    buffer[rec_count++] = data;
                    rec_step = 2;
                } else {
                    rec_step = 1;
                    rec_count = 0;
                }
                break;
            case 2:
                if(data == 0xA1) {
                    buffer[rec_count++] = data;
                    rec_step = 3;
                } else {
                    spdlog::warn("STAR2帧头错误: 0x74 0x{:02X}", data);
                    rec_step = 1;
                    rec_count = 0;
                }
                break;
            case 3:
                buffer[rec_count++] = data;
                if(rec_count == 5) {
                    if(buffer[4] == 0x15) {
                        spdlog::info("STAR-2数据接收完成, 长度: {}", rec_count);
                        // 处理STAR-2指令并生成响应
                        handle_star_2_command();
                    } else {
                        spdlog::warn("STAR-2帧尾错误: 0x{:02X}", buffer[4]);
                    }
                    rec_step = 1;
                    rec_count = 0;
                }
                break;
            default:
                rec_step = 1;
                rec_count = 0;
                break;
        }
    }
    
    void process_wheel_1_data(uint8_t data) {
        static uint8_t rec_step = 1;
        static uint16_t rec_count = 0;
        static uint8_t checksum = 0;
        
        switch(rec_step) {
            case 1:
                if((data == 0x58) || (data == 0x59)) {
                    checksum = 0;
                    buffer[rec_count++] = data;
                    checksum += data;
                    rec_step = 2;
                } else {
                    rec_step = 1;
                    rec_count = 0;
                    checksum = 0;
                }
                break;
            case 2:
                buffer[rec_count++] = data;
                if(rec_count == 11) {
                    if(checksum == data) {
                        spdlog::info("WHEEL-1数据接收完成, 长度: {}", rec_count);
                        // 处理WHEEL-1指令并生成响应
                        handle_wheel_1_command();
                    } else {
                        spdlog::warn("WHEEL-1校验和错误");
                    }
                    rec_step = 1;
                    rec_count = 0;
                    checksum = 0;
                } else {
                    checksum += data;
                }
                break;
            default:
                rec_step = 1;
                rec_count = 0;
                checksum = 0;
                break;
        }
    }
    
    void process_wheel_2_data(uint8_t data) {
        static uint8_t rec_step = 1;
        static uint16_t rec_count = 0;
        static uint8_t checksum = 0;
        
        switch(rec_step) {
            case 1:
                if((data == 0x58) || (data == 0x59)) {
                    checksum = 0;
                    buffer[rec_count++] = data;
                    checksum += data;
                    rec_step = 2;
                } else {
                    rec_step = 1;
                    rec_count = 0;
                    checksum = 0;
                }
                break;
            case 2:
                buffer[rec_count++] = data;
                if(rec_count == 11) {
                    if(checksum == data) {
                        spdlog::info("WHEEL-2数据接收完成, 长度: {}", rec_count);
                        // 处理WHEEL-2指令并生成响应
                        handle_wheel_2_command();
                    } else {
                        spdlog::warn("WHEEL-2校验和错误");
                    }
                    rec_step = 1;
                    rec_count = 0;
                    checksum = 0;
                } else {
                    checksum += data;
                }
                break;
            default:
                rec_step = 1;
                rec_count = 0;
                checksum = 0;
                break;
        }
    }
    
    void process_wheel_3_data(uint8_t data) {
        static uint8_t rec_step = 1;
        static uint16_t rec_count = 0;
        static uint8_t checksum = 0;
        
        switch(rec_step) {
            case 1:
                if((data == 0x58) || (data == 0x59)) {
                    checksum = 0;
                    buffer[rec_count++] = data;
                    checksum += data;
                    rec_step = 2;
                } else {
                    rec_step = 1;
                    rec_count = 0;
                    checksum = 0;
                }
                break;
            case 2:
                buffer[rec_count++] = data;
                if(rec_count == 11) {
                    if(checksum == data) {
                        spdlog::info("WHEEL-3数据接收完成, 长度: {}", rec_count);
                        // 处理WHEEL-3指令并生成响应
                        handle_wheel_3_command();
                    } else {
                        spdlog::warn("WHEEL-3校验和错误");
                    }
                    rec_step = 1;
                    rec_count = 0;
                    checksum = 0;
                } else {
                    checksum += data;
                }
                break;
            default:
                rec_step = 1;
                rec_count = 0;
                checksum = 0;
                break;
        }
    }
    
    void process_optical_data(uint8_t data) {
        static uint8_t rec_step = 1;
        static uint16_t rec_count = 0;
        
        switch(rec_step) {
            case 1:
                if(data == 0xCD) {
                    buffer[rec_count++] = data;
                    rec_step = 2;
                } else if(data == 0xEB) {
                    buffer[rec_count++] = data;
                    rec_step = 12;
                } else {
                    rec_step = 1;
                    rec_count = 0;
                }
                break;
            case 2:
                if(data == 0x90) {
                    buffer[rec_count++] = data;
                    rec_step = 3;
                } else {
                    spdlog::warn("OPTICAL帧头错误: 0xCD 0x{:02X}", data);
                    rec_step = 1;
                    rec_count = 0;
                }
                break;
            case 3:
                buffer[rec_count++] = data;
                if(rec_count == 5) {
                    if(buffer[4] == 0xFF) {
                        spdlog::info("OPTICAL数据接收完成, 长度: {}", rec_count);
                        // 处理OPTICAL指令并生成响应
                        handle_optical_command();
                    } else {
                        spdlog::warn("OPTICAL帧尾错误: 0x{:02X}", buffer[4]);
                    }
                    rec_step = 1;
                    rec_count = 0;
                }
                break;
            case 12:
                if(data == 0x90) {
                    buffer[rec_count++] = data;
                    rec_step = 13;
                } else {
                    spdlog::warn("OPTICAL扩展帧头错误: 0xEB 0x{:02X}", data);
                    rec_step = 1;
                    rec_count = 0;
                }
                break;
            case 13:
                buffer[rec_count++] = data;
                if(rec_count == 8) {
                    uint8_t checksum = 0;
                    for(int kc = 0; kc < rec_count - 1; kc++) {
                        checksum += buffer[kc];
                    }
                    
                    if(buffer[7] == checksum) {
                        spdlog::info("OPTICAL带参指令接收完成, 长度: {}", rec_count);
                        // 处理OPTICAL带参指令
                        handle_optical_command();
                    } else {
                        spdlog::warn("OPTICAL带参指令校验错误");
                    }
                    rec_step = 1;
                    rec_count = 0;
                }
                break;
            default:
                rec_step = 1;
                rec_count = 0;
                break;
        }
    }
    
    void process_vision_data(uint8_t data) {
        static uint8_t rec_step = 1;
        static uint16_t rec_count = 0;
        static uint16_t frame_len = 0;
        
        switch(rec_step) {
            case 1:
                if(data == 0xEB) {
                    buffer[rec_count++] = data;
                    rec_step = 2;
                } else {
                    rec_step = 1;
                    rec_count = 0;
                }
                break;
            case 2:
                if(data == 0x90) {
                    buffer[rec_count++] = data;
                    rec_step = 3;
                } else {
                    spdlog::warn("VISION帧头错误: 0xEB 0x{:02X}", data);
                    rec_step = 1;
                    rec_count = 0;
                }
                break;
            case 3:
                if((data == 0x1E) || (data == 0x0F)) {
                    buffer[rec_count++] = data;
                    rec_step = 4;
                } else {
                    spdlog::warn("VISION指令类型错误: 0x{:02X}", data);
                    rec_step = 1;
                    rec_count = 0;
                }
                break;
            case 4:
                buffer[rec_count++] = data;
                rec_step = 5;
                break;
            case 5:
                buffer[rec_count++] = data;
                frame_len = (buffer[3] * 256) + buffer[4];
                rec_step = 6;
                break;
            case 6:
                buffer[rec_count++] = data;
                if(rec_count == (frame_len + 6)) {
                    uint8_t checksum = 0;
                    for(int kc = 2; kc < rec_count - 1; kc++) {
                        checksum += buffer[kc];
                    }
                    if(buffer[rec_count - 1] == checksum) {
                        spdlog::info("VISION数据接收完成, 长度: {}", rec_count);
                        // 处理VISION指令并生成响应
                        handle_vision_command();
                    } else {
                        spdlog::warn("VISION校验和错误");
                    }
                    rec_step = 1;
                    rec_count = 0;
                    frame_len = 0;
                }
                break;
            default:
                rec_step = 1;
                rec_count = 0;
                frame_len = 0;
                break;
        }
    }
    
    // ==================================== 命令处理函数 ====================================
    void handle_gnss_command() {
        if (!hw_status.gnss_onoff) {
            spdlog::info("GNSS功能已关闭，不生成响应");
            return;
        }
        
        // 直接调用现有的GNSS数据生成函数
        generate_gnss_frame(gnss_frame, ChaRVECI, mjd, Epoch2017);
        
        // 发送响应
        write(gnss_frame, GPS_Hk_FRAME_LENGTH);
        spdlog::info("已发送GNSS响应数据，长度: {}", GPS_Hk_FRAME_LENGTH);
    }
    
    void handle_gyro_command() {
        if (!hw_status.gyro_onoff) {
            spdlog::info("GYRO功能已关闭，不生成响应");
            return;
        }
        
        // 直接调用现有的GYRO数据生成函数
        generate_gyro_frame(gyro_frame, AttiVelECI);
        
        // 发送响应
        write(gyro_frame, GYRO_FRAME_LENGTH);
        spdlog::info("已发送GYRO响应数据，长度: {}", GYRO_FRAME_LENGTH);
    }
    
    void handle_mems_command() {
        if (!hw_status.mems_onoff) {
            spdlog::info("MEMS功能已关闭，不生成响应");
            return;
        }
        
        // 直接调用现有的MEMS数据生成函数
        generate_mems_frame(mems_frame, AttiVelECI);
        
        // 发送响应
        write(mems_frame, MEMS_FRAME_LENGTH);
        spdlog::info("已发送MEMS响应数据，长度: {}", MEMS_FRAME_LENGTH);
    }
    
    void handle_star_1_command() {
        if (!hw_status.star_onoff) {
            spdlog::info("STAR-1功能已关闭，不生成响应");
            return;
        }
        
        // 获取四元数
        get_orbit_Q(QuatECI);
        
        // 直接调用现有的STAR-1数据生成函数
        generate_star_1_frame(star_frame, QuatECI, AttiVelECI);
        
        // 发送响应
        write(star_frame, CAK_TLM_1_Info_Length);
        spdlog::info("已发送STAR-1响应数据，长度: {}", CAK_TLM_1_Info_Length);
    }
    
    void handle_star_2_command() {
        if (!hw_status.star_onoff) {
            spdlog::info("STAR-2功能已关闭，不生成响应");
            return;
        }
        
        // 获取四元数
        get_orbit_Q(QuatECI);
        
        // 直接调用现有的STAR-2数据生成函数
        generate_star_2_frame(star_frame, QuatECI, AttiVelECI);
        
        // 发送响应
        write(star_frame, CAK_TLM_1_Info_Length);
        spdlog::info("已发送STAR-2响应数据，长度: {}", CAK_TLM_1_Info_Length);
    }
    
    void handle_wheel_1_command() {
        if (!hw_status.wheel_onoff) {
            spdlog::info("WHEEL-1功能已关闭，不生成响应");
            return;
        }
        
        // 解析转速命令（从buffer中解析）
        if (buffer_index >= 5) {
            hw_status.CmdWheel[0] = cal_message_para(buffer[1], buffer[2], buffer[3], buffer[4], WHEEL_EXP);
            spdlog::info("WHEEL-1转速命令: {:.2f} rpm", hw_status.CmdWheel[0]);
            
            // 更新动量轮仿真数据
            sensor_WheelMeas(hw_status.WheelTorqueBody, hw_status.WheelAM, hw_status.CmdWheel);
        }
        
        // 直接调用现有的WHEEL数据生成函数
        generate_wheel_frame(wheel_frame, hw_status.CmdWheel[0]);
        
        // 发送响应
        write(wheel_frame, WHEEL_FRAME_LENGTH);
        spdlog::info("已发送WHEEL-1响应数据，长度: {}", WHEEL_FRAME_LENGTH);
    }
    
    void handle_wheel_2_command() {
        if (!hw_status.wheel_onoff) {
            spdlog::info("WHEEL-2功能已关闭，不生成响应");
            return;
        }
        
        // 解析转速命令
        if (buffer_index >= 5) {
            hw_status.CmdWheel[1] = cal_message_para(buffer[1], buffer[2], buffer[3], buffer[4], WHEEL_EXP);
            spdlog::info("WHEEL-2转速命令: {:.2f} rpm", hw_status.CmdWheel[1]);
            
            // 更新动量轮仿真数据
            sensor_WheelMeas(hw_status.WheelTorqueBody, hw_status.WheelAM, hw_status.CmdWheel);
        }
        
        // 直接调用现有的WHEEL数据生成函数
        generate_wheel_frame(wheel_frame, hw_status.CmdWheel[1]);
        
        // 发送响应
        write(wheel_frame, WHEEL_FRAME_LENGTH);
        spdlog::info("已发送WHEEL-2响应数据，长度: {}", WHEEL_FRAME_LENGTH);
    }
    
    void handle_wheel_3_command() {
        if (!hw_status.wheel_onoff) {
            spdlog::info("WHEEL-3功能已关闭，不生成响应");
            return;
        }
        
        // 解析转速命令
        if (buffer_index >= 5) {
            hw_status.CmdWheel[2] = cal_message_para(buffer[1], buffer[2], buffer[3], buffer[4], WHEEL_EXP);
            spdlog::info("WHEEL-3转速命令: {:.2f} rpm", hw_status.CmdWheel[2]);
            
            // 更新动量轮仿真数据
            sensor_WheelMeas(hw_status.WheelTorqueBody, hw_status.WheelAM, hw_status.CmdWheel);
        }
        
        // 直接调用现有的WHEEL数据生成函数
        generate_wheel_frame(wheel_frame, hw_status.CmdWheel[2]);
        
        // 发送响应
        write(wheel_frame, WHEEL_FRAME_LENGTH);
        spdlog::info("已发送WHEEL-3响应数据，长度: {}", WHEEL_FRAME_LENGTH);
    }
    
    void handle_optical_command() {
        if (!hw_status.optical_onoff) {
            spdlog::info("OPTICAL功能已关闭，不生成响应");
            return;
        }
        
        // 根据指令类型处理
        if (buffer_index >= 4) {
            if (buffer[2] == 0x06 && buffer[3] == 0x00) {
                // 遥测指令 - 直接调用现有的光电数据生成函数
                generate_xgd_frame(xgd_frame, TarRVECI, ChaRVECI, MtxECI2VVLH, 
                                 MtxVVLH2Body, LOS_Remote, EulerVVLH, R_Plane, TargetFLAG);
                write(xgd_frame, OPTIC_FRAME_LENGTH);
                spdlog::info("已发送OPTICAL遥测数据，长度: {}", OPTIC_FRAME_LENGTH);
            } else if ((buffer[2] == 0x07 && buffer[3] == 0x01) || 
                      (buffer[2] == 0x08 && buffer[3] == 0x01)) {
                // 图像指令 - 直接调用现有的图像生成函数
                generate_photo_frame(photo_frame);
                write(photo_frame, 249);
                spdlog::info("已发送OPTICAL图像数据，长度: 249");
            } else {
                spdlog::warn("未知的OPTICAL指令: 0x{:02X} 0x{:02X}", buffer[2], buffer[3]);
            }
        }
    }
    
    void handle_vision_command() {
        if (buffer_index >= 3 && buffer[2] == 0x0F) {
            // 遥测指令 - 直接调用现有的TX2数据生成函数
            generate_tx2_frame(tx2_frame, LOS_Remote, R_Plane, TarRVECI, ChaRVECI, 
                             MtxVVLH2Body, MtxECI2VVLH, TargetFLAG);
            write(tx2_frame, VISION_Info_Length);
            spdlog::info("已发送VISION遥测数据，长度: {}", VISION_Info_Length);
        } else {
            spdlog::warn("未知的VISION指令类型: 0x{:02X}", buffer[2]);
        }
    }
};

// ==================================== 主初始化函数 ====================================
void initialize_simulation_system() {
    cout << "=== 星载硬件仿真系统初始化 ===" << endl;
    cout << "初始化Linux串口仿真系统..." << endl;
    
    // 初始化轨道信息
    orbit_info_initz("./settings/setting.json");
    
    uint8_t dummy_orbit_frame[89] = {0};
    
    set_orbit_info(dummy_orbit_frame, AttiVelECI, EulerVVLH, MtxVVLH2Body, MtxECI2VVLH);
    
    spdlog::info("初始化后角速度: [{:.6f}, {:.6f}, {:.6f}] 弧度/秒", 
                AttiVelECI[0], AttiVelECI[1], AttiVelECI[2]);
    spdlog::info("转换为度/秒: [{:.6f}, {:.6f}, {:.6f}] 度/秒", 
                AttiVelECI[0] * 180.0 / M_PI, 
                AttiVelECI[1] * 180.0 / M_PI, 
                AttiVelECI[2] * 180.0 / M_PI);
    
    // 获取四元数
    get_orbit_Q(QuatECI);
    
    // 串口设备映射配置
    cout << "1. 配置串口设备..." << endl;
    map<string, string> port_config = {
        {"GNSS",    "/dev/tnt0"},   // GNSS接收机
        {"GYRO",    "/dev/tnt2"},   // 陀螺仪
        {"MEMS",    "/dev/tnt4"},   // MEMS加速度计
        {"STAR1",   "/dev/tnt6"},   // 星敏感器1
        {"STAR2",   "/dev/tnt8"},   // 星敏感器2
        {"WHEEL1",  "/dev/tnt10"},  // 动量轮1
        {"WHEEL2",  "/dev/tnt12"},  // 动量轮2
        {"WHEEL3",  "/dev/tnt14"},  // 动量轮3
        {"OPTICAL", "/dev/tnt16"},  // 小光电
        {"VISION",  "/dev/tnt18"}   // TX2视觉
    };
    
    // 打开所有串口设备
    cout << "2. 开始打开串口设备..." << endl;
    int opened_ports = 0;
    for (const auto& config : port_config) {
        cout << "   尝试打开: " << config.first << " -> " << config.second << "... ";
        cout.flush();
        
        auto port = make_unique<SerialPort>(config.first, config.second);
        
        if (port->open(115200)) {
            port->start();
            serial_ports[config.first] = move(port);
            opened_ports++;
            cout << "✅ 成功" << endl;
        } else {
            cout << "❌ 失败" << endl;
        }
    }
    
    cout << "===============================================" << endl;
    cout << "✅ 星载硬件仿真系统初始化完成" << endl;
    cout << "   成功打开 " << opened_ports << "/" << port_config.size() << " 个串口设备" << endl;
    cout << "   系统已就绪，开始监听串口指令..." << endl;
    cout << "=== 初始化完成 ===" << endl;
}

// ==================================== 清理函数 ====================================
void cleanup_simulation_system() {
    spdlog::info("清理仿真系统资源...");
    for (auto& pair : serial_ports) {
        spdlog::info("关闭串口: {}", pair.first);
        pair.second->stop();
        pair.second->close();
    }
    serial_ports.clear();
    spdlog::info("仿真系统资源清理完成");
}

// ==================================== 主函数 ====================================
int main(int argc, char** argv) {
    // 初始化日志系统
    spdlog_initz("./settings/setting.json");
    
    cout << "**************************************************" << endl;
    cout << "*       星载硬件仿真系统 - 单程序版本           *" << endl;
    cout << "*                                                *" << endl;
    cout << "* 功能：监听串口 -> 解析指令 -> 调用现有仿真函数*" << endl;
    cout << "*                                                *" << endl;
    cout << "**************************************************" << endl;
    cout << endl;
    
    // 初始化仿真系统
    initialize_simulation_system();
    
    // 注册信号处理
    std::signal(SIGINT, [](int) {
        cout << "\n收到中断信号，正在关闭系统..." << endl;
        cleanup_simulation_system();
        exit(0);
    });
    
    std::signal(SIGTERM, [](int) {
        cout << "\n收到终止信号，正在关闭系统..." << endl;
        cleanup_simulation_system();
        exit(0);
    });
    
    // 主循环
    while (true) {
        std::this_thread::sleep_for(std::chrono::seconds(1));
        
    }
    
    return 0;
}