/*
 * msp_TL720D.c
 *
 *  Created on: Jul 19, 2024
 *      Author: bihon
 *
 *      SHI(2026.4)
 *      倾角仪TL720D与电子罗盘DDM360B，自动输出模式，改为问答模式
 *      地址为   00 和 02
 *      波特率 115200
 */

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

#define addr_TL720D          0x00//地址
#define addr_DDM360B         0x02//地址

//Roll：0.01°(-180 +180)
int32_t *RobotAngle;//机器人角度（陀螺仪TL720D）
//Yaw:0.01°(0 360)
int32_t *RobotAngle_DDM;//磁北角度（电子罗盘DDM360B）


struct UARTHandler *TL720D_UART_Handler;
DispacherController *TL720D_DispacherController;

MSP_TL720DParameters* SP_MSP_RF_TL720D_Parameters_In;
MSP_TL720DParameters* MSP_DDM360B_Parameters_In;

static void decode_TL720D(uint8_t *buffer, uint16_t length);
static void decode_DDM360B(uint8_t *buffer, uint16_t length);
static int32_t getDeci(uint8_t *data);
static int changeAngle_LeftZerotoUpZero(int angle);

static void reading_Task_TL720D(void);
static void reading_Task_DDM360B(void);

void TL720D_intialize(struct UARTHandler *Handler)
{
	//TL720D_UART_Handler->UART_Decode = NULL;
	TL720D_UART_Handler = Handler;
	TL720D_UART_Handler->Wait_time = 8;
//	TL720D_UART_Handler->UART_Decode = decode_TL720D;//收到数据时调用decode_TL720D函数进行处理
//	TL720D_UART_Handler->UART_Decode = decode_DDM360B;

	TL720D_DispacherController = Handler->dispacherController;
	TL720D_DispacherController->Dispacher_Enable = 1; //不周期性发送
//	TL720D_DispacherController->DispacherCallTime = 40;//
	TL720D_DispacherController->DispacherCallTime = 0;//
	TL720D_DispacherController->Add_Dispatcher_List(TL720D_DispacherController, reading_Task_TL720D);
	TL720D_DispacherController->Add_Dispatcher_List(TL720D_DispacherController, reading_Task_DDM360B);

	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"TL720D",0,ComError_TL720D);//注册硬件错误处理
	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"DDM360B",0,ComError_DDM360B);
	//log_info("TL720D_intialize");
	LOG("TL720D initialized success");//记录初始化日志
	LOG("DDM360B initialized success");
}

uint8_t Buffer_DDM360B[100];//调试用
uint8_t Length_DDM360B = 0;//调试用
static void decode_DDM360B(uint8_t *buffer, uint16_t length)
{
		if(length < 100){
			Length_DDM360B = length;
			memcpy(Buffer_DDM360B, buffer, length);//调试用
		}

//	// DDM360B: 帧长0x0D(13字节), 地址0x02
	if (buffer[0] == 0x68 && buffer[1] == 0x0D && buffer[2] == 0x02
			&& buffer[3] == 0x84 && length == 14)
	{
		// 验证校验和（不含字头）
		uint8_t calc_checksum = 0;
		for (int i = 1; i < (14-1); i++) {
			calc_checksum += buffer[i];
		}
		if (calc_checksum != buffer[14-1]) {
			return;
		}
		MSP_DDM360B_Parameters_In->RF_Angle_Roll 	= getDeci(&buffer[4]);
		MSP_DDM360B_Parameters_In->RF_Angle_Pitch 	= getDeci(&buffer[7]);
		MSP_DDM360B_Parameters_In->RF_Angle_Yaw 	= getDeci(&buffer[10]);
		*RobotAngle_DDM = MSP_DDM360B_Parameters_In->RF_Angle_Yaw;

		HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"DDM360B",1);
		LOG("DDM360B decoding succeeded");
	}else
	{
		//log_error("TL720D decoding failed");
		LOGFF(DL_ERROR,"DDM360B decoding failed");
	}
}

