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

#include "msp_Steering_Engine.h"
#include "SteerEngine/msp_fashion_star_uart_servo.h"
#define USART_RECV_BUF_SIZE 2048
#define USART_SEND_BUF_SIZE 2048
#include <bsp_Log.h>
#include "BHBF_ROBOT.h"

int32_t* Desulfurizer_Steering_Set_Speed;
int32_t* Desulfurizer_Steering_Angle;
int32_t* Desulfurizer_Steering_Current;
int32_t* Desulfurizer_Steering_Real_Angle;
void steering_set_velocity();
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 = 30; //等待10ms 最低不要低于4；
	//log_info("steering_engine_intialize");
	steering_dispacher=Handler->dispacherController;
	steering_dispacher->Add_Dispatcher_List(steering_dispacher,steering_set_angle);

	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"steering_engine",0,steering_engine);
	LOG("steering_engine_intialize");
}



void steering_set_velocity()
{
	//FSUS_STATUS FSUS_WheelKeepMove(uint8_t servo_id, uint8_t is_cw, uint16_t speed,uint8_t *sendbytes, uint8_t *length)
	steering_engine_UART_Handler->UART_Decode = NULL;
	if(*Desulfurizer_Steering_Set_Speed>0)
	{
		FSUS_WheelKeepMove(00,1,*Desulfurizer_Steering_Set_Speed,&steering_engine_UART_Handler->Tx_Buf,
				&steering_engine_UART_Handler->TxCount);
	}else if(*Desulfurizer_Steering_Set_Speed<0)
	{
		FSUS_WheelKeepMove(00,0,-*Desulfurizer_Steering_Set_Speed,&steering_engine_UART_Handler->Tx_Buf,
				&steering_engine_UART_Handler->TxCount);
	}
	else
	{
		 FSUS_WheelStop(0,&steering_engine_UART_Handler->Tx_Buf,
				&steering_engine_UART_Handler->TxCount);
	}



	steering_engine_UART_Handler->UART_Tx(steering_engine_UART_Handler);
}


void steering_set_angle()
{

	steering_engine_UART_Handler->UART_Decode = decode_steering_current;
	FSUS_SetServoAngle(00, *Desulfurizer_Steering_Angle, 6, 400, 200,
			&steering_engine_UART_Handler->Tx_Buf,
			&steering_engine_UART_Handler->TxCount);

	steering_engine_UART_Handler->UART_Tx(steering_engine_UART_Handler);
}

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(0, 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);
}
void read_steering_Angle()
{
	LOG("read the Angle of the steering engine");

	steering_engine_UART_Handler->UART_Decode = decode_steering_angle;
	FSUS_QueryServoAngle(0, &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);
}
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)
{
	char bufferchar[100];
	int i = 0;
	for (i = 0; i < length; i++)
	{

		bufferchar[i] = buffer[i];
	}

	int16_t angle = 0;

	if (length == 8) //返回的数据个数是9
	{
		memcpy(&angle, &buffer[5], 2);
		*Desulfurizer_Steering_Real_Angle=angle;
		LOG("the value of the steering engine angle is %d", angle);

	} else
	{

		LOGFF(DL_ERROR,"the data length is not 9, please check whether the steer engine is properly powered");
	}

}