/*
 * steering_engine.c
 *
 *  Created on: Jul 18, 2024
 *      Author: akeguo
 */

#include "msp_Steering_Engine.h"
#include "msp_fashion_star_uart_servo.h"
#define USART_RECV_BUF_SIZE 2048
#define USART_SEND_BUF_SIZE 2048
#include "BHBF_ROBOT.h"

int32_t* Desulfurizer_Steering_Set_Speed_1;
int32_t* Desulfurizer_Steering_Set_Speed_2;
int32_t* Desulfurizer_Steering_Set_Speed_3;
int32_t* Desulfurizer_Steering_Set_Speed_4;


int32_t* Steering_damping_power_1;
int32_t* Steering_damping_power_2;
int32_t* Steering_damping_power_3;
int32_t* Steering_damping_power_4;
uint8_t steering_id_10 = 1;
int32_t* Desulfurizer_Steering_Angle_3;
int32_t* Desulfurizer_Steering_Angle_4;
int32_t* Desulfurizer_Steering_Current;
int32_t* Desulfurizer_Steering_Real_Angle_3;
int32_t* Desulfurizer_Steering_Real_Angle_4;
void steering_set_velocity(uint8_t id,int32_t Steering_Speed);
void steering_velocity_control();
void SteeringDampingPowerControl(uint8_t steering_id1,
								 int32_t Steering_damping_power);
void steering_control();
void steering_homing_control();
void decode_steering_angle(uint8_t *buffer, uint16_t length);
void decode_steering_current(uint8_t *buffer, uint16_t length);
struct UARTHandler *steering_engine_UART_Handler;
DispacherController* steering_dispacher;
void steering_engine_intialize(struct UARTHandler *Handler)
{
	steering_engine_UART_Handler = Handler;
	steering_engine_UART_Handler->Wait_time = 10; //等待10ms 最低不要低于4；
	steering_engine_UART_Handler->UART_Decode = decode_steering_angle;
	steering_dispacher=Handler->dispacherController;
	steering_dispacher->Dispacher_Enable = 1;
	steering_dispacher->DispacherCallTime = 100;
	steering_dispacher->Add_Dispatcher_List(steering_dispacher,read_steering_Angle);

	steering_dispacher->Add_Dispatcher_List(steering_dispacher,steering_control);
}

uint8_t set_speed_command[20];
uint8_t steering_id = 0;
uint8_t set_speed_command_sum = 0;
uint8_t set_speed_command_check = 0;
uint8_t steering_velocity_flag = 0;
uint8_t update_flag = 0;
uint8_t update_counts = 0;

void steering_control()
{
	if(GV.MK32_Key.CH5_SB == -1000)
	{
		switch(update_flag)
		{
			case 0:
				update_counts ++;
				steering_homing_control();
				if(update_counts >= 10)
				{
					update_flag = 1;
					update_counts = 0;
				}
				break;
			case 1:
				update_counts ++;
				update_steering_state();
				if(update_counts >= 10)
				{
					update_flag = 0;
					update_counts = 0;
				}
				break;
		}
	}
	else {
		steering_velocity_control();
	}
}

void steering_velocity_control()
{
	switch(steering_id)
	{
		case 0:
			steering_set_velocity_with_damping(steering_id,
											   *Desulfurizer_Steering_Set_Speed_1,
											   *Steering_damping_power_1);//停止后进入阻尼状态
			steering_id = 1;
			break;
		case 1:
			steering_set_velocity_with_damping(steering_id,
											   *Desulfurizer_Steering_Set_Speed_2,
											   *Steering_damping_power_2);//停止后进入阻尼状态
			steering_id = 2;
			break;
		case 2:
			steering_set_velocity_with_locking_force(steering_id,*Desulfurizer_Steering_Set_Speed_3);//停止后进入锁力状态
			steering_id = 3;
			break;
		case 3:
			steering_set_velocity_with_locking_force(steering_id,*Desulfurizer_Steering_Set_Speed_4);//停止后进入锁力状态
			steering_id = 0;
			break;
	}
}


uint8_t set_damping_power_command[20];
uint8_t set_damping_power_command_sum = 0;
uint8_t set_damping_power_command_check = 0;

