/**
 * @file test_pitch_up.cpp
 * @brief 测试程序：单独控制 pitch 轴运动（支持命令行参数）
 *
 * 用法: ./test_pitch_up [pitch角度] [运行秒数]
 * 示例: ./test_pitch_up 1.5 10  (向上1.5度，运行10秒)
 *
 * 协议帧格式（15字节）：
 * | 0xBB | 0x77 | x_move(2B) | y_move(2B) | yaw(2B) | pitch(2B) | feed(2B) | switch(1B) | CRC8 | 0xEE |
 */

#include <cstring>
#include <iostream>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <chrono>
#include <thread>
#include <cstdlib>

// 帧定义 - 与示例程序完全一致
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 *SERIAL_PORT = "/dev/ttyCH340";
constexpr int BAUDRATE = 115200;

// CRC8 计算 - 与示例程序完全一致
uint8_t calculateCRC8(const uint8_t *data, size_t len) {
    uint8_t crc = 0xFF;
    for (size_t i = 0; i < len; i++) {
        crc ^= data[i];
        for (int j = 0; j < 8; j++) {
            if (crc & 0x80) {
                crc = (crc << 1) ^ 0x31;
            } else {
                crc <<= 1;
            }
        }
    }
    return crc;
}

// 波特率转换 - 与示例程序一致
speed_t convertBaudrate(int baudrate) {
    switch (baudrate) {
        case 9600: return B9600;
        case 19200: return B19200;
        case 38400: return B38400;
        case 57600: return B57600;
        case 115200: return B115200;
        case 230400: return B230400;
        case 460800: return B460800;
        case 921600: return B921600;
        default: return B115200;
    }
}

int initSerial(const char *port, int baudrate) {
    std::cout << "正在打开串口 " << port << "..." << std::endl;

    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;
    }

    // 设置波特率
    speed_t baud = convertBaudrate(baudrate);
    cfsetospeed(&tty, baud);
    cfsetispeed(&tty, baud);

    // 8N1 配置 - 与示例程序一致
    tty.c_cflag &= ~PARENB;  // 无校验
    tty.c_cflag &= ~CSTOPB;  // 1位停止位
    tty.c_cflag &= ~CSIZE;
    tty.c_cflag |= CS8;             // 8位数据
    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;  // 100ms 超时

    if (tcsetattr(fd, TCSANOW, &tty) != 0) {
        std::cerr << "tcsetattr 错误: " << strerror(errno) << std::endl;
        close(fd);
        return -1;
    }

    // 清空缓冲区
    tcflush(fd, TCIOFLUSH);
    std::cout << "串口打开成功" << std::endl;
    return fd;
}

void sendControlFrame(int fd, int16_t pitch_val) {
    // 控制参数
    int16_t x_move = -100;           // 平动左右 [-660, 660]
    int16_t y_move = -200;           // 平动前后 [-660, 660]
    int16_t yaw_val = -300;          // 云台偏航 [-660, 660]
    int16_t feed = -400;             // 拨弹轮 [-660, 660]
    uint8_t left_switch = 3;      // 左拨杆 [1, 3] 2 摩擦轮打开 3 关闭
    uint8_t right_switch = 3;     // 右拨杆 [1, 3] 2 小陀螺 3 关闭

    // 构建数据帧 - 与示例程序完全一致
    uint8_t frame[FRAME_LENGTH];
    int idx = 0;

    // 帧头
    frame[idx++] = FRAME_HEADER_1;
    frame[idx++] = FRAME_HEADER_2;

    // 平动左右 (2 bytes, int16, 小端序)
    frame[idx++] = x_move & 0xFF;
    frame[idx++] = (x_move >> 8) & 0xFF;

    // 平动前后 (2 bytes, 小端序)
    frame[idx++] = y_move & 0xFF;
    frame[idx++] = (y_move >> 8) & 0xFF;

    // 云台偏航 (2 bytes, 小端序)
    frame[idx++] = yaw_val & 0xFF;
    frame[idx++] = (yaw_val >> 8) & 0xFF;

    // 云台俯仰 (2 bytes, 小端序) - pitch 值
    frame[idx++] = pitch_val & 0xFF;
    frame[idx++] = (pitch_val >> 8) & 0xFF;

    // 拨弹轮 (2 bytes, 小端序)
    frame[idx++] = feed & 0xFF;
    frame[idx++] = (feed >> 8) & 0xFF;

    // 拨杆 (1 byte: 高4位左拨杆，低4位右拨杆)
    frame[idx++] = (left_switch << 4) | right_switch;

    // CRC8 (固定为 0xCC)
    frame[idx++] = 0xCC;

    // 帧尾
    frame[idx++] = FRAME_TAIL;

    // 调试输出：打印完整帧内容 - 与示例程序一致
    std::string frame_hex;
    char buf[4];
    for (int i = 0; i < FRAME_LENGTH; i++) {
        snprintf(buf, sizeof(buf), "%02X ", frame[i]);
        frame_hex += buf;
    }
    std::cout << "[TX](" << FRAME_LENGTH << "B): " << frame_hex
              << "| pitch=" << pitch_val << std::endl;

    // 发送数据
    ssize_t written = write(fd, frame, FRAME_LENGTH);
    if (written != FRAME_LENGTH) {
        std::cerr << "发送数据不完整: " << written << "/" << FRAME_LENGTH << std::endl;
    }
}

