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

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

 ******************************************************************************
  文 件 名   : motor_example.c
  版 本 号   : 初稿
  作    者   : radkil
  生成日期   : 2026年7月13日
  最近修改   :
  功能描述   : 电机模块使用示例
  
  修改历史   :
  1.日    期   : 2026年7月13日
    作    者   : 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;
/*----------------------------------------------*
 * 常量定义                                     *
 *----------------------------------------------*/

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

#if 0
/*****************************************************************************
 函 数 名  : MotorExample_LeiSai
 功能描述  : 雷赛电机使用示例
 输入参数  : 无
 输出参数  : 无
 返 回 值  : void
*****************************************************************************/
void MotorExample_LeiSai(void)
{
    // 1. 初始化电机管理模块
    MotorMgr_Init();

    // 2. 配置左轮电机
    Motor_Config_t stLeftConfig = {
        .m_ucMotorID = 1,
        .m_uiPulsePerRound = 10000,
        .m_uiReductionRatio = 70,
        .m_fWheelDiameter = 0.26f
    };

    // 3. 创建电机实例（使用雷赛协议）
    Motor_Instance_t *pstLeftMotor = MotorMgr_Create(&stLeftConfig, g_ptFDCAN1, LeiSai_GetProtocol(), NULL);

    // 4. 配置右轮电机
    Motor_Config_t stRightConfig = {
        .m_ucMotorID = 2,
        .m_uiPulsePerRound = 10000,
        .m_uiReductionRatio = 70,
        .m_fWheelDiameter = 0.26f
    };
    Motor_Instance_t *pstRightMotor = MotorMgr_Create(&stRightConfig, g_ptFDCAN1, LeiSai_GetProtocol(), NULL);

    // 5. 复位并激活所有电机
    if (pstLeftMotor != NULL)
    {
        MotorMgr_ResetAll(pstLeftMotor);
        Rd_Delay(1000);
        MotorMgr_ActivateAll(pstLeftMotor);
        Rd_Delay(1000);
    }

    // 6. 设置当前位置为零点
    if (pstLeftMotor != NULL)
    {
        MotorMgr_SetHome(pstLeftMotor);
        Rd_Delay(100);
    }
    if (pstRightMotor != NULL)
    {
        MotorMgr_SetHome(pstRightMotor);
        Rd_Delay(100);
    }

    // 7. 初始化速度模式
    if (pstLeftMotor != NULL)
    {
        MotorMgr_SpeedModeInit(pstLeftMotor);
    }
    if (pstRightMotor != NULL)
    {
        MotorMgr_SpeedModeInit(pstRightMotor);
    }

    // 8. 设置目标速度
    if (pstLeftMotor != NULL)
    {
        MotorMgr_SetTargetSpeed(pstLeftMotor, 100000);
    }
    if (pstRightMotor != NULL)
    {
        MotorMgr_SetTargetSpeed(pstRightMotor, -100000);
    }

    // 9. 定期读取电机状态
    while(1)
    {
        if (pstLeftMotor != NULL)
        {
            MotorMgr_RequestPosition(pstLeftMotor);
            MotorMgr_RequestVelocity(pstLeftMotor);
            MotorMgr_RequestFault(pstLeftMotor);

            const Motor_Data_t *pstData = MotorMgr_GetData(pstLeftMotor);
            if (pstData != NULL)
            {
                RD_PRINTF("Left Motor: Pos=%ld, Vel=%ld, Dist=%.3f m\n",
                          pstData->m_iRealPosition,
                          pstData->m_iRealVelocity,
                          pstData->m_dRealDistance);
            }
        }

        Rd_Delay(100);
    }
}

/*****************************************************************************
 函 数 名  : MotorExample_MultiProtocol
 功能描述  : 多协议使用示例（展示架构扩展性）
 输入参数  : 无
 输出参数  : 无
 返 回 值  : void

 说明：假设未来有其他协议实现，只需：
 1. 实现 Motor_Protocol_t 函数表
 2. 调用 MotorMgr_Create 时传入不同的协议
*****************************************************************************/
void MotorExample_MultiProtocol(void)
{
    MotorMgr_Init();

    // 使用雷赛协议的电机
    Motor_Config_t stConfig1 = { .m_ucMotorID = 1 };
    Motor_Instance_t *pstMotor1 = MotorMgr_Create(&stConfig1, g_ptFDCAN1, LeiSai_GetProtocol(), NULL);

    // 假设未来有其他协议
    // Motor_Config_t stConfig2 = { .m_ucMotorID = 2 };
    // Motor_Instance_t *pstMotor2 = MotorMgr_Create(&stConfig2, g_ptFDCAN2, OtherProtocol_GetProtocol(), NULL);

    // 统一的管理接口
    MotorMgr_SpeedModeInit(pstMotor1);
    MotorMgr_SetTargetSpeed(pstMotor1, 50000);

    // 通过名称查找
    Motor_Instance_t *pstFound = MotorMgr_FindByName("Motor_1");
    if (pstFound != NULL)
    {
        RD_PRINTF("Found motor: %s\n", pstFound->m_szName);
    }
}
#endif

void MotorTask(void *argument)
{
    // 等待系统稳定
    Rd_Delay(1000);

    // 复位并激活所有电机
    for (int i = 0; i < MOTOR_INSTANCE_MAX; i++)
    {
        if (g_apstMotors[i] != NULL)
        {
            MotorMgr_ResetAll(g_apstMotors[i]);
            Rd_Delay(500);
            MotorMgr_ActivateAll(g_apstMotors[i]);
            Rd_Delay(500);
            MotorMgr_SetHome(g_apstMotors[i]);
            Rd_Delay(100);
            MotorMgr_SpeedModeInit(g_apstMotors[i]);
        }
    }

    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]);
            }
        }

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

/*==============================================*
 *      电机模块消息处理                         *
 *==============================================*/

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

    switch (pstMsg->m_uiMsgID)
    {
        case MOTOR_CMD_SET_SPEED:
        {
            if (pstMsg->m_uiDataLen >= sizeof(Motor_CmdData_t))
            {
                Motor_CmdData_t *pstData = (Motor_CmdData_t *)pstMsg->m_aucData;
                if (pstData->m_ucMotorIndex < 2 && g_apstMotors[pstData->m_ucMotorIndex] != NULL)
                {
                    MotorMgr_SetTargetSpeed(g_apstMotors[pstData->m_ucMotorIndex], pstData->m_iValue);
                }
            }
            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:
        {
            if (pstMsg->m_uiDataLen >= sizeof(Motor_CmdData_t))
            {
                Motor_CmdData_t *pstData = (Motor_CmdData_t *)pstMsg->m_aucData;
                if (pstData->m_ucMotorIndex < 2 && g_apstMotors[pstData->m_ucMotorIndex] != NULL)
                {
                    MotorMgr_SetHome(g_apstMotors[pstData->m_ucMotorIndex]);
                }
            }
            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;
    }
}

void MotorInit(void)
{
    // 初始化电机管理模块
    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_ptFDCAN1, 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_ptFDCAN1, LeiSai_GetProtocol(), NULL);

    g_uiMotorModuleID = MsgCenter_Register(MODULE_NAME_MOTOR, Motor_ModuleHandler);

    // 创建电机任务
    const osThreadAttr_t motor_task_attributes = {
        .name = MODULE_NAME_MOTOR,
        .stack_size = 1024,
        .priority = (osPriority_t) osPriorityHigh5,
    };
    (void)osThreadNew(MotorTask, NULL, &motor_task_attributes);
}