/*******************************************************************************
 * File Name         : PythonLink.c
 * Description       : Python通信链路模块实现
 * Created on        : 2025-11-13
 * Author            : Auto-generated
 *******************************************************************************/

/*******************************************************************************
 *                              Include Files                                  *
 *******************************************************************************/
#include "PythonLink.h"
#include "Uart.h"
#include "HIDO_Debug.h"
#include <string.h>
#include <math.h>

/*******************************************************************************
 *                             Global Variables                                *
 *******************************************************************************/
// UART 缓冲区
static HIDO_UINT8 g_au8PythonLinkTxFrameBuf[PYTHONLINK_TX_BUF_SIZE];   // 用于组帧
static HIDO_UINT8 g_au8PythonLinkTxQueueBuf[PYTHONLINK_TX_BUF_SIZE];   // 提供给UART FIFO
static HIDO_UINT8 g_au8PythonLinkRxBuf[PYTHONLINK_RX_BUF_SIZE];
typedef struct
{
    HIDO_UINT32 m_u32LastPollTick;
    HIDO_UINT32 m_u32LastDataTick;
    HIDO_UINT32 m_u32TotalBytes;
    HIDO_UINT32 m_u32ParsedFrames;
    HIDO_UINT32 m_u32HandleFailCount;
    HIDO_UINT32 m_u32DmaNullCount;
    HIDO_UINT32 m_u32NoDataWarningTick;
    HIDO_UINT32 m_u32LastLogTick;
} ST_PythonLink_RxDebug;

static ST_PythonLink_RxDebug g_stPythonLinkRxDebug;

// 统计计数
static HIDO_UINT32 g_u32GPSPacketCount = 0;
static HIDO_UINT32 g_u32IMUPacketCount = 0;
static HIDO_UINT32 g_u32ErrorCount = 0;
static HIDO_UINT32 g_u32ControlRxCount = 0;

// 控制命令缓存
static volatile HIDO_UINT16 g_u16LatestSteeringPWM = 1500;  // 中位值
static volatile HIDO_UINT16 g_u16LatestThrottlePWM = 1500;  // 中位值
static volatile HIDO_UINT32 g_u32ControlTimestamp = 0;      // 命令时间戳
static volatile HIDO_BOOL g_bControlValid = HIDO_FALSE;     // 命令有效标志

// DMA接收位置跟踪
static HIDO_UINT16 g_u16LastDMAPos = 0;

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

/**
 * @brief 计算16位累加校验和 (Checksum_u16)
 * @param _pu8Data: 数据指针
 * @param _u32Len: 数据长度
 * @return 16位校验和
 */
static HIDO_UINT16 PythonLink_CalcChecksum(const HIDO_UINT8 *_pu8Data, HIDO_UINT32 _u32Len)
{
    HIDO_UINT32 sum = 0;
    HIDO_UINT32 i;
    
    // 16位累加和
    for (i = 0; i < _u32Len; i++)
    {
        sum += _pu8Data[i];
    }
    
    // 返回低16位
    return (HIDO_UINT16)(sum & 0xFFFF);
}

#if (PYTHONLINK_FORCE_FIXED_PAYLOAD != 0)
/**
 * @brief 调试: 生成固定内容的负载数据，便于观测串口是否乱码
 * @param _pu8Dst  负载存放位置
 * @param _u16Len  负载长度
 * @param _u8Type  数据类型, 用于区分不同包
 */