void SteeringDampingPowerControl(uint8_t steering_id1,
								 int32_t Steering_damping_power)
{
	if(Steering_damping_power == 0)
	{
		set_speed_command[0] = 0x12;
		set_speed_command[1] = 0x4C;
		set_speed_command[2] = 0x18;
		set_speed_command[3] = 0x04;
		set_speed_command[4] = steering_id1;
		set_speed_command[5] = 0x11;
		set_speed_command[6] = 0x70;
		set_speed_command[7] = 0x17;

		set_speed_command_sum = (set_speed_command[0] + set_speed_command[1]+ set_speed_command[2]
							 + set_speed_command[3]+ set_speed_command[4]+ set_speed_command[5]+ set_speed_command[6]
							 + set_speed_command[7]);

		set_speed_command_check = set_speed_command_sum % 256;
		set_speed_command[8] = set_speed_command_check;

		memcpy(&steering_engine_UART_Handler->Tx_Buf,&set_speed_command,9);
		steering_engine_UART_Handler->TxCount = 9;
		steering_engine_UART_Handler->UART_Tx(steering_engine_UART_Handler);

	}
	else {
		set_damping_power_command[0] = 0x12;
		set_damping_power_command[1] = 0x4C;
		set_damping_power_command[2] = 0x09;
		set_damping_power_command[3] = 0x03;
		set_damping_power_command[4] = steering_id1;
		set_damping_power_command[5] = Steering_damping_power;
		set_damping_power_command[6] = Steering_damping_power >> 8;

		set_damping_power_command_sum = set_damping_power_command[0] + set_damping_power_command[1] + set_damping_power_command[2]
										+ set_damping_power_command[3] + set_damping_power_command[4] + set_damping_power_command[5]
										+ set_damping_power_command[6];
		set_damping_power_command[7] = set_damping_power_command_sum % 256;

		memcpy(&steering_engine_UART_Handler->Tx_Buf,&set_damping_power_command,8);
		steering_engine_UART_Handler->TxCount = 8;
		steering_engine_UART_Handler->UART_Tx(steering_engine_UART_Handler);
	}
}

uint8_t status_flag = 0;
uint8_t clear_current_laps_command[10];
uint8_t clear_current_laps_check = 0;
uint8_t steering_id2 = 0;
uint8_t status_counts = 0;
void update_steering_state()
{
	switch(steering_id2)
	{
		case 0:
			status_counts ++;
			clear_current_laps();
			if(status_counts >= 10)
			{
				steering_id2 = 1;
				status_counts = 0;
			}
			break;
		case 1:
			status_counts ++;
			clear_current_laps();
			if(status_counts >= 10)
			{
				steering_id2 = 0;
				status_counts = 0;
			}
			break;
//		case 2:
//			clear_current_laps();
//			steering_id2 = 3;
//			break;
//		case 3:
//			clear_current_laps();
//			steering_id2 = 0;
//			break;
	}
}

void clear_current_laps()
{
	clear_current_laps_command[0] = 0x12;
	clear_current_laps_command[1] = 0x4C;
	clear_current_laps_command[2] = 0x11;
	clear_current_laps_command[3] = 0x01;
	clear_current_laps_command[4] = steering_id2;

	clear_current_laps_check = (clear_current_laps_command[0] + clear_current_laps_command[1] + clear_current_laps_command[2]
								+ clear_current_laps_command[3] + clear_current_laps_command[4]) % 256;

	clear_current_laps_command[5] = clear_current_laps_check;

	memcpy(&steering_engine_UART_Handler->Tx_Buf,&clear_current_laps_command,6);
	steering_engine_UART_Handler->TxCount = 6;
	steering_engine_UART_Handler->UART_Tx(steering_engine_UART_Handler);
}


