/*
 * @Author: mypx
 * @Date: 2025-10-15 16:40:13
 * @LastEditTime: 2025-10-15 16:47:38
 * @LastEditors: mypx mypx_coder@163.com
 * @Description: 
 */
#include "bsp_motor.h"
#include "et_log.h"
#include "rtthread.h"
#include "usart.h"

#if (DEBUG_MB_ENABLE == 1)
#define DBG_MB(...) ET_DBG("[MB] " __VA_ARGS__)
#else
#define DBG_MB(...)
#endif

struct rt_messagequeue servo_mq;
static char            mq_pool[SERVO_RX_QUEUE_SIZE];


int bsp_servo_init(void)
{
    MX_USART3_UART_Init();

    rt_err_t result = rt_mq_init(&servo_mq, "servo_mq", mq_pool, 1, SERVO_RX_QUEUE_SIZE, RT_IPC_FLAG_FIFO);
    if (result != RT_EOK)
    {
        ET_ERR("servo mq init failed\n");
        return -1;
    }

    __HAL_UART_ENABLE_IT(&SERVO_COM, UART_IT_RXNE);
    return 0;
}

int32_t bsp_servo_uart_read(uint8_t *buf, uint16_t count, int32_t byte_timeout_ms, void *arg)
{
    UNUSED(arg);
    uint16_t  bytes_read = 0;
    uint8_t   byte;
    rt_tick_t timeout_ticks = rt_tick_from_millisecond(byte_timeout_ms);
    DBG_MB("start read:count:%d, timeout:%d\n", count, byte_timeout_ms);

    while (bytes_read < count)
    {
        rt_err_t result = rt_mq_recv(&servo_mq, &byte, sizeof(byte), timeout_ticks);
        if (result == RT_EOK)
        {
            DBG_MB("rx: %02X\n", byte);
            buf[bytes_read++] = byte;
        }
        else
        {
            if (bytes_read > 0)
            {
                DBG_MB("Modbus read finished %d\n", bytes_read);
                return bytes_read;
            }
            DBG_MB("Servo read timeout, no data received\n");
            return 0;
        }
    }
    DBG_MB("Actually read:%d, count:%d\r\n", bytes_read, count);

    return bytes_read;
}

int32_t bsp_servo_uart_write(const uint8_t *buf, uint16_t count, int32_t byte_timeout_ms, void *arg)
{
    UNUSED(arg);
    rt_thread_mdelay(2);
    if (HAL_UART_Transmit(&SERVO_COM, buf, count, byte_timeout_ms) == HAL_OK)
    {
        return count;
    }
    else
    {
        return 0;
    }
}