/*******************************************************************************
 * File Name         : SBUS.c
 * Description       : SBUS RC receiver protocol parser implementation
 * Created on        : 2025-11-13
 * Author            : Auto-generated
 *******************************************************************************/

/*******************************************************************************
 *                              Include Files                                  *
 *******************************************************************************/
#include "SBUS.h"
#include "Uart.h"
#include "stm32h7xx_hal.h"
#include "stm32h7xx_hal_uart.h"
#include "stm32h7xx_hal_dma.h"
#include <string.h>
#include "pwm_ctrol.h"
#include "PythonLink.h"

/*******************************************************************************
 *                                  Macro                                      *
 *******************************************************************************/
#define SBUS_TIMEOUT_MS 100

// 调试开关：设置为 1 启用调试输出（需要 printf 重定向），设置为 0 禁用
#define SBUS_DEBUG_ENABLE           0

#if SBUS_DEBUG_ENABLE
    #define SBUS_DEBUG_PRINT(...)   printf(__VA_ARGS__)
#else
    #define SBUS_DEBUG_PRINT(...)   ((void)0)
#endif

/*******************************************************************************
 *                             Global Variables                                *
 *******************************************************************************/
// SBUS 接收缓冲区 (DMA 写入)
static HIDO_UINT8 g_au8SBUSRxBuf[SBUS_UART_RX_BUF_SIZE] = {0};

// SBUS 当前通道数据
static ST_SBUSData g_stSBUSData = {0};

// 临时解析缓冲区 (用于帧边界对齐)
static HIDO_UINT8 g_au8ParseBuf[SBUS_FRAME_SIZE] = {0};
static HIDO_UINT8 g_u8ParseBufIdx = 0;

// 调试统计
static HIDO_UINT32 g_u32DebugPollCount = 0;
static HIDO_UINT32 g_u32DebugLastDmaRemaining = 0;
// SBUS failsafe active flag
static volatile HIDO_UINT8 g_bSBUSFailsafeActive = 0;

// 控制模式状态 (0=手动遥控器, 1=自动Python)
static volatile HIDO_UINT8 g_u8ControlMode = 0;

/*******************************************************************************
 *                             Local Function                                  *
 *******************************************************************************/

/**
 * @brief 解析单个 SBUS 帧 (25 字节)
 * @param _pu8Frame: 帧数据指针 (必须指向完整的 25 字节)
 * @return HIDO_TRUE: 解析成功, HIDO_FALSE: 校验失败
 */
