/*
 * thickness_sensor.c
 *
 *  Created on: Mar 21, 2025
 *      Author: SQ-X
 */

#include "thickness_sensor.h"
#include "BHBF_ROBOT.h"
#include "bsp_Error.pb.h"

void decode_thickness(uint8_t *buffer, uint16_t length);
void GF_TS_Inquiry();

int32_t* ThicknessValue;
struct UARTHandler *Thickness_sensor;
DispacherController *Thickness_sensor_dispacherController;
void thickness_sensor_intialize(struct UARTHandler *Handler)
{
	//uartHandler_intialize(&Force_sensor,Handler,10);
	Thickness_sensor = Handler;
	Thickness_sensor->UART_Decode=decode_thickness;
	Thickness_sensor->Wait_time = 10; //等待10ms 最低不要低于4；
	Thickness_sensor_dispacherController=Handler->dispacherController;
	Thickness_sensor_dispacherController->DispacherCallTime = 50;//10 是100ms 难道这个2ms的定时器是10ms的？
	LOG("angle_encoder_intialize");
		//log_info("angle_encoder_intialize");
	Thickness_sensor_dispacherController->Add_Dispatcher_List(Thickness_sensor_dispacherController,GF_TS_Inquiry);


	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"thickness_sensor",0,Thickness_sensor);
		//uartHandler->Insert_HardWare_Entry_UART
	 LOG("steering_engine_intialize");
}

uint8_t Inquiry_Order1[40]={0XFE, 0XE1, 0X90, 0X0D, 0X00, 0X01, 0X00, 0X00,0x01,0x01,0x06, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00,0x0C,
		0x45,0x4D,0x32,0x34,0x31,0x31,0x31,0x38,0x53,0x30,0x30,0x31,0xC8,0x43,0x0D,0x90,0xE1,0xFE };//40个
void GF_TS_Inquiry()
{

//	MB_ReadHoldingReg(&Thickness_sensor->Tx_Buf,
//			&Thickness_sensor->TxCount, 1, 0x07D0, 2); //03 command ; read 2 registers
	for(int i=0;i<40;i++)
	{
		Thickness_sensor->Tx_Buf[i]=Inquiry_Order1[i];
	}

	Thickness_sensor->TxCount=40;
	Thickness_sensor->UART_Tx(Thickness_sensor);//send the data from the modbus command;
	LOG("GF_TS_Inquiry Second Version");

}

uint8_t data1[42];
void decode_thickness(uint8_t *buffer, uint16_t length)
{
	memcpy(data1,buffer,length);
	LOG("start decoding and the length is %d",length);
	/* CRC 校验 */
	uint16_t crc_check = ((buffer[36] << 8) | buffer[37]);
	/* CRC 校验正确 */
	if (crc_check == MB_CRC16(&buffer[22], 14))
	{
		*ThicknessValue= (int32_t)((int32_t)buffer[34]<<8|buffer[35]);

		HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"thickness_sensor",1);
		LOG("decode_force  succeeded and the thickness is %d",*ForceValue);

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

	}

}