void steering_set_velocity_with_locking_force(uint8_t id,int32_t Steering_Speed)
{
	if(Steering_Speed > 0)  //顺时针
	{
		set_speed_command[0] = 0x12;
		set_speed_command[1] = 0x4C;
		set_speed_command[2] = 0x0F;
		set_speed_command[3] = 0x0D;
		set_speed_command[4] = id;
		set_speed_command[5] = 0x00;
		set_speed_command[6] = 0x40;
		set_speed_command[7] = 0x38;
		set_speed_command[8] = 0x00;
		set_speed_command[9] = (uint8_t)(Steering_Speed * 10);
		set_speed_command[10] = (uint8_t)((Steering_Speed * 10) >> 8);
		set_speed_command[11] = 0x64;
		set_speed_command[12] = 0x00;
		set_speed_command[13] = 0x64;
		set_speed_command[14] = 0x00;
		set_speed_command[15] = 0x00;
		set_speed_command[16] = 0x00;

		set_speed_command_sum = (set_speed_command[0] + set_speed_command[1]+ set_speed_command[2]
							 + set_speed_command[3]+ set_speed_command[4]+ set_speed_command[5]+ set_speed_command[6]
							 + set_speed_command[7]+ set_speed_command[8]+ set_speed_command[9]+ set_speed_command[10]+ set_speed_command[11]
							 + set_speed_command[12]+ set_speed_command[13] + set_speed_command[14]+ set_speed_command[15]+ set_speed_command[16]);

		set_speed_command_check = set_speed_command_sum % 256;
		set_speed_command[17] = set_speed_command_check;

		memcpy(&steering_engine_UART_Handler->Tx_Buf,&set_speed_command,18);
		steering_engine_UART_Handler->TxCount = 18;
		steering_engine_UART_Handler->UART_Tx(steering_engine_UART_Handler);
	}
	else if(Steering_Speed < 0)     //逆时针
	{
		set_speed_command[0] = 0x12;
		set_speed_command[1] = 0x4C;
		set_speed_command[2] = 0x0F;
		set_speed_command[3] = 0x0D;
		set_speed_command[4] = id;
		set_speed_command[5] = 0x00;
		set_speed_command[6] = 0xC0;
		set_speed_command[7] = 0xC7;
		set_speed_command[8] = 0xFF;
		set_speed_command[9] = (uint8_t)(-Steering_Speed * 10);
		set_speed_command[10] = (uint8_t)((-Steering_Speed * 10) >> 8);
		set_speed_command[11] = 0x64;
		set_speed_command[12] = 0x00;
		set_speed_command[13] = 0x64;
		set_speed_command[14] = 0x00;
		set_speed_command[15] = 0x00;
		set_speed_command[16] = 0x00;

		set_speed_command_sum = (set_speed_command[0] + set_speed_command[1]+ set_speed_command[2]
							 + set_speed_command[3]+ set_speed_command[4]+ set_speed_command[5]+ set_speed_command[6]
							 + set_speed_command[7]+ set_speed_command[8]+ set_speed_command[9]+ set_speed_command[10]+ set_speed_command[11]
							 + set_speed_command[12]+ set_speed_command[13] + set_speed_command[14]+ set_speed_command[15]+ set_speed_command[16]);

		set_speed_command_check = set_speed_command_sum % 256;
		set_speed_command[17] = set_speed_command_check;

		memcpy(&steering_engine_UART_Handler->Tx_Buf,&set_speed_command,18);
		steering_engine_UART_Handler->TxCount = 18;
		steering_engine_UART_Handler->UART_Tx(steering_engine_UART_Handler);
	}
	else {                 //停止
		set_speed_command[0] = 0x12;
		set_speed_command[1] = 0x4C;
		set_speed_command[2] = 0x18;
		set_speed_command[3] = 0x04;
		set_speed_command[4] = id;
		set_speed_command[5] = 0x11;
		set_speed_command[6] = 0x70;
		set_speed_command[7] = 0x17;

		set_speed_command_sum = (set_speed_command[0] + set_speed_command[1]+ set_speed_command[2]
							 + set_speed_command[3]+ set_speed_command[4]+ set_speed_command[5]+ set_speed_command[6]
							 + set_speed_command[7]);

		set_speed_command_check = set_speed_command_sum % 256;
		set_speed_command[8] = set_speed_command_check;

		memcpy(&steering_engine_UART_Handler->Tx_Buf,&set_speed_command,9);
		steering_engine_UART_Handler->TxCount = 9;
		steering_engine_UART_Handler->UART_Tx(steering_engine_UART_Handler);
	}
}