static HIDO_INT32 SBUS_ParseFrame(const HIDO_UINT8 *_pu8Frame)
{
    // 1. 校验帧头和帧尾
    if (_pu8Frame[0] != SBUS_HEADER || _pu8Frame[24] != SBUS_FOOTER)
    {
        g_stSBUSData.m_u32ErrorCount++;
        return HIDO_FALSE;
    }

    // 2. 提取 16 个 11-bit 通道 (bit-packed in bytes 1~22)
    // SBUS 协议: 176 bits (16 channels * 11 bits) 存储在 22 字节中
    // Channel layout (LSB first):
    // Byte 1:   Ch0[0:7]
    // Byte 2:   Ch0[8:10] | Ch1[0:4]
    // Byte 3:   Ch1[5:10] | Ch2[0:1]
    // ... (依次类推)
    
    g_stSBUSData.m_au16Channels[0]  = (HIDO_UINT16)((_pu8Frame[1]      | _pu8Frame[2] << 8)                    & 0x07FF);
    g_stSBUSData.m_au16Channels[1]  = (HIDO_UINT16)((_pu8Frame[2] >> 3 | _pu8Frame[3] << 5)                    & 0x07FF);
    g_stSBUSData.m_au16Channels[2]  = (HIDO_UINT16)((_pu8Frame[3] >> 6 | _pu8Frame[4] << 2 | _pu8Frame[5] << 10) & 0x07FF);
    g_stSBUSData.m_au16Channels[3]  = (HIDO_UINT16)((_pu8Frame[5] >> 1 | _pu8Frame[6] << 7)                    & 0x07FF);
    g_stSBUSData.m_au16Channels[4]  = (HIDO_UINT16)((_pu8Frame[6] >> 4 | _pu8Frame[7] << 4)                    & 0x07FF);
    g_stSBUSData.m_au16Channels[5]  = (HIDO_UINT16)((_pu8Frame[7] >> 7 | _pu8Frame[8] << 1 | _pu8Frame[9] << 9)  & 0x07FF);
    g_stSBUSData.m_au16Channels[6]  = (HIDO_UINT16)((_pu8Frame[9] >> 2 | _pu8Frame[10] << 6)                   & 0x07FF);
    g_stSBUSData.m_au16Channels[7]  = (HIDO_UINT16)((_pu8Frame[10] >> 5 | _pu8Frame[11] << 3)                  & 0x07FF);
    g_stSBUSData.m_au16Channels[8]  = (HIDO_UINT16)((_pu8Frame[12]      | _pu8Frame[13] << 8)                  & 0x07FF);
    g_stSBUSData.m_au16Channels[9]  = (HIDO_UINT16)((_pu8Frame[13] >> 3 | _pu8Frame[14] << 5)                  & 0x07FF);
    g_stSBUSData.m_au16Channels[10] = (HIDO_UINT16)((_pu8Frame[14] >> 6 | _pu8Frame[15] << 2 | _pu8Frame[16] << 10) & 0x07FF);
    g_stSBUSData.m_au16Channels[11] = (HIDO_UINT16)((_pu8Frame[16] >> 1 | _pu8Frame[17] << 7)                  & 0x07FF);
    g_stSBUSData.m_au16Channels[12] = (HIDO_UINT16)((_pu8Frame[17] >> 4 | _pu8Frame[18] << 4)                  & 0x07FF);
    g_stSBUSData.m_au16Channels[13] = (HIDO_UINT16)((_pu8Frame[18] >> 7 | _pu8Frame[19] << 1 | _pu8Frame[20] << 9) & 0x07FF);
    g_stSBUSData.m_au16Channels[14] = (HIDO_UINT16)((_pu8Frame[20] >> 2 | _pu8Frame[21] << 6)                  & 0x07FF);
    g_stSBUSData.m_au16Channels[15] = (HIDO_UINT16)((_pu8Frame[21] >> 5 | _pu8Frame[22] << 3)                  & 0x07FF);

    // 3. 提取标志位 (Byte 23)
    // Bit 0: CH17 digital
    // Bit 1: CH18 digital
    // Bit 2: Frame Lost
    // Bit 3: Failsafe
    g_stSBUSData.m_u8Ch17       = (_pu8Frame[23] & 0x01) ? 1 : 0;
    g_stSBUSData.m_u8Ch18       = (_pu8Frame[23] & 0x02) ? 1 : 0;
    g_stSBUSData.m_u8FrameLost  = (_pu8Frame[23] & 0x04) ? 1 : 0;
    g_stSBUSData.m_u8Failsafe   = (_pu8Frame[23] & 0x08) ? 1 : 0;

    // 4. 更新时间戳和计数
    g_stSBUSData.m_u32LastUpdateTick = HAL_GetTick();
    g_stSBUSData.m_u32FrameCount++;

    // 5. 检查 CH8 (通道7, 索引7) 决定控制模式
    // CH8 > 1500: 自动模式 (Python控制)
    // CH8 < 1500: 手动模式 (遥控器控制)
    HIDO_UINT16 ch8_value = g_stSBUSData.m_au16Channels[7];
    if (ch8_value > 1500)
    {
        // 自动模式: 不更新 PWM, 由 Python 控制
        g_u8ControlMode = 1;
    }
    else
    {
        // 手动模式: 使用遥控器信号控制
        g_u8ControlMode = 0;
    }

    // 解析成功后立即触发 PWM 控制（最小延迟）
    // 同时清除 failsafe 标志（如果之前触发过）
    if (g_bSBUSFailsafeActive)
    {
        g_bSBUSFailsafeActive = 0;
    }
    
    // 只有在手动模式下才更新 PWM
    if (g_u8ControlMode == 0)
    {
        SBUS_Control_PWM();
    }

    return HIDO_TRUE;
}

/**
 * @brief 从 DMA 缓冲区中查找并解析 SBUS 帧
 * @param _pu8Buf: DMA 缓冲区指针
 * @param _u32Len: 可读数据长度
 * @return 无
 */
