/******************************************************************************

                  版权所有 (C), 2018-2099, Radkil

 ******************************************************************************
  文 件 名   : paint_robot_new_motors.c
  版 本 号   : 初稿
  作    者   : radkil
  生成日期   : 2026年8月21日
  最近修改   :
  功能描述   : 喷涂机器人电机逻辑
  
  修改历史   :
  1.日    期   : 2026年8月21日
    作    者   : radkil
    修改内容   : 创建文件

******************************************************************************/
#include "BHBF.h"
#include "motor_manager.h"
#include "msp_motor_leisai.h"

/*----------------------------------------------*
 * 外部变量说明                                 *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 外部函数原型说明                             *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 内部函数原型说明                             *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 全局变量                                     *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 模块级变量                                   *
 *----------------------------------------------*/
static Motor_Instance_t *g_apstMotors[MOTOR_INSTANCE_MAX] = {NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL};
static uint32_t g_uiMotorModuleID = 0;
static int g_aiMotorSpeed[MOTOR_INSTANCE_MAX] = {0};

/*----------------------------------------------*
 * 常量定义                                     *
 *----------------------------------------------*/

/*----------------------------------------------*
 * 宏定义                                       *
 *----------------------------------------------*/

static void Motor_ModuleHandler(const Msg_t *pstMsg)
{
    if (NULL == pstMsg)
    {
        return;
    }

    switch (pstMsg->m_uiMsgID)
    {
        case MOTOR_CMD_SET_SPEED:
        {
            int iSpeed = (int)pstMsg->m_aucData;
            log_i("Set Speed %d", iSpeed);
//            g_aiMotorSpeed[0] = iSpeed;
//            g_aiMotorSpeed[1] = iSpeed;
            break;
        }
        case MOTOR_CMD_STOP:
        {
            for (int i = 0; i < 2; i++)
            {
                if (g_apstMotors[i] != NULL)
                {
                    MotorMgr_SetTargetSpeed(g_apstMotors[i], 0);
                }
            }
            break;
        }
        case MOTOR_CMD_SET_HOME:
        {
            log_i("Not Support [%d]", pstMsg->m_uiMsgID);
            break;
        }
        case MOTOR_CMD_GET_STATUS:
        {
            // 回复状态给请求者
            Motor_StatusData_t stStatus;
            for (int i = 0; i < 2; i++)
            {
                if (g_apstMotors[i] != NULL)
                {
                    const Motor_Data_t *pstData = MotorMgr_GetData(g_apstMotors[i]);
                    if (pstData != NULL)
                    {
                        stStatus.m_ucMotorIndex = i;
                        stStatus.m_iPosition = pstData->m_iRealPosition;
                        stStatus.m_iVelocity = pstData->m_iRealVelocity;
                        stStatus.m_uiFaultCode = pstData->m_uiFaultCode;

                        // 回复给请求者
                        MsgCenter_SendTo(MODULE_NAME_RBCORE, 0x10, &stStatus, sizeof(stStatus));
                    }
                }
            }
            break;
        }
        default:
            break;
    }
}

// 雷赛电机CAN响应检查函数
// 雷赛响应ID范围：0x581 - 0x5FF (0x580 + MotorID)
static int check_LeiSaiMotor(char *_pBuffer, uint32_t _iSize)
{
    if (NULL == _pBuffer || _iSize < 4)
    {
        return 0;
    }

    // 从buffer中提取CAN ID（前4字节）
    uint32_t uiFrameID = (uint32_t)(_pBuffer[0] & 0xFF)
                        | ((_pBuffer[1] & 0xFF) << 8)
                        | ((_pBuffer[2] & 0xFF) << 16)
                        | ((_pBuffer[3] & 0xFF) << 24);

    // 雷赛响应ID范围：0x581 - 0x5FF
    if (uiFrameID >= 0x581 && uiFrameID <= 0x5FF)
    {
        return 8 + 4; // 数据长度8 + CAN ID长度4
    }

    return 0;
}

