/*
 * msp_Angle_Encoder_YL159.c
 *
 *  Created on: Jul 24, 2024
 *      Author: akeguo
 */

#include "msp_Angle_Encoder_YL159.h"
#include "BHBF_ROBOT.h"
#include "bsp_Log.h"

int32_t* Desulfurizer_Angle;

//int32_t* Desulfurizer_Thickness;
MB_REG_DATA mb_reg_value;
struct UARTHandler *encoder_UART_Handler;
DispacherController *encoder_dispacherController;
void angle_encoder_intialize(struct UARTHandler *Handler)
{

	encoder_UART_Handler = Handler;
	encoder_UART_Handler->Wait_time = 10; //等待10ms 最低不要低于4；
	LOG("angle_encoder_intialize");
	encoder_dispacherController=Handler->dispacherController;
	encoder_dispacherController->DispacherCallTime=100;
	//encoder_UART_Handler->DispacherCallPeriod=100;
	//log_info("angle_encoder_intialize");
	encoder_dispacherController->Add_Dispatcher_List(
			encoder_dispacherController,read_angle);

	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"angle_encoder",0,AngleEncoderErrorBitFlag);

}



//set current position home/ aka 0
void reset_current_value_0()
{


	LOG("Set Angle Encoder position 0");

	encoder_UART_Handler->UART_Decode = NULL;
	//RS_485_2_UART_Handler.UART_Decode=;
	mb_reg_value.DATA_10H[0] = 0;
	mb_reg_value.DATA_10H[1] = 0;
	//set 16-17 为0
	MB_WriteNumHoldingReg((encoder_UART_Handler->Tx_Buf),
			&(encoder_UART_Handler->TxCount), 1, 16, 2, mb_reg_value.DATA_10H); //清零
	encoder_UART_Handler->UART_Tx(encoder_UART_Handler);
}
void reset_position_value()
{


	LOG("Set Angle Encoder position 0");

	//set current decode function NULL, Indicate that there is no need to decode the received data;
	//RS_485_2_UART_Handler.UART_Decode = NULL;
	encoder_UART_Handler->UART_Decode = NULL;
	MB_WriteHoldingReg(encoder_UART_Handler->Tx_Buf,
			&encoder_UART_Handler->TxCount, 1, 67, 10); //read from the data;
	encoder_UART_Handler->UART_Tx(encoder_UART_Handler);
}

void read_angle()
{

	LOG("call data from Angle Encoder    from 0x08  mqtt 444  update 00");


	//RS_485_2_UART_Handler.UART_Decode = decode_Angle;
	encoder_UART_Handler->UART_Decode = decode_Angle;
	MB_ReadHoldingReg(&encoder_UART_Handler->Tx_Buf,
			&encoder_UART_Handler->TxCount, 1, 16, 2); //read from the data;
	if(encoder_UART_Handler->uart!=NULL)
	{
		encoder_UART_Handler->UART_Tx(encoder_UART_Handler);
	}else
	{
		LOGFF(DL_ERROR,"encoder_UART_Handler did not intialized");

	}


}
static int32_t angle;
void decode_Angle(uint8_t *buffer, uint16_t length)
{
	/* CRC 校验 */
	uint16_t crc_check = ((buffer[length - 1] << 8) | buffer[length - 2]);
	/* CRC 校验正确 */
	if (crc_check == MB_CRC16(buffer, length - 2))
	{
		angle = buffer[5] << 24 | buffer[6] << 16 | buffer[3] << 8 | buffer[4];
		*Desulfurizer_Angle = angle;

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

		//Decoded successfully
	} else
	{
		//LOGFF(DL_ERROR,"angle encoder decoding failed");
		LOGFF(DL_ERROR,"angle encoder decoding failed");

	}

}