/*
 * DMK_485_Motor.c
 *
 *  Created on: Jan 21, 2025
 *      Author: akeguo
 */
//DMKE Motor
#include "msp_DMKE_485_Motor.h"

DMKE_struct_define *DMKE_Motors[3];

struct UARTHandler *DMAKE_UART_Handler;
DispacherController *DMAKE_dispacherController;

char Is_First_Run = 0;

void decode_DMAKE(uint8_t *buffer, uint16_t length);
void DMAKE_Set();
void DMAKE_intialize(struct UARTHandler *Handler)
{
	//TL720D_UART_Handler->UART_Decode = NULL;
	DMAKE_UART_Handler = Handler;
	DMAKE_UART_Handler->Wait_time = 30;
	DMAKE_UART_Handler->UART_Decode = decode_DMAKE;


	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"DMAKE_1",0,DMAKE_1);
	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"DMAKE_2",0,DMAKE_2);
	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"DMAKE_3",0,DMAKE_3);


	DMAKE_dispacherController = Handler->dispacherController;
	DMAKE_dispacherController->DispacherCallTime = 20;

	DMAKE_dispacherController->Add_Dispatcher_List(DMAKE_dispacherController,
	        DMAKE_Set);

	//log_info("TL720D_intialize");
	LOGFF(DL_ERROR, "TL720D_intialize");
}
void decode_DMAKE(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))
	{

		if (buffer[0] == 1)
		{
			HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
			        "DMAKE_1", 1);
		}
		else if (buffer[0] == 2)
		{
			HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
			        "DMAKE_2", 1);
		}
		else if (buffer[0] == 3)
		{
			HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
			        "DMAKE_3", 1);
		}
	}
	else
	{

	}

}

uint8_t DMAKE_UART_Tx_Buf[200];
uint8_t DMAKE_UART_TxCount = 0;
void DMAKE_Set()
{
	if (Is_First_Run == 0)
	{

		for (int i = 0; i < 3; i++)
		{
			//clear faults
			MB_WriteHoldingReg(&DMAKE_UART_Tx_Buf, &DMAKE_UART_TxCount, (uint8_t)(DMKE_Motors[i]->Slave_ID),
			        0x0131, 0x08);

			DMAKE_UART_Handler->AddSendList(DMAKE_UART_Handler,
			        DMAKE_UART_Tx_Buf, DMAKE_UART_TxCount, 80, NULL);
			////set paramters
			MB_WriteHoldingReg(&DMAKE_UART_Tx_Buf, &DMAKE_UART_TxCount, (uint8_t)(DMKE_Motors[i]->Slave_ID),
			        0x0131, 0x23);

			DMAKE_UART_Handler->AddSendList(DMAKE_UART_Handler,
			        DMAKE_UART_Tx_Buf, DMAKE_UART_TxCount, 80, NULL);
		}

		Is_First_Run = 1;
	}
	else
	{
		for (int i = 0; i < 3; i++)
		{
			//set speed
			MB_WriteHoldingReg(&DMAKE_UART_Tx_Buf, &DMAKE_UART_TxCount, (uint8_t)(DMKE_Motors[i]->Slave_ID),
			        0x06a, DMKE_Motors[i]->TargetSpeedRpm);

			DMAKE_UART_Handler->AddSendList(DMAKE_UART_Handler,
			        DMAKE_UART_Tx_Buf, DMAKE_UART_TxCount, 40, NULL);
			//set direction
		}

		for (int i = 0; i < 3; i++)
				{

					//set direction

					if (DMKE_Motors[i]->Command == -1)
					{
						//正转
						MB_WriteHoldingReg(&DMAKE_UART_Tx_Buf, &DMAKE_UART_TxCount, (uint8_t)(DMKE_Motors[i]->Slave_ID),
						        0x033, 0x01);

					}
					else if (DMKE_Motors[i]->Command == 0)
					{
						//stop
						MB_WriteHoldingReg(&DMAKE_UART_Tx_Buf, &DMAKE_UART_TxCount, (uint8_t)(DMKE_Motors[i]->Slave_ID),
						        0x033, 0);

					}
					else if (DMKE_Motors[i]->Command == 1)
					{
						//reverse
						MB_WriteHoldingReg(&DMAKE_UART_Tx_Buf, &DMAKE_UART_TxCount, (uint8_t)(DMKE_Motors[i]->Slave_ID),
						        0x033, 0x14);
					}
					DMAKE_UART_Handler->AddSendList(DMAKE_UART_Handler,
					        DMAKE_UART_Tx_Buf, DMAKE_UART_TxCount, 40, NULL);
				}
	}

}