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

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

 ******************************************************************************
  文 件 名   : msp_motor_leisai.c
  版 本 号   : 初稿
  作    者   : radkil
  生成日期   : 2026年7月13日
  最近修改   :
  功能描述   : 雷赛ISV2电机CAN协议实现
  
  修改历史   :
  1.日    期   : 2026年7月13日
    作    者   : radkil
    修改内容   : 创建文件

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

#include "msp_motor_leisai.h"
#include <math.h>

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

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

/*----------------------------------------------*
 * 内部函数原型说明                             *
 *----------------------------------------------*/
static int LeiSai_SendSDO(Motor_Instance_t *pstMotor, uint8_t ucCmd, 
                          uint16_t usIndex, uint8_t ucSubIndex, 
                          int32_t iValue);
static void LeiSai_UpdateDistance(Motor_Instance_t *pstMotor);

static int LeiSai_Init(Motor_Instance_t *pstMotor);
static int LeiSai_ResetAll(Motor_Instance_t *pstMotor);
static int LeiSai_ActivateAll(Motor_Instance_t *pstMotor);
static int LeiSai_SetHome(Motor_Instance_t *pstMotor);
static int LeiSai_SetWatchdog(Motor_Instance_t *pstMotor, uint32_t uiTimeMs);
static int LeiSai_RequestPosition(Motor_Instance_t *pstMotor);
static int LeiSai_RequestVelocity(Motor_Instance_t *pstMotor);
static int LeiSai_RequestTorque(Motor_Instance_t *pstMotor);
static int LeiSai_RequestFault(Motor_Instance_t *pstMotor);
static int LeiSai_SpeedModeInit(Motor_Instance_t *pstMotor);
static int LeiSai_SetTargetSpeed(Motor_Instance_t *pstMotor, int32_t iTargetSpeed);
static int LeiSai_PositionModeInit(Motor_Instance_t *pstMotor, int32_t iAccel, int32_t iDecel, int32_t iTargetSpeed);
static int LeiSai_SetTargetPosition(Motor_Instance_t *pstMotor, int32_t iTargetPos);
static int LeiSai_SetDO(Motor_Instance_t *pstMotor, uint8_t ucDoIndex, uint8_t ucState);
static int LeiSai_ParseResponse(Motor_Instance_t *pstMotor, const uint8_t *pucData, uint32_t uiLen);
static int32_t LeiSai_SpeedConvert(Motor_Instance_t *pstMotor, int32_t iSpeedE01Mmin);

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

// 雷赛协议操作函数表
const Motor_Protocol_t g_stLeiSaiProtocol = {
    .m_szName = "LeiSai_ISV2",
    .pfInit = LeiSai_Init,
    .pfResetAll = LeiSai_ResetAll,
    .pfActivateAll = LeiSai_ActivateAll,
    .pfSetHome = LeiSai_SetHome,
    .pfSetWatchdog = LeiSai_SetWatchdog,
    .pfRequestPosition = LeiSai_RequestPosition,
    .pfRequestVelocity = LeiSai_RequestVelocity,
    .pfRequestTorque = LeiSai_RequestTorque,
    .pfRequestFault = LeiSai_RequestFault,
    .pfSpeedModeInit = LeiSai_SpeedModeInit,
    .pfSetTargetSpeed = LeiSai_SetTargetSpeed,
    .pfPositionModeInit = LeiSai_PositionModeInit,
    .pfSetTargetPosition = LeiSai_SetTargetPosition,
    .pfSetDO = LeiSai_SetDO,
    .pfParseResponse = LeiSai_ParseResponse,
    .pfSpeedConvert = LeiSai_SpeedConvert,
};

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

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

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

/*****************************************************************************
 函 数 名  : LeiSai_GetProtocol
 功能描述  : 获取雷赛协议操作函数表
 输入参数  : 无
 输出参数  : 无
 返 回 值  : const Motor_Protocol_t*
*****************************************************************************/
const Motor_Protocol_t* LeiSai_GetProtocol(void)
{
    return &g_stLeiSaiProtocol;
}

