#include "nuc.h"
#include "crc16.h"
#include <string.h>
#include "error_detect.h"
volatile uint32_t drop_message = 0;

static osThreadId_t thread_alert;

uint8_t nucbuf[25];
char SendBuffer[7];

static void NUC_CBLTCallback(void)
{
  osThreadFlagsSet(thread_alert, SIGNAL_NUC_RAW_REDY);
  // detect_hook(NUC_TOE);
}
static void NUC_ERRORCALLBACK(void)
{
  NUC_Restart();
  uint32_t error_code = HAL_UART_GetError(BSP_UART_GetHandle(BSP_UART_NUC));
  // osThreadFlagsSet(thread_alert, SIGNAL_NUC_ERROR);
}
int8_t NUC_Init(NUC_t *nuc)
{
  if (nuc == NULL) 
    return DEVICE_ERR_NULL;
  if ((thread_alert = osThreadGetId()) == NULL)
    return DEVICE_ERR_NULL;
  BSP_UART_RegisterCallback(BSP_UART_NUC, BSP_UART_RX_CPLT_CB,
                            NUC_CBLTCallback);
  BSP_UART_RegisterCallback(BSP_UART_NUC, BSP_UART_ERROR_CB,
                            NUC_ERRORCALLBACK);
  return DEVICE_OK;
}
int8_t NUC_StartReceiving()
{
  if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_NUC),
                           (uint8_t *)nucbuf,
                           sizeof(nucbuf)) == HAL_OK)
    return DEVICE_OK;
  return DEVICE_ERR;
}
int8_t NUC_StartSending(fp32 *data)
{

  union
  {
    float x[1];
    char data[4];
  } instance;

  //      for (int i = 0; i < 1; i++) {
  instance.x[0] = data[0];
  //    }

  SendBuffer[0] = 0xFC; // 帧头
  SendBuffer[1] = 0x01; // 控制帧
  for (int i = 2; i < 6; i++)
  {
    SendBuffer[i] = instance.data[i - 2];
  }
  SendBuffer[6] = 0xFD; // 帧尾

  if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_NUC),
                            (uint8_t *)SendBuffer,
                            sizeof(SendBuffer)) == HAL_OK)
    return DEVICE_OK;
  return DEVICE_ERR;
}
int8_t NUC_Restart(void)
{
  __HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_NUC));
  __HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_NUC));
  return DEVICE_OK;
}
bool_t NUC_WaitDmaCplt(void)
{
  return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll, 0) ==
          SIGNAL_NUC_RAW_REDY);
}