// 雷赛电机CAN响应解码函数
static void decode_LeiSaiMotor(const char *_pBuffer, uint32_t _iSize)
{
    if (NULL == _pBuffer || _iSize < 12) // 4字节CAN ID + 8字节数据
    {
        return;
    }

    // 从buffer中提取CAN ID
    uint32_t uiFrameID = (uint32_t)(_pBuffer[0] & 0xFF)
                        | ((_pBuffer[1] & 0xFF) << 8)
                        | ((_pBuffer[2] & 0xFF) << 16)
                        | ((_pBuffer[3] & 0xFF) << 24);

    // 提取电机ID (0x580 + MotorID)
    uint8_t ucMotorID = (uint8_t)(uiFrameID - 0x580);

    // 调用电机管理模块解析响应
    MotorMgr_ParseResponse(ucMotorID, (const uint8_t *)&_pBuffer[4], 8);
}

static void MotorInit(void)
{
	// 初始化FDCAN1，使用雷赛电机的回调
	TCANUserData *ptCANUserData = CAN_userdata_init(&hfdcan1, 32, 0);
	g_ptCAN1 = rd_ComCreate(check_LeiSaiMotor, decode_LeiSaiMotor, FDCAN1_Send, CONFIG_UART_BUFFER_SIZE, ptCANUserData);
	CAN_IT_init(g_ptCAN1);

    // 初始化电机管理模块
    MotorMgr_Init();

    // 配置左轮电机
    Motor_Config_t stLeftMotorConfig = {
        .m_ucMotorID = 1,
        .m_uiPulsePerRound = 10000,
        .m_uiReductionRatio = 70,
        .m_fWheelDiameter = 0.26f
    };
    g_apstMotors[0] = MotorMgr_Create(&stLeftMotorConfig, g_ptCAN1, LeiSai_GetProtocol(), NULL);

    // 配置右轮电机
    Motor_Config_t stRightMotorConfig = {
        .m_ucMotorID = 2,
        .m_uiPulsePerRound = 10000,
        .m_uiReductionRatio = 70,
        .m_fWheelDiameter = 0.26f
    };
    g_apstMotors[1] = MotorMgr_Create(&stRightMotorConfig, g_ptCAN1, LeiSai_GetProtocol(), NULL);

    g_uiMotorModuleID = MsgCenter_Register(MODULE_NAME_MOTOR, Motor_ModuleHandler);
}

void MotorTask(void *argument)
{
	MotorInit();

	MotorMgr_ResetAll(g_apstMotors[0]);
	Rd_Delay(500);
	MotorMgr_ResetAll(g_apstMotors[1]);
	Rd_Delay(500);

	int iCount = 6;
	while(iCount--)
	{
		MotorMgr_ActivateAll(g_apstMotors[0]);
		Rd_Delay(500);
		MotorMgr_ActivateAll(g_apstMotors[1]);
		Rd_Delay(500);
	}

	MotorMgr_SetHome(g_apstMotors[0]);
	MotorMgr_SetHome(g_apstMotors[1]);
	MotorMgr_SpeedModeInit(g_apstMotors[0]);
	MotorMgr_SpeedModeInit(g_apstMotors[1]);

    while(1)
    {
        // 处理消息
        MsgCenter_ProcessWait(g_uiMotorModuleID, 2);

        for (int i = 0; i < MOTOR_INSTANCE_MAX; i++)
        {
            if (g_apstMotors[i] != NULL)
            {
                // 读取位置、速度、故障
                MotorMgr_RequestPosition(g_apstMotors[i]);
                MotorMgr_RequestFault(g_apstMotors[i]);
                MotorMgr_RequestVelocity(g_apstMotors[i]);
                MotorMgr_SetTargetSpeed(g_apstMotors[i], g_aiMotorSpeed[i]);
            }
        }

        Rd_Delay(2); // 2ms周期
    }
}