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

static osThreadId_t thread_alert;

uint8_t nucbuf[31];


static void NUC_IdleCallback(void) {	
	osThreadFlagsSet(thread_alert,SIGNAL_NUC_RAW_REDY);
	detect_hook(NUC_TOE);
}
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_IDLE_LINE_CB,
                            NUC_IdleCallback);
	
   return DEVICE_OK;
}
int8_t NUC_StartReceiving() {
  if (HAL_UARTEx_ReceiveToIdle_DMA(BSP_UART_GetHandle(BSP_UART_NUC),
                           (uint8_t *)nucbuf,
                           sizeof(nucbuf)) == HAL_OK)
    return DEVICE_OK;
  return DEVICE_ERR;
}
  char SendBuffer[19];

int8_t NUC_StartSending(fp32 *data) {

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

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

  SendBuffer[0] = 0xFC; //发送ID
  SendBuffer[1] = 0x01; //控制帧
  for(int i = 2; i < 18; i++)
  {
    SendBuffer[i] = instance.data[i-2];
  }
  SendBuffer[18] = 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,500) ==
          SIGNAL_NUC_RAW_REDY);
}


int8_t NUC_RawParse(CMD_NUC_t *n){
	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 MID://控制帧0x09
//				/* 协议格式 
//		   0xFF  HEAD
//		   0x09  控制帧
//		   0x01  相机帧
//		   vx    fp32
//		   vy    fp32
//		   wz    fp32
//		   0xFE  TAIL
//		  */
////		if(nucbuf[15]!=TAIL)goto error;
//       instance.data[3] = nucbuf[6];
//       instance.data[2] = nucbuf[5];
//       instance.data[1] = nucbuf[4];
//       instance.data[0] = nucbuf[3];
//      n->navi.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->navi.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->navi.wz = 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->navi.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->navi.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->navi.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->navi.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->navi.angle  = instance.x[4];//

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


    }
     return DEVICE_OK;
  }
    error:
    drop_message++;
    return DEVICE_ERR;
}

int8_t NUC_HandleOffline(CMD_NUC_t *cmd)
{
	if(cmd == NULL) return DEVICE_ERR_NULL;
	 memset(cmd, 0, sizeof(*cmd));
	return 0;
}