static HIDO_VOID PythonLink_DebugFillFixedPayload(HIDO_UINT8 *_pu8Dst, HIDO_UINT16 _u16Len, HIDO_UINT8 _u8Type)
{
    static const HIDO_UINT8 s_au8TagGPS[] = "GPSDBG";
    static const HIDO_UINT8 s_au8TagIMU[] = "IMUDBG";
    static const HIDO_UINT8 s_au8TagCTRL[] = "CTRDBG";
    const HIDO_UINT8 *pTag = HIDO_NULL;
    HIDO_UINT8 tagLen = 0;
    HIDO_UINT16 i;

    switch (_u8Type)
    {
    case PYTHONLINK_TYPE_GPS:
        pTag = s_au8TagGPS;
        tagLen = sizeof(s_au8TagGPS) - 1;
        break;
    case PYTHONLINK_TYPE_IMU:
        pTag = s_au8TagIMU;
        tagLen = sizeof(s_au8TagIMU) - 1;
        break;
    case PYTHONLINK_TYPE_CONTROL:
        pTag = s_au8TagCTRL;
        tagLen = sizeof(s_au8TagCTRL) - 1;
        break;
    default:
        tagLen = 0;
        break;
    }

    for (i = 0; i < _u16Len; i++)
    {
        if (i < tagLen && pTag != HIDO_NULL)
        {
            _pu8Dst[i] = pTag[i];
        }
        else
        {
            _pu8Dst[i] = (HIDO_UINT8)((_u8Type << 4) + (i & 0x0F));
        }
    }
}
#endif

/**
 * @brief 解析并处理接收到的控制命令帧
 * @param _pu8Data: 帧数据指针 (从帧头开始)
 * @param _u16Len: 数据长度
 * @return HIDO_OK: 解析成功, HIDO_ERR: 失败
 */
static HIDO_INT32 PythonLink_ParseControlFrame(const HIDO_UINT8 *_pu8Data, HIDO_UINT16 _u16Len)
{
    ST_PythonLink_FrameHeader *pHeader;
    ST_PythonLink_Control *pControl;
    HIDO_UINT16 checksum_received, checksum_calc;
    HIDO_UINT16 payload_len;
    
    // 检查最小长度: 帧头(5) + 负载(4) + 校验和(2) + 帧尾(2) = 13字节
    if (_u16Len < 13)
    {
        return HIDO_ERR;
    }
    
    // 解析帧头
    pHeader = (ST_PythonLink_FrameHeader *)_pu8Data;
    
    // 检查帧头
    if (pHeader->m_u8Header1 != PYTHONLINK_FRAME_HEADER1 ||
        pHeader->m_u8Header2 != PYTHONLINK_FRAME_HEADER2)
    {
        return HIDO_ERR;
    }
    
    // 检查类型
    if (pHeader->m_u8Type != PYTHONLINK_TYPE_CONTROL)
    {
        return HIDO_ERR;
    }
    
    // 检查负载长度
    payload_len = pHeader->m_u16Length;
    if (payload_len != sizeof(ST_PythonLink_Control))
    {
        return HIDO_ERR;
    }
    
    // 检查总长度
    HIDO_UINT16 expected_len = sizeof(ST_PythonLink_FrameHeader) + payload_len + 
                                sizeof(ST_PythonLink_FrameFooter);
    if (_u16Len < expected_len)
    {
        return HIDO_ERR;
    }
    
    // 校验Checksum (从类型字节开始，跳过帧头AA 55)
    // 校验范围: Type + Length + Payload
    checksum_calc = PythonLink_CalcChecksum(_pu8Data + 2, 
                                             sizeof(ST_PythonLink_FrameHeader) - 2 + payload_len);
    checksum_received = *((HIDO_UINT16 *)(_pu8Data + sizeof(ST_PythonLink_FrameHeader) + payload_len));
    
    if (checksum_received != checksum_calc)
    {
        g_u32ErrorCount++;
        return HIDO_ERR;
    }
    
    // 检查帧尾
    HIDO_UINT8 footer1 = _pu8Data[sizeof(ST_PythonLink_FrameHeader) + payload_len + 2];
    HIDO_UINT8 footer2 = _pu8Data[sizeof(ST_PythonLink_FrameHeader) + payload_len + 3];
    if (footer1 != PYTHONLINK_FRAME_FOOTER1 || footer2 != PYTHONLINK_FRAME_FOOTER2)
    {
        g_u32ErrorCount++;
        return HIDO_ERR;
    }
    
    // 解析控制命令
    pControl = (ST_PythonLink_Control *)(_pu8Data + sizeof(ST_PythonLink_FrameHeader));
    
    // 限制PWM范围并更新全局变量
    g_u16LatestSteeringPWM = (pControl->m_u16SteeringPWM < 1000) ? 1000 : 
                              (pControl->m_u16SteeringPWM > 2000) ? 2000 : 
                              pControl->m_u16SteeringPWM;
    
    g_u16LatestThrottlePWM = (pControl->m_u16ThrottlePWM < 1000) ? 1000 : 
                              (pControl->m_u16ThrottlePWM > 2000) ? 2000 : 
                              pControl->m_u16ThrottlePWM;
    
    g_u32ControlTimestamp = HAL_GetTick();
    g_bControlValid = HIDO_TRUE;
    g_u32ControlRxCount++;
    
    return HIDO_OK;
}

