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

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

 ******************************************************************************
  文 件 名   : paint_robot_new_motors.c
  版 本 号   : 初稿
  作    者   : radkil
  生成日期   : 2026年8月21日
  最近修改   :
  功能描述   : 喷涂机器人电机逻辑
  
  修改历史   :
  1.日    期   : 2026年8月21日
    作    者   : radkil
    修改内容   : 创建文件

******************************************************************************/
#include "BHBF.h"
#include "motor_manager.h"
#include "msp_motor_leisai.h"
#include "msg_center.h"
#include "lua_base.h"

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

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

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

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

/*----------------------------------------------*
 * 模块级变量                                   *
 *----------------------------------------------*/
static Motor_Instance_t *g_apstMotors[MOTOR_INSTANCE_MAX] = {NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL};
static uint32_t g_uiMotorModuleID = 0;
static int g_aiMotorSpeed[MOTOR_INSTANCE_MAX] = {0};
static int g_isMotorInit = -1; // -1表示还在等待遥控器初始化完毕，0表示可以进行电机初始化，1表示电机初始化完毕，运行电机主循环
static int g_iLastVehicleMove = 0;
static int g_iInitOnce = 0; // 防止重复初始化标志

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

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

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

    static int iVehicleSpeed = 1;
	static uint32_t uiLastTime = 0;
    switch (pstMsg->m_uiMsgID)
    {
        case MOTOR_SET_SPEED:
        {
            int aiMotorSpeed[2] = {0};
            if (pstMsg->m_uiDataLen >= sizeof(aiMotorSpeed))
            {
                RD_MEMCPY(aiMotorSpeed, pstMsg->m_aucData, sizeof(aiMotorSpeed));
            }
            g_aiMotorSpeed[0] = MotorMgr_SpeedConvert(g_apstMotors[0], aiMotorSpeed[0]);
            g_aiMotorSpeed[1] = MotorMgr_SpeedConvert(g_apstMotors[1], aiMotorSpeed[1]);
            if (0 == g_aiMotorSpeed[0] && 0 == g_aiMotorSpeed[1])
			{
				g_iLastVehicleMove = 0;
			}
			else
			{
				g_iLastVehicleMove = 1;
			}
			uiLastTime = Rd_GetTime();
			break;
        }
        case MOTOR_POWER_ENABLE:
		{
			g_isMotorInit = 0;
			break;
		}
        case MOTOR_POWER_DISABLE:
		{
			HAL_GPIO_WritePin(OUT_0_GPIO_Port, OUT_0_Pin, 1);
			break;
		}
        case GET_VEHICLE_SPEED:
        {
        	if (pstMsg->m_uiDataLen >= sizeof(int))
			{
				RD_MEMCPY(&iVehicleSpeed, pstMsg->m_aucData, sizeof(int));
			}
        	break;
        }
        case MOTOR_CMD_STOP_AFTER:
        {
        	static int iIsDelayStop = 0;

			if (1 == g_iLastVehicleMove)
			{
				MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_SET_STOP_OFF_PAINT, NULL, 0);
				MsgCenter_SendTo(MODULE_NAME_CUSTOM, CMD_STOP_ALL, NULL, 0);//先关枪
				iIsDelayStop = 1;
			}

			g_iLastVehicleMove = 0;

			if (1 == iIsDelayStop)
			{
				if (Rd_GetTime() - uiLastTime >= 600 * g_stCV.Paint_Gun_Shutdown_Distance / iVehicleSpeed)
				{
					memset(g_aiMotorSpeed, 0, sizeof(g_aiMotorSpeed));
					iIsDelayStop = 0;
					MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_RESET_PAINT, NULL, 0);
				}
				break;
			}
			else
			{
				uiLastTime = Rd_GetTime();
			}
            break;
        }
		case CMD_STOP_ALL:
		{
			memset(g_aiMotorSpeed, 0, sizeof(g_aiMotorSpeed));
			uiLastTime = Rd_GetTime();
			g_iLastVehicleMove = 0;
			break;
		}
		case CMD_SHOW_INFO:
		{
			lua_print("iVehicleSpeed = %d\nuiLastTime = %d\ng_isMotorInit = %d\ng_iLastVehicleMove = %d\n",
				iVehicleSpeed, uiLastTime, g_isMotorInit, g_iLastVehicleMove);
			break;
		}
        default:
            break;
    }
}

// 雷赛电机CAN响应检查函数
// 雷赛响应ID范围：0x581 - 0x5FF (0x580 + MotorID)
static int check_LeiSaiMotor(char *_pBuffer, uint32_t _iSize)
{
	TCANUserData *ptCANUserData = (TCANUserData *)g_ptCAN1->m_pUserData;
	uint32_t iCANId = (uint32_t)ptCANUserData->m_canrx->Identifier;

    if (iCANId >= 0x581 && iCANId <= 0x5FF)
    {
        return _iSize;
    }

    return 0;
}