/*****************************************************************************
 函 数 名  : LeiSai_SendSDO
 功能描述  : 发送SDO命令
*****************************************************************************/
static int LeiSai_SendSDO(Motor_Instance_t *pstMotor, uint8_t ucCmd, 
                          uint16_t usIndex, uint8_t ucSubIndex, 
                          int32_t iValue)
{
    if (NULL == pstMotor || NULL == pstMotor->m_ptCAN)
    {
        return RD_NULL;
    }

    uint8_t aucData[8];
    aucData[0] = ucCmd;
    aucData[1] = (uint8_t)(usIndex & 0xFF);
    aucData[2] = (uint8_t)((usIndex >> 8) & 0xFF);
    aucData[3] = ucSubIndex;
    aucData[4] = (uint8_t)(iValue & 0xFF);
    aucData[5] = (uint8_t)((iValue >> 8) & 0xFF);
    aucData[6] = (uint8_t)((iValue >> 16) & 0xFF);
    aucData[7] = (uint8_t)((iValue >> 24) & 0xFF);

    uint32_t uiFrameID = 0x600 + pstMotor->m_stConfig.m_ucMotorID;
    return rd_ComIDSend(pstMotor->m_ptCAN, uiFrameID, (char *)aucData, 8);
}

/*****************************************************************************
 函 数 名  : LeiSai_UpdateDistance
 功能描述  : 更新距离计算
*****************************************************************************/
static void LeiSai_UpdateDistance(Motor_Instance_t *pstMotor)
{
    double dCircleLength = 3.14 * pstMotor->m_stConfig.m_fWheelDiameter;
    double dDeltaCounts = pstMotor->m_stData.m_iRealPosition - pstMotor->m_stData.m_iStartMeasurePos;
    
    pstMotor->m_stData.m_dRealDistance = (dDeltaCounts / pstMotor->m_stConfig.m_uiPulsePerRound 
                                          / pstMotor->m_stConfig.m_uiReductionRatio
                                          + (double)pstMotor->m_stData.m_iRoundCount) 
                                         * dCircleLength;
}

/*****************************************************************************
 函 数 名  : LeiSai_Init
 功能描述  : 初始化电机
*****************************************************************************/
static int LeiSai_Init(Motor_Instance_t *pstMotor)
{
    if (NULL == pstMotor)
    {
        return RD_NULL;
    }
    pstMotor->m_eState = MOTOR_STATE_IDLE;
    return RD_SUCCESS;
}

/*****************************************************************************
 函 数 名  : LeiSai_ResetAll
 功能描述  : 复位所有节点（NMT命令）
*****************************************************************************/
static int LeiSai_ResetAll(Motor_Instance_t *pstMotor)
{
    if (NULL == pstMotor || NULL == pstMotor->m_ptCAN)
    {
        return RD_NULL;
    }

    uint8_t aucData[2] = {0x81, 0x00};
    return rd_ComIDSend(pstMotor->m_ptCAN, 0x000, (char *)aucData, 2);
}

/*****************************************************************************
 函 数 名  : LeiSai_ActivateAll
 功能描述  : 激活所有节点（NMT命令）
*****************************************************************************/
static int LeiSai_ActivateAll(Motor_Instance_t *pstMotor)
{
    if (NULL == pstMotor || NULL == pstMotor->m_ptCAN)
    {
        return RD_NULL;
    }

    uint8_t aucData1[2] = {0x01, 0x01};
    rd_ComIDSend(pstMotor->m_ptCAN, 0x000, (char *)aucData1, 2);

    uint8_t aucData2[2] = {0x01, 0x02};
    return rd_ComIDSend(pstMotor->m_ptCAN, 0x000, (char *)aucData2, 2);
}

/*****************************************************************************
 函 数 名  : LeiSai_SetHome
 功能描述  : 设置当前位置为零点
*****************************************************************************/
static int LeiSai_SetHome(Motor_Instance_t *pstMotor)
{
    if (NULL == pstMotor)
    {
        return RD_NULL;
    }

    LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_1B, LEISAI_OD_MODE_OF_OPERATION, 0x00, LEISAI_MODE_HOMING);
    LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_1B, LEISAI_OD_HOMING_METHOD, 0x00, 0x23);
    LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_SHUTDOWN);
    LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_SWITCH_ON);
    LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_ENABLE);
    LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_START_MOVE);

    return RD_SUCCESS;
}

/*****************************************************************************
 函 数 名  : LeiSai_SetWatchdog
 功能描述  : 设置看门狗时间
*****************************************************************************/
static int LeiSai_SetWatchdog(Motor_Instance_t *pstMotor, uint32_t uiTimeMs)
{
    if (NULL == pstMotor)
    {
        return RD_NULL;
    }

    return LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_WATCHDOG_TIME, 0x06, (int32_t)uiTimeMs);
}