static HIDO_VOID SBUS_ProcessBuffer(const HIDO_UINT8 *_pu8Buf, HIDO_UINT32 _u32Len)
{
    static HIDO_UINT32 s_u32TotalBytesProcessed = 0;
    
    for (HIDO_UINT32 i = 0; i < _u32Len; i++)
    {
        HIDO_UINT8 byte = _pu8Buf[i];
        s_u32TotalBytesProcessed++;

        // 状态机: 寻找帧头 0x0F
        if (g_u8ParseBufIdx == 0)
        {
            if (byte == SBUS_HEADER)
            {
                g_au8ParseBuf[g_u8ParseBufIdx++] = byte;
            }
            // 否则丢弃非帧头字节
        }
        else
        {
            g_au8ParseBuf[g_u8ParseBufIdx++] = byte;

            // 收满 25 字节后尝试解析
            if (g_u8ParseBufIdx >= SBUS_FRAME_SIZE)
            {
                SBUS_ParseFrame(g_au8ParseBuf);
                g_u8ParseBufIdx = 0;  // 重置状态机
            }
        }
    }
}

/*******************************************************************************
 *                             Global Function                                 *
 *******************************************************************************/

/**
 * @brief 初始化 SBUS 接收模块
 */
HIDO_INT32 SBUS_Init(HIDO_VOID)
{
    HIDO_INT32 ret;
    ST_UartInit stUartInit;
    UART_HandleTypeDef *pUart = NULL;

    // 先检查 UART 是否已经注册
    ret = Uart_GetHandle(UART_ID_SBUS, (HIDO_VOID **)&pUart);
    
    if (pUart == NULL)
    {
        return HIDO_FALSE;
    }

    // 初始化 SBUS 数据结构
    memset(&g_stSBUSData, 0, sizeof(g_stSBUSData));
    memset(g_au8ParseBuf, 0, sizeof(g_au8ParseBuf));
    memset(g_au8SBUSRxBuf, 0, sizeof(g_au8SBUSRxBuf));
    g_u8ParseBufIdx = 0;
    g_u32DebugPollCount = 0;
    g_u32DebugLastDmaRemaining = 0;

    // 配置 UART4 接收 (DMA 模式)
    stUartInit.m_eRxMode = UART_RX_MODE_DMA;
    stUartInit.m_pu8RxBuf = g_au8SBUSRxBuf;
    stUartInit.m_u32RxBufSize = SBUS_UART_RX_BUF_SIZE;

    // 不使用发送功能 (SBUS 仅单向接收)
    stUartInit.m_eTxMode = UART_TX_MODE_POLL;
    stUartInit.m_pu8TxBuf = NULL;
    stUartInit.m_u32TxBufSize = 0;
    stUartInit.m_u32TxQueueMemberCnt = 0;
    stUartInit.m_fnRxISR = NULL;

    ret = Uart_Init(UART_ID_SBUS, &stUartInit);
    if (ret != HIDO_OK)
    {
        return HIDO_FALSE;
    }

    // 获取 UART 句柄以验证初始化
    ret = Uart_GetHandle(UART_ID_SBUS, (HIDO_VOID **)&pUart);
    if (ret != HIDO_OK || pUart == NULL)
    {
        return HIDO_FALSE;
    }

    return HIDO_TRUE;
}

/**
 * @brief SBUS 轮询函数 (主循环调用)
 */
