/*
AI
*/

/* Includes ----------------------------------------------------------------- */
#include "ai.h"

#include <string.h>

#include "bsp\delay.h"
#include "bsp\uart.h"
#include "component\crc16.h"
#include "component\crc8.h"
#include "component\user_math.h"
#include "component\filter.h"


/* Private define ----------------------------------------------------------- */
#define AI_LEN_RX_BUFF (sizeof(Protocol_DownPackage_t))

/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static volatile uint32_t drop_message = 0;

static uint8_t rxbuf[AI_LEN_RX_BUFF];

static bool inited = false;

static osThreadId_t thread_alert;

/* Private function  -------------------------------------------------------- */

static void Ai_RxCpltCallback(void) {
  osThreadFlagsSet(thread_alert, SIGNAL_AI_RAW_REDY);
}

static void Ai_IdleLineCallback(void) {
  osThreadFlagsSet(thread_alert, SIGNAL_AI_RAW_REDY);
}

/* Exported functions ------------------------------------------------------- */
int8_t AI_Init(AI_t *ai) {
  UNUSED(ai);
  ASSERT(ai);
  if (inited) return DEVICE_ERR_INITED;
  VERIFY((thread_alert = osThreadGetId()) != NULL);

  BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_RX_CPLT_CB,
                            Ai_RxCpltCallback);
  BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_IDLE_LINE_CB,
                            Ai_IdleLineCallback);
  
  ai->search_velocity.yaw = 0.0025f;
  ai->search_velocity.pit = 0.0015f;
  inited = true;
  return 0;
}

int8_t AI_Restart(void) {
  __HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_AI));
  __HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_AI));
  return DEVICE_OK;
}

int8_t AI_StartReceiving(AI_t *ai) {
  UNUSED(ai);
  if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AI), rxbuf,
                           AI_LEN_RX_BUFF) == HAL_OK)
    return DEVICE_OK;
  return DEVICE_ERR;
}

bool AI_WaitDmaCplt(void) {
  return (osThreadFlagsWait(SIGNAL_AI_RAW_REDY, osFlagsWaitAll, 0) ==
          SIGNAL_AI_RAW_REDY);
}

int8_t AI_ParseHost(AI_t *ai) {
  if (!CRC16_Verify((const uint8_t *)&(rxbuf), sizeof(ai->from_host)))
    goto error;
  ai->ai_online = true;
  memcpy(&(ai->from_host), rxbuf, sizeof(ai->from_host));
  memset(rxbuf, 0, AI_LEN_RX_BUFF);
  return DEVICE_OK;

error:
  drop_message++;
  return DEVICE_ERR;
}

void AI_PackCmd(AI_t *ai, CMD_Host_t *cmd_host) {
  cmd_host->gimbal_delta.yaw = ai->from_host.data.gimbal.yaw;
  cmd_host->gimbal_delta.pit = ai->from_host.data.gimbal.pit;
  cmd_host->fire = (ai->from_host.data.notice & AI_NOTICE_FIRE);
  cmd_host->chassis_move_vec.vx = ai->from_host.data.chassis_move_vec.vx;
  cmd_host->chassis_move_vec.vy = ai->from_host.data.chassis_move_vec.vy;
  cmd_host->chassis_move_vec.wz = ai->from_host.data.chassis_move_vec.wz;
}

void AI_SearchArmor(AI_t *ai, CMD_Host_t *cmd_host , AHRS_Eulr_t *gimbal_ai){
  if (gimbal_ai->pit > 1.95){
    ai->search_velocity.pit = fAbs(ai->search_velocity.pit);
  } else if (gimbal_ai->pit < 1.75) {
    ai->search_velocity.pit = -fAbs(ai->search_velocity.pit);
  }
  
  if (gimbal_ai->yaw > 4.6 && gimbal_ai->yaw < 4.9) {
    ai->search_velocity.yaw = -fAbs(ai->search_velocity.yaw);
  } else if (gimbal_ai->yaw > 2.4 && gimbal_ai->yaw < 2.7) {
    ai->search_velocity.yaw = fAbs(ai->search_velocity.yaw);
  }

  cmd_host->gimbal_delta.yaw = ai->search_velocity.yaw;
  cmd_host->gimbal_delta.pit = ai->search_velocity.pit;
}

int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host) {
  if (ai == NULL) return DEVICE_ERR_NULL;
  if (cmd_host == NULL) return DEVICE_ERR_NULL;
  ai->ai_online = false;
  memset(&(ai->from_host), 0, sizeof(ai->from_host));
  memset(cmd_host, 0, sizeof(*cmd_host));
  return 0;
}

int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat, const AHRS_Eulr_t *gimbal_ai) {
  ai->to_host.mcu.id = AI_ID_MCU;
  memcpy((void *)&(ai->to_host.mcu.package.data.quat), (const void *)quat,
         sizeof(*quat));
  memcpy((void *)&(ai->to_host.mcu.package.data.gimbal), (const void *)gimbal_ai,
         sizeof(*gimbal_ai));
  ai->to_host.mcu.package.data.notice = 0;
  if (ai->status == AI_STATUS_AUTOAIM)
    ai->to_host.mcu.package.data.notice |= AI_NOTICE_AUTOAIM;
  else if (ai->status == AI_STATUS_HITSWITCH)
    ai->to_host.mcu.package.data.notice |= AI_NOTICE_HITBUFF;
  else if (ai->status == AI_STATUS_AUTOMATIC)
    ai->to_host.mcu.package.data.notice |= AI_NOTICE_AUTOMATIC;

  ai->to_host.mcu.package.crc16 = CRC16_Calc(
      (const uint8_t *)&(ai->to_host.mcu.package),
      sizeof(ai->to_host.mcu.package) - sizeof(uint16_t), CRC16_INIT);
  return DEVICE_OK;
}

int8_t AI_PackRef(AI_t *ai, const Referee_ForAI_t *ref) {
  (void)ref;
  ai->to_host.ref.id = AI_ID_REF;
  ai->to_host.ref.package.crc16 = CRC16_Calc(
      (const uint8_t *)&(ai->to_host.ref.package),
      sizeof(ai->to_host.ref.package) - sizeof(uint16_t), CRC16_INIT);
  return DEVICE_OK;
}

int8_t AI_StartSend(AI_t *ai, bool ref_update) {
  if (ref_update) {
    if (HAL_UART_Transmit_DMA(
            BSP_UART_GetHandle(BSP_UART_AI), (uint8_t *)&(ai->to_host),
            sizeof(ai->to_host.ref) + sizeof(ai->to_host.mcu)) == HAL_OK)
      return DEVICE_OK;
    else
      return DEVICE_ERR;
  } else {
    if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_AI),
                              (uint8_t *)&(ai->to_host.mcu),
                              sizeof(ai->to_host.mcu)) == HAL_OK)
      return DEVICE_OK;
    else
      return DEVICE_ERR;
  }
}