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

static int8_t Package_BuildAndVerify(uint8_t *raw, uint16_t len, uint8_t id) {
  uint16_t *crc = NULL;

  if (raw == NULL || len <= sizeof(uint16_t)) {
    return DEVICE_ERR;
  }

  raw[0] = id;
  crc = (uint16_t *)(void *)(raw + len - sizeof(uint16_t));
  *crc = CRC16_Calc(raw, len - sizeof(uint16_t), CRC16_INIT);

  if (CRC16_Verify(raw, len) != true) {
    return DEVICE_ERR;
  }

  return DEVICE_OK;
}

static int8_t Package_Check(const uint8_t *raw, uint16_t len, uint8_t expected_id) {
  if (raw == NULL || len <= sizeof(uint16_t)) {
    return DEVICE_ERR;
  }

  if (raw[0] != expected_id) {
    return DEVICE_ERR;
  }

  if (CRC16_Verify(raw, len) != true) {
    return DEVICE_ERR;
  }

  return DEVICE_OK;
}


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 == NULL || result == NULL) {
			return DEVICE_ERR;
		}

		if (Package_Check((const uint8_t *)ai, sizeof(*ai), ID_AI) != DEVICE_OK) {
			return DEVICE_ERR;
		}

		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 ){ 
		(void)quat;
		if (mcu == NULL || motor == NULL) {
			return DEVICE_ERR;
		}

		mcu->data.mode=1;
		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;			

		return Package_BuildAndVerify((uint8_t *)mcu, sizeof(*mcu), ID_MCU);
}

int8_t  REF_Send(PackageReferee_t *referee,Referee_RobotStatus_t* robot_status,Referee_GameStatus_t *game_status) {
		if (referee == NULL || robot_status == NULL || game_status == NULL) {
			return DEVICE_ERR;
		}

		referee->data.remain_hp=robot_status->current_HP;
		referee->data.game_progress = game_status->game_progress & 0x0F;
		referee->data.stage_remain_time= game_status->stage_remain_time;

		return Package_BuildAndVerify((uint8_t *)referee, sizeof(*referee), ID_REF);
}

int8_t REF_StartSend(PackageReferee_t *referee) {
	if (referee == NULL) {
		return DEVICE_ERR;
	}

	if(BSP_UART_Transmit(BSP_UART_1, (uint8_t *)referee, sizeof(*referee), true)==HAL_OK)
	  return DEVICE_OK;
	return DEVICE_ERR;
}


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;
	return DEVICE_ERR;

}