/*****************************************************************************
 函 数 名  : LeiSai_RequestPosition
 功能描述  : 读取电机位置
*****************************************************************************/
static int LeiSai_RequestPosition(Motor_Instance_t *pstMotor)
{
    if (NULL == pstMotor)
    {
        return RD_NULL;
    }

    return LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_READ, LEISAI_OD_POSITION_ACTUAL, 0x00, 0);
}

/*****************************************************************************
 函 数 名  : LeiSai_RequestVelocity
 功能描述  : 读取电机速度
*****************************************************************************/
static int LeiSai_RequestVelocity(Motor_Instance_t *pstMotor)
{
    if (NULL == pstMotor)
    {
        return RD_NULL;
    }

    return LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_READ, LEISAI_OD_VELOCITY_ACTUAL, 0x00, 0);
}

/*****************************************************************************
 函 数 名  : LeiSai_RequestTorque
 功能描述  : 读取电机电流/转矩
*****************************************************************************/
static int LeiSai_RequestTorque(Motor_Instance_t *pstMotor)
{
    if (NULL == pstMotor)
    {
        return RD_NULL;
    }

    return LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_READ, LEISAI_OD_TORQUE_ACTUAL, 0x00, 0);
}

/*****************************************************************************
 函 数 名  : LeiSai_RequestFault
 功能描述  : 读取故障码
*****************************************************************************/
static int LeiSai_RequestFault(Motor_Instance_t *pstMotor)
{
    if (NULL == pstMotor)
    {
        return RD_NULL;
    }

    return LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_READ, LEISAI_OD_FAULT_CODE, 0x00, 0);
}

/*****************************************************************************
 函 数 名  : LeiSai_SpeedModeInit
 功能描述  : 速度模式初始化
*****************************************************************************/
static int LeiSai_SpeedModeInit(Motor_Instance_t *pstMotor)
{
    if (NULL == pstMotor)
    {
        return RD_NULL;
    }

    LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_SHUTDOWN);
    LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_SWITCH_ON);
    LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_ENABLE);
    LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_1B, LEISAI_OD_MODE_OF_OPERATION, 0x00, LEISAI_MODE_PROFILE_VEL);
    LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_4B, LEISAI_OD_TARGET_VELOCITY, 0x00, 0);

    return RD_SUCCESS;
}

/*****************************************************************************
 函 数 名  : LeiSai_SetTargetSpeed
 功能描述  : 设置目标速度
*****************************************************************************/
static int LeiSai_SetTargetSpeed(Motor_Instance_t *pstMotor, int32_t iTargetSpeed)
{
    if (NULL == pstMotor)
    {
        return RD_NULL;
    }

    return LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_4B, LEISAI_OD_TARGET_VELOCITY, 0x00, iTargetSpeed);
}

/*****************************************************************************
 函 数 名  : LeiSai_PositionModeInit
 功能描述  : 位置模式初始化
*****************************************************************************/
static int LeiSai_PositionModeInit(Motor_Instance_t *pstMotor, int32_t iAccel, int32_t iDecel, int32_t iTargetSpeed)
{
    if (NULL == pstMotor)
    {
        return RD_NULL;
    }

    LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_SHUTDOWN);
    LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_SWITCH_ON);
    LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_ENABLE);
    LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_1B, LEISAI_OD_MODE_OF_OPERATION, 0x00, LEISAI_MODE_PROFILE_POS);
    LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_4B, LEISAI_OD_ACCELERATION, 0x00, iAccel);
    LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_4B, LEISAI_OD_DECELERATION, 0x00, iDecel);
    LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_4B, LEISAI_OD_TARGET_VELOCITY, 0x00, iTargetSpeed);

    return RD_SUCCESS;
}

/*****************************************************************************
 函 数 名  : LeiSai_SetTargetPosition
 功能描述  : 设置目标位置（相对运动）
*****************************************************************************/
static int LeiSai_SetTargetPosition(Motor_Instance_t *pstMotor, int32_t iTargetPos)
{
    if (NULL == pstMotor)
    {
        return RD_NULL;
    }

    LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_4B, LEISAI_OD_TARGET_POSITION, 0x00, iTargetPos);
    LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_RELATIVE_MOVE);
    LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_2B, LEISAI_OD_CONTROL_WORD, 0x00, LEISAI_CTRL_START_MOVE);

    return RD_SUCCESS;
}