void printUsage(const char* program) {
    std::cout << "用法: " << program << " [pitch角度] [运行秒数]" << std::endl;
    std::cout << "  pitch角度: 目标角度，正值向上，负值向下，范围建议 (-3, 3) 度" << std::endl;
    std::cout << "  运行秒数:  发送时长，0或不填表示无限循环" << std::endl;
    std::cout << std::endl;
    std::cout << "示例:" << std::endl;
    std::cout << "  " << program << "          # 默认向上1度，无限循环" << std::endl;
    std::cout << "  " << program << " 1.5      # 向上1.5度，无限循环" << std::endl;
    std::cout << "  " << program << " -1 5     # 向下1度，运行5秒" << std::endl;
    std::cout << "  " << program << " 2 10     # 向上2度，运行10秒" << std::endl;
}

int main(int argc, char* argv[]) {
    // 默认参数
    double pitch_angle = -1;  // 默认向上1度
    int duration_sec = 0;      // 默认无限循环

    // 解析命令行参数
    if (argc > 1) {
        pitch_angle = std::atof(argv[1]);
    }
    if (argc > 2) {
        duration_sec = std::atoi(argv[2]);
    }
    if (argc > 3) {
        printUsage(argv[0]);
        return 1;
    }

    std::cout << "========================================" << std::endl;
    std::cout << "Pitch 轴控制测试程序" << std::endl;
    std::cout << "目标角度: " << (pitch_angle > 0 ? "向上 " : "向下 ") 
              << std::abs(pitch_angle) << " 度" << std::endl;
    if (duration_sec > 0) {
        std::cout << "运行时长: " << duration_sec << " 秒" << std::endl;
    } else {
        std::cout << "运行时长: 无限循环 (按 Ctrl+C 停止)" << std::endl;
    }
    std::cout << "发送频率: 50 Hz" << std::endl;
    std::cout << "========================================" << std::endl;

    // 初始化串口
    int fd = initSerial(SERIAL_PORT, BAUDRATE);
    if (fd < 0) {
        return -1;
    }

    // 计算 pitch 值
    // 映射系数 220: 3度 -> 660
    int16_t pitch_val = static_cast<int16_t>(pitch_angle * 220);
    if (pitch_val > 660) pitch_val = 660;
    if (pitch_val < -660) pitch_val = -660;

    std::cout << "Pitch 发送值: " << pitch_val << std::endl;
    std::cout << "----------------------------------------" << std::endl;

    // 计算结束时间
    auto start_time = std::chrono::steady_clock::now();
    auto end_time = start_time + std::chrono::seconds(duration_sec);

    // 持续发送，频率 50Hz
    while (true) {
        // 检查是否超时
        if (duration_sec > 0 && std::chrono::steady_clock::now() >= end_time) {
            std::cout << "运行时间到达 " << duration_sec << " 秒，停止发送" << std::endl;
            break;
        }

        sendControlFrame(fd, pitch_val);
        std::this_thread::sleep_for(std::chrono::milliseconds(20));  // 50Hz
    }

    close(fd);
    return 0;
}