/*
 * LS_CAN_Motor_Frames.c
 *
 *  Created on: Mar 21, 2025
 *      Author: BingooRobotFJX
 */

#include <motors.h>
#include "fsm_state_control.h"
#include "BHBF_ROBOT.h"
FDCANHandler *LS_Motor_Controller;
DispacherController *LS_DispacherController;

void MotorCommandsLoop();
void MotorDecodeCAN(uint32_t canID, uint8_t *buffer, uint32_t length);
int32_t speed_E01m_per_min_to_pulse_s(int32_t speed_E01m_per_min);



char LS_Motor_Need_To_Activate = 0;
char LS_Motor_Config_Count = 0;

/*
 *
 * */
void LS_Motor_Controller_intialize(FDCANHandler *Handler)
{


	//初始化
	LS_Motor_Controller = Handler;
	LS_Motor_Controller->CAN_Decode = MotorDecodeCAN;

	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,
	        "LS_CAN_ID1_LeftMotor", 0, ComError_LS_LeftMotor);

	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,
	        "LS_CAN_ID2_RightMotor", 0, ComError_LS_RightMotor);

	LS_DispacherController = Handler->dispacherController;
	LS_DispacherController->DispacherCallTime = 2;
	LS_DispacherController->Add_Dispatcher_List(LS_DispacherController,
	        MotorCommandsLoop);
}

/*
 *
 * */
void MotorCommandsLoop()
{
	if (LS_Motor_Need_To_Activate == 1)
	{


		LS_Reset_All_Motors(LS_Motor_Controller, 1000);//等待1s
		LS_Reset_All_Motors(LS_Motor_Controller, 500);

		LS_Activate_All_Motors(LS_Motor_Controller, 1000);
		LS_Activate_All_Motors(LS_Motor_Controller, 1000);
		LS_Activate_All_Motors(LS_Motor_Controller, 500);
		LS_Activate_All_Motors(LS_Motor_Controller, 500);
		LS_Activate_All_Motors(LS_Motor_Controller, 500);


		LS_Set_Current_Positon_Zero(GV.LeftMotor.MotorID, LS_Motor_Controller, 50);
		LS_Set_Current_Positon_Zero(GV.RightMotor.MotorID, LS_Motor_Controller, 50);
		LS_SpeedModeSetup(GV.LeftMotor.MotorID, LS_Motor_Controller, 10, 1500000, 1500000,
		        0);
		LS_SpeedModeSetup(GV.RightMotor.MotorID, LS_Motor_Controller, 10, 1500000, 1500000,
		        0);

		LS_Motor_Need_To_Activate = 0;
	}
	else
	{
		if(LS_Motor_Config_Count <= 200) /*confirm complete config -->stop send*/
		{
			LS_Motor_Config_Count++;
			LS_Set_WatchDogTime(GV.LeftMotor.MotorID, LS_Motor_Controller, 5);
			LS_Set_WatchDogTime(GV.RightMotor.MotorID, LS_Motor_Controller, 5);

		}
		LS_Request_Position(GV.LeftMotor.MotorID, LS_Motor_Controller, 2);
		LS_Request_Fault(GV.LeftMotor.MotorID, LS_Motor_Controller, 2);
		LS_Request_Velocity(GV.LeftMotor.MotorID, LS_Motor_Controller, 2);
		LS_Request_Position(GV.RightMotor.MotorID, LS_Motor_Controller, 2);
		LS_Request_Fault(GV.RightMotor.MotorID, LS_Motor_Controller, 2);

		LS_SpeedMode_Set_TargetSpeed(GV.LeftMotor.MotorID, LS_Motor_Controller, 2,
				speed_E01m_per_min_to_pulse_s(GV.LeftMotor.Target_Velcity)  );

		LS_SpeedMode_Set_TargetSpeed(GV.RightMotor.MotorID, LS_Motor_Controller, 2,
				speed_E01m_per_min_to_pulse_s(GV.RightMotor.Target_Velcity) );

	}

}

/*
 * 解析函数
 *
 * */
void MotorDecodeCAN(uint32_t canID, uint8_t *buffer, uint32_t length)
{
	switch (canID - 0x580)
	{

		case 1:
		{
			HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
			        "LS_CAN_ID1_LeftMotor", 1);
			LS_Analytic_Fun(GV.LeftMotor.MotorID, buffer); //ID 和数据
		}
			break;
		case 2:
		{
			HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
			        "LS_CAN_ID2_RightMotor", 1);
			LS_Analytic_Fun(GV.RightMotor.MotorID, buffer);
		}
			break;
	}

}

/* DO输出
 * @state: 1:ON; 0:OFF;
 * */
void LS_Motor_DOSet(uint8_t state)
{
	if (state == 1)
	{
		LS_DO_Set(GV.LeftMotor.MotorID, 1, state, LS_Motor_Controller, 10);
	}
	else if (state == 0)
	{
		LS_DO_Set(GV.LeftMotor.MotorID, 1, state, LS_Motor_Controller, 10);
	}
}

/*
 * SpeedMPMin:0.1 m/min
 * P*60/10000/70/(Πd)=V
 * return:1 pulse/s
 * */
int32_t speed_E01m_per_min_to_pulse_s(int32_t speed_E01m_per_min)
{
	return (int32_t) (speed_E01m_per_min*0.1 *CV.pulse_Per_Circle * CV.wheel_Reduction_Ratio / 60
		        / (3.14 * CV.wheel_Diameter_m));
}