/**
	*go控制程序
	*/
#include "GO_M8010_6_Driver.h"
#include "usart.h"

GO_Motorfield GO_motor_info[GO_NUM];//存放电机数据
const float gravit_const =9.81;//计算前馈力矩有关

//数据处理函数
static void GO_M8010_recv_data(uint8_t* Temp_buffer);
// void GO_M8010_recv_data(uint8_t* Temp_buffer);
/**
	*@brief		电机初始化
	*/
void GO_M8010_init (){
    for (uint8_t id= 0; id <GO_NUM ;id++)
    {
        GO_motor_info[id].id        = id;
        GO_motor_info[id].mode      = 1;
        GO_motor_info[id].correct   = 1;
        GO_motor_info[id].MError    = 0;
        GO_motor_info[id].Temp      = 0;
        GO_motor_info[id].tar_pos   = 0;
        GO_motor_info[id].tar_w     = 0;
        GO_motor_info[id].T         = 0;
        GO_motor_info[id].W         = 0;
        GO_motor_info[id].Pos       = 0;
        GO_motor_info[id].footForce = 0;
        GO_motor_info[id].buffer[0] = 0xFE;
        GO_motor_info[id].buffer[1] = 0xEE;
        
    }
	//	HAL_UART_RegisterCallback(&huart1, HAL_UART_TX_COMPLETE_CB_ID, uartTxCB);
		HAL_UART_RegisterCallback(&huart6, HAL_UART_TX_COMPLETE_CB_ID, uartTxCB);
}

//暂存接收数据
static uint8_t Temp_buffer[16];
//dma传输完成后进入此回调函数
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
    // CRC checking. if CRC checking fail the led will indicate and return 
    uint16_t crc = do_crc_table(Temp_buffer,sizeof(Temp_buffer)-2);
    if ((Temp_buffer[14] != (crc&0xFF)) || (Temp_buffer[15] != ((crc>>8) & 0xFF)))
    { 
        HAL_GPIO_WritePin(LED_B_GPIO_Port, LED_B_Pin, GPIO_PIN_SET);    //蓝色灯亮      // indicate CRC incorrect
        HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_G_Pin, GPIO_PIN_RESET);      //红色灯灭
        return;
    }
    // CRC pass and start converting data to the motor 
    HAL_GPIO_WritePin(GPIOH, GPIO_PIN_11, GPIO_PIN_SET);                 // indicate CRC correct
    HAL_GPIO_WritePin(LED_R_GPIO_Port, LED_G_Pin, GPIO_PIN_RESET);      //红色灯灭
    GO_M8010_recv_data(Temp_buffer);

}


/**
	*@brief		用户自定义串口发送完成中断回调函数
	*/
void uartTxCB(UART_HandleTypeDef *huart)
{
    //发送完成后开启接受dma，并将传输完成中断回调函数指向串口接收完成中断回调函数
    HAL_UART_Receive_DMA(huart,Temp_buffer,sizeof(Temp_buffer));
}

/**
	*@brief				go发送控制命令
	*@input[in]		huart 所用串口指针
	*@input[in]		id 被控电机id
	*@input[in]		rev 暂时不知道干啥用的，github为回答issue
	*@input[in]		T	力矩，单位N·m
	*@input[in]		Pos 目标位置，单位rad
	*@input[in]		W 目标速度，单位rad/s
	*@input[in]		K_P 位置环环kp
	*@input[in]		K_W 速度环kp
	*@note				控制公式 ： t = T + （设定位置 - 实际位置）*K_P + （设定速度 - 实际速度）*K_W
	*/
