/**
 * @file serial_debug.cpp
 * @brief 串口调试程序：接收数据并打印解算结果和原始数据
 * 
 * 用法: ./serial_debug [串口设备]
 * 示例: ./serial_debug /dev/ttyCH340
 */

#include <iostream>
#include <iomanip>
#include <cstring>
#include <vector>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <chrono>
#include <thread>
#include <csignal>

// 协议定义
constexpr uint8_t FRAME_HEADER_1 = 0xBB;
constexpr uint8_t FRAME_HEADER_2 = 0x77;
constexpr uint8_t FRAME_TAIL = 0xEE;
constexpr int FRAME_LENGTH = 15;
constexpr const char *DEFAULT_SERIAL_PORT = "/dev/ttyCH340";
constexpr int BAUDRATE = 115200;

// 传感器数据结构
struct SensorData {
    float gyro_x = 0.0f;   // 陀螺仪x
    float accel_x = 0.0f;  // 加速度计x
    float accel_y = 0.0f;  // 加速度计y
    float accel_z = 0.0f;  // 加速度计z
    bool valid = false;
};

// 全局标志用于退出
volatile bool running = true;

void signalHandler(int) {
    running = false;
}

// 解析接收到的数据
bool parseFrame(const uint8_t* data, SensorData& sensor) {
    // 检查帧头帧尾
    if (data[0] != FRAME_HEADER_1 || data[1] != FRAME_HEADER_2 || data[14] != FRAME_TAIL) {
        return false;
    }
    
    // 解析4个杆量值（小端序 int16）
    int16_t x_move = data[2] | (data[3] << 8);
    int16_t y_move = data[4] | (data[5] << 8);
    int16_t yaw_val = data[6] | (data[7] << 8);
    int16_t pitch_val = data[8] | (data[9] << 8);
    
    // 转换为float（假设原始数据是放大了100倍的）
    sensor.gyro_x = x_move / 100.0f;
    sensor.accel_x = y_move / 100.0f;
    sensor.accel_y = yaw_val / 100.0f;
    sensor.accel_z = pitch_val / 100.0f;
    sensor.valid = true;
    
    return true;
}

// 打印原始数据帧（十六进制）
void printRawData(const uint8_t* data, int len) {
    std::cout << "Raw Data: ";
    for (int i = 0; i < len; i++) {
        std::cout << std::hex << std::uppercase << std::setw(2) << std::setfill('0') 
                  << static_cast<int>(data[i]) << " ";
    }
    std::cout << std::dec << std::nouppercase << std::endl;
}

// 打印解算的传感器数据
void printSensorData(const SensorData& sensor) {
    std::cout << "========================================" << std::endl;
    std::cout << "  gyro_x: " << std::setw(8) << std::fixed << std::setprecision(3) << sensor.gyro_x << std::endl;
    std::cout << " accel_x: " << std::setw(8) << std::fixed << std::setprecision(3) << sensor.accel_x << std::endl;
    std::cout << " accel_y: " << std::setw(8) << std::fixed << std::setprecision(3) << sensor.accel_y << std::endl;
    std::cout << " accel_z: " << std::setw(8) << std::fixed << std::setprecision(3) << sensor.accel_z << std::endl;
    std::cout << "========================================" << std::endl;
}

// 解析并打印帧中其他字段
void printFrameDetails(const uint8_t* data) {
    int16_t x_move = data[2] | (data[3] << 8);
    int16_t y_move = data[4] | (data[5] << 8);
    int16_t yaw = data[6] | (data[7] << 8);
    int16_t pitch = data[8] | (data[9] << 8);
    int16_t feed = data[10] | (data[11] << 8);
    uint8_t switches = data[12];
    uint8_t left_switch = (switches >> 4) & 0x0F;
    uint8_t right_switch = switches & 0x0F;
    uint8_t crc = data[13];
    
    std::cout << "Frame Details:" << std::endl;
    std::cout << "  Header: 0x" << std::hex << std::uppercase << std::setw(2) << std::setfill('0') 
              << static_cast<int>(data[0]) << " 0x" << std::setw(2) << static_cast<int>(data[1]) << std::endl;
    std::cout << std::dec << std::nouppercase;
    std::cout << "  x_move:  " << std::setw(5) << x_move << std::endl;
    std::cout << "  y_move:  " << std::setw(5) << y_move << std::endl;
    std::cout << "  yaw:     " << std::setw(5) << yaw << std::endl;
    std::cout << "  pitch:   " << std::setw(5) << pitch << std::endl;
    std::cout << "  feed:    " << std::setw(5) << feed << std::endl;
    std::cout << "  switch:  L=" << static_cast<int>(left_switch) << " R=" << static_cast<int>(right_switch) << std::endl;
    std::cout << "  CRC:     0x" << std::hex << std::uppercase << std::setw(2) << std::setfill('0') 
              << static_cast<int>(crc) << std::endl;
    std::cout << std::dec << std::nouppercase;
    std::cout << "  Tail:    0x" << std::hex << std::uppercase << std::setw(2) << std::setfill('0') 
              << static_cast<int>(data[14]) << std::endl;
    std::cout << std::dec << std::nouppercase << std::endl;
}

