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

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

 ******************************************************************************
  文 件 名   : msp_Timer.c
  版 本 号   : 初稿
  作    者   : radkil
  生成日期   : 2026年9月17日
  最近修改   :
  功能描述   : 硬件定时器任务
  
  修改历史   :
  1.日    期   : 2026年9月17日
    作    者   : radkil
    修改内容   : 创建文件

******************************************************************************/
#include "BHBF.h"
#include "msg_center.h"
#include "rd_time.h"
#include "tim.h"

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

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

/*----------------------------------------------*
 * 模块级变量                                   *
 *----------------------------------------------*/
static uint32_t g_uiTimModuleID = 0;
/*----------------------------------------------*
 * 常量定义                                     *
 *----------------------------------------------*/

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

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

    static int aiMotorSpeed[2] = {0};

    static int g_dletAngle = 0; //PID最终计算补偿值
    static uint32_t uiLastSendTick = 0; //上一次时间戳，用于控制直线行驶距离
    static int bTimerStarted = 0; //直线行驶距离单次触发标志
    switch (pstMsg->m_uiMsgID)
    {
        case TIMER_CMD_RESET_STRAIGHT:
        {
        	uiLastSendTick = 0;
        	bTimerStarted = 0;
        	break;
        }
        case TIMER_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_RF_Angle_Roll - stCmd.m_iAngle) <= g_stCV.PID_mid.PID_Angle)
				{
					if (abs(g_RF_Angle_Roll - stCmd.m_iAngle) < g_stCV.PID_low.PID_Angle)
					{
						g_dletAngle = Angle_Tune_PID((double)g_RF_Angle_Roll, 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_RF_Angle_Roll, 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_RF_Angle_Roll, 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 TIMER_CMD_TURN_ANGLE:
		{
			int iTargetAngle = 0;
			if (pstMsg->m_uiDataLen >= sizeof(int))
			{
				RD_MEMCPY(&iTargetAngle, pstMsg->m_aucData, sizeof(int));
				if (abs(g_RF_Angle_Roll - iTargetAngle) <= 50)
				{
					aiMotorSpeed[0] = 0;
					aiMotorSpeed[1] = 0;
				}
				else if (abs(g_RF_Angle_Roll - iTargetAngle) <= 1000)
				{
					g_dletAngle = Angle_Tune_PID((double)g_RF_Angle_Roll, iTargetAngle, 1, 0,0.5, 10);
					aiMotorSpeed[0] = -g_dletAngle ;
					aiMotorSpeed[1] = g_dletAngle;
				}
				else
				{
					g_dletAngle = Angle_Tune_PID((double)g_RF_Angle_Roll, 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;
		}
        default:
            break;
    }
}

void Timer_Task(void)
{
    MsgCenter_Process(g_uiTimModuleID);
}

void Timer_Init(void)
{
    HAL_TIM_Base_Start_IT(&htim8);
    g_uiTimModuleID = MsgCenter_Register(MODULE_NAME_TIMER, Timer_ModuleHandler);
}