/*
 * msp_DH_Roughening.c
 *
 *  Created on: Nov 22, 2024
 *      Author: akeguo
 */

#include "msp_DH_Roughening.h"
#include "bsp_Error_Detect.h"
#include "robot_state.h"
#include "BHBF_ROBOT.h"
DHRoughening DHRougheningContrller; //遥控器数据



DispacherController *DHRoughening_dispacherController;
struct UARTHandler *DHRoughening_Controller_UART_Handler;

void DHRoughening_Controller_intialize(struct UARTHandler *Handler) {

	DHRoughening_Controller_UART_Handler = Handler;
	DHRoughening_Controller_UART_Handler->Wait_time = 20; //等待10ms 最低不要低于4；
	DHRoughening_dispacherController = Handler->dispacherController;
	DHRoughening_dispacherController->Dispacher_Enable=1;
	DHRoughening_dispacherController->DispacherCallTime = 200;
	DHRoughening_dispacherController->Add_Dispatcher_List(
	        DHRoughening_dispacherController, read_DHRoughening_Data);
	DHRoughening_dispacherController->Add_Dispatcher_List(
	        DHRoughening_dispacherController, send_to_DHRoughening);

	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,
	        "DHRoughening", 0, DHRougheningController);

}

void read_DHRoughening_Data(void) {
	LOGFF(DL_ERROR, "read data from DeChi");

	DHRoughening_Controller_UART_Handler->UART_Decode = decode_DHRoughening;
	MB_ReadInputReg(&DHRoughening_Controller_UART_Handler->Tx_Buf,
	        &DHRoughening_Controller_UART_Handler->TxCount, 3, 0, 6);

	DHRoughening_Controller_UART_Handler->UART_Tx(
	        DHRoughening_Controller_UART_Handler);

}
uint8_t data5555[2000];
uint8_t Wireless_LastState;

void decode_DHRoughening(uint8_t *buffer, uint16_t length) {

	memcpy(data5555,buffer,length);
	uint16_t crc_check = ((buffer[length - 1] << 8) | buffer[length - 2]);
	/* CRC 校验正确 */
	if (crc_check == MB_CRC16(buffer, length - 2))
	{

		HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
		        "DHRoughening", 1);

		 memcpy(&DHRougheningContrller,&buffer[3],12);//16个字节数字
		 if(Wireless_LastState = 0
				 && DHRougheningContrller.Wireless_State == 1)
		 {
			 DHRougheningContrller.Wireless_FirstFlag = 1;
		 }

		 Wireless_LastState = DHRougheningContrller.Wireless_State;
		// *Roughenging_LeftCompensation=(*Roughenging_LeftCompensation_Range)*DHRougheningContrller.Left_Compensation*100/65535;
		// *Roughenging_RightCompensation=(*Roughenging_RightCompensation_Range)*DHRougheningContrller.Right_Compensation*100/65535;
	} else
	{

	}

}
void send_to_DHRoughening()
{

	set_DHRoughening_Display((uint16_t) (GV.Left_Compensation * 100),
	        (uint16_t) (GV.Right_Compensation / 6.5 * 100),
	        (uint16_t) abs(GV.Robot_Angle.RF_Angle_Roll),
	        (uint16_t) abs(GV.Robot_ForceValue ));

}

//左补偿  xxx.xx 度   0x0000—0xFFFF==0—655.35 度
//右补偿  xxx.xx 度   0x0000—0xFFFF==0—655.35 度
//当前角度 xxx.xx 度   0x0000—0xFFFF==0—655.35 度
//移动速度  xxx 米/分钟   0x0000—0xFFFF==0—65535 米/分钟
MB_REG_DATA mb_reg_value_DHRoughening;
void set_DHRoughening_Display(uint16_t LeftCompensation,
        uint16_t RightCompensation, uint16_t Angle, uint16_t Speed) {


	mb_reg_value_DHRoughening.DATA_10H[0] = (LeftCompensation >> 8) & 0xff;
	mb_reg_value_DHRoughening.DATA_10H[1] = (LeftCompensation) & 0xff;

	mb_reg_value_DHRoughening.DATA_10H[2] = (RightCompensation >> 8) & 0xff;
	mb_reg_value_DHRoughening.DATA_10H[3] = RightCompensation & 0xff;

	mb_reg_value_DHRoughening.DATA_10H[4] = (Angle >> 8) & 0xff;
	mb_reg_value_DHRoughening.DATA_10H[5] = Angle & 0xff;

	mb_reg_value_DHRoughening.DATA_10H[6] = (Speed >> 8) & 0xff;
	mb_reg_value_DHRoughening.DATA_10H[7] = Speed & 0xff;

	DHRoughening_Controller_UART_Handler->UART_Decode = NULL; //表示不需要解析读回来的数据

	MB_WriteNumHoldingReg(&DHRoughening_Controller_UART_Handler->Tx_Buf,
	        &DHRoughening_Controller_UART_Handler->TxCount, 3, 0, 5,
	        mb_reg_value_DHRoughening.DATA_10H);

	//read from the data;
	DHRoughening_Controller_UART_Handler->UART_Tx(
	        DHRoughening_Controller_UART_Handler);

}

int32_t DHRougheningGetSpeed(DHRoughening DHRougheningContrller)
{
//	if (DHRougheningContrller.Vehicle_Speed1 == 1) {
//		return *DHRoughening_Speed[0];
//	}
//	if (DHRougheningContrller.Vehicle_Speed2 == 1) {
//		return *DHRoughening_Speed[1];
//	}
//	if (DHRougheningContrller.Vehicle_Speed3 == 1) {
//		return *DHRoughening_Speed[2];
//	}
//	if (DHRougheningContrller.Vehicle_Speed4 == 1) {
//		return *DHRoughening_Speed[3];
//	}
//	if (DHRougheningContrller.Vehicle_Speed5 == 1) {
//		return *DHRoughening_Speed[4];
//	}
//	if (DHRougheningContrller.Vehicle_Speed6 == 1) {
//		return *DHRoughening_Speed[5];
//	}
//	if (DHRougheningContrller.Vehicle_Speed7 == 1) {
//		return *DHRoughening_Speed[6];
//	}
//	if (DHRougheningContrller.Vehicle_Speed8 == 1) {
//		return *DHRoughening_Speed[7];
//	}
//	if (DHRougheningContrller.Vehicle_Speed9 == 1) {
//		return *DHRoughening_Speed[8];
//	}
//	if (DHRougheningContrller.Vehicle_Speed10 == 1) {
//		return *DHRoughening_Speed[9];
//	}
	return 0;
}

int32_t DHRougheningGetLaneChangeDistance(DHRoughening DHRougheningContrller) {

//	if (DHRougheningContrller.LaneChangeDistance_0 == 1) {
//		return *DHRoughening_LaneChangeDistance[0];
//	}
//
//	if (DHRougheningContrller.LaneChangeDistance_1 == 1) {
//		return *DHRoughening_LaneChangeDistance[1];
//	}
//	if (DHRougheningContrller.LaneChangeDistance_2 == 1) {
//		return *DHRoughening_LaneChangeDistance[2];
//	}
//	if (DHRougheningContrller.LaneChangeDistance_3 == 1) {
//		return *DHRoughening_LaneChangeDistance[3];
//	}
//	if (DHRougheningContrller.LaneChangeDistance_4 == 1) {
//		return *DHRoughening_LaneChangeDistance[4];
//	}
//	if (DHRougheningContrller.LaneChangeDistance_5 == 1) {
//		return *DHRoughening_LaneChangeDistance[5];
//	}
	return 0;
}