/**
 * 协议层实现
 * NRF24 发射 / SBUS 接收 / USB HID Joystick
 */
#include "protocol.h"
#include "usart.h"
#include "usb.h"
#include <string.h>

/* 回传数据 */
uint16_t telem_voltage = 0;
uint8_t  telem_rssi = 0;
uint8_t  telem_packet_loss = 0;
uint16_t telem_temp = 0;

/* 协议状态 */
static uint32_t last_rf_send = 0;
static uint32_t rf_interval_ms = 10; /* 默认 100Hz */

/* SBUS 接收缓冲 */
static uint8_t  sbus_buf[SBUS_FRAME_SIZE];
static uint8_t  sbus_idx = 0;
static uint8_t  sbus_got_frame = 0;

/* USB HID 报告缓冲 */
static uint8_t hid_report[HID_REPORT_SIZE];

void Protocol_Init(void) {
    last_rf_send = 0;
    sbus_idx = 0;
    sbus_got_frame = 0;
    memset(hid_report, 0, sizeof(hid_report));
}

/* ---- NRF24 协议 ---- */
void Protocol_SendRFPacket(void) {
    uint8_t packet[RF_PACKET_SIZE];
    memset(packet, 0, RF_PACKET_SIZE);

    /* 包头 */
    packet[0] = RF_SYNC_BYTE0;
    packet[1] = RF_SYNC_BYTE1;

    /* 对频短语 (6字节) */
    memcpy(&packet[2], g_config.nrf_phrase, 6);

    /* 通道数据 (16通道 * 2字节 = 32字节, 但只取前16字节核心通道) */
    /* 每个通道用 int16_t, 压缩为 2 bytes */
    for (uint8_t i = 0; i < 8; i++) {
        int16_t val = g_channels[i];
        /* 范围 -1000 ~ +1000, 映射到 0~2000 */
        uint16_t mapped = (uint16_t)(val + 1000);
        packet[8 + i * 2] = (mapped >> 8) & 0xFF;
        packet[8 + i * 2 + 1] = mapped & 0xFF;
    }

    /* 校验和 (简单 XOR) */
    uint8_t checksum = 0;
    for (uint8_t i = 0; i < RF_PACKET_SIZE - 1; i++)
        checksum ^= packet[i];
    packet[RF_PACKET_SIZE - 1] = checksum;

    /* 发送 */
    NRF24L01_TxPacket(packet, RF_PACKET_SIZE);
}

void Protocol_ProcessRF(void) {
    /* 接收回传数据 */
    uint8_t rx_buf[32];
    if (NRF24L01_RxPacket(rx_buf)) {
        /* 解析回传包 */
        if (rx_buf[0] == 0xBB && rx_buf[1] == 0x44) {
            telem_voltage = (rx_buf[2] << 8) | rx_buf[3];
            telem_rssi = rx_buf[4];
            telem_packet_loss = rx_buf[5];
            telem_temp = (rx_buf[6] << 8) | rx_buf[7];
        }
    }
}

/* ---- SBUS 协议 ---- */
void Protocol_SBUS_Init(void) {
    /* USART1 配置为 SBUS 模式 (100000 baud, 8E2) */
    huart1.Init.BaudRate = SBUS_BAUDRATE;
    huart1.Init.WordLength = UART_WORDLENGTH_9B;
    huart1.Init.Parity = UART_PARITY_EVEN;
    huart1.Init.StopBits = UART_STOPBITS_2;
    if (HAL_UART_Init(&huart1) != HAL_OK) {
        Error_Handler();
    }
    /* 启动接收中断 */
    HAL_UART_Receive_IT(&huart1, sbus_buf, SBUS_FRAME_SIZE);
}

