/**
	*go控制程序
	*/
#include "GO_M8010_6_Driver.h"
#include "usart.h"
#include "bsp_usart.h"
#include <cmsis_os2.h>
#include "crc16.h"
#include <string.h>

#define SATURATE(_IN, _MIN, _MAX) \
	{                             \
		if ((_IN) <= (_MIN))      \
			(_IN) = (_MIN);       \
		else if ((_IN) >= (_MAX)) \
			(_IN) = (_MAX);       \
	}
GO_MotorCmd_t cmd_gogogo ;
RIS_ControlData_t motor_send_data;
	
	
GO_MotorData_t data = {0}; // 确保 data 定义在此处
uint8_t rx_buffer[sizeof(data.motor_recv_data)] = {0}; // 确保 rx_buffer 定义在此处






/// @brief 将发送给电机的浮点参数转换为定点类型参数
/// @param motor_s 要转换的电机指令结构体
void modify_data(GO_MotorCmd_t *motor_s)
{
	motor_send_data.head[0] = 0xFE;
	motor_send_data.head[1] = 0xEE;

	SATURATE(motor_s->id, 0, 15);
	SATURATE(motor_s->mode, 0, 7);
	SATURATE(motor_s->K_P, 0.0f, 25.599f);
	SATURATE(motor_s->K_W, 0.0f, 25.599f);
	SATURATE(motor_s->T, -127.99f, 127.99f);
	SATURATE(motor_s->W, -804.00f, 804.00f);
	SATURATE(motor_s->Pos, -411774.0f, 411774.0f);

	motor_send_data.mode.id = motor_s->id;
	motor_send_data.mode.status = motor_s->mode;
	motor_send_data.comd.k_pos = motor_s->K_P / 25.6f * 32768.0f;
	motor_send_data.comd.k_spd = motor_s->K_W / 25.6f * 32768.0f;
	motor_send_data.comd.pos_des = motor_s->Pos / 6.28318f * 32768.0f;
	motor_send_data.comd.spd_des = motor_s->W / 6.28318f * 256.0f;
	motor_send_data.comd.tor_des = motor_s->T * 256.0f;
	motor_send_data.CRC16 = CRC16_Calc( (uint8_t *)&motor_send_data, sizeof(RIS_ControlData_t) - sizeof(motor_send_data.CRC16),0);
}

/// @brief 将接收到的定点类型原始数据转换为浮点参数类型
/// @param motor_r 要转换的电机反馈结构体
void extract_data(GO_MotorData_t *motor_r)
{
	if (motor_r->motor_recv_data.head[0] != 0xFD || motor_r->motor_recv_data.head[1] != 0xEE)
	{
		motor_r->correct = 0;
		return;
	}
	motor_r->calc_crc = CRC16_Calc((uint8_t *)&motor_r->motor_recv_data, sizeof(RIS_MotorData_t) - sizeof(motor_r->motor_recv_data.CRC16),0);
	if (motor_r->motor_recv_data.CRC16 != motor_r->calc_crc)
	{
		memset(&motor_r->motor_recv_data, 0, sizeof(RIS_MotorData_t));
		motor_r->correct = 0;
		motor_r->bad_msg++;
		return;
	}
	else
	{
		motor_r->motor_id = motor_r->motor_recv_data.mode.id;
		motor_r->mode = motor_r->motor_recv_data.mode.status;
		motor_r->Temp = motor_r->motor_recv_data.fbk.temp;
		motor_r->MError = motor_r->motor_recv_data.fbk.MError;
		motor_r->W = ((float)motor_r->motor_recv_data.fbk.speed / 256.0f) * 6.28318f;
		motor_r->T = ((float)motor_r->motor_recv_data.fbk.torque) / 256.0f;
		motor_r->Pos = 6.28318f * ((float)motor_r->motor_recv_data.fbk.pos) / 32768.0f;
		motor_r->footForce = motor_r->motor_recv_data.fbk.force;
		motor_r->correct = 1;
		return;
	}
}
void GO_M8010_send_data(GO_MotorCmd_t *cmd)
{
    modify_data(cmd);
	
    HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_RS485), (uint8_t *)(&motor_send_data), sizeof(motor_send_data));   
    osDelay (1);
	  HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_RS485), rx_buffer, sizeof(rx_buffer));

}

void USART6_RxCompleteCallback(void ) 
{	

	  UART_HandleTypeDef *huart6 = BSP_UART_GetHandle(BSP_UART_RS485);
    uint16_t crc = CRC16_Calc(rx_buffer,sizeof(rx_buffer)-2,0x0000);

	  memcpy(&data.motor_recv_data, rx_buffer, sizeof(data.motor_recv_data)); // Copy received data
    extract_data(&data); 
}
GO_MotorData_t *get_GO_measure_point(void)
{
    return &data;
}