/**
 * @brief 在DMA接收缓冲区中查找并解析控制命令帧
 * @return 解析成功的帧数量
 */
static HIDO_UINT32 PythonLink_ProcessRxBuffer(HIDO_VOID)
{
    UART_HandleTypeDef *pUart = NULL;
    HIDO_UINT16 dma_pos, available_bytes;
    HIDO_UINT32 parsed_count = 0;
    HIDO_INT32 ret;
    
    // 获取UART句柄
    HIDO_UINT32 tick_now = HAL_GetTick();
    g_stPythonLinkRxDebug.m_u32LastPollTick = tick_now;

    ret = Uart_GetHandle(UART_ID_PYTHON, (HIDO_VOID **)&pUart);
    if (ret != HIDO_OK || pUart == NULL)
    {
        g_stPythonLinkRxDebug.m_u32HandleFailCount++;
        return 0;
    }
    if (pUart->hdmarx == NULL)
    {
        g_stPythonLinkRxDebug.m_u32DmaNullCount++;
        return 0;
    }
    
    // 获取DMA当前写入位置
    dma_pos = PYTHONLINK_RX_BUF_SIZE - __HAL_DMA_GET_COUNTER(pUart->hdmarx);
    
    // 计算可读字节数
    if (dma_pos >= g_u16LastDMAPos)
    {
        available_bytes = dma_pos - g_u16LastDMAPos;
    }
    else
    {
        // DMA循环缓冲区回绕
        available_bytes = PYTHONLINK_RX_BUF_SIZE - g_u16LastDMAPos + dma_pos;
    }
    
    // 如果没有新数据，直接返回
    if (available_bytes == 0)
    {
        if ((tick_now - g_stPythonLinkRxDebug.m_u32LastDataTick) > 1000 &&
            (tick_now - g_stPythonLinkRxDebug.m_u32NoDataWarningTick) > 1000)
        {
            g_stPythonLinkRxDebug.m_u32NoDataWarningTick = tick_now;
        }
        return 0;
    }

    g_stPythonLinkRxDebug.m_u32LastDataTick = tick_now;
    g_stPythonLinkRxDebug.m_u32TotalBytes += available_bytes;
    
    // 查找帧头并解析
    HIDO_UINT16 search_pos = g_u16LastDMAPos;
    while (available_bytes >= 13)  // 最小帧长度
    {
        // 查找帧头 AA 55
        HIDO_UINT8 byte1 = g_au8PythonLinkRxBuf[search_pos];
        HIDO_UINT8 byte2 = g_au8PythonLinkRxBuf[(search_pos + 1) % PYTHONLINK_RX_BUF_SIZE];
        
        if (byte1 == PYTHONLINK_FRAME_HEADER1 && byte2 == PYTHONLINK_FRAME_HEADER2)
        {
            // 找到帧头，尝试解析
            HIDO_UINT8 temp_buf[64];  // 临时缓冲区
            HIDO_UINT16 copy_len = (available_bytes > 64) ? 64 : available_bytes;
            
            // 从循环缓冲区拷贝数据到临时缓冲区
            for (HIDO_UINT16 i = 0; i < copy_len; i++)
            {
                temp_buf[i] = g_au8PythonLinkRxBuf[(search_pos + i) % PYTHONLINK_RX_BUF_SIZE];
            }
            
            // 尝试解析
            if (PythonLink_ParseControlFrame(temp_buf, copy_len) == HIDO_OK)
            {
                // 解析成功，跳过这个帧
                HIDO_UINT16 frame_len = 13;  // 控制帧固定长度
                search_pos = (search_pos + frame_len) % PYTHONLINK_RX_BUF_SIZE;
                available_bytes -= frame_len;
                parsed_count++;
                g_stPythonLinkRxDebug.m_u32ParsedFrames++;
            }
            else
            {
                // 解析失败，跳过一个字节继续搜索
                search_pos = (search_pos + 1) % PYTHONLINK_RX_BUF_SIZE;
                available_bytes--;
            }
        }
        else
        {
            // 不是帧头，继续搜索
            search_pos = (search_pos + 1) % PYTHONLINK_RX_BUF_SIZE;
            available_bytes--;
        }
    }
    
    // 更新读取位置
    g_u16LastDMAPos = search_pos;
    
    return parsed_count;
}

