#include "vision_bridge.h"
#include "device/device.h"
#include "bsp/uart.h"
#include "component/crc16.h"
#include <string.h>

int8_t AI_Init(AI_t *ai) {
  if (ai == NULL) return DEVICE_ERR_NULL;
	memset(ai, 0, sizeof(AI_t));
  return 0;
}

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

int8_t AI_ParseForHost(AI_t* ai, AI_RawData_t* raw_data){
		ai->tohost.head[0]='M';
		ai->tohost.head[1]='R';	
		ai->tohost.mode=raw_data->mode;
		ai->tohost.pitch=raw_data->pitch;
		ai->tohost.yaw=raw_data->yaw;	
		ai->tohost.pitch_vel=raw_data->pitch_vel;
		ai->tohost.yaw_vel=raw_data->yaw_vel;	
		ai->tohost.q[0]=raw_data->q[0];	
		ai->tohost.q[1]=raw_data->q[1];	
		ai->tohost.q[2]=raw_data->q[2];	
		ai->tohost.q[3]=raw_data->q[3];		
		ai->tohost.bullet_count=10;
		ai->tohost.bullet_speed=17.5;	
		
		ai->tohost.crc16=CRC16_Calc(((const uint8_t*)&(ai->tohost)),sizeof(ai->tohost)-sizeof(uint16_t), CRC16_INIT );
		if(CRC16_Verify(((const uint8_t*)&(ai->tohost)), sizeof(ai->tohost))!=true){
				  return DEVICE_ERR;
		}
		      return DEVICE_OK;
}

int8_t AI_StartSend(AI_t *ai) {
      if (BSP_UART_Transmit(BSP_UART_AI,(uint8_t *)&ai->tohost,sizeof(ai->tohost), true)==HAL_OK)  
      return DEVICE_OK;
}

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