#include "device/ai.h"
#include "device/device.h"
#include "bsp/uart.h"
#include "component/crc16.h"


int8_t AI_Init(AI_t *ai) {
  if (ai == NULL) return DEVICE_ERR_NULL;
//  if (inited) return DEVICE_ERR_INITED;
//  if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;

  BSP_UART_RegisterCallback(BSP_UART_NUC, BSP_UART_NUC,
                            BSP_UART_RX_CPLT_CB);
//  inited = true;
  return 0;
}


int8_t AI_StartReceiving(AI_t *ai) {
  if (BSP_UART_Receive(BSP_UART_NUC,&ai->RX,sizeof(ai->RX), true)==HAL_OK) {
    return DEVICE_OK;}
  return DEVICE_ERR;
}

int8_t AI_Get_NUC(AI_t *ai,AI_cmd_t* ai_cmd) {
		if(ai->RX.head[0]!='M'&&ai->RX.head[1]!='R'){
					return DEVICE_ERR;
		}
//		CRC16_Calc(&ai->RX,sizeof(ai->RX),ai->RX.crc16);
		if(CRC16_Verify((const uint8_t*)&(ai->RX), sizeof(ai->RX))!=true){
				  return DEVICE_ERR;
		}
		ai_cmd->gimbal_t.setpoint.pit=ai->RX.pitch;
		ai_cmd->gimbal_t.setpoint.yaw=ai->RX.yaw;
		ai_cmd->mode=ai->RX.mode;
		ai_cmd->gimbal_t.accl.pit=ai->RX.pitch_acc;
		ai_cmd->gimbal_t.vel.pit=ai->RX.pitch_vel;
		ai_cmd->gimbal_t.accl.yaw=ai->RX.yaw_acc;
		ai_cmd->gimbal_t.vel.yaw=ai->RX.yaw_vel;		
  return DEVICE_OK;
}


int8_t AI_ParseHost(AI_t* ai,Gimbal_feedback_t* motor,AHRS_Quaternion_t* quat) {
		ai->TX.head[0]='M';
		ai->TX.head[1]='R';	
//		ai->TX.mode=2;

		ai->TX.pitch=motor->motor.pitch_4310_motor_feedback.rotor_abs_angle;
		ai->TX.yaw=motor->motor.yaw_4310_motor_feedback.rotor_abs_angle;	
		ai->TX.pitch_vel=motor->motor.pitch_4310_motor_feedback.rotor_speed;
		ai->TX.yaw_vel=motor->motor.yaw_4310_motor_feedback.rotor_speed;	
		ai->TX.q[0]=motor->imu.quat.q0;	
		ai->TX.q[1]=motor->imu.quat.q1;	
		ai->TX.q[2]=motor->imu.quat.q2;	
		ai->TX.q[3]=motor->imu.quat.q3;		

		ai->TX.bullet_count=10;
		ai->TX.bullet_speed=22;	
		
		ai->TX.crc16=CRC16_Calc(((const uint8_t*)&(ai->TX)),sizeof(ai->TX)-sizeof(uint16_t), CRC16_INIT );
		if(CRC16_Verify(((const uint8_t*)&(ai->TX)), sizeof(ai->TX))!=true){
				  return DEVICE_ERR;
		}
		      return DEVICE_OK;
}

int8_t AI_StartSend(AI_t *ai) {

      if (BSP_UART_Transmit(BSP_UART_NUC,&ai->TX,sizeof(ai->TX), true)==HAL_OK)  
      return DEVICE_OK;
//    else
//      return DEVICE_ERR;
//  } else {
//    if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_AI),
//                              (uint8_t *)&(ai->TX),
//                              sizeof(ai->TX)) == HAL_OK)
//      return DEVICE_OK;
//    else
//      return DEVICE_ERR;
//  }
}


// #include "ai.h"
// #include "bsp/uart.h"
// #include "component/crc16.h"



// // void ai_init(AI_t* ai){
// // 		ai->RX.head[0]="M";
// // 		ai->RX.head[0]="R";	
// // 		ai->TX.head[0]="M";
// // 		ai->TX.head[0]="R";		
// // } 





// int8_t AI_StartReceiving(AI_t *ai) {
//   if (BSP_UART_Receive(BSP_UART_NUC,&ai->RX,sizeof(ai->RX), false)) 
//     return DEVICE_OK;
//   return DEVICE_ERR;
// }

// int8_t AI_Get_NUC(AI_t *ai,AI_cmd_t* ai_cmd) {
// 		if(ai->RX.head[0]!='M'&&ai->RX.head[1]!='R'){
// 					return DEVICE_ERR;
// 		}
// //		CRC16_Calc(&ai->RX,sizeof(ai->RX),ai->RX.crc16);
// //		if(CRC16_Verify((const uint8_t*)&(ai->RX), sizeof(ai->RX.crc16))!=true){
// //				  return DEVICE_ERR;
// //		}
// 		ai_cmd->gimbal_t.setpoint.pit=ai->RX.pitch;
// 		ai_cmd->gimbal_t.setpoint.yaw=ai->RX.yaw;
// 		ai_cmd->mode=ai->RX.mode;
// 		ai_cmd->gimbal_t.accl.pit=ai->RX.pitch_acc;
// 		ai_cmd->gimbal_t.vel.pit=ai->RX.pitch_vel;
// 		ai_cmd->gimbal_t.accl.yaw=ai->RX.yaw_acc;
// 		ai_cmd->gimbal_t.vel.yaw=ai->RX.yaw_vel;		
//   return DEVICE_OK;
// }


// int8_t AI_ParseHost(AI_t *ai,Gimbal_feedback_t* motor,AHRS_Quaternion_t* quat) {
		
// 		ai->TX.head[0]='M';
// 		ai->TX.head[1]='R';	
// //		ai->TX.mode=2;
// 		ai->TX.pitch=motor->motor.pitch_4310_motor_feedback.rotor_abs_angle;
// 		ai->TX.yaw=motor->motor.yaw_4310_motor_feedback.rotor_abs_angle;	
// 		ai->TX.pitch_vel=motor->motor.pitch_4310_motor_feedback.rotor_speed;
// 		ai->TX.yaw_vel=motor->motor.yaw_4310_motor_feedback.rotor_speed;	
// 		ai->TX.q[0]=motor->imu.quat.q1;	
// 		ai->TX.q[1]=motor->imu.quat.q2;	
// 		ai->TX.q[2]=motor->imu.quat.q3;	
// 		ai->TX.q[3]=motor->imu.quat.q0;	

// 		ai->TX.bullet_count=10;
// 		ai->TX.bullet_speed=10;	
		
// 		ai->TX.crc16=CRC16_Calc(((const uint8_t*)&(ai->TX)),sizeof(ai->TX)-sizeof(uint16_t),ai->TX.crc16);
// 		if(CRC16_Verify(((const uint8_t*)&(ai->TX)), sizeof(ai->TX))!=true){
// 				  return DEVICE_ERR;
// 		}
// 		      return DEVICE_OK;
// }

// int8_t AI_StartSend(AI_t *ai) {
// //  if (ref_update) {
//       if (BSP_UART_Transmit(BSP_UART_NUC,&ai->TX,sizeof(ai->TX), false))  
//       return DEVICE_OK;
// //    else
// //      return DEVICE_ERR;
// //  } else {
// //    if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_AI),
// //                              (uint8_t *)&(ai->TX),
// //                              sizeof(ai->TX)) == HAL_OK)
// //      return DEVICE_OK;
// //    else
// //      return DEVICE_ERR;
// //  }
// }