/**
 * @brief 发送数据帧
 * @param _u8Type: 数据类型 (GPS/IMU)
 * @param _pu8Payload: 数据负载指针
 * @param _u16PayloadLen: 负载长度
 * @return HIDO_OK: 成功, HIDO_ERR: 失败
 */
static HIDO_INT32 PythonLink_SendFrame(HIDO_UINT8 _u8Type, 
                                        const HIDO_UINT8 *_pu8Payload, 
                                        HIDO_UINT16 _u16PayloadLen)
{
    HIDO_UINT32 frameLen = 0;
    HIDO_UINT16 crc = 0;
    ST_PythonLink_FrameHeader header;
    ST_PythonLink_FrameFooter footer;

    // 检查参数
    if (_pu8Payload == NULL || _u16PayloadLen == 0)
    {
        g_u32ErrorCount++;
        return HIDO_ERR;
    }

    // 检查缓冲区空间
    if ((sizeof(header) + _u16PayloadLen + sizeof(footer)) > PYTHONLINK_TX_BUF_SIZE)
    {
        g_u32ErrorCount++;
        return HIDO_ERR;
    }

    // 构造帧头
    header.m_u8Header1 = PYTHONLINK_FRAME_HEADER1;
    header.m_u8Header2 = PYTHONLINK_FRAME_HEADER2;
    header.m_u8Type = _u8Type;
    header.m_u16Length = _u16PayloadLen;

    // 拷贝帧头到发送缓冲区
    memcpy(&g_au8PythonLinkTxFrameBuf[frameLen], &header, sizeof(header));
    frameLen += sizeof(header);

    // 拷贝数据负载
#if (PYTHONLINK_FORCE_FIXED_PAYLOAD != 0)
    PythonLink_DebugFillFixedPayload(&g_au8PythonLinkTxFrameBuf[frameLen], _u16PayloadLen, _u8Type);
#else
    memcpy(&g_au8PythonLinkTxFrameBuf[frameLen], _pu8Payload, _u16PayloadLen);
#endif
    frameLen += _u16PayloadLen;

    // 计算Checksum (从类型字节开始，跳过帧头AA 55)
    // 校验范围: Type + Length + Payload
    crc = PythonLink_CalcChecksum(&g_au8PythonLinkTxFrameBuf[2], frameLen - 2);
    
    // 构造帧尾
    footer.m_u16CRC = crc;
    footer.m_u8Footer1 = PYTHONLINK_FRAME_FOOTER1;
    footer.m_u8Footer2 = PYTHONLINK_FRAME_FOOTER2;

    // 拷贝帧尾
    memcpy(&g_au8PythonLinkTxFrameBuf[frameLen], &footer, sizeof(footer));
    frameLen += sizeof(footer);

    // 通过UART发送
    if (Uart_Send(UART_ID_PYTHON, g_au8PythonLinkTxFrameBuf, frameLen) != HIDO_OK)
    {
        g_u32ErrorCount++;
        return HIDO_ERR;
    }

    return HIDO_OK;
}

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

/**
 * @brief 初始化 PythonLink 模块
 */
