/**
 * @file test_yaw_swing.cpp
 * @brief 测试程序：yaw 轴左右摆动测试
 *
 * 发送序列: yaw=100 保持1秒 -> yaw=-100 保持1秒 -> 循环
 * 用法: ./test_yaw_swing [循环次数]
 * 示例: ./test_yaw_swing 3  (循环3次后停止，默认无限循环)
 *
 * 协议帧格式（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;

// 测试参数
constexpr int16_t YAW_RIGHT = 100;   // 向右100
constexpr int16_t YAW_LEFT = -100;   // 向左-100
constexpr int HOLD_TIME_MS = 1000;   // 保持1秒
constexpr int SEND_INTERVAL_MS = 20; // 50Hz

// 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;
        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);

    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 << "串口打开成功" << std::endl;
    return fd;
}

void sendControlFrame(int fd, int16_t yaw_val) {
    // 控制参数
    int16_t x_move = 0;
    int16_t y_move = 0;
    int16_t pitch_val = 0;
    int16_t feed = 0;
    uint8_t left_switch = 3;
    uint8_t right_switch = 3;

    uint8_t frame[FRAME_LENGTH];
    int idx = 0;

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

    // 平动左右
    frame[idx++] = x_move & 0xFF;
    frame[idx++] = (x_move >> 8) & 0xFF;

    // 平动前后
    frame[idx++] = y_move & 0xFF;
    frame[idx++] = (y_move >> 8) & 0xFF;

    // 云台偏航 (yaw)
    frame[idx++] = yaw_val & 0xFF;
    frame[idx++] = (yaw_val >> 8) & 0xFF;

    // 云台俯仰
    frame[idx++] = pitch_val & 0xFF;
    frame[idx++] = (pitch_val >> 8) & 0xFF;

    // 拨弹轮
    frame[idx++] = feed & 0xFF;
    frame[idx++] = (feed >> 8) & 0xFF;

    // 拨杆
    frame[idx++] = (left_switch << 4) | right_switch;

    // CRC8
    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::string direction = (yaw_val > 0) ? "右" : "左";
    std::cout << "[TX](" << FRAME_LENGTH << "B): " << frame_hex
              << "| yaw=" << yaw_val << " (" << direction << ")" << std::endl;

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

// 发送指定yaw值，保持指定时间
void sendYawForDuration(int fd, int16_t yaw_val, int duration_ms) {
    auto start = std::chrono::steady_clock::now();
    int count = 0;
    
    while (true) {
        auto now = std::chrono::steady_clock::now();
        auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(now - start).count();
        
        if (elapsed >= duration_ms) {
            break;
        }
        
        sendControlFrame(fd, yaw_val);
        count++;
        std::this_thread::sleep_for(std::chrono::milliseconds(SEND_INTERVAL_MS));
    }
    
    std::cout << "  发送了 " << count << " 帧，时长 " << duration_ms << "ms" << std::endl;
}

void printUsage(const char* program) {
    std::cout << "用法: " << program << " [循环次数]" << std::endl;
    std::cout << "  循环次数: 测试循环次数，0或不填表示无限循环" << std::endl;
    std::cout << std::endl;
    std::cout << "示例:" << std::endl;
    std::cout << "  " << program << "       # 无限循环 (按 Ctrl+C 停止)" << std::endl;
    std::cout << "  " << program << " 5     # 循环5次后停止" << std::endl;
}

int main(int argc, char* argv[]) {
    int loop_count = 0;  // 0表示无限循环
    int current_loop = 0;

    if (argc > 1) {
        loop_count = std::atoi(argv[1]);
    }
    if (argc > 2) {
        printUsage(argv[0]);
        return 1;
    }

    std::cout << "========================================" << std::endl;
    std::cout << "Yaw 轴摆动测试程序" << std::endl;
    std::cout << "测试序列: yaw=" << YAW_RIGHT << "(右,1s) -> yaw=" << YAW_LEFT << "(左,1s)" << std::endl;
    if (loop_count > 0) {
        std::cout << "循环次数: " << loop_count << " 次" << 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;
    }

    std::cout << "----------------------------------------" << std::endl;

    // 主循环
    while (true) {
        if (loop_count > 0 && current_loop >= loop_count) {
            std::cout << "完成 " << loop_count << " 次循环，测试结束" << std::endl;
            break;
        }

        current_loop++;
        if (loop_count > 0) {
            std::cout << "\n=== 第 " << current_loop << "/" << loop_count << " 次循环 ===" << std::endl;
        } else {
            std::cout << "\n=== 第 " << current_loop << " 次循环 ===" << std::endl;
        }

        // 步骤1: yaw=100 (向右) 保持1秒
        std::cout << "【步骤1】yaw=" << YAW_RIGHT << " (向右) 保持 " << HOLD_TIME_MS << "ms" << std::endl;
        sendYawForDuration(fd, YAW_RIGHT, HOLD_TIME_MS);

        // 步骤2: yaw=-100 (向左) 保持1秒
        std::cout << "【步骤2】yaw=" << YAW_LEFT << " (向左) 保持 " << HOLD_TIME_MS << "ms" << std::endl;
        sendYawForDuration(fd, YAW_LEFT, HOLD_TIME_MS);
    }

    close(fd);
    std::cout << "串口已关闭" << std::endl;
    return 0;
}