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


int8_t AI_StartReceiving(PackageAI_t *ai) {
  if (BSP_UART_Receive(BSP_UART_1,(uint8_t *)ai,sizeof(*ai), true)==HAL_OK) {
    return DEVICE_OK;}
  return DEVICE_ERR;
}

int8_t AI_Get_NUC(PackageAI_t *ai,AI_result_t* result) {

        if(ai->id==ID_AI){
		// if(CRC16_Verify((const uint8_t*)&(ai), sizeof(&ai))==true)
		// {
		result->mode=ai->data.mode;
		result->gimbal_t.setpoint.yaw=ai->data.yaw;
		result->gimbal_t.vel.yaw=ai->data.yaw_vel;
		result->gimbal_t.accl.yaw=ai->data.yaw_acc;
		result->gimbal_t.setpoint.pit=ai->data.pitch;
		result->gimbal_t.vel.pit=ai->data.pitch_vel;
		result->gimbal_t.accl.pit=ai->data.pitch_acc;
		
		result->chassis_t.Vx=ai->data.vx;
		result->chassis_t.Vy=ai->data.vy;
		result->chassis_t.Vw=ai->data.wz;
		// }
			
        return DEVICE_OK;
	    }
}


int8_t MCU_Send(PackageMCU_t* mcu,Gimbal_feedback_t* motor,AHRS_Quaternion_t* quat) {

		mcu->id=ID_MCU;
		mcu->data.mode=0;
		mcu->data.q[0]=motor->imu.quat.q0;	  
		mcu->data.q[1]=motor->imu.quat.q1;	
		mcu->data.q[2]=motor->imu.quat.q2;	
		mcu->data.q[3]=motor->imu.quat.q3;
		mcu->data.yaw=motor->imu.eulr.yaw;
		mcu->data.yaw_vel=motor->imu.gyro.z;			
		mcu->data.pitch=motor->imu.eulr.rol;
		mcu->data.pitch_vel=motor->imu.gyro.x;
		mcu->data.bullet_count=0;
		mcu->data.bullet_speed=22;			
		
		mcu->crc16=CRC16_Calc(((const uint8_t*)&(mcu)),sizeof(&mcu)-sizeof(uint16_t), CRC16_INIT );
		if(CRC16_Verify(((const uint8_t*)&(mcu)), sizeof(&mcu))!=true){
				  return DEVICE_ERR;
		}
		      return DEVICE_OK;
}

int8_t MCU_StartSend(PackageMCU_t *mcu) {
	if(BSP_UART_Transmit(BSP_UART_1, (uint8_t *)mcu, sizeof(*mcu), true)==HAL_OK)
	  return DEVICE_OK;

}