HIDO_INT32 PythonLink_Init(HIDO_VOID)
{
    ST_UartInit stUartInit;
    UART_HandleTypeDef *pUart = NULL;
    HIDO_INT32 ret;

    // 检查 UART 是否已注册
    ret = Uart_GetHandle(UART_ID_PYTHON, (HIDO_VOID **)&pUart);
    if (ret != HIDO_OK || pUart == NULL)
    {
        HIDO_Debug2("[PythonLink] UART_ID_PYTHON not registered!\r\n");
        return HIDO_ERR;
    }

    // 清空缓冲区
    memset(g_au8PythonLinkTxFrameBuf, 0, sizeof(g_au8PythonLinkTxFrameBuf));
    memset(g_au8PythonLinkTxQueueBuf, 0, sizeof(g_au8PythonLinkTxQueueBuf));
    memset(g_au8PythonLinkRxBuf, 0, sizeof(g_au8PythonLinkRxBuf));

    // 配置 UART (DMA 模式)
    stUartInit.m_eTxMode = UART_TX_MODE_DMA;
    stUartInit.m_pu8TxBuf = g_au8PythonLinkTxQueueBuf;
    stUartInit.m_u32TxBufSize = PYTHONLINK_TX_BUF_SIZE;
    stUartInit.m_u32TxQueueMemberCnt = 16;  // 发送队列深度 (增大以缓冲更多 DMA 事务)

    stUartInit.m_eRxMode = UART_RX_MODE_DMA;
    stUartInit.m_pu8RxBuf = g_au8PythonLinkRxBuf;
    stUartInit.m_u32RxBufSize = PYTHONLINK_RX_BUF_SIZE;
    stUartInit.m_fnRxISR = NULL;

    ret = Uart_Init(UART_ID_PYTHON, &stUartInit);
    if (ret != HIDO_OK)
    {
        HIDO_Debug2("[PythonLink] UART init failed!\r\n");
        return HIDO_ERR;
    }

    // 重置统计计数
    g_u32GPSPacketCount = 0;
    g_u32IMUPacketCount = 0;
    g_u32ErrorCount = 0;

    HIDO_Debug2("[PythonLink] Init success (UART5, 921600 bps, DMA)\r\n");
    return HIDO_OK;
}

/**
 * @brief PythonLink 轮询函数
 */
HIDO_VOID PythonLink_Poll(HIDO_VOID)
{
    // 处理接收缓冲区中的控制命令
    PythonLink_ProcessRxBuffer();
    
    // 检查控制命令超时 (1秒无新命令则标记为无效)
    if (g_bControlValid == HIDO_TRUE)
    {
        HIDO_UINT32 elapsed = HAL_GetTick() - g_u32ControlTimestamp;
        if (elapsed > 1000)
        {
            g_bControlValid = HIDO_FALSE;
            // 超时后恢复到中位值
            g_u16LatestSteeringPWM = 1500;
            g_u16LatestThrottlePWM = 1500;
        }
    }
}

/**
 * @brief 发送GPS数据包
 */
