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

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

 ******************************************************************************
  文 件 名   : motor_example.c
  版 本 号   : 初稿
  作    者   : radkil
  生成日期   : 2026年7月13日
  最近修改   :
  功能描述   : 电机模块使用示例
  
  修改历史   :
  1.日    期   : 2026年7月13日
    作    者   : radkil
    修改内容   : 创建文件

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

#include "BHBF_robot.h"
#include "motor_manager.h"
#include "msp_motor_leisai.h"

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

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

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

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

/*----------------------------------------------*
 * 模块级变量                                   *
 *----------------------------------------------*/

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

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

/*****************************************************************************
 函 数 名  : 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);
    }
}