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

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

 ******************************************************************************
  文 件 名   : motor_manager.c
  版 本 号   : 初稿
  作    者   : radkil
  生成日期   : 2026年7月13日
  最近修改   :
  功能描述   : 电机管理模块（多实例管理，协议无关）
  
  修改历史   :
  1.日    期   : 2026年7月13日
    作    者   : radkil
    修改内容   : 创建文件

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

#include "motor_manager.h"

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

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

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

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

/*----------------------------------------------*
 * 模块级变量                                   *
 *----------------------------------------------*/
static rd_slist_t g_stMotorList;  // 电机链表头

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

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

// 宏：检查协议函数是否存在并调用
#define MOTOR_PROTOCOL_CALL(pstMotor, func, ...) \
    ((pstMotor)->m_pstProtocol && (pstMotor)->m_pstProtocol->func) ? \
    (pstMotor)->m_pstProtocol->func(pstMotor, ##__VA_ARGS__) : RD_NULL

/*****************************************************************************
 函 数 名  : MotorMgr_Init
 功能描述  : 初始化电机管理模块
 输入参数  : 无
 输出参数  : 无
 返 回 值  : void
 调用函数  : 
 被调函数  : 
 
 修改历史      :
  1.日    期   : 2026年7月13日
    作    者   : radkil
    修改内容   : 新生成函数

*****************************************************************************/
void MotorMgr_Init(void)
{
    rd_slist_init(&g_stMotorList);
}

/*****************************************************************************
 函 数 名  : MotorMgr_Create
 功能描述  : 创建电机实例
 输入参数  : const Motor_Config_t *pstConfig 电机配置
             TComCtrl *ptCAN 绑定的CAN控制器
             const Motor_Protocol_t *pstProtocol 协议操作函数表
             void *pstPrivData 协议私有数据
 输出参数  : 无
 返 回 值  : Motor_Instance_t* 电机实例指针
 调用函数  : 
 被调函数  : 
 
 修改历史      :
  1.日    期   : 2026年7月13日
    作    者   : radkil
    修改内容   : 新生成函数

*****************************************************************************/
Motor_Instance_t* MotorMgr_Create(const Motor_Config_t *pstConfig, 
                                   TComCtrl *ptCAN,
                                   const Motor_Protocol_t *pstProtocol,
                                   void *pstPrivData)
{
    if (NULL == pstConfig || NULL == ptCAN || NULL == pstProtocol)
    {
        return NULL;
    }

    // 检查是否已存在相同ID的电机
    rd_slist_t *pos;
    rd_slist_t *n;
    rd_slist_for_each(pos, n, &g_stMotorList)
    {
        Motor_Instance_t *pstCheck = rd_slist_entry(pos, Motor_Instance_t, node);
        if (pstCheck->m_stConfig.m_ucMotorID == pstConfig->m_ucMotorID)
        {
            return NULL; // ID重复
        }
    }

    // 分配内存
    Motor_Instance_t *pstMotor = (Motor_Instance_t *)RD_CALLOC(1, sizeof(Motor_Instance_t));
    if (NULL == pstMotor)
    {
        return NULL;
    }

    // 初始化链表节点
    pstMotor->node.next = NULL;

    // 复制配置
    pstMotor->m_stConfig.m_ucMotorID = pstConfig->m_ucMotorID;
    pstMotor->m_stConfig.m_uiPulsePerRound = pstConfig->m_uiPulsePerRound > 0 ? pstConfig->m_uiPulsePerRound : 10000;
    pstMotor->m_stConfig.m_uiReductionRatio = pstConfig->m_uiReductionRatio > 0 ? pstConfig->m_uiReductionRatio : 70;
    pstMotor->m_stConfig.m_fWheelDiameter = pstConfig->m_fWheelDiameter > 0 ? pstConfig->m_fWheelDiameter : 0.26f;

    // 初始化运行数据
    pstMotor->m_stData.m_iRealPosition = 0;
    pstMotor->m_stData.m_iRealVelocity = 0;
    pstMotor->m_stData.m_iRealTorque = 0;
    pstMotor->m_stData.m_uiFaultCode = 0;
    pstMotor->m_stData.m_dRealDistance = 0.0;
    pstMotor->m_stData.m_iStartMeasurePos = 0;
    pstMotor->m_stData.m_iLastPosition = 0;
    pstMotor->m_stData.m_iRoundCount = 0;
    pstMotor->m_stData.m_ucStartMeasureFlag = 1;

    // 绑定CAN控制器和协议
    pstMotor->m_ptCAN = ptCAN;
    pstMotor->m_eState = MOTOR_STATE_IDLE;
    pstMotor->m_uiLastHeartbeat = 0;
    pstMotor->m_pstProtocol = pstProtocol;
    pstMotor->m_pstPrivData = pstPrivData;

    // 设置实例名称
    RD_SNPRINTF(pstMotor->m_szName, MOTOR_MAX_NAME_LEN, "Motor_%d", pstConfig->m_ucMotorID);

    // 添加到链表尾部
    rd_slist_append(&g_stMotorList, &pstMotor->node);

    return pstMotor;
}

/*****************************************************************************
 函 数 名  : MotorMgr_Destroy
 功能描述  : 销毁电机实例
 输入参数  : Motor_Instance_t *pstMotor 电机实例
 输出参数  : 无
 返 回 值  : int 0成功，非0失败
 调用函数  : 
 被调函数  : 
 
 修改历史      :
  1.日    期   : 2026年7月13日
    作    者   : radkil
    修改内容   : 新生成函数

*****************************************************************************/
int MotorMgr_Destroy(Motor_Instance_t *pstMotor)
{
    if (NULL == pstMotor)
    {
        return RD_NULL;
    }

    // 从链表中移除
    rd_slist_remove(&g_stMotorList, &pstMotor->node);
    RD_FREE(pstMotor);

    return RD_SUCCESS;
}

/*****************************************************************************
 函 数 名  : MotorMgr_FindByName
 功能描述  : 通过名称查找电机实例
 输入参数  : const char *pszName 实例名称
 输出参数  : 无
 返 回 值  : Motor_Instance_t* 电机实例指针
 调用函数  : 
 被调函数  : 
 
 修改历史      :
  1.日    期   : 2026年7月13日
    作    者   : radkil
    修改内容   : 新生成函数

*****************************************************************************/
Motor_Instance_t* MotorMgr_FindByName(const char *pszName)
{
    if (NULL == pszName)
    {
        return NULL;
    }

    rd_slist_t *pos;
    rd_slist_t *n;
    rd_slist_for_each(pos, n, &g_stMotorList)
    {
        Motor_Instance_t *pstMotor = rd_slist_entry(pos, Motor_Instance_t, node);
        if (RD_STRCMP(pstMotor->m_szName, pszName) == 0)
        {
            return pstMotor;
        }
    }
    return NULL;
}

/*****************************************************************************
 函 数 名  : MotorMgr_FindByID
 功能描述  : 通过ID查找电机实例
 输入参数  : uint8_t ucMotorID 电机节点ID
 输出参数  : 无
 返 回 值  : Motor_Instance_t* 电机实例指针
 调用函数  : 
 被调函数  : 
 
 修改历史      :
  1.日    期   : 2026年7月13日
    作    者   : radkil
    修改内容   : 新生成函数

*****************************************************************************/
Motor_Instance_t* MotorMgr_FindByID(uint8_t ucMotorID)
{
    rd_slist_t *pos;
    rd_slist_t *n;
    rd_slist_for_each(pos, n, &g_stMotorList)
    {
        Motor_Instance_t *pstMotor = rd_slist_entry(pos, Motor_Instance_t, node);
        if (pstMotor->m_stConfig.m_ucMotorID == ucMotorID)
        {
            return pstMotor;
        }
    }
    return NULL;
}

/*****************************************************************************
 函 数 名  : MotorMgr_ResetAll
 功能描述  : 复位所有节点
 输入参数  : Motor_Instance_t *pstMotor 电机实例
 输出参数  : 无
 返 回 值  : int 0成功，非0失败
*****************************************************************************/
int MotorMgr_ResetAll(Motor_Instance_t *pstMotor)
{
    return MOTOR_PROTOCOL_CALL(pstMotor, pfResetAll);
}

/*****************************************************************************
 函 数 名  : MotorMgr_ActivateAll
 功能描述  : 激活所有节点
 输入参数  : Motor_Instance_t *pstMotor 电机实例
 输出参数  : 无
 返 回 值  : int 0成功，非0失败
*****************************************************************************/
int MotorMgr_ActivateAll(Motor_Instance_t *pstMotor)
{
    return MOTOR_PROTOCOL_CALL(pstMotor, pfActivateAll);
}

/*****************************************************************************
 函 数 名  : MotorMgr_SetHome
 功能描述  : 设置当前位置为零点
 输入参数  : Motor_Instance_t *pstMotor 电机实例
 输出参数  : 无
 返 回 值  : int 0成功，非0失败
*****************************************************************************/
int MotorMgr_SetHome(Motor_Instance_t *pstMotor)
{
    return MOTOR_PROTOCOL_CALL(pstMotor, pfSetHome);
}

/*****************************************************************************
 函 数 名  : MotorMgr_SetWatchdog
 功能描述  : 设置看门狗时间
 输入参数  : Motor_Instance_t *pstMotor 电机实例
             uint32_t uiTimeMs 毫秒
 输出参数  : 无
 返 回 值  : int 0成功，非0失败
*****************************************************************************/
int MotorMgr_SetWatchdog(Motor_Instance_t *pstMotor, uint32_t uiTimeMs)
{
    return MOTOR_PROTOCOL_CALL(pstMotor, pfSetWatchdog, uiTimeMs);
}

/*****************************************************************************
 函 数 名  : MotorMgr_RequestPosition
 功能描述  : 读取电机位置
 输入参数  : Motor_Instance_t *pstMotor 电机实例
 输出参数  : 无
 返 回 值  : int 0成功，非0失败
*****************************************************************************/
int MotorMgr_RequestPosition(Motor_Instance_t *pstMotor)
{
    return MOTOR_PROTOCOL_CALL(pstMotor, pfRequestPosition);
}

/*****************************************************************************
 函 数 名  : MotorMgr_RequestVelocity
 功能描述  : 读取电机速度
 输入参数  : Motor_Instance_t *pstMotor 电机实例
 输出参数  : 无
 返 回 值  : int 0成功，非0失败
*****************************************************************************/
int MotorMgr_RequestVelocity(Motor_Instance_t *pstMotor)
{
    return MOTOR_PROTOCOL_CALL(pstMotor, pfRequestVelocity);
}

/*****************************************************************************
 函 数 名  : MotorMgr_RequestTorque
 功能描述  : 读取电机电流/转矩
 输入参数  : Motor_Instance_t *pstMotor 电机实例
 输出参数  : 无
 返 回 值  : int 0成功，非0失败
*****************************************************************************/
int MotorMgr_RequestTorque(Motor_Instance_t *pstMotor)
{
    return MOTOR_PROTOCOL_CALL(pstMotor, pfRequestTorque);
}

/*****************************************************************************
 函 数 名  : MotorMgr_RequestFault
 功能描述  : 读取故障码
 输入参数  : Motor_Instance_t *pstMotor 电机实例
 输出参数  : 无
 返 回 值  : int 0成功，非0失败
*****************************************************************************/
int MotorMgr_RequestFault(Motor_Instance_t *pstMotor)
{
    return MOTOR_PROTOCOL_CALL(pstMotor, pfRequestFault);
}

/*****************************************************************************
 函 数 名  : MotorMgr_SpeedModeInit
 功能描述  : 速度模式初始化
 输入参数  : Motor_Instance_t *pstMotor 电机实例
 输出参数  : 无
 返 回 值  : int 0成功，非0失败
*****************************************************************************/
int MotorMgr_SpeedModeInit(Motor_Instance_t *pstMotor)
{
    int iRet = MOTOR_PROTOCOL_CALL(pstMotor, pfSpeedModeInit);
    if (iRet == RD_SUCCESS)
    {
        pstMotor->m_eState = MOTOR_STATE_READY;
    }
    return iRet;
}

/*****************************************************************************
 函 数 名  : MotorMgr_SetTargetSpeed
 功能描述  : 设置目标速度
 输入参数  : Motor_Instance_t *pstMotor 电机实例
             int32_t iTargetSpeed 目标速度(脉冲/秒)
 输出参数  : 无
 返 回 值  : int 0成功，非0失败
*****************************************************************************/
int MotorMgr_SetTargetSpeed(Motor_Instance_t *pstMotor, int32_t iTargetSpeed)
{
    int iRet = MOTOR_PROTOCOL_CALL(pstMotor, pfSetTargetSpeed, iTargetSpeed);
    if (iRet == RD_SUCCESS)
    {
        pstMotor->m_eState = MOTOR_STATE_RUNNING;
    }
    return iRet;
}

/*****************************************************************************
 函 数 名  : MotorMgr_PositionModeInit
 功能描述  : 位置模式初始化
 输入参数  : Motor_Instance_t *pstMotor 电机实例
             int32_t iAccel 加速度(脉冲/秒²)
             int32_t iDecel 减速度(脉冲/秒²)
             int32_t iTargetSpeed 目标速度(脉冲/秒)
 输出参数  : 无
 返 回 值  : int 0成功，非0失败
*****************************************************************************/
int MotorMgr_PositionModeInit(Motor_Instance_t *pstMotor, int32_t iAccel, int32_t iDecel, int32_t iTargetSpeed)
{
    int iRet = MOTOR_PROTOCOL_CALL(pstMotor, pfPositionModeInit, iAccel, iDecel, iTargetSpeed);
    if (iRet == RD_SUCCESS)
    {
        pstMotor->m_eState = MOTOR_STATE_READY;
    }
    return iRet;
}

/*****************************************************************************
 函 数 名  : MotorMgr_SetTargetPosition
 功能描述  : 设置目标位置（相对运动）
 输入参数  : Motor_Instance_t *pstMotor 电机实例
             int32_t iTargetPos 目标位置(脉冲)
 输出参数  : 无
 返 回 值  : int 0成功，非0失败
*****************************************************************************/
int MotorMgr_SetTargetPosition(Motor_Instance_t *pstMotor, int32_t iTargetPos)
{
    int iRet = MOTOR_PROTOCOL_CALL(pstMotor, pfSetTargetPosition, iTargetPos);
    if (iRet == RD_SUCCESS)
    {
        pstMotor->m_eState = MOTOR_STATE_RUNNING;
    }
    return iRet;
}

/*****************************************************************************
 函 数 名  : MotorMgr_SetDO
 功能描述  : 设置数字输出
 输入参数  : Motor_Instance_t *pstMotor 电机实例
             uint8_t ucDoIndex 输出索引(1或2)
             uint8_t ucState 状态(0或1)
 输出参数  : 无
 返 回 值  : int 0成功，非0失败
*****************************************************************************/
int MotorMgr_SetDO(Motor_Instance_t *pstMotor, uint8_t ucDoIndex, uint8_t ucState)
{
    return MOTOR_PROTOCOL_CALL(pstMotor, pfSetDO, ucDoIndex, ucState);
}

/*****************************************************************************
 函 数 名  : MotorMgr_ParseResponse
 功能描述  : 解析CAN响应数据（遍历所有电机实例尝试解析）
 输入参数  : uint8_t ucMotorID 电机节点ID
             const uint8_t *pucData 数据指针
             uint32_t uiLen 数据长度
 输出参数  : 无
 返 回 值  : void
*****************************************************************************/
void MotorMgr_ParseResponse(uint8_t ucMotorID, const uint8_t *pucData, uint32_t uiLen)
{
    if (NULL == pucData || uiLen < 8)
    {
        return;
    }

    Motor_Instance_t *pstMotor = MotorMgr_FindByID(ucMotorID);
    if (NULL == pstMotor)
    {
        return;
    }

    if (pstMotor->m_pstProtocol && pstMotor->m_pstProtocol->pfParseResponse)
    {
        pstMotor->m_pstProtocol->pfParseResponse(pstMotor, pucData, uiLen);
    }
}

/*****************************************************************************
 函 数 名  : MotorMgr_SpeedConvert
 功能描述  : 速度单位转换
 输入参数  : Motor_Instance_t *pstMotor 电机实例
             int32_t iSpeedUserUnit 用户速度单位
 输出参数  : 无
 返 回 值  : int32_t 脉冲/秒
*****************************************************************************/
int32_t MotorMgr_SpeedConvert(Motor_Instance_t *pstMotor, int32_t iSpeedUserUnit)
{
    if (pstMotor->m_pstProtocol && pstMotor->m_pstProtocol->pfSpeedConvert)
    {
        return pstMotor->m_pstProtocol->pfSpeedConvert(pstMotor, iSpeedUserUnit);
    }
    return 0;
}

/*****************************************************************************
 函 数 名  : MotorMgr_GetData
 功能描述  : 获取电机数据
 输入参数  : Motor_Instance_t *pstMotor 电机实例
 输出参数  : 无
 返 回 值  : const Motor_Data_t*
*****************************************************************************/
const Motor_Data_t* MotorMgr_GetData(Motor_Instance_t *pstMotor)
{
    if (NULL == pstMotor)
    {
        return NULL;
    }
    return &pstMotor->m_stData;
}

/*****************************************************************************
 函 数 名  : MotorMgr_GetState
 功能描述  : 获取电机状态
 输入参数  : Motor_Instance_t *pstMotor 电机实例
 输出参数  : 无
 返 回 值  : Motor_State_e
*****************************************************************************/
Motor_State_e MotorMgr_GetState(Motor_Instance_t *pstMotor)
{
    if (NULL == pstMotor)
    {
        return MOTOR_STATE_IDLE;
    }
    return pstMotor->m_eState;
}