HIDO_INT32 PythonLink_SendGPSData(const ST_GPRMI *_pstGPRMI)
{
    ST_PythonLink_GPS gpsPacket;
    HIDO_INT32 ret;

    // 检查参数
    if (_pstGPRMI == NULL || _pstGPRMI->m_bValid == HIDO_FALSE)
    {
        return HIDO_ERR;
    }

    // 填充GPS数据包
    memset(&gpsPacket, 0, sizeof(gpsPacket));
    gpsPacket.m_dLatitude = _pstGPRMI->m_dLatitude;
    gpsPacket.m_dLongitude = _pstGPRMI->m_dLongitude;
    
    // 默认使用GPRMI提供的航向角, 若不可用则回退到速度计算
    HIDO_FLOAT heading = _pstGPRMI->m_fHeadingAngle;
    if (!isfinite(heading))
    {
        heading = 0.0f;
    }

    if ((!isfinite(heading) || heading == 0.0f) &&
        (_pstGPRMI->m_fEastVelocity != 0.0f || _pstGPRMI->m_fNorthVelocity != 0.0f))
    {
        // atan2(东,北) 得到相对正北方向的角度
        heading = atan2f(_pstGPRMI->m_fEastVelocity, _pstGPRMI->m_fNorthVelocity) * 180.0f / 3.14159265f;
        if (heading < 0.0f)
        {
            heading += 360.0f;
        }
    }

    gpsPacket.m_fHeadingAngle = heading;
    gpsPacket.m_fPitchAngle = _pstGPRMI->m_fPitchAngle;
    gpsPacket.m_fRollAngle = _pstGPRMI->m_fRollAngle;
    if (!isfinite(gpsPacket.m_fPitchAngle))
    {
        gpsPacket.m_fPitchAngle = 0.0f;
    }
    if (!isfinite(gpsPacket.m_fRollAngle))
    {
        gpsPacket.m_fRollAngle = 0.0f;
    }

    gpsPacket.m_fEastVelocity = _pstGPRMI->m_fEastVelocity;
    gpsPacket.m_fNorthVelocity = _pstGPRMI->m_fNorthVelocity;
    gpsPacket.m_fUpVelocity = _pstGPRMI->m_fUpVelocity;
    gpsPacket.m_fAltitude = _pstGPRMI->m_fAltitude;
    gpsPacket.m_u32UTCTime = _pstGPRMI->m_u32UTCTime;
    gpsPacket.m_u8PositionQuality = _pstGPRMI->m_u8PositionQuality;
    gpsPacket.m_u8SatelliteCount = _pstGPRMI->m_u8SatelliteCount;

    // 发送数据帧
    ret = PythonLink_SendFrame(PYTHONLINK_TYPE_GPS, 
                                (const HIDO_UINT8 *)&gpsPacket, 
                                sizeof(gpsPacket));
    
    if (ret == HIDO_OK)
    {
        g_u32GPSPacketCount++;
    }

    return ret;
}

/**
 * @brief 发送IMU数据包
 */
HIDO_INT32 PythonLink_SendIMUData(const ST_GPIMU *_pstGPIMU)
{
    ST_PythonLink_IMU imuPacket;
    HIDO_INT32 ret;

    // 检查参数
    if (_pstGPIMU == NULL || _pstGPIMU->m_bValid == HIDO_FALSE)
    {
        return HIDO_ERR;
    }

    // 填充IMU数据包
    memset(&imuPacket, 0, sizeof(imuPacket));
    imuPacket.m_fAccelX = _pstGPIMU->m_fAccelX;
    imuPacket.m_fAccelY = _pstGPIMU->m_fAccelY;
    imuPacket.m_fAccelZ = _pstGPIMU->m_fAccelZ;
    imuPacket.m_fGyroX = _pstGPIMU->m_fGyroX;
    imuPacket.m_fGyroY = _pstGPIMU->m_fGyroY;
    imuPacket.m_fGyroZ = _pstGPIMU->m_fGyroZ;
    imuPacket.m_fTemperature = _pstGPIMU->m_fTemperature;
    imuPacket.m_u32UTCTime = _pstGPIMU->m_u32UTCTime;

    // 发送数据帧
    ret = PythonLink_SendFrame(PYTHONLINK_TYPE_IMU, 
                                (const HIDO_UINT8 *)&imuPacket, 
                                sizeof(imuPacket));
    
    if (ret == HIDO_OK)
    {
        g_u32IMUPacketCount++;
    }

    return ret;
}

/**
 * @brief 获取统计信息
 */
HIDO_VOID PythonLink_GetStats(HIDO_UINT32 *_pu32GPSCount, 
                               HIDO_UINT32 *_pu32IMUCount, 
                               HIDO_UINT32 *_pu32ErrorCount)
{
    if (_pu32GPSCount != NULL)
    {
        *_pu32GPSCount = g_u32GPSPacketCount;
    }
    if (_pu32IMUCount != NULL)
    {
        *_pu32IMUCount = g_u32IMUPacketCount;
    }
    if (_pu32ErrorCount != NULL)
    {
        *_pu32ErrorCount = g_u32ErrorCount;
    }
}