int8_t NUC_RawParse(CMD_NUC_t *n, NUC_t *nuc)
{
  if (n == NULL)
    return DEVICE_ERR_NULL;
  union
  {
    float x[5];
    char data[20];
  } instance;
  if (nucbuf[0] != HEAD)
    goto error; // 发送ID不是底盘
  else
  {
    n->status_fromnuc = nucbuf[1];
    n->ctrl_status = nucbuf[2];
    switch (n->status_fromnuc)
    {
    case VISION: // 控制帧0x02
      /* 协议格式
     0xFF  HEAD
     0x02  控制帧
     0x01  相机帧
     vx    fp32
     vy    fp32
     wz    fp32
     0xFE  TAIL
    */

      instance.data[3] = nucbuf[3];
      instance.data[2] = nucbuf[4];
      instance.data[1] = nucbuf[5];
      instance.data[0] = nucbuf[6];
      n->camera.data1 = instance.x[0]; //
      instance.data[7] = nucbuf[7];
      instance.data[6] = nucbuf[8];
      instance.data[5] = nucbuf[9];
      instance.data[4] = nucbuf[10];
      n->camera.data2 = instance.x[1]; //
      instance.data[11] = nucbuf[11];
      instance.data[10] = nucbuf[12];
      instance.data[9] = nucbuf[13];
      instance.data[8] = nucbuf[14];
      n->camera.data3 = instance.x[2]; //
      break;
    case MID: // 控制帧0x08
      /* 协议格式
     0xFF  HEAD
     0x09  控制帧
     0x01  相机帧
     vx    fp32
     vy    fp32
     wz    fp32
     0xFE  TAIL
    */
      if (nucbuf[24] != TAIL)
        goto error;
      instance.data[3] = nucbuf[6];
      instance.data[2] = nucbuf[5];
      instance.data[1] = nucbuf[4];
      instance.data[0] = nucbuf[3];
      n->MID360.vx = instance.x[0]; //
      instance.data[7] = nucbuf[10];
      instance.data[6] = nucbuf[9];
      instance.data[5] = nucbuf[8];
      instance.data[4] = nucbuf[7];
      n->MID360.vy = instance.x[1]; //
      instance.data[11] = nucbuf[14];
      instance.data[10] = nucbuf[13];
      instance.data[9] = nucbuf[12];
      instance.data[8] = nucbuf[11];
      n->MID360.wz = instance.x[2]; //
      instance.data[15] = nucbuf[18];
      instance.data[14] = nucbuf[17];
      instance.data[13] = nucbuf[16];
      instance.data[12] = nucbuf[15];
      n->MID360.pos = instance.x[3]; //
      instance.data[19] = nucbuf[22];
      instance.data[18] = nucbuf[21];
      instance.data[17] = nucbuf[20];
      instance.data[16] = nucbuf[19];
      n->MID360.angle = instance.x[4]; //

      n->MID360.flag = nucbuf[23]; //

      break;
    }
    nuc->unc_online = true; // 设置为在线状态
    return DEVICE_OK;
  }
error:
  drop_message++;
  NUC_Restart();
  return DEVICE_ERR;
}

int8_t NUC_HandleOffline(CMD_NUC_t *cmd, NUC_t *nuc)
{
  if (cmd == NULL)
    return DEVICE_ERR_NULL;
  nuc->unc_online = false; // 设置为离线状态
  //	 memset(cmd, 0, sizeof(*cmd));
  return 0;
}

int8_t NUC_PackIMU(NUC_t *nuc, const AHRS_Quaternion_t *quat, const AHRS_Accl_t *accl, const AHRS_Gyro_t *gyro){
  nuc->to_nuc.imu.head = HEAD;
  nuc->to_nuc.imu.id = IMU_ID;
  nuc->to_nuc.imu.type = TYPE;
  nuc->to_nuc.imu.end = TAIL;
  memcpy((void *)&(nuc->to_nuc.imu.package.quat), (const void *)quat,sizeof(*quat));
  memcpy((void *)&(nuc->to_nuc.imu.package.gyro), (const void *)gyro,sizeof(*gyro));
  memcpy((void *)&(nuc->to_nuc.imu.package.accl), (const void *)accl,sizeof(*accl));
  return DEVICE_OK;
}

int8_t NUC_PackCMD(NUC_t *nuc, const NUC_send_t *send){
  nuc->to_nuc.cmd.head = HEAD;
  nuc->to_nuc.cmd.id = IMU_ID;
  nuc->to_nuc.cmd.type = TYPE;
  nuc->to_nuc.cmd.end = TAIL;
  // memcpy((void *)&(nuc->to_nuc.cmd.package.status), (const void *)send,sizeof(*send));
  /*在这里添加你们的控制命令*/
  return DEVICE_OK;
}

int8_t NUC_StartSend(NUC_t *nuc, bool cmd_update){
  if (cmd_update) {
    if (HAL_UART_Transmit_DMA(
            BSP_UART_GetHandle(BSP_UART_NUC), (uint8_t *)&(nuc->to_nuc.cmd),
            sizeof(nuc->to_nuc.cmd) + sizeof(nuc->to_nuc.imu)) == HAL_OK)
      return DEVICE_OK;
    else
      return DEVICE_ERR;
  } else {
    if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_NUC),
                              (uint8_t *)&(nuc->to_nuc.imu),
                              sizeof(nuc->to_nuc.imu)) == HAL_OK)
      return DEVICE_OK;
    else
      return DEVICE_ERR;
  }
}