/*****************************************************************************
 函 数 名  : LeiSai_SetDO
 功能描述  : 设置数字输出
*****************************************************************************/
static int LeiSai_SetDO(Motor_Instance_t *pstMotor, uint8_t ucDoIndex, uint8_t ucState)
{
    if (NULL == pstMotor)
    {
        return RD_NULL;
    }

    uint16_t usIndex = 0;
    switch (ucDoIndex)
    {
        case 1:
            usIndex = 0x2410;
            break;
        case 2:
            usIndex = 0x2411;
            break;
        default:
            return RD_INVALUE;
    }

    return LeiSai_SendSDO(pstMotor, LEISAI_SDO_CMD_WRITE_4B, usIndex, 0x00, ucState ? 1 : 0);
}

/*****************************************************************************
 函 数 名  : LeiSai_ParseResponse
 功能描述  : 解析CAN响应数据
*****************************************************************************/
static int LeiSai_ParseResponse(Motor_Instance_t *pstMotor, const uint8_t *pucData, uint32_t uiLen)
{
    if (NULL == pstMotor || NULL == pucData || uiLen < 8)
    {
        return RD_NULL;
    }

    uint16_t usFunctionCode = (uint16_t)(pucData[1] | (pucData[2] << 8));

    switch (usFunctionCode)
    {
        case LEISAI_OD_POSITION_ACTUAL:
        {
            pstMotor->m_stData.m_iRealPosition = (int32_t)(pucData[4] 
                                                     | (pucData[5] << 8) 
                                                     | (pucData[6] << 16) 
                                                     | (pucData[7] << 24));

            if (pstMotor->m_stData.m_ucStartMeasureFlag == 1)
            {
                pstMotor->m_stData.m_ucStartMeasureFlag = 0;
                pstMotor->m_stData.m_iRoundCount = 0;
                pstMotor->m_stData.m_iStartMeasurePos = pstMotor->m_stData.m_iRealPosition;
                pstMotor->m_stData.m_iLastPosition = pstMotor->m_stData.m_iRealPosition;
            }
            else
            {
                if (abs(pstMotor->m_stData.m_iRealPosition - pstMotor->m_stData.m_iLastPosition) >= 0x3FFFFFFF)
                {
                    if (pstMotor->m_stData.m_iRealPosition > 0)
                    {
                        pstMotor->m_stData.m_iRoundCount -= 1;
                    }
                    else
                    {
                        pstMotor->m_stData.m_iRoundCount += 1;
                    }
                }
            }

            pstMotor->m_stData.m_iLastPosition = pstMotor->m_stData.m_iRealPosition;
            LeiSai_UpdateDistance(pstMotor);
            break;
        }
        case LEISAI_OD_FAULT_CODE:
        {
            pstMotor->m_stData.m_uiFaultCode = (uint32_t)(pucData[4] 
                                                       | (pucData[5] << 8) 
                                                       | (pucData[6] << 16) 
                                                       | (pucData[7] << 24));
            if (pstMotor->m_stData.m_uiFaultCode != 0)
            {
                pstMotor->m_eState = MOTOR_STATE_ERROR;
            }
            break;
        }
        case LEISAI_OD_VELOCITY_ACTUAL:
        {
            pstMotor->m_stData.m_iRealVelocity = (int32_t)(pucData[4] 
                                                        | (pucData[5] << 8) 
                                                        | (pucData[6] << 16) 
                                                        | (pucData[7] << 24));
            break;
        }
        case LEISAI_OD_TORQUE_ACTUAL:
        {
            pstMotor->m_stData.m_iRealTorque = (int32_t)(pucData[4] 
                                                      | (pucData[5] << 8) 
                                                      | (pucData[6] << 16) 
                                                      | (pucData[7] << 24));
            break;
        }
        default:
            break;
    }

    return RD_SUCCESS;
}

/*****************************************************************************
 函 数 名  : LeiSai_SpeedConvert
 功能描述  : 速度单位转换：0.1m/min -> 脉冲/秒
*****************************************************************************/
static int32_t LeiSai_SpeedConvert(Motor_Instance_t *pstMotor, int32_t iSpeedE01Mmin)
{
    if (NULL == pstMotor)
    {
        return 0;
    }

    double dSpeedMs = iSpeedE01Mmin * 0.1 / 60.0;
    double dWheelCircumference = 3.14 * pstMotor->m_stConfig.m_fWheelDiameter;
    double dPulsePerSec = dSpeedMs / dWheelCircumference 
                          * pstMotor->m_stConfig.m_uiReductionRatio 
                          * pstMotor->m_stConfig.m_uiPulsePerRound;

    return (int32_t)dPulsePerSec;
}