/*
 * map_DH_Remote_Controller.c
 *
 *  Created on: Jul 31, 2024
 *      Author: akeguo
 *
 *     this file is targeted to DCH-D24
 *     SN23080533
 *     IP 65
 *
 */
#include "msp_DH_Remote_Controller.h"
#include <bsp_Log.h>
#include "bsp_MB_host.h"
#include "BHBF_ROBOT.h"
uint16_t De_Chi_For_Back_Wards_Speed; //dechi 前进后退速度
uint16_t De_Chi_Left_Rigth_Wards_Speed; //dechi 左转右转速度
struct DeChiMoveState *DeChiMoveCurrentState;
struct DeChiFunctionState *DeChiFunctionCurrentState;
DispacherController *DeChi_dispacherController;

struct UARTHandler *DH_Remote_Controller_UART_Handler;

void DH_Remote_Controller_intialize(struct UARTHandler *Handler)
{

	DH_Remote_Controller_UART_Handler = Handler;
	DH_Remote_Controller_UART_Handler->Wait_time = 6; //等待10ms 最低不要低于4；
	DeChi_dispacherController=Handler->dispacherController;
	DeChi_dispacherController->DispacherCallTime=100;
	DeChi_dispacherController->Add_Dispatcher_List(DeChi_dispacherController,read_DeChi_Data);


	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"DH_Remote",0,DH_Remote);
	LOG(DL_ERROR,"angle encoder decoding failed");

}

void read_DeChi_Data(void)
{

	LOGFF(DL_ERROR,"read data from DeChi");

	DH_Remote_Controller_UART_Handler->UART_Decode = decode_DeChi;
	MB_ReadInputReg(&DH_Remote_Controller_UART_Handler->Tx_Buf,
			&DH_Remote_Controller_UART_Handler->TxCount, 3, 0, 6);

	DH_Remote_Controller_UART_Handler->UART_Tx(
			DH_Remote_Controller_UART_Handler);

}
uint16_t Decoded_Reg_Value[6];
void decode_DeChi(uint8_t *buffer, uint16_t length)
{

	int decoded_result;
	decoded_result = MB_Decode_InputRegs(buffer, length, 6, Decoded_Reg_Value);

	if (decoded_result == 1)
	{
		De_Chi_For_Back_Wards_Speed = Decoded_Reg_Value[0];
		De_Chi_Left_Rigth_Wards_Speed = Decoded_Reg_Value[1];

		DeChiMoveCurrentState = &Decoded_Reg_Value[2];
		DeChiFunctionCurrentState = &Decoded_Reg_Value[3];
		LOG("DeChi decoding  succeeded");

		HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
							"DH_Remote", 1);
	} else
	{
		//log_error("DeChi decoding failed");
		LOGFF(DL_ERROR,"DeChi decoding failed");
	}

}
MB_REG_DATA mb_reg_value_DH;
void set_DeChi_Display(uint16_t Angle, uint16_t Speed)
{

	DH_Remote_Controller_UART_Handler->UART_Decode = NULL; //表示不需要解析读回来的数据
	mb_reg_value_DH.DATA_10H[0] = (Angle >> 8) & 0xff;
	mb_reg_value_DH.DATA_10H[1] = (Angle) & 0xff;

	mb_reg_value_DH.DATA_10H[2] = (Speed >> 8) & 0xff;
	mb_reg_value_DH.DATA_10H[3] = Speed & 0xff;

	MB_WriteNumHoldingReg(&DH_Remote_Controller_UART_Handler->Tx_Buf,
			&DH_Remote_Controller_UART_Handler->TxCount, 3, 0, 2,
			mb_reg_value_DH.DATA_10H);

	//read from the data;
	DH_Remote_Controller_UART_Handler->UART_Tx(
			DH_Remote_Controller_UART_Handler);

}