/*
 * msp_UltrasonicSensor.c
 *
 *  Created on: Oct 8, 2024
 *      Author: akeguo
 */
#include "msp_Ultrasonic_Sensor.h"
#include "bsp_Error.pb.h"
#include "BHBF_ROBOT.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;

int32_t *USValue_1;
int32_t *USValue_2;

int32_t USValue_in1;
int32_t USValue_in2;

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_dispacherController = Handler->dispacherController;

	LOG("Ultrasonic_sensor_encoder_intialize");

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

	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,
			"Ultrasonic_sensor", 0, ComError_Ultrasonic_sensor);
	//uartHandler->Insert_HardWare_Entry_UART

	USValue_1 = &USValue_in1;
	USValue_2 = &USValue_in2;

	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

void GF_US_Inquiry()
{

	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");
	Ultrasonic_sensor->AddSendList(Ultrasonic_sensor, Ultrasonic_sensor->Tx_Buf,
			Ultrasonic_sensor->TxCount, OneLineWaitTime,
			decode_UltrasonicSensor);

	MB_ReadInputReg(&Ultrasonic_sensor->Tx_Buf, &Ultrasonic_sensor->TxCount, 2,
			0x0000, 1); //03 command ; read 2 registers

	Ultrasonic_sensor->AddSendList(Ultrasonic_sensor, Ultrasonic_sensor->Tx_Buf,
			Ultrasonic_sensor->TxCount, OneLineWaitTime,
			decode_UltrasonicSensor);
	LOG("Ultrasonic_sensor_2 Version");

}



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_in1 = (int32_t) ((int32_t) buffer[3] << 8 | buffer[4]);
	HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
			"Ultrasonic_sensor", 1);
	LOG("decode_force  succeeded and the force is %d", *USValue_1);
} else if (buffer[0] == 2 && crc_check == MB_CRC16(buffer, length - 2))
{
	USValue_in2 = (int32_t) ((int32_t) buffer[3] << 8 | buffer[4]);
	HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
			"Ultrasonic_sensor", 1);
	LOG("decode_force  succeeded and the force is %d", *USValue_2);
} else
{
	//Decode Error；
	//log_error("wire sensor decoding failed");
	LOGFF(DL_ERROR, "Ultrasonic_sensor decoding failed");

}

}