uint8_t xx;
void GO_M8010_send_data(UART_HandleTypeDef *huart, int id,int rev,float T,float Pos,
                         float W,float K_P,float K_W)
{
    // a pointer to target motor
    GO_Motorfield* motor;
    motor = GO_motor_info+id;
		
	  //这个rev是干啥的
    // send preious cmd
    if (rev==1){
        HAL_UART_Transmit_IT(huart,motor->buffer,sizeof(motor->buffer));
        return;
    }

    // assign motor target goal to the buffer
    motor->motor_send_data.head[0]=0xFE;
    motor->motor_send_data.head[1]=0xEE;

    motor->motor_send_data.mode.id = motor->id & 0xF;
    motor->motor_send_data.mode.status = motor->mode & 0x7;
    motor->motor_send_data.mode.none = 0x0;
    motor->motor_send_data.comd.tor_des = T*256;

    motor->motor_send_data.comd.spd_des = (W*256*6.33f)/(6.28319f);
    motor->motor_send_data.comd.pos_des = (Pos*32768*6.33f)/(6.28319f);
    motor->motor_send_data.comd.k_pos = K_P*1280;
    motor->motor_send_data.comd.k_spd = K_W*1280;

    uint8_t* mode = (uint8_t *)&(motor->motor_send_data.mode);
    motor->buffer[2] = *mode;
    motor->buffer[3] = (motor->motor_send_data.comd.tor_des) & 0xFF;
    motor->buffer[4] =  (motor->motor_send_data.comd.tor_des>>8) & 0xFF;
    motor->buffer[5] =  (motor->motor_send_data.comd.spd_des) & 0xFF;
    motor->buffer[6] =  (motor->motor_send_data.comd.spd_des>>8) & 0xFF;
    motor->buffer[7] =  (motor->motor_send_data.comd.pos_des) & 0xFF;
    motor->buffer[8] =  (motor->motor_send_data.comd.pos_des>>8) & 0xFF;
    motor->buffer[9] =  (motor->motor_send_data.comd.pos_des>>16) & 0xFF;
    motor->buffer[10] =  (motor->motor_send_data.comd.pos_des>>24) & 0xFF;
    motor->buffer[11] =  (motor->motor_send_data.comd.k_pos) & 0xFF;
    motor->buffer[12] = (motor->motor_send_data.comd.k_pos>>8) & 0xFF;
    motor->buffer[13] =  (motor->motor_send_data.comd.k_spd) & 0xFF;
    motor->buffer[14] =  (motor->motor_send_data.comd.k_spd>>8) & 0xFF;

    //crc calulation 
    uint16_t crc = do_crc_table(motor->buffer, sizeof(motor->buffer)-2);
    motor->buffer[15] = (crc) & 0xFF;
    motor->buffer[16] = (crc>>8) & 0xFF;

    // interrupt buffer sending
    xx = HAL_UART_Transmit_IT(huart,motor->buffer,sizeof(motor->buffer));
    HAL_Delay(100);
    HAL_UART_Receive_DMA(huart, Temp_buffer, 16);//有无都可以

    
}

//数据处理函数
void GO_M8010_recv_data(uint8_t* Temp_buffer)
{
    // check for ID
    int8_t ID=-1;
    GO_Motorfield* motor;
    ID = Temp_buffer[2] & 0xF;

    // a pointer to target pointer
    motor = GO_motor_info + ID;
    memcpy(motor->Rec_buffer,Temp_buffer,16);

    // assign buffer to the target motor
    motor->mode = Temp_buffer[2]>>4 & 0xF;
    motor->correct = 1;
    motor->MError = 0;
    int16_t T = Temp_buffer[4]<<8 | Temp_buffer[3];
    motor->T = T;
    motor->T /= 256;
    int16_t W = Temp_buffer[6]<<8 | Temp_buffer[5];
    motor->W = W;
    motor->W = (motor->W * 6.28319f )/(256*6.33f);
    int32_t Pos = Temp_buffer[10]<<24 | Temp_buffer[9]<<16 | Temp_buffer[8]<<8 | Temp_buffer[7];
    motor->Pos = Pos;
    motor->Pos = (motor->Pos * 6.28319f )/(32768*6.33f);
    int8_t Temp = Temp_buffer[11] & 0xFF;
    motor->Temp = Temp;
    motor->MError = Temp_buffer[12] & 0x7;
    
}

//力位混合控制
// for differnt robot should have different define. Now just for testing first
/**
    *@brief				力位混合控制
    *@input[in]		huart 所用串口指针
    *@input[in]		id 被控电机id
    *@input[in]		bias 前馈力矩
    *@input[in]		length 机械臂长度
    *@input[in]		mass 机械臂质量
    *@input[in]		tar_pos 目标位置
    *@input[in]		tar_w 目标速度
    *@input[in]		K_P 位置环kp
    *@input[in]		K_W 速度环kp
    */
void basic_ForceControl (UART_HandleTypeDef *huart, int id, float bias, float length, float mass, 
                        float tar_pos, float tar_w, float K_P,float K_W)
{
    float forw_T = mass* gravit_const* length* (cos(GO_motor_info[id].Pos));//计算前馈力矩
    forw_T += bias;
    GO_M8010_send_data(huart,id,0,forw_T,tar_pos,tar_w,K_P,K_W);
}

/**
	*@brief				go发送控制命令
	*@input[in]		id 被控电机id
	*@revtal			返回电机数据指针
	*/
GO_Motorfield* getGoPoint(uint8_t id)
{
	return &GO_motor_info[id];
}