/**
 * @brief 打印调试信息
 */
HIDO_VOID PythonLink_PrintDebugInfo(HIDO_VOID)
{
    HIDO_Debug2("\r\n========== PythonLink Debug Info ==========\r\n");
    HIDO_Debug2("GPS packets sent:   %u\r\n", g_u32GPSPacketCount);
    HIDO_Debug2("IMU packets sent:   %u\r\n", g_u32IMUPacketCount);
    HIDO_Debug2("Control RX count:   %u\r\n", g_u32ControlRxCount);
    HIDO_Debug2("Error count:        %u\r\n", g_u32ErrorCount);
    HIDO_Debug2("RX total bytes:     %u\r\n", g_stPythonLinkRxDebug.m_u32TotalBytes);
    HIDO_Debug2("RX parsed frames:   %u\r\n", g_stPythonLinkRxDebug.m_u32ParsedFrames);
    HIDO_Debug2("RX last data age:   %u ms\r\n",
                HAL_GetTick() - g_stPythonLinkRxDebug.m_u32LastDataTick);
    HIDO_Debug2("RX handle fails:    %u\r\n", g_stPythonLinkRxDebug.m_u32HandleFailCount);
    HIDO_Debug2("RX DMA null count:  %u\r\n", g_stPythonLinkRxDebug.m_u32DmaNullCount);
    HIDO_Debug2("Control valid:      %s\r\n", g_bControlValid ? "YES" : "NO");
    if (g_bControlValid)
    {
        HIDO_Debug2("  Steering PWM:     %u us\r\n", g_u16LatestSteeringPWM);
        HIDO_Debug2("  Throttle PWM:     %u us\r\n", g_u16LatestThrottlePWM);
        HIDO_Debug2("  Age:              %u ms\r\n", HAL_GetTick() - g_u32ControlTimestamp);
    }
    HIDO_Debug2("===========================================\r\n\r\n");
}

HIDO_VOID PythonLink_PrintRxDebug(HIDO_VOID)
{
    HIDO_Debug2("\r\n----- PythonLink RX Debug Dump -----\r\n");
    HIDO_Debug2("Last poll tick:     %u\r\n", g_stPythonLinkRxDebug.m_u32LastPollTick);
    HIDO_Debug2("Last data tick:     %u\r\n", g_stPythonLinkRxDebug.m_u32LastDataTick);
    HIDO_Debug2("Total bytes:        %u\r\n", g_stPythonLinkRxDebug.m_u32TotalBytes);
    HIDO_Debug2("Parsed frames:      %u\r\n", g_stPythonLinkRxDebug.m_u32ParsedFrames);
    HIDO_Debug2("Handle fail count:  %u\r\n", g_stPythonLinkRxDebug.m_u32HandleFailCount);
    HIDO_Debug2("DMA null count:     %u\r\n", g_stPythonLinkRxDebug.m_u32DmaNullCount);
    HIDO_Debug2("Last log tick:      %u\r\n", g_stPythonLinkRxDebug.m_u32LastLogTick);
    HIDO_Debug2("---------------------------------------\r\n\r\n");
}

/**
 * @brief 获取最新的控制命令
 */
HIDO_BOOL PythonLink_GetControl(HIDO_UINT16 *_pu16SteeringPWM, 
                                 HIDO_UINT16 *_pu16ThrottlePWM,
                                 HIDO_UINT32 *_pu32Timestamp)
{
    if (g_bControlValid == HIDO_FALSE)
    {
        return HIDO_FALSE;
    }
    
    if (_pu16SteeringPWM != NULL)
    {
        *_pu16SteeringPWM = g_u16LatestSteeringPWM;
    }
    
    if (_pu16ThrottlePWM != NULL)
    {
        *_pu16ThrottlePWM = g_u16LatestThrottlePWM;
    }
    
    if (_pu32Timestamp != NULL)
    {
        *_pu32Timestamp = g_u32ControlTimestamp;
    }
    
    return HIDO_TRUE;
}