/*
 * msp_TL720D.c
 *
 *  Created on: Jul 19, 2024
 *      Author: bihon
 */

#include "msp_TL720D.h"
#include "BHBF_ROBOT.h"
#include "msp_TL720D.pb.h"




struct UARTHandler *TL720D_UART_Handler;

MSP_TL720DParameters* SP_MSP_RF_TL720D_Parameters_In;

void decode_TL720D(uint8_t *buffer, uint16_t length);
int16_t getDeci(uint8_t *data);

void TL720D_intialize(struct UARTHandler *Handler)
{
	//TL720D_UART_Handler->UART_Decode = NULL;
	TL720D_UART_Handler = Handler;

	TL720D_UART_Handler->UART_Decode = decode_TL720D;
	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"TL720D",0,TL720D);
	//log_info("TL720D_intialize");

	LOGFF(DL_ERROR,"TL720D_intialize");
}
void decode_TL720D(uint8_t *buffer, uint16_t length)
{

	if (buffer[0] == 0x68 && buffer[1] == 0x1F && buffer[2] == 0x00
			&& buffer[3] == 0x84)
	{
		//SP_MSP_RF_TL720D_Parameters_In.
		SP_MSP_RF_TL720D_Parameters_In->RF_Angle_Roll = getDeci(&buffer[4]);
		SP_MSP_RF_TL720D_Parameters_In->RF_Angle_Pitch = getDeci(&buffer[7]);
		SP_MSP_RF_TL720D_Parameters_In->RF_Angle_Yaw = getDeci(&buffer[10]);
		SP_MSP_RF_TL720D_Parameters_In->RF_Acc_X = getDeci(&buffer[13]);
		SP_MSP_RF_TL720D_Parameters_In->RF_Acc_Y = getDeci(&buffer[16]);
		SP_MSP_RF_TL720D_Parameters_In->RF_Acc_Z = getDeci(&buffer[19]);
		SP_MSP_RF_TL720D_Parameters_In->RF_Gro_X = getDeci(&buffer[22]);
		SP_MSP_RF_TL720D_Parameters_In->RF_Gro_Y = getDeci(&buffer[25]);
		SP_MSP_RF_TL720D_Parameters_In->RF_Gro_Z = getDeci(&buffer[28]);
		LOG("TL720D decoding  succeeded");

		//Is_TL720_Updating_Flag=true;
		 HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"TL720D",1);
	} else
	{
		//log_error("TL720D decoding failed");
		LOGFF(DL_ERROR,"TL720D decoding failed");

	}

}
int16_t getDeci(uint8_t *data)
{
	char isNegative = 0;

	if (*data >> 4)
	{
		isNegative = 1;
	} else
	{
		isNegative = 0;
	}

	int16_t data_value = 0;
	data_value = ((*data) & 0x0f) * 10000;
	data++;
	data_value += (*data >> 4) * 1000;
	data_value += ((*data) & 0x0f) * 100;
	data++;

	int16_t xiaoshu = 0;
	xiaoshu = (*data >> 4) * 10;
	xiaoshu += (*data) & 0x0f;
	if (isNegative)
	{
		return -(data_value + xiaoshu);
	} else
	{
		return (data_value + xiaoshu);
	}

}