#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;

static uint8_t nucbuf[32];

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;
}
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);
}

 	union 
	{
		float x[3];
		char data[12];
	}instance;
int8_t NUC_RawParse(CMD_NUC_t *n){
	if(n ==NULL) return DEVICE_ERR_NULL;

  if(nucbuf[0]!=HEAD) goto error;  
  else{
    n->status_fromnuc =nucbuf[1];
		n->ctrl_status =nucbuf[2];
    switch (n->status_fromnuc)
    {
		case MID://控制帧0x09
				/* 协议格式 
		   0xFF  HEAD
		   0x0X  控制帧
		   0x01  相机帧
		   vx    fp32
		   vy    fp32
		   wz    fp32
		   0xFE  TAIL
	使用的是串口1
		  */
		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 PICK:
			/* 协议格式 
		   0xFF  HEAD
		   0x06  控制帧
		   0x01  相机帧
		   cmd   8位
		   dis   相机深度值
		   posx  相机yaw轴值
		   posy  相机pitch轴值
		   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->pick.posx = instance.x[0]; //距离球中心的角度值
       instance.data[7] = nucbuf[10];
       instance.data[6] = nucbuf[9];
       instance.data[5] = nucbuf[8];
       instance.data[4] = nucbuf[7];
		 n->pick.posy = instance.x[1];// 相机yaw轴
		   instance.data[11]  = nucbuf[14];
		   instance.data[10]  = nucbuf[13];
		   instance.data[9]  = nucbuf[12];
		   instance.data[8]  = nucbuf[11];
		 n->pick.posw= instance.x[2];// 暂未用到
       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;
}