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

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

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

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

#include "BHBF_robot.h"
#include "msg_center.h"
#include "motor_manager.h"
#include "msp_motor_leisai.h"
#include "cmsis_os.h"
#include "FreeRTOS.h"

/*==============================================*
 *      模块定义                                *
 *==============================================*/

// 模块名称定义
#define MODULE_NAME_MOTOR       "motor"
#define MODULE_NAME_CHASSIS     "chassis"
#define MODULE_NAME_UI          "ui"

/*==============================================*
 *      电机模块命令字定义                       *
 *==============================================*/

typedef enum {
    MOTOR_CMD_SET_SPEED = 0x01,         // 设置速度
    MOTOR_CMD_SET_POSITION = 0x02,      // 设置位置
    MOTOR_CMD_STOP = 0x03,              // 停止
    MOTOR_CMD_GET_STATUS = 0x04,        // 获取状态
    MOTOR_CMD_SET_HOME = 0x05,          // 设置零点
    MOTOR_CMD_SPEED_MODE = 0x06,        // 速度模式
    MOTOR_CMD_POSITION_MODE = 0x07,     // 位置模式
} Motor_Cmd_e;

// 电机命令数据结构
typedef struct {
    uint8_t     m_ucMotorIndex;     // 电机索引（0=左轮，1=右轮）
    int32_t     m_iValue;           // 值（速度/位置等）
} Motor_CmdData_t;

// 电机状态数据结构
typedef struct {
    uint8_t     m_ucMotorIndex;
    int32_t     m_iPosition;
    int32_t     m_iVelocity;
    uint32_t    m_uiFaultCode;
} Motor_StatusData_t;

/*==============================================*
 *      底盘模块命令字定义                       *
 *==============================================*/

typedef enum {
    CHASSIS_CMD_MOVE = 0x01,            // 移动
    CHASSIS_CMD_STOP = 0x02,            // 停止
    CHASSIS_CMD_SET_SPEED = 0x03,       // 设置速度
} Chassis_Cmd_e;

typedef struct {
    int32_t     m_iSpeedX;           // X方向速度
    int32_t     m_iSpeedY;           // Y方向速度
    int32_t     m_iSpeedW;           // 旋转速度
} Chassis_MoveData_t;

/*==============================================*
 *      模块级变量                               *
 *==============================================*/

static uint32_t g_uiMotorModuleID = 0;
static uint32_t g_uiChassisModuleID = 0;
static uint32_t g_uiUIModuleID = 0;

static Motor_Instance_t *g_apstMotors[2] = {NULL, NULL};

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

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("ui", 0x10, &stStatus, sizeof(stStatus));
                    }
                }
            }
            break;
        }
        default:
            break;
    }
}

/*==============================================*
 *      底盘模块消息处理                         *
 *==============================================*/

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

    switch (pstMsg->m_uiMsgID)
    {
        case CHASSIS_CMD_MOVE:
        {
            if (pstMsg->m_uiDataLen >= sizeof(Chassis_MoveData_t))
            {
                Chassis_MoveData_t *pstData = (Chassis_MoveData_t *)pstMsg->m_aucData;
                
                // 转换为左右轮速度（差速模型）
                int32_t iLeftSpeed = pstData->m_iSpeedX - pstData->m_iSpeedW;
                int32_t iRightSpeed = pstData->m_iSpeedX + pstData->m_iSpeedW;

                // 发送给电机模块
                Motor_CmdData_t stMotorCmd;
                stMotorCmd.m_ucMotorIndex = 0;
                stMotorCmd.m_iValue = iLeftSpeed;
                MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_CMD_SET_SPEED, &stMotorCmd, sizeof(stMotorCmd));

                stMotorCmd.m_ucMotorIndex = 1;
                stMotorCmd.m_iValue = iRightSpeed;
                MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_CMD_SET_SPEED, &stMotorCmd, sizeof(stMotorCmd));
            }
            break;
        }
        case CHASSIS_CMD_STOP:
        {
            MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_CMD_STOP, NULL, 0);
            break;
        }
        default:
            break;
    }
}

/*==============================================*
 *      UI模块消息处理                           *
 *==============================================*/

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

    // 处理来自其他模块的回复
    switch (pstMsg->m_uiMsgID)
    {
        case 0x10:  // 电机状态回复
        {
            if (pstMsg->m_uiDataLen >= sizeof(Motor_StatusData_t))
            {
                Motor_StatusData_t *pstStatus = (Motor_StatusData_t *)pstMsg->m_aucData;
                RD_PRINTF("Motor[%d] Status: Pos=%ld, Vel=%ld, Fault=%ld\n",
                          pstStatus->m_ucMotorIndex,
                          pstStatus->m_iPosition,
                          pstStatus->m_iVelocity,
                          pstStatus->m_uiFaultCode);
            }
            break;
        }
        default:
            break;
    }
}