HIDO_VOID SBUS_Poll(HIDO_VOID)
{
    UART_HandleTypeDef *pUart = NULL;
    HIDO_INT32 ret;

    g_u32DebugPollCount++;

    // 获取 UART 句柄
    ret = Uart_GetHandle(UART_ID_SBUS, (HIDO_VOID **)&pUart);
    if (ret != HIDO_OK || pUart == NULL)
    {
        return;
    }

    // 检查 DMA 句柄
    if (pUart->hdmarx == NULL)
    {
        return;
    }

    // 计算 DMA 已接收的数据量
    // DMA->CNDTR 返回剩余未传输的数据个数，因此已接收 = BufSize - CNDTR
    HIDO_UINT32 dmaRemaining = __HAL_DMA_GET_COUNTER(pUart->hdmarx);
    HIDO_UINT32 receivedBytes = SBUS_UART_RX_BUF_SIZE - dmaRemaining;

    g_u32DebugLastDmaRemaining = dmaRemaining;

    // 处理新接收的数据
    // 注意: 这里使用环形缓冲区简单读取策略 (适合短周期轮询)
    static HIDO_UINT32 s_u32LastProcessedIdx = 0;

    if (receivedBytes > s_u32LastProcessedIdx)
    {
        HIDO_UINT32 newBytes = receivedBytes - s_u32LastProcessedIdx;
        SBUS_ProcessBuffer(&g_au8SBUSRxBuf[s_u32LastProcessedIdx], newBytes);
        s_u32LastProcessedIdx = receivedBytes;
    }

    // 处理 DMA 环形回绕 (当 CNDTR 重载时)
    if (receivedBytes < s_u32LastProcessedIdx)
    {
        // 先处理缓冲区尾部
        HIDO_UINT32 tailBytes = SBUS_UART_RX_BUF_SIZE - s_u32LastProcessedIdx;
        if (tailBytes > 0)
        {
            SBUS_ProcessBuffer(&g_au8SBUSRxBuf[s_u32LastProcessedIdx], tailBytes);
        }
        // 再处理缓冲区头部
        if (receivedBytes > 0)
        {
            SBUS_ProcessBuffer(&g_au8SBUSRxBuf[0], receivedBytes);
        }
        s_u32LastProcessedIdx = receivedBytes;
    }

    // Failsafe: 如果距离上次有效帧超过 1000ms，触发中立脉宽（1500us）
    if (!g_bSBUSFailsafeActive)
    {
        HIDO_UINT32 currentTick = HAL_GetTick();
        HIDO_UINT32 elapsed = currentTick - g_stSBUSData.m_u32LastUpdateTick;
        if (elapsed >= 1000)
        {
            // 触发一次性 failsafe：将转向与电机脉宽设为中立 1500us
            Set_Steering_Pulse(1500);
            Set_Motor_Pulse(1500);
            g_bSBUSFailsafeActive = 1;
        }
    }
    
    // 自动模式: 应用Python下发的控制命令
    if (g_u8ControlMode == 1)
    {
        HIDO_UINT16 steering_pwm = 1500;
        HIDO_UINT16 throttle_pwm = 1500;
        HIDO_UINT32 cmd_timestamp = 0;
        
        // 获取Python控制命令
        if (PythonLink_GetControl(&steering_pwm, &throttle_pwm, &cmd_timestamp) == HIDO_TRUE)
        {
            // 应用控制命令到PWM输出
            Set_Steering_Pulse(steering_pwm);
            Set_Motor_Pulse(throttle_pwm);
        }
        else
        {
            // 没有有效的Python命令时，使用安全的中位值
            Set_Steering_Pulse(1500);
            Set_Motor_Pulse(1500);
        }
    }
}

/**
 * @brief 获取 SBUS 通道数据副本
 */
HIDO_INT32 SBUS_GetData(ST_SBUSData *_pstData)
{
    if (_pstData == NULL)
    {
        return HIDO_FALSE;
    }

    // 检查信号是否有效 (最近 100ms 内收到帧)
    if (!SBUS_IsSignalValid(SBUS_TIMEOUT_MS))
    {
        return HIDO_FALSE;
    }

    // 拷贝数据 (简单拷贝, 若需线程安全可加临界区)
    memcpy(_pstData, &g_stSBUSData, sizeof(ST_SBUSData));
    return HIDO_TRUE;
}

/**
 * @brief 获取单个通道值
 */
HIDO_UINT16 SBUS_GetChannel(HIDO_UINT8 _u8Channel)
{
    if (_u8Channel >= SBUS_NUM_CHANNELS)
    {
        return SBUS_CENTER_VALUE;
    }

    if (!SBUS_IsSignalValid(SBUS_TIMEOUT_MS))
    {
        return SBUS_CENTER_VALUE;
    }

    return g_stSBUSData.m_au16Channels[_u8Channel];
}

/**
 * @brief 获取数字通道状态
 */
HIDO_UINT8 SBUS_GetDigitalChannel(HIDO_UINT8 _u8DigitalChannel)
{
    if (!SBUS_IsSignalValid(SBUS_TIMEOUT_MS))
    {
        return 0;
    }

    if (_u8DigitalChannel == 17)
    {
        return g_stSBUSData.m_u8Ch17;
    }
    else if (_u8DigitalChannel == 18)
    {
        return g_stSBUSData.m_u8Ch18;
    }

    return 0;
}

/**
 * @brief 检查 SBUS 信号是否有效
 */