uint8_t Protocol_SBUS_Parse(const uint8_t *frame, int16_t *channels) {
    /* SBUS 帧: 0x0F 起始, 0x00 结束 */
    if (frame[0] != 0x0F || frame[24] != 0x00) return 0;

    /* 解析 16 通道 (11-bit each) */
    channels[0]  = ((frame[1]     | frame[2]<<8)                     & 0x07FF);
    channels[1]  = ((frame[2]>>3  | frame[3]<<5)                     & 0x07FF);
    channels[2]  = ((frame[3]>>6  | frame[4]<<2 | frame[5]<<10)      & 0x07FF);
    channels[3]  = ((frame[5]>>1  | frame[6]<<7)                     & 0x07FF);
    channels[4]  = ((frame[6]>>4  | frame[7]<<4)                     & 0x07FF);
    channels[5]  = ((frame[7]>>7  | frame[8]<<1 | frame[9]<<9)       & 0x07FF);
    channels[6]  = ((frame[9]>>2  | frame[10]<<6)                    & 0x07FF);
    channels[7]  = ((frame[10]>>5 | frame[11]<<3)                    & 0x07FF);
    channels[8]  = ((frame[12]    | frame[13]<<8)                    & 0x07FF);
    channels[9]  = ((frame[13]>>3 | frame[14]<<5)                    & 0x07FF);
    channels[10] = ((frame[14]>>6 | frame[15]<<2 | frame[16]<<10)    & 0x07FF);
    channels[11] = ((frame[16]>>1 | frame[17]<<7)                    & 0x07FF);
    channels[12] = ((frame[17]>>4 | frame[18]<<4)                    & 0x07FF);
    channels[13] = ((frame[18]>>7 | frame[19]<<1 | frame[20]<<9)     & 0x07FF);
    channels[14] = ((frame[20]>>2 | frame[21]<<6)                    & 0x07FF);
    channels[15] = ((frame[21]>>5 | frame[22]<<3)                    & 0x07FF);

    /* SBUS 范围: 172~1811, 映射到 -1000~+1000 */
    for (uint8_t i = 0; i < 16; i++) {
        channels[i] = (int16_t)((int32_t)(channels[i] - 992) * 1000 / 819);
        if (channels[i] > 1000) channels[i] = 1000;
        if (channels[i] < -1000) channels[i] = -1000;
    }

    return 1;
}

/* ---- USB HID Joystick ---- */
void Protocol_HID_Init(void) {
    /* USB 已在 MX_USB_PCD_Init 中初始化 */
    /* HID 描述符需在 usbd_hid.c 中配置 (通常由 CubeMX 生成) */
}

void Protocol_HID_Send(void) {
    /* 构造 HID Joystick 报告 */
    /* 标准 Joystick 报告: 8 axis (16-bit) + buttons */
    memset(hid_report, 0, HID_REPORT_SIZE);

    /* X, Y, Z, Rx, Ry, Rz, Slider, Dial */
    for (uint8_t i = 0; i < 8 && i < CHANNEL_COUNT; i++) {
        /* 映射 -1000~+1000 到 16-bit */
        int16_t val = g_channels[i];
        hid_report[i * 2] = val & 0xFF;
        hid_report[i * 2 + 1] = (val >> 8) & 0xFF;
    }

    /* 发送 HID 报告 (需要 USB HID 库支持) */
    /* USBD_HID_SendReport(&hUsbDeviceFS, hid_report, HID_REPORT_SIZE); */
}

/* ---- 主循环 ---- */
void Protocol_Run(void) {
    uint32_t now = HAL_GetTick();

    /* 根据刷新率发送 RF 数据 */
    if (now - last_rf_send >= rf_interval_ms) {
        last_rf_send = now;

        if (g_config.rf_type == 0) {
            /* NRF24 模式 */
            Protocol_SendRFPacket();
            /* 短暂切换到接收模式收遥测 */
            NRF24L01_RXMode();
            HAL_Delay(1);
            Protocol_ProcessRF();
            NRF24L01_TXMode();
        } else {
            /* CRFS 模式 (通过 USART3 发送) */
            /* TODO: CRFS 协议实现 */
        }
    }

    /* USB HID 发送 (每 10ms) */
    static uint32_t last_hid = 0;
    if (now - last_hid >= 10) {
        last_hid = now;
        Protocol_HID_Send();
    }
}