static void reading_Task_DDM360B(void)
{
	uint8_t addr = addr_DDM360B;
	uint8_t cmd[5];
    uint8_t checksum = 0;

    cmd[0] = 0x68;           // 帧头
    cmd[1] = 0x04;           // 数据长度
    cmd[2] = addr;           // 地址码
    cmd[3] = 0x04;           // 命令字（读取角度）

    // 校验和 = 数据长度 + 地址码 + 命令字
    checksum = cmd[1] + cmd[2] + cmd[3];
    cmd[4] = checksum;

    memcpy(TL720D_UART_Handler->Tx_Buf, cmd, 5);
    TL720D_UART_Handler->TxCount = 5;

	TL720D_UART_Handler->AddSendList(
			TL720D_UART_Handler,
			TL720D_UART_Handler->Tx_Buf,
			TL720D_UART_Handler->TxCount,
			20,
			decode_DDM360B);
//    TL720D_UART_Handler->UART_Tx(TL720D_UART_Handler);
}

static void reading_Task_TL720D(void)
{
//	static uint8_t count_read_Task = 0;
//	count_read_Task++;
//	if((count_read_Task % 2) == 0)
//	{
//		Send_Query_Command(addr_TL720D);  // 查询TL720D（地址0x00）
//	}
//	else
//	{
//		Send_Query_Command(addr_DDM360B);  // 查询DDM360B（地址0x02）
//	}
	uint8_t addr = addr_TL720D;
		uint8_t cmd[5];
	    uint8_t checksum = 0;

	    cmd[0] = 0x68;           // 帧头
	    cmd[1] = 0x04;           // 数据长度
	    cmd[2] = addr;           // 地址码
	    cmd[3] = 0x04;           // 命令字（读取角度）

	    // 校验和 = 数据长度 + 地址码 + 命令字
	    checksum = cmd[1] + cmd[2] + cmd[3];
	    cmd[4] = checksum;

	    memcpy(TL720D_UART_Handler->Tx_Buf, cmd, 5);
	    TL720D_UART_Handler->TxCount = 5;

		TL720D_UART_Handler->AddSendList(
				TL720D_UART_Handler,
				TL720D_UART_Handler->Tx_Buf,
				TL720D_UART_Handler->TxCount,
				20,
				decode_TL720D);

}

uint8_t Buffer_TL720D[100];//调试用
uint8_t Length_TL720D = 0;//调试用
static void decode_TL720D(uint8_t *buffer, uint16_t length)
{
		if(length < 100){
			Length_TL720D = length;
			memcpy(Buffer_TL720D, buffer, length);//调试用
		}

//	// TL720D: 帧长0x1F(31字节), 地址0x00
	if (buffer[0] == 0x68 && buffer[1] == 0x1F && buffer[2] == 0x00
			&& buffer[3] == 0x84 && length ==32)
	{
		// 验证校验和（不含字头）
		uint8_t calc_checksum = 0;
		for (int i = 1; i < (32-1); i++) {
			calc_checksum += buffer[i];
		}
		if (calc_checksum != buffer[32-1]) {
			return;  // 校验失败，直接返回
		}
		//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]);

//		*RobotAngle = SP_MSP_RF_TL720D_Parameters_In->RF_Angle_Roll;

		//水平向左为0°，转换为竖直向上为0°;
		*RobotAngle = changeAngle_LeftZerotoUpZero(SP_MSP_RF_TL720D_Parameters_In->RF_Angle_Roll);//1号机器人
		//水平向右左为0°，转换为竖直向上为0°;
		*RobotAngle = *RobotAngle * -1;// 2号

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

}

//水平向左为0°，转换为竖直向上为0°;
static int changeAngle_LeftZerotoUpZero(int angle)
{
	int32_t RobotAngle_2 = angle -90*100;
	if(RobotAngle_2 < -180*100)
	{
		RobotAngle_2 = RobotAngle_2 + 360*100;
	}
	return RobotAngle_2;
}
/* 数值计算
 * data: 数据地址
 * return: 单位: 0.01
 * */
static int32_t getDeci(uint8_t *data)//int16_t不满足0° ~ 360.00°范围
{
	char isNegative = 0;

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

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

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

}
//Yaw（航向角）：0° ~ 360.00°，单位 0.01° → 范围 0 ~ 36000 ❌
//int16_t 最大只有 32767，但当角度 > 327.67° 时 //就会溢出变成负数