// 初始化串口
int initSerial(const char* port, int baudrate) {
    int fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY);
    if (fd < 0) {
        std::cerr << "无法打开串口 " << port << ": " << strerror(errno) << std::endl;
        return -1;
    }
    
    struct termios tty;
    memset(&tty, 0, sizeof(tty));
    
    if (tcgetattr(fd, &tty) != 0) {
        std::cerr << "tcgetattr 错误: " << strerror(errno) << std::endl;
        close(fd);
        return -1;
    }
    
    // 设置波特率
    cfsetospeed(&tty, B115200);
    cfsetispeed(&tty, B115200);
    
    // 8N1
    tty.c_cflag &= ~PARENB;
    tty.c_cflag &= ~CSTOPB;
    tty.c_cflag &= ~CSIZE;
    tty.c_cflag |= CS8;
    tty.c_cflag |= CREAD | CLOCAL;
    tty.c_cflag &= ~CRTSCTS;
    
    // 原始模式
    tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
    tty.c_iflag &= ~(IXON | IXOFF | IXANY);
    tty.c_oflag &= ~OPOST;
    
    // 设置超时
    tty.c_cc[VMIN] = 0;
    tty.c_cc[VTIME] = 1;
    
    if (tcsetattr(fd, TCSANOW, &tty) != 0) {
        std::cerr << "tcsetattr 错误: " << strerror(errno) << std::endl;
        close(fd);
        return -1;
    }
    
    tcflush(fd, TCIOFLUSH);
    std::cout << "串口 " << port << " 打开成功" << std::endl;
    return fd;
}

int main(int argc, char* argv[]) {
    // 设置信号处理
    signal(SIGINT, signalHandler);
    signal(SIGTERM, signalHandler);
    
    // 获取串口设备
    const char* port = (argc > 1) ? argv[1] : DEFAULT_SERIAL_PORT;
    
    std::cout << "========================================" << std::endl;
    std::cout << "  串口调试程序 - Serial Debugger" << std::endl;
    std::cout << "  设备: " << port << std::endl;
    std::cout << "  波特率: " << BAUDRATE << std::endl;
    std::cout << "  按 Ctrl+C 停止" << std::endl;
    std::cout << "========================================" << std::endl << std::endl;
    
    // 初始化串口
    int fd = initSerial(port, BAUDRATE);
    if (fd < 0) {
        return -1;
    }
    
    uint8_t buffer[256];
    std::vector<uint8_t> frame_buffer;
    frame_buffer.reserve(FRAME_LENGTH);
    
    int frame_count = 0;
    int valid_count = 0;
    auto start_time = std::chrono::steady_clock::now();
    
    while (running) {
        // 读取数据
        ssize_t n = read(fd, buffer, sizeof(buffer));
        
        if (n > 0) {
            for (ssize_t i = 0; i < n; i++) {
                frame_buffer.push_back(buffer[i]);
                
                // 查找帧头
                if (frame_buffer.size() >= 2 && 
                    frame_buffer[0] == FRAME_HEADER_1 && 
                    frame_buffer[1] == FRAME_HEADER_2) {
                    
                    // 等待完整帧
                    if (frame_buffer.size() >= FRAME_LENGTH) {
                        frame_count++;
                        
                        // 打印帧号和时间
                        auto now = std::chrono::steady_clock::now();
                        auto elapsed = std::chrono::duration_cast<std::chrono::seconds>(now - start_time).count();
                        std::cout << "\n========== Frame #" << frame_count 
                                  << " (Time: " << elapsed << "s) ==========" << std::endl;
                        
                        // 打印原始数据
                        printRawData(frame_buffer.data(), FRAME_LENGTH);
                        
                        // 检查帧尾
                        if (frame_buffer[FRAME_LENGTH - 1] == FRAME_TAIL) {
                            // 解析并打印数据
                            SensorData sensor;
                            if (parseFrame(frame_buffer.data(), sensor)) {
                                valid_count++;
                                std::cout << "Status: VALID" << std::endl;
                                printSensorData(sensor);
                            }
                            
                            // 打印帧详情
                            printFrameDetails(frame_buffer.data());
                        } else {
                            std::cout << "Status: INVALID (Frame tail error)" << std::endl;
                            std::cout << "Expected: 0x" << std::hex << std::uppercase 
                                      << std::setw(2) << std::setfill('0') 
                                      << static_cast<int>(FRAME_TAIL) << std::endl;
                            std::cout << "Got:      0x" << std::setw(2) 
                                      << static_cast<int>(frame_buffer[FRAME_LENGTH - 1]) << std::endl;
                            std::cout << std::dec << std::nouppercase << std::endl;
                        }
                        
                        // 清空缓冲区
                        frame_buffer.clear();
                    }
                } else if (frame_buffer.size() > FRAME_LENGTH) {
                    // 缓冲区溢出，丢弃数据
                    frame_buffer.erase(frame_buffer.begin());
                }
            }
        } else if (n < 0 && errno != EAGAIN) {
            std::cerr << "读取错误: " << strerror(errno) << std::endl;
        }
        
        std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }
    
    // 打印统计信息
    std::cout << "\n========================================" << std::endl;
    std::cout << "  统计信息:" << std::endl;
    std::cout << "  总帧数: " << frame_count << std::endl;
    std::cout << "  有效帧: " << valid_count << std::endl;
    std::cout << "  有效率: " << (frame_count > 0 ? (100.0 * valid_count / frame_count) : 0) << "%" << std::endl;
    std::cout << "========================================" << std::endl;
    
    close(fd);
    std::cout << "串口已关闭" << std::endl;
    return 0;
}