#include "nuc.h"
#include "crc16.h"
#include <string.h>
#include "define.h"
#include "error_detect.h"

static volatile uint32_t drop_message = 0;

static osThreadId_t thread_alert;

uint8_t nucbuf[32];     //接收缓存区
uint8_t SendBuffer[11]; //发送缓存区
int b=0;
int c =0;
int e=0;
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;
}

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

//发送函数
int8_t NUC_Send(fp32 *data) {
  union {
		float x[2];
		uint8_t  data[8];
	}instance;
  for (int i = 0; i < 2; i++) {
    instance.x[i] = data[i];
  }
  SendBuffer[0] = 0xFC; //发送ID
  SendBuffer[1] = 0x01; //控制帧
	
  for(int i = 2; i < 10; i++){
   SendBuffer[i] = instance.data[i-2];
  }
  SendBuffer[10] = 0xFD; //结束符

  if (HAL_UART_Transmit(BSP_UART_GetHandle(BSP_UART_NUC),
      (uint8_t *)SendBuffer,sizeof(SendBuffer),1000) == HAL_OK){
				b++;
		return DEVICE_OK;	
	}   
	return DEVICE_ERR;
}

//发送控制函数
void NUC_Send_control(CMD_NUC_t *n,CMD_t *cmd,Chassis_t *chassis) {
	static fp32 send_data[2];
	
	//send_data[0]：校准标志位         1：sick校准成功 
	//send_data[1]：锁框锁球标志位     0：锁框 1：锁车
	
	  //启动校准		 
    if (chassis->radar_reset_flag == 1) {
      send_data[0] = 1;
      send_data[1] = 0;
      NUC_Send(send_data); 		 				
     } 
		 //锁框模式
		else if(cmd->C_cmd.communicate == NO){
      send_data[0] = 0;
      send_data[1] = 0;
      NUC_Send(send_data); 			 			 
     } 		
		 //传球模式
		else if(cmd->C_cmd.communicate == YES){
      send_data[0] = 0;
      send_data[1] = 1;	
      NUC_Send(send_data);  	 	 
		 }
}

//接收函数
/*车偏左需右旋，角度为正     车偏右需左旋，角度为负*/
int8_t NUC_RawParse(CMD_NUC_t *n) {
  if (n == NULL) return DEVICE_ERR_NULL;
  union {
    float x[3];
    char data[12];
  } instance; // 方便在浮点数和字符数组之间进行数据转换

 if(nucbuf[0]!=HEAD) goto error;  //发送ID不是底盘
 else{
#ifdef nuc_1  
	 /* 协议格式 
      0xFF  HEAD
      0x09  控制帧
      0x01  相机帧
      flag  fp32
      angle fp32
      yaw   fp32
      0xFE  TAIL
    */
    if (nucbuf[15] != TAIL) goto error;
       instance.data[0] = nucbuf[3];
       instance.data[1] = nucbuf[4];
       instance.data[2] = nucbuf[5];
       instance.data[3] = nucbuf[6];
       n->nuc.flag = instance.x[0];

       instance.data[4] = nucbuf[7];
       instance.data[5] = nucbuf[8];
       instance.data[6] = nucbuf[9];
       instance.data[7] = nucbuf[10];
       n->nuc.angle = instance.x[1]/3.1415926535f*360;
					
       instance.data[8] = nucbuf[11];
       instance.data[9] = nucbuf[12];
       instance.data[10] = nucbuf[13];
       instance.data[11] = nucbuf[14];
			 n->nuc.yaw = instance.x[2];
			//将 n->nuc.yaw映射到（0，360）
       n->nuc.yaw = instance.x[2]/3.1415926535f*180;
			 if(n->nuc.yaw<0)	n->nuc.yaw +=360;	
#elif defined(nuc_2)
	 /* 协议格式 
      0xFF         HEAD
      angle        fp32     //框的角度
      yaw          fp32     //r2的角度
      0xFE         TAIL
   */
	     if (nucbuf[17] != TAIL) goto error;
       instance.data[0] = nucbuf[1];
       instance.data[1] = nucbuf[2];
       instance.data[2] = nucbuf[3];
       instance.data[3] = nucbuf[4];
       n->nuc.angle = (instance.x[0] != 0.0f) ? (instance.x[0]/3.1415926535f*360) : 0.0f;

       instance.data[4] = nucbuf[9];
       instance.data[5] = nucbuf[10];
       instance.data[6] = nucbuf[11];
       instance.data[7] = nucbuf[12];
       n->nuc.yaw = (instance.x[2] != 0.0f) ? (instance.x[2]/3.1415926535f*360) : 0.0f;
    
#endif
			 //接收正常时绿灯翻转	
       HAL_GPIO_TogglePin(GPIOH,LED_G_Pin);						
		   e++;				 
   return DEVICE_OK;
  }

error:
	c++;
	//接收不正常时红灯翻转	
  HAL_GPIO_TogglePin(GPIOH,LED_R_Pin);	
  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;
}