/*==============================================*
 *      模块任务函数                             *
 *==============================================*/

// 电机模块任务
void Motor_ModuleTask(void *argument)
{
    // 初始化电机
    MotorMgr_Init();

    Motor_Config_t stConfig = {
        .m_ucMotorID = 1,
        .m_uiPulsePerRound = 10000,
        .m_uiReductionRatio = 70,
        .m_fWheelDiameter = 0.26f
    };
    g_apstMotors[0] = MotorMgr_Create(&stConfig, g_ptFDCAN1, LeiSai_GetProtocol(), NULL);

    stConfig.m_ucMotorID = 2;
    g_apstMotors[1] = MotorMgr_Create(&stConfig, g_ptFDCAN1, LeiSai_GetProtocol(), NULL);

    // 初始化电机
    for (int i = 0; i < 2; i++)
    {
        if (g_apstMotors[i] != NULL)
        {
            MotorMgr_ResetAll(g_apstMotors[i]);
            MotorMgr_ActivateAll(g_apstMotors[i]);
            MotorMgr_SpeedModeInit(g_apstMotors[i]);
        }
    }

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

        // 周期性读取电机状态
        for (int i = 0; i < 2; i++)
        {
            if (g_apstMotors[i] != NULL)
            {
                MotorMgr_RequestPosition(g_apstMotors[i]);
                MotorMgr_RequestVelocity(g_apstMotors[i]);
                MotorMgr_RequestFault(g_apstMotors[i]);
            }
        }
    }
}

// 底盘模块任务
void Chassis_ModuleTask(void *argument)
{
    while (1)
    {
        // 处理消息
        MsgCenter_ProcessWait(g_uiChassisModuleID, 10);
    }
}

// UI模块任务
void UI_ModuleTask(void *argument)
{
    while (1)
    {
        // 处理消息
        MsgCenter_ProcessWait(g_uiUIModuleID, 100);

        // 定期请求电机状态
        MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_CMD_GET_STATUS, NULL, 0);
    }
}

/*==============================================*
 *      初始化示例                               *
 *==============================================*/

void MsgExample_Init(void)
{
    // 初始化消息中心
    MsgCenter_Init();

    // 注册模块
    g_uiMotorModuleID = MsgCenter_Register(MODULE_NAME_MOTOR, Motor_ModuleHandler);
    g_uiChassisModuleID = MsgCenter_Register(MODULE_NAME_CHASSIS, Chassis_ModuleHandler);
    g_uiUIModuleID = MsgCenter_Register(MODULE_NAME_UI, UI_ModuleHandler);

    // 创建模块任务
    const osThreadAttr_t motor_task_attr = {
        .name = "motor_task",
        .stack_size = 1024,
        .priority = (osPriority_t) osPriorityHigh5,
    };
    (void)osThreadNew(Motor_ModuleTask, NULL, &motor_task_attr);

    const osThreadAttr_t chassis_task_attr = {
        .name = "chassis_task",
        .stack_size = 512,
        .priority = (osPriority_t) osPriorityHigh4,
    };
    (void)osThreadNew(Chassis_ModuleTask, NULL, &chassis_task_attr);

    const osThreadAttr_t ui_task_attr = {
        .name = "ui_task",
        .stack_size = 512,
        .priority = (osPriority_t) osPriorityNormal3,
    };
    (void)osThreadNew(UI_ModuleTask, NULL, &ui_task_attr);
}

/*==============================================*
 *      外部调用示例                             *
 *==============================================*/

// 从任何线程调用这些函数即可跨线程控制电机
void Example_SetMotorSpeed(uint8_t ucMotorIndex, int32_t iSpeed)
{
    Motor_CmdData_t stData;
    stData.m_ucMotorIndex = ucMotorIndex;
    stData.m_iValue = iSpeed;
    MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_CMD_SET_SPEED, &stData, sizeof(stData));
}

void Example_StopAllMotors(void)
{
    MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_CMD_STOP, NULL, 0);
}

void Example_ChassisMove(int32_t iSpeedX, int32_t iSpeedW)
{
    Chassis_MoveData_t stData;
    stData.m_iSpeedX = iSpeedX;
    stData.m_iSpeedY = 0;
    stData.m_iSpeedW = iSpeedW;
    MsgCenter_SendTo(MODULE_NAME_CHASSIS, CHASSIS_CMD_MOVE, &stData, sizeof(stData));
}