#include "user_task.h"
#include "nuc.h"

#ifdef DEBUG

NUC_t nuc_raw;
CMD_NUC_t cmd_fromnuc;
AHRS_Quaternion_t quat;
AHRS_Accl_t accl;
AHRS_Gyro_t gyro;

NUC_send_t NUC_send;
#else 
static NUC_t nuc_raw;
static CMD_NUC_t cmd_fromnuc;
static AHRS_Quaternion_t quat;
static AHRS_Accl_t accl;
static AHRS_Gyro_t gyro;
NUC_send_t NUC_send;

#endif
fp32 send[4]={11.0f,45.0,1.f,4.0f};
int a=0;


void Task_nuc(void *argument){
 (void)argument; /**/

	 osDelay(TASK_INIT_DELAY_NUC); 
  const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_NUC;

   NUC_Init(&nuc_raw);

   uint32_t tick = osKernelGetTickCount();
   uint32_t last_online_tick = tick;
   while (1) 
		 {	 
#ifdef DEBUG
    task_runtime.stack_water_mark.nuc= osThreadGetStackSpace(osThreadGetId());    
#endif
			 a++;
			NUC_StartReceiving();
      if (NUC_WaitDmaCplt()){ 
         NUC_RawParse(&cmd_fromnuc, &nuc_raw);
         last_online_tick = tick;
      }
      else{
        if (tick - last_online_tick > 300) NUC_HandleOffline(&cmd_fromnuc, &nuc_raw);
      }

      if (nuc_raw.unc_online) {
        osMessageQueueReset(task_runtime.msgq.cmd.raw.nuc);
        osMessageQueuePut(task_runtime.msgq.cmd.raw.nuc,&(cmd_fromnuc),0,0);
      }

      osMessageQueueGet(task_runtime.msgq.nuc.quat, &(quat), NULL, 0);
      osMessageQueueGet(task_runtime.msgq.nuc.accl, &(accl), NULL, 0);
      osMessageQueueGet(task_runtime.msgq.nuc.gyro, &(gyro), NULL, 0);

		//  osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc_send, &NUC_send, NULL, 0);

     bool cmd_update = (osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc_send,
                                         &(NUC_send), NULL, 0) == osOK);

      NUC_PackIMU(&nuc_raw, &quat, &accl, &gyro);
      if (cmd_update) {
        NUC_PackCMD(&nuc_raw,&NUC_send);
      }


    // NUC_StartSend(&nuc_raw, cmd_update);

		 tick += delay_tick; /* 计算下一个唤醒时刄1ķ*/
		 osDelayUntil(tick);
		 }
		
}