void steering_set_velocity_with_damping(uint8_t id,int32_t Steering_Speed,int32_t Steering_Power)
{
	if(Steering_Speed > 0)  //顺时针
	{
		set_speed_command[0] = 0x12;
		set_speed_command[1] = 0x4C;
		set_speed_command[2] = 0x0F;
		set_speed_command[3] = 0x0D;
		set_speed_command[4] = id;
		set_speed_command[5] = 0x00;
		set_speed_command[6] = 0x40;
		set_speed_command[7] = 0x38;
		set_speed_command[8] = 0x00;
		set_speed_command[9] = (uint8_t)(Steering_Speed * 10);
		set_speed_command[10] = (uint8_t)((Steering_Speed * 10) >> 8);
		set_speed_command[11] = 0x64;
		set_speed_command[12] = 0x00;
		set_speed_command[13] = 0x64;
		set_speed_command[14] = 0x00;
		set_speed_command[15] = 0x00;
		set_speed_command[16] = 0x00;

		set_speed_command_sum = (set_speed_command[0] + set_speed_command[1]+ set_speed_command[2]
							 + set_speed_command[3]+ set_speed_command[4]+ set_speed_command[5]+ set_speed_command[6]
							 + set_speed_command[7]+ set_speed_command[8]+ set_speed_command[9]+ set_speed_command[10]+ set_speed_command[11]
							 + set_speed_command[12]+ set_speed_command[13] + set_speed_command[14]+ set_speed_command[15]+ set_speed_command[16]);

		set_speed_command_check = set_speed_command_sum % 256;
		set_speed_command[17] = set_speed_command_check;

		memcpy(&steering_engine_UART_Handler->Tx_Buf,&set_speed_command,18);
		steering_engine_UART_Handler->TxCount = 18;
		steering_engine_UART_Handler->UART_Tx(steering_engine_UART_Handler);
	}
	else if(Steering_Speed < 0)     //逆时针
	{
		set_speed_command[0] = 0x12;
		set_speed_command[1] = 0x4C;
		set_speed_command[2] = 0x0F;
		set_speed_command[3] = 0x0D;
		set_speed_command[4] = id;
		set_speed_command[5] = 0x00;
		set_speed_command[6] = 0xC0;
		set_speed_command[7] = 0xC7;
		set_speed_command[8] = 0xFF;
		set_speed_command[9] = (uint8_t)(-Steering_Speed * 10);
		set_speed_command[10] = (uint8_t)((-Steering_Speed * 10) >> 8);
		set_speed_command[11] = 0x64;
		set_speed_command[12] = 0x00;
		set_speed_command[13] = 0x64;
		set_speed_command[14] = 0x00;
		set_speed_command[15] = 0x00;
		set_speed_command[16] = 0x00;

		set_speed_command_sum = (set_speed_command[0] + set_speed_command[1]+ set_speed_command[2]
							 + set_speed_command[3]+ set_speed_command[4]+ set_speed_command[5]+ set_speed_command[6]
							 + set_speed_command[7]+ set_speed_command[8]+ set_speed_command[9]+ set_speed_command[10]+ set_speed_command[11]
							 + set_speed_command[12]+ set_speed_command[13] + set_speed_command[14]+ set_speed_command[15]+ set_speed_command[16]);

		set_speed_command_check = set_speed_command_sum % 256;
		set_speed_command[17] = set_speed_command_check;

		memcpy(&steering_engine_UART_Handler->Tx_Buf,&set_speed_command,18);
		steering_engine_UART_Handler->TxCount = 18;
		steering_engine_UART_Handler->UART_Tx(steering_engine_UART_Handler);
	}
	else {                 //停止
		set_speed_command[0] = 0x12;
		set_speed_command[1] = 0x4C;
		set_speed_command[2] = 0x18;
		set_speed_command[3] = 0x04;
		set_speed_command[4] = id;
		set_speed_command[5] = 0x12;
		set_speed_command[6] = Steering_Power;
		set_speed_command[7] = Steering_Power >> 8;

		set_speed_command_sum = (set_speed_command[0] + set_speed_command[1]+ set_speed_command[2]
							 + set_speed_command[3]+ set_speed_command[4]+ set_speed_command[5]+ set_speed_command[6]
							 + set_speed_command[7]);

		set_speed_command_check = set_speed_command_sum % 256;
		set_speed_command[8] = set_speed_command_check;

		memcpy(&steering_engine_UART_Handler->Tx_Buf,&set_speed_command,9);
		steering_engine_UART_Handler->TxCount = 9;
		steering_engine_UART_Handler->UART_Tx(steering_engine_UART_Handler);
	}
}

uint32_t homing_flag = 0;
uint8_t homing_counts = 0;

void steering_homing_control()
{
	switch(homing_flag)
	{
		case 0:
			homing_counts ++;
			FSUS_SetServoAngle(02, *Desulfurizer_Steering_Angle_3, 500, 0, 200,
					&steering_engine_UART_Handler->Tx_Buf,
					&steering_engine_UART_Handler->TxCount);
			steering_engine_UART_Handler->UART_Tx(steering_engine_UART_Handler);
			if(homing_counts >= 10)
			{
				homing_flag = 1;
				homing_counts = 0;
			}

			break;
		case 1:
			homing_counts ++;
			FSUS_SetServoAngle(03, *Desulfurizer_Steering_Angle_4, 500, 0, 200,
					&steering_engine_UART_Handler->Tx_Buf,
					&steering_engine_UART_Handler->TxCount);
			steering_engine_UART_Handler->UART_Tx(steering_engine_UART_Handler);
			if(homing_counts >= 10)
			{
				homing_flag = 0;
				homing_counts = 0;
			}
			break;
	}
}