HIDO_INT32 SBUS_IsSignalValid(HIDO_UINT32 _u32TimeoutMs)
{
    HIDO_UINT32 currentTick = HAL_GetTick();
    HIDO_UINT32 elapsed = currentTick - g_stSBUSData.m_u32LastUpdateTick;

    return (elapsed < _u32TimeoutMs) ? HIDO_TRUE : HIDO_FALSE;
}

/**
 * @brief 获取接收统计信息
 */
HIDO_VOID SBUS_GetStats(HIDO_UINT32 *_pu32FrameCount, HIDO_UINT32 *_pu32ErrorCount)
{
    if (_pu32FrameCount != NULL)
    {
        *_pu32FrameCount = g_stSBUSData.m_u32FrameCount;
    }
    if (_pu32ErrorCount != NULL)
    {
        *_pu32ErrorCount = g_stSBUSData.m_u32ErrorCount;
    }
}

/**
 * @brief 调试函数：打印 SBUS 状态和统计信息
 */
HIDO_VOID SBUS_PrintDebugInfo(HIDO_VOID)
{
    UART_HandleTypeDef *pUart = NULL;
    HIDO_INT32 ret;

    HIDO_Debug2("\r\n========== SBUS Debug Info ==========\r\n");
    HIDO_Debug2("Poll count:      %u\r\n", g_u32DebugPollCount);
    HIDO_Debug2("Frame count:     %u\r\n", g_stSBUSData.m_u32FrameCount);
    HIDO_Debug2("Error count:     %u\r\n", g_stSBUSData.m_u32ErrorCount);
    HIDO_Debug2("Last update:     %u ms ago\r\n", 
                HAL_GetTick() - g_stSBUSData.m_u32LastUpdateTick);
    HIDO_Debug2("Parse buf idx:   %u\r\n", g_u8ParseBufIdx);
    
    ret = Uart_GetHandle(UART_ID_SBUS, (HIDO_VOID **)&pUart);
    if (ret == HIDO_OK && pUart != NULL)
    {
        if (pUart->hdmarx != NULL)
        {
            HIDO_UINT32 dmaRemaining = __HAL_DMA_GET_COUNTER(pUart->hdmarx);
            HIDO_Debug2("DMA remaining:   %u\r\n", dmaRemaining);
            HIDO_Debug2("DMA received:    %u\r\n", SBUS_UART_RX_BUF_SIZE - dmaRemaining);
            HIDO_Debug2("DMA state:       %u\r\n", HAL_DMA_GetState(pUart->hdmarx));
        }
        else
        {
            HIDO_Debug2("DMA handle:      NULL!\r\n");
        }
    }
    else
    {
        HIDO_Debug2("UART handle:     NULL!\r\n");
    }

    if (g_stSBUSData.m_u32FrameCount > 0)
    {
        HIDO_Debug2("\r\nLast frame channels:\r\n");
        for (HIDO_UINT8 i = 0; i < 8; i++)
        {
            HIDO_Debug2("  CH%02u: %4u", i, g_stSBUSData.m_au16Channels[i]);
            if ((i + 1) % 4 == 0) HIDO_Debug2("\r\n");
        }
        HIDO_Debug2("  CH17: %u, CH18: %u\r\n", 
                    g_stSBUSData.m_u8Ch17, g_stSBUSData.m_u8Ch18);
        HIDO_Debug2("  FrameLost: %u, Failsafe: %u\r\n",
                    g_stSBUSData.m_u8FrameLost, g_stSBUSData.m_u8Failsafe);
    }

    HIDO_Debug2("\r\nRaw RX buffer (first 32 bytes):\r\n");
    for (HIDO_UINT8 i = 0; i < 32; i++)
    {
        HIDO_Debug2("%02X ", g_au8SBUSRxBuf[i]);
        if ((i + 1) % 16 == 0) HIDO_Debug2("\r\n");
    }
    HIDO_Debug2("====================================\r\n\r\n");
}

/**
 * @brief 判断是否处于自动模式
 * @return HIDO_TRUE: 自动模式 (CH8 > 1500, Python控制)
 *         HIDO_FALSE: 手动模式 (CH8 < 1500, 遥控器控制)
 */
HIDO_BOOL SBUS_IsAutoMode(HIDO_VOID)
{
    return (g_u8ControlMode == 1) ? HIDO_TRUE : HIDO_FALSE;
}