/*
 * msp_UltrasonicSensor.c
 *
 *  Created on: Oct 8, 2024
 *      Author: akeguo
 */
#include "msp_Ultrasonic_Sensor.h"
#include "bsp_Error.pb.h"
#include "BHBF_ROBOT.h"
#include "msp_UltrasonicParameters.pb.h"

void decode_UltrasonicSensor(uint8_t *buffer, uint16_t length);
void GF_US_Inquiry();//inqure data from the, in fact this is a modbus 03 function;


MSP_UltrasonicParameters* USValue_in;


struct UARTHandler *Ultrasonic_sensor;

DispacherController *Ultrasonic_sensor_dispacherController;
void Ultrasonic_sensor_intialize(struct UARTHandler *Handler)
{
	//uartHandler_intialize(&Force_sensor,Handler,10);
	Ultrasonic_sensor = Handler;
	Ultrasonic_sensor->UART_Decode=decode_UltrasonicSensor;
	Ultrasonic_sensor->Wait_time = 20; //等待10ms 最低不要低于4；
	Ultrasonic_sensor_dispacherController=Handler->dispacherController;
	Ultrasonic_sensor_dispacherController->DispacherCallTime = 100;//10 是100ms 难道这个2ms的定时器是10ms的？
	LOG("Ultrasonic_sensor_encoder_intialize");

		//log_info("angle_encoder_intialize");
	Ultrasonic_sensor_dispacherController->Add_Dispatcher_List(Ultrasonic_sensor_dispacherController,GF_US_Inquiry);

	Ultrasonic_sensor_dispacherController->Dispacher_Enable=1;
	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"Ultrasonic_sensor",0,Ultrasonic_sensor);
		//uartHandler->Insert_HardWare_Entry_UART


	LOG("Ultrasonic_sensor_intialize");
}

uint8_t Inquiry_Order_1[8]={0X01, 0X04, 0X00, 0X00, 0X00, 0X01, 0X31, 0XCA}; //01 04 00 00 00 01 31 CA
uint8_t Inquiry_Order_2[8]={0X02, 0X04, 0X00, 0X00, 0X00, 0X01, 0X31, 0XF9}; //02 04 00 00 00 01 31 F9
uint8_t GF_US_count;
void GF_US_Inquiry()
{
	switch (GF_US_count)
	{
		case 0:
			MB_ReadInputReg(&Ultrasonic_sensor->Tx_Buf,
					&Ultrasonic_sensor->TxCount, 1, 0x0000, 1); //03 command ; read 2 registers

			Ultrasonic_sensor->UART_Tx(Ultrasonic_sensor);//send the data from the modbus command;
			LOG("Ultrasonic_sensor_1 Version");
			GF_US_count = 1;
			break;
		case 1:
			MB_ReadInputReg(&Ultrasonic_sensor->Tx_Buf,
					&Ultrasonic_sensor->TxCount, 2, 0x0000, 1); //03 command ; read 2 registers

			Ultrasonic_sensor->UART_Tx(Ultrasonic_sensor);//send the data from the modbus command;
			LOG("Ultrasonic_sensor_2 Version");
			GF_US_count = 2;
			break;
		case 2:
			MB_ReadInputReg(&Ultrasonic_sensor->Tx_Buf,
					&Ultrasonic_sensor->TxCount, 3, 0x0000, 1); //03 command ; read 2 registers

			Ultrasonic_sensor->UART_Tx(Ultrasonic_sensor);//send the data from the modbus command;
			LOG("Ultrasonic_sensor_3 Version");
			GF_US_count = 3;
			break;
		case 3:
			MB_ReadInputReg(&Ultrasonic_sensor->Tx_Buf,
					&Ultrasonic_sensor->TxCount, 4, 0x0000, 1); //03 command ; read 2 registers

			Ultrasonic_sensor->UART_Tx(Ultrasonic_sensor);//send the data from the modbus command;
			LOG("Ultrasonic_sensor_4 Version");
			GF_US_count = 4;
			break;
		case 4:
			MB_ReadHoldingReg(&Ultrasonic_sensor->Tx_Buf,
					&Ultrasonic_sensor->TxCount, 5, 0x0004, 1); //03 command ; read 2 registers

			Ultrasonic_sensor->UART_Tx(Ultrasonic_sensor);//send the data from the modbus command;
			LOG("Dynamometer_sensor_5 Version");
			GF_US_count = 0;
			break;
		default:
			break;
	}


}


void decode_UltrasonicSensor(uint8_t *buffer, uint16_t length)
{
	uint8_t data[20];
	memcpy(data,buffer,length);
	LOG("start decoding and the length is %d",length);
	/* CRC 校验 */
	uint16_t crc_check = ((buffer[length - 1] << 8) | buffer[length - 2]);
	/* CRC 校验正确 */
	if (buffer[0] == 1 && crc_check == MB_CRC16(buffer, length - 2))
	{
		USValue_in->USValue_1 = (int32_t)((int32_t)buffer[3]<<8|buffer[4]);
		HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"Ultrasonic_sensor",1);

	}
	else if(buffer[0] == 2 && crc_check == MB_CRC16(buffer, length - 2))
	{
		USValue_in->USValue_2 = (int32_t)((int32_t)buffer[3]<<8|buffer[4]);
		HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"Ultrasonic_sensor",1);

	}
	else if(buffer[0] == 3 && crc_check == MB_CRC16(buffer, length - 2))
	{
		USValue_in->USValue_3 = (int32_t)((int32_t)buffer[3]<<8|buffer[4]);
		HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"Ultrasonic_sensor",1);

	}
	else if(buffer[0] == 4 && crc_check == MB_CRC16(buffer, length - 2))
	{
		USValue_in->USValue_4 = (int32_t)((int32_t)buffer[3]<<8|buffer[4]);
		HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"Ultrasonic_sensor",1);

	}
	else if(buffer[0] == 5 && crc_check == MB_CRC16(buffer, length - 2))
	{
		USValue_in->Dym_Value = (int32_t)((int32_t)buffer[3]<<8|buffer[4]);
		HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"Dynamometer_sensor",1);

	}
	else
	{
		//Decode Error；
		//log_error("wire sensor decoding failed");
		LOGFF(DL_ERROR,"Ultrasonic_sensor decoding failed");

	}

}