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

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

 ******************************************************************************
  文 件 名   : BHBF.c
  版 本 号   : 初稿
  作    者   : radkil
  生成日期   : 2026年6月8日
  最近修改   :
  功能描述   : 机器人主程序
  
  修改历史   :
  1.日    期   : 2026年6月8日
    作    者   : radkil
    修改内容   : 创建文件

******************************************************************************/
#include "BHBF.h"
#include "common.h"
#include "msg_center.h"

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

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

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

/*----------------------------------------------*
 * 全局变量                                     *
 *----------------------------------------------*/
static uint32_t g_uiRBcoreModuleID = 0;

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

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

/*----------------------------------------------*
 * 宏定义                                       *
 *----------------------------------------------*/
static void RBcore_ModuleHandler(const Msg_t *pstMsg)
{
    if (NULL == pstMsg)
    {
        return;
    }

    static int iSpeed = -1;
    static int aiMotorSpeed[2] = {0}; //0索引表示左，1索引表示右，多轮时到对应motor的MOTOR_CMD_SET_SPEED命令自行处理
    switch (pstMsg->m_uiMsgID)
    {
		case CMD_STOP_ALL:
		{
    		MsgCenter_SendTo(MODULE_NAME_MOTOR, pstMsg->m_uiMsgID, NULL, 0);
			break;
		}
        case RBCORE_CMD_MANUAL_FORWARD:
        {
        	if (iSpeed >= 0)
        	{
        		aiMotorSpeed[0] = iSpeed;
        		aiMotorSpeed[1] = iSpeed;
        		MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_CMD_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
        	}
            break;
        }
        case RBCORE_CMD_MANUAL_BACKWARD:
        {
        	if (iSpeed >= 0)
			{
				aiMotorSpeed[0] = -iSpeed;
				aiMotorSpeed[1] = -iSpeed;
				MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_CMD_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
			}
            break;
        }
        case RBCORE_CMD_MANUAL_TURNLEFT:
		{
			if (g_stCV.LeftTurnSpeed > 0 && g_stCV.RightTurnSpeed > 0)
			{
				aiMotorSpeed[0] = -g_stCV.LeftTurnSpeed;
				aiMotorSpeed[1] = g_stCV.RightTurnSpeed;
				MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_CMD_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
			}
			break;
		}
        case RBCORE_CMD_MANUAL_TURNRIGHT:
		{
			if (g_stCV.LeftTurnSpeed > 0 && g_stCV.RightTurnSpeed > 0)
			{
				aiMotorSpeed[0] = g_stCV.LeftTurnSpeed;
				aiMotorSpeed[1] = -g_stCV.RightTurnSpeed;
				MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_CMD_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
			}
			break;
		}
        case RBCORE_GET_VEHICLE_SPEED:
		{
			if (pstMsg->m_uiDataLen >= sizeof(int))
			{
				RD_MEMCPY(&iSpeed, pstMsg->m_aucData, sizeof(int));
				iSpeed *= 10;
			}
			break;
		}
        default:
            break;
    }
}

void RBcore_Task(void *argument)
{
    while(1)
    {
        // 处理消息
        MsgCenter_ProcessWait(g_uiRBcoreModuleID, 2);
    }
}

void RBcore_Init(void)
{
    g_uiRBcoreModuleID = MsgCenter_Register(MODULE_NAME_RBCORE, RBcore_ModuleHandler);

    const osThreadAttr_t GF_Dispatch_attributes = {
        .name = MODULE_NAME_RBCORE,
        .stack_size = 2048,
        .priority = (osPriority_t) osPriorityRealtime1,
    };
    (void)osThreadNew(RBcore_Task, NULL, &GF_Dispatch_attributes);
}