int i = 0;
void read_steering_current()
{
	LOG("read the current of the steering engine");
	//log_info("read the current of the steering engine");
	steering_engine_UART_Handler->UART_Decode = decode_steering_current;
	FSUS_ReadData(2, 2, &steering_engine_UART_Handler->Tx_Buf,
			&steering_engine_UART_Handler->TxCount);

	char bufferchar[100];
	int i = 0;
	for (i = 0; i < steering_engine_UART_Handler->TxCount; i++)
	{

		bufferchar[i] = steering_engine_UART_Handler->Tx_Buf[i];
	}

	//log_trace(log_buf);
	steering_engine_UART_Handler->UART_Tx(steering_engine_UART_Handler);
}

uint8_t read_steering_2_Angle_command[6] = {0x12,0x4c,0x0a,0x01,0x02,0x6b};
uint8_t read_steering_3_Angle_command[6] = {0x12,0x4c,0x0a,0x01,0x03,0x6c};
uint8_t read_steering_Angle_flag = 0;
void read_steering_Angle()
{
	switch(read_steering_Angle_flag)
	{
		case 0:
			memcpy(&steering_engine_UART_Handler->Tx_Buf,&read_steering_2_Angle_command,6);
			steering_engine_UART_Handler->TxCount = 6;
			steering_engine_UART_Handler->UART_Tx(steering_engine_UART_Handler);
			read_steering_Angle_flag = 1;
			break;
		case 1:
			memcpy(&steering_engine_UART_Handler->Tx_Buf,&read_steering_3_Angle_command,6);
			steering_engine_UART_Handler->TxCount = 6;
			steering_engine_UART_Handler->UART_Tx(steering_engine_UART_Handler);
			read_steering_Angle_flag = 0;
			break;
	}
}
void setSteeringRobotProtectionMode()
{
	uint8_t setvalue[2];
	setvalue[0] = 0;

	steering_engine_UART_Handler->UART_Decode = NULL;

//	data_id	参数名称(en)	参数名称(cn)	字节类型	字节长度	单位	默认值
//	37	stall_protect_mode	舵机堵转保护模式	uint8_t	1		0x00
	FSUS_WriteData(0, 37, setvalue, 1, &steering_engine_UART_Handler->Tx_Buf,
			&steering_engine_UART_Handler->TxCount); //设置id37 为0
	steering_engine_UART_Handler->UART_Tx(steering_engine_UART_Handler);

	uint16_t Power = 600;
	HAL_Delay(300);
	steering_engine_UART_Handler->UART_Decode = NULL;
	//	data_id	参数名称(en)	参数名称(cn)	字节类型	字节长度	单位	默认值
	//	42	over_power	功率上限	uint16_t	2	mW
	FSUS_WriteData(0, 42, (uint8_t*) &Power, 2,
			&steering_engine_UART_Handler->Tx_Buf,
			&steering_engine_UART_Handler->TxCount);	//设置id37 为0
	steering_engine_UART_Handler->UART_Tx(steering_engine_UART_Handler);

	Power = 500;
	HAL_Delay(300);
	steering_engine_UART_Handler->UART_Decode = NULL;
	//38	stall_power_limit	舵机堵转功率上限	uint16_t	2	mW
	FSUS_WriteData(0, 38, (uint8_t*) &Power, 2,
			&steering_engine_UART_Handler->Tx_Buf,
			&steering_engine_UART_Handler->TxCount);	//设置id37 为0
	steering_engine_UART_Handler->UART_Tx(steering_engine_UART_Handler);
}

uint16_t current = 0;
void decode_steering_current(uint8_t *buffer, uint16_t length)
{

	if (length == 9) //返回的数据个数是9
	{
		memcpy(&current, &buffer[6], 2);

		*Desulfurizer_Steering_Current=current;

		LOG("the value of the steering engine current is %d", current);
		//steering_engine_UART_Handler->Set_PCOMHardWare(steering_engine_UART_Handler,"steering_engine",1);

	} else
	{
		//log_error("the data length is not 9, please check whether the steer engine is properly powered");
		//LOGFF(DL_ERROR,"the data length is not 9, please check whether the steer engine is properly powered");
	}
}
void decode_steering_angle(uint8_t *buffer, uint16_t length)
{
	if((buffer[0] == 0x05) && (buffer[1] == 0x1c) && (buffer[2] == 0x0a) && (buffer[3] == 0x03))
	{
		switch(buffer[4])
		{
			case 0x02:
				*Desulfurizer_Steering_Real_Angle_3 = ((buffer[6] << 8) | buffer[5]) / 10;
				break;
			case 0x03:
				*Desulfurizer_Steering_Real_Angle_4 = ((buffer[6] << 8) | buffer[5]) / 10;
				break;
		}
	}
}