// 雷赛电机CAN响应解码函数
static void decode_LeiSaiMotor(const char *_pBuffer, uint32_t _iSize)
{
	TCANUserData *ptCANUserData = (TCANUserData *)g_ptCAN1->m_pUserData;
	uint32_t iCANId = (uint32_t)ptCANUserData->m_canrx->Identifier;

    // 提取电机ID (0x580 + MotorID)
    uint8_t ucMotorID = (uint8_t)(iCANId - 0x580);

    // 调用电机管理模块解析响应
    MotorMgr_ParseResponse(ucMotorID, (const uint8_t *)_pBuffer, _iSize);
}

void MotorTask(void *argument)
{
	g_uiMotorModuleID = MsgCenter_Register(MODULE_NAME_MOTOR, Motor_ModuleHandler);
    while(1)
    {
    	// 处理消息
    	MsgCenter_ProcessWait(g_uiMotorModuleID, 2);

    	if (0 == g_isMotorInit && 0 == g_iInitOnce)
    	{
    		HAL_GPIO_WritePin(OUT_0_GPIO_Port, OUT_0_Pin, 0);
			// 初始化FDCAN1，使用雷赛电机的回调
			TCANUserData *ptCANUserData = CAN_userdata_init(&hfdcan1, 32, 0);
			g_ptCAN1 = rd_ComCreate(check_LeiSaiMotor, decode_LeiSaiMotor, FDCAN1_Send, CONFIG_UART_BUFFER_SIZE, ptCANUserData);
			CAN_IT_init(g_ptCAN1);

			// 初始化电机管理模块
			MotorMgr_Init();

			// 配置左轮电机
			Motor_Config_t stLeftMotorConfig = {
				.m_ucMotorID = 1,
				.m_uiPulsePerRound = g_stCV.pulse_Per_Circle,
				.m_uiReductionRatio = g_stCV.wheel_Reduction_Ratio,
				.m_fWheelDiameter = g_stCV.wheel_Diameter_m
			};
			g_apstMotors[0] = MotorMgr_Create(&stLeftMotorConfig, g_ptCAN1, LeiSai_GetProtocol(), NULL);

			// 配置右轮电机
			Motor_Config_t stRightMotorConfig = {
				.m_ucMotorID = 2,
				.m_uiPulsePerRound = g_stCV.pulse_Per_Circle,
				.m_uiReductionRatio = g_stCV.wheel_Reduction_Ratio,
				.m_fWheelDiameter = g_stCV.wheel_Diameter_m
			};
			g_apstMotors[1] = MotorMgr_Create(&stRightMotorConfig, g_ptCAN1, LeiSai_GetProtocol(), NULL);

			MotorMgr_ResetAll(g_apstMotors[0]);
			Rd_Delay(500);
			MotorMgr_ResetAll(g_apstMotors[1]);
			Rd_Delay(500);

			int iCount = 6;
			while(iCount--)
			{
				MotorMgr_ActivateAll(g_apstMotors[0]);
				Rd_Delay(500);
				MotorMgr_ActivateAll(g_apstMotors[1]);
				Rd_Delay(500);
			}

			MotorMgr_SetHome(g_apstMotors[0]);
			MotorMgr_SetHome(g_apstMotors[1]);
			MotorMgr_SpeedModeInit(g_apstMotors[0]);
			MotorMgr_SpeedModeInit(g_apstMotors[1]);
    		g_isMotorInit = 1;
			g_iInitOnce = 1;
			MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_GET_MOTOR_OK, NULL, 0);
    	}
    	else if (g_isMotorInit > 0)
    	{
    		static int LS_Motor_Config_Count = 0;
    		static int iStatusQueryCycle = 0;
    		static int iLastiCount = 0;

			if (LS_Motor_Config_Count <= 200)
			{
				LS_Motor_Config_Count++;
				MotorMgr_SetWatchdog(g_apstMotors[0], 1000);
				Rd_Delay(4);
				MotorMgr_SetWatchdog(g_apstMotors[1], 1000);
				Rd_Delay(4);
			}
			
			int iCurrentCount = Rd_GetTime();
			if (iCurrentCount - iLastiCount >= 1000)
			{
				log_e("Send motor timeout (%d) !", iCurrentCount - iLastiCount);
			}

			MotorMgr_SetTargetSpeed(g_apstMotors[0], g_aiMotorSpeed[0]);
			Rd_Delay(4);
			MotorMgr_SetTargetSpeed(g_apstMotors[1], g_aiMotorSpeed[1]);
			Rd_Delay(4);
			iLastiCount = Rd_GetTime();

			if (++iStatusQueryCycle >= 10)
			{
				iStatusQueryCycle = 0;
				MotorMgr_RequestPosition(g_apstMotors[0]);
				Rd_Delay(4);
				MotorMgr_RequestPosition(g_apstMotors[1]);
				Rd_Delay(4);
				MotorMgr_RequestFault(g_apstMotors[0]);
				Rd_Delay(4);
				MotorMgr_RequestFault(g_apstMotors[1]);
				Rd_Delay(4);
				MotorMgr_RequestVelocity(g_apstMotors[0]);
				Rd_Delay(4);
				MotorMgr_RequestVelocity(g_apstMotors[1]);
				Rd_Delay(4);
			}
    	}
    }
}