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

                  版权所有 (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"

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

/*----------------------------------------------*
 * 外部函数原型说明                             *
 *----------------------------------------------*/
double Angle_Tune_PID(double CurrentAngle, double TargetAngle, double Position_KP,
		double Position_KI, double Position_KD, double MaxValue);
/*----------------------------------------------*
 * 内部函数原型说明                             *
 *----------------------------------------------*/

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

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

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

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

    static int iSpeed = -1; // 表示由RD1转换来的速度值
    static int aiMotorSpeed[2] = {0}; //0索引表示左，1索引表示右，多轮时到对应motor的MOTOR_CMD_SET_SPEED命令自行处理
    static int g_dletAngle = 0; //PID最终计算补偿值
    static int32_t g_iPIDAngle = 0; //当前角度

    static uint32_t uiLastSendTick = 0; //上一次时间戳，用于控制直线行驶距离
    static int bTimerStarted = 0; //直线行驶距离单次触发标志
    switch (pstMsg->m_uiMsgID)
    {
		case RBCORE_CMD_STOP_ALL:
		{
    		MsgCenter_SendTo(MODULE_NAME_MOTOR, COMMON_CMD_STOP_ALL, NULL, 0);
    		MsgCenter_SendTo(MODULE_NAME_CUSTOM, COMMON_CMD_STOP_ALL, NULL, 0);
			break;
		}
		case RBCORE_GET_TL720D_ROLL:
		{
			if (pstMsg->m_uiDataLen >= sizeof(int32_t))
			{
				RD_MEMCPY(&g_iPIDAngle, pstMsg->m_aucData, sizeof(int32_t));
			}
			break;
		}
        case RBCORE_CMD_MANUAL_FORWARD:
        {
        	if (iSpeed >= 0)
        	{
        		aiMotorSpeed[0] = iSpeed;
        		aiMotorSpeed[1] = iSpeed;
        		MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_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_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_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_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
			}
			break;
		}
        case RBCORE_CMD_RESET_STRAIGHT:
        {
        	uiLastSendTick = 0;
        	bTimerStarted = 0;
        	break;
        }
        case RBCORE_CMD_STRAIGHT_DRIVE:
		{
			BHBF_straight_drive_Cmd stCmd = {0};
			if (pstMsg->m_uiDataLen >= sizeof(stCmd))
			{
				RD_MEMCPY(&stCmd, pstMsg->m_aucData, sizeof(stCmd));

				if (stCmd.m_iTime > 0 && 0 == bTimerStarted)
				{
					uiLastSendTick = Rd_GetTime();
					bTimerStarted = 1; // 开始计时
				}

				if (0 == stCmd.m_iMode)
				{
					break;
				}

				if (abs(g_iPIDAngle - stCmd.m_iAngle) <= g_stCV.PID_mid.PID_Angle)
				{
					if (abs(g_iPIDAngle - stCmd.m_iAngle) < g_stCV.PID_low.PID_Angle)
					{
						g_dletAngle = Angle_Tune_PID((double)g_iPIDAngle, stCmd.m_iAngle,
								g_stCV.PID_low.Kp, g_stCV.PID_low.Ki, g_stCV.PID_low.Kd, 10);
					}
					else
					{
						g_dletAngle = Angle_Tune_PID((double)g_iPIDAngle, stCmd.m_iAngle,
								g_stCV.PID_mid.Kp, g_stCV.PID_mid.Ki, g_stCV.PID_mid.Kd, 10);
					}
					if (stCmd.m_iMode > 0)
					{
						aiMotorSpeed[0] = (g_stCV.Lane_Change_Speed_m_per_min * 10) - g_dletAngle ;
						aiMotorSpeed[1] = (g_stCV.Lane_Change_Speed_m_per_min * 10) + g_dletAngle ;
					}
					else
					{
						aiMotorSpeed[0] = -(g_stCV.Lane_Change_Speed_m_per_min * 10) - g_dletAngle ;
						aiMotorSpeed[1] = -(g_stCV.Lane_Change_Speed_m_per_min * 10) + g_dletAngle ;
					}
				}
				else
				{
					g_dletAngle = Angle_Tune_PID((double)g_iPIDAngle, stCmd.m_iAngle,
							g_stCV.PID_high.Kp, g_stCV.PID_high.Ki, g_stCV.PID_high.Kd,50);
					aiMotorSpeed[0] = -g_dletAngle ;
					aiMotorSpeed[1] = g_dletAngle ;
				}

				if (stCmd.m_iTime > 0 && (int32_t)(Rd_GetTime() - uiLastSendTick) >= stCmd.m_iTime)// 到时间停止
				{
					aiMotorSpeed[0] = 0;
					aiMotorSpeed[1] = 0;
				}
				MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));

				if (aiMotorSpeed[0] == 0 && aiMotorSpeed[1] == 0)
				{
					MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
				}
			}
			break;
		}
        case RBCORE_CMD_TURN_ANGLE:
		{
			int iTargetAngle = 0;
			if (pstMsg->m_uiDataLen >= sizeof(int))
			{
				RD_MEMCPY(&iTargetAngle, pstMsg->m_aucData, sizeof(int));
				if (abs(g_iPIDAngle - iTargetAngle) <= 50)
				{
					aiMotorSpeed[0] = 0;
					aiMotorSpeed[1] = 0;
				}
				else if (abs(g_iPIDAngle - iTargetAngle) <= 1000)
				{
					g_dletAngle = Angle_Tune_PID((double)g_iPIDAngle, iTargetAngle, 1, 0,0.5, 10);
					aiMotorSpeed[0] = -g_dletAngle ;
					aiMotorSpeed[1] = g_dletAngle;
				}
				else
				{
					g_dletAngle = Angle_Tune_PID((double)g_iPIDAngle, iTargetAngle, 2, 0, 0.5, 85);
					aiMotorSpeed[0] = -g_dletAngle ;
					aiMotorSpeed[1] = g_dletAngle ;
				}
				MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));

				if (aiMotorSpeed[0] == 0 && aiMotorSpeed[1] == 0)
				{
					MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
				}
			}
			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 = THREAD_DEFAULT_STACK,
        .priority = (osPriority_t) osPriorityRealtime1,
    };
    (void)osThreadNew(RBcore_Task, NULL, &GF_Dispatch_attributes);
}