/**@brief Thread for handling the Application's BLE Stack events.
 *
 * @details This thread is responsible for handling BLE Stack events sent from on_ble_evt().
 *
 * @param[in]   arg   Pointer used for passing some arbitrary information (context) from the
 *                    osThreadCreate() call to the thread.
 */
void ble_stack_thread(void const * arg)
{
    uint32_t    err_code;
    osEvent     evt;
    ble_evt_t * p_ble_evt;

    UNUSED_PARAMETER(arg);

    while (1)
    {
        evt = osMessageGet(ble_stack_msg_box, osWaitForever); // wait for message

        if (evt.status == osEventMessage)
        {
            p_ble_evt = evt.value.p;

            switch (p_ble_evt->header.evt_id)
            {
                case BLE_GAP_EVT_CONNECTED:

                    err_code = bsp_indication_set(BSP_INDICATE_CONNECTED);
                    APP_ERROR_CHECK(err_code);
                    m_conn_handle = p_ble_evt->evt.gap_evt.conn_handle;
                    break;

                default:
                    // No implementation needed.
                    break;
            }
            (void)osPoolFree(ble_evt_pool, p_ble_evt);
        }
    }
}

static void ecg_2_lead_gui_tick(state_ptr state)
{
  uint32_t i = 0;
  osEvent event;

  uint32_t bpm = 0;
  char str_bpm[4] = "-?-";

  int32_t lead_I = 0, lead_II = 0;

  /* Do transition actions */
  /* Get bpm data */
  event = osMessageGet(queue_ecg_bpm_screenHandle, 0);
  if (event.status == osEventMessage)
  {
    /* Retrieve value */
    bpm = (((uint32_t) event.value.v) >> 10);
    sprintf(str_bpm, "%u", bpm);

    item_area_set_text(&menu_ecg.items[1].item.area, str_bpm);
    while (osMailPut(queue_lcdHandle, (void *) &menu_ecg.items[1]) != osOK)
    {
      osDelay(1);
    }
  }

/**
  * @brief  Storage Thread
  * @param  argument: pointer that is passed to the thread function as start argument.
  * @retval None
  */
static void StorageThread(void const * argument)
{
  osEvent event;
  
  for( ;; )
  {
    event = osMessageGet( StorageEvent, osWaitForever );
    
    if( event.status == osEventMessage )
    {
      switch(event.value.v)
      {
      case USBDISK_CONNECTION_EVENT:
        f_mount(&USBDISK_FatFs,USBDISK_Drive,  0);
        StorageStatus[USB_DISK_UNIT] = 1;
        break;
        
      case USBDISK_DISCONNECTION_EVENT:
        f_mount(0, USBDISK_Drive, 0);
        StorageStatus[USB_DISK_UNIT] = 0;
        break;  
      }
    }
  }
}

// Handle command event processing.
void commandProcessingThread(void const *argument)
{
	UINT8 currentIndex;

	for (;;)
	{
		// Wait for a command message.
		osEvent commandEvent = osMessageGet(commandQueue, osWaitForever);

		// Is this a command event?
		if (commandEvent.status == osEventMessage)
		{
			// Point to the message.
			commandMessage *message = (commandMessage *) commandEvent.value.p;

			// Execute all the commands
			currentIndex = 0;
			while (currentIndex < message->length) {
				currentIndex += handleCommand(message, currentIndex);
			}

			// Free the message.
			osPoolFree(commandMemPool, message);
		}
	}
}

/**
  * @brief  Message Queue Consumer Thread.
  * @param  argument: Not used
  * @retval None
  */
static void MessageQueueConsumer (const void *argument)
{
  osEvent event;
  
  for(;;)
  {
    /* Get the message from the queue */
    event = osMessageGet(osQueue, 100);
    
    if(event.status == osEventMessage)
    {
      if(event.value.v != ConsumerValue)
      {
        /* Catch-up. */
        ConsumerValue = event.value.v;
        
        /* Toggle LED3 to indicate error */
        BSP_LED_Toggle(LED3);
      }
      else
      {  
        /* Increment the value we expect to remove from the queue next time
        round. */
        ++ConsumerValue;
      }			
    }		
  }
}

/*---------------------------------------------------------------------------*
 * Routine:  sys_arch_mbox_tryfetch
 *---------------------------------------------------------------------------*
 * Description:
 *      Similar to sys_arch_mbox_fetch, but if message is not ready
 *      immediately, we'll return with SYS_MBOX_EMPTY.  On success, 0 is
 *      returned.
 * Inputs:
 *      sys_mbox_t mbox         -- Handle of mailbox
 *      void **msg              -- Pointer to pointer to msg received
 * Outputs:
 *      u32_t                   -- SYS_MBOX_EMPTY if no messages.  Otherwise,
 *                                  return ERR_OK.
 *---------------------------------------------------------------------------*/
u32_t sys_arch_mbox_tryfetch(sys_mbox_t *mbox, void **msg) {
    osEvent event = osMessageGet(mbox->id, 0);
    if (event.status != osEventMessage)
        return SYS_MBOX_EMPTY;
    
    *msg = (void *)event.value.v;
    
    return ERR_OK;
}

void MainTask()
{	
	for(;;)
	{
		// Wait for adc result and write it to the UART connected to the PC.
		osEvent result = osMessageGet(mainTaskMessageQId, osWaitForever);
		HAL_UART_Transmit(&huart2, (uint8_t*)&result.value.v, 4, 100);
	}
}

/**
  * @brief  Audio task
  * @param  argument: pointer that is passed to the thread function as start argument.
  * @retval None
  */
static void Audio_Thread(void const * argument)
{
  uint32_t numOfReadBytes;    
  osEvent event;  
  for(;;)
  {
    event = osMessageGet(AudioEvent, 100 );
    
    if( event.status == osEventMessage )
    {
      if(haudio.state == AUDIOPLAYER_PLAY)
      {
        switch(event.value.v)
        {
        case BUFFER_OFFSET_HALF:
          if(f_read(&wav_file, 
                    &haudio.buffer[0], 
                    AUDIO_BUFFER_SIZE/2, 
                    (void *)&numOfReadBytes) == FR_OK)
          { 
            if(numOfReadBytes == 0)
            {  
              haudio.state = AUDIOPLAYER_EOF;
            } 
          }
          else
          {
              haudio.state = AUDIOPLAYER_ERROR;    
          }
          break;  
          
        case BUFFER_OFFSET_FULL:
          if(f_read(&wav_file, 
                    &haudio.buffer[AUDIO_BUFFER_SIZE/2], 
                    AUDIO_BUFFER_SIZE/2, 
                    (void *)&numOfReadBytes) == FR_OK)
          { 
            if(numOfReadBytes == 0)
            { 
              haudio.state = AUDIOPLAYER_EOF;                     
            } 
          }
          else
          {
              haudio.state = AUDIOPLAYER_ERROR;   
          }
          break;   
          
        default:
          break;
        }
      }
      
    }
  }
}

/**@brief Thread for handling the Application's BLE Stack events.
 *
 * @details This thread is responsible for handling BLE Stack events sended from on_ble_evt().
 *
 * @param[in]   arg   Pointer used for passing some arbitrary information (context) from the
 *                    osThreadCreate() call to the thread.
 */
void ble_stack_thread(void const * arg)
{
    uint32_t    err_code;
    osEvent     evt;
    ble_evt_t * p_ble_evt;

    UNUSED_PARAMETER(arg);

    while (1)
    {
        evt = osMessageGet(ble_stack_msg_box, osWaitForever); // wait for message

        if (evt.status == osEventMessage)
        {
            p_ble_evt = evt.value.p;

            switch (p_ble_evt->header.evt_id)
            {
                case BLE_GAP_EVT_CONNECTED:

                    err_code = bsp_indication_set(BSP_INDICATE_CONNECTED);
                    APP_ERROR_CHECK(err_code);
                    m_conn_handle = p_ble_evt->evt.gap_evt.conn_handle;
                    break;

                case BLE_GAP_EVT_DISCONNECTED:
                    err_code = bsp_indication_set(BSP_INDICATE_IDLE);
                    APP_ERROR_CHECK(err_code);
                    advertising_start();
                    break;

                case BLE_GAP_EVT_TIMEOUT:

                    if (p_ble_evt->evt.gap_evt.params.timeout.src ==
                        BLE_GAP_TIMEOUT_SRC_ADVERTISING)
                    {
                        err_code = bsp_indication_set(BSP_INDICATE_IDLE);
                        APP_ERROR_CHECK(err_code);
                        // enable buttons to wake-up from power off
                        bsp_buttons_enable( (1 <<
                                             WAKEUP_BUTTON_ID) | (1 << BOND_DELETE_ALL_BUTTON_ID) );
                        // Go to system-off mode (this function will not return; wakeup will cause a reset).
                        err_code = sd_power_system_off();
                        APP_ERROR_CHECK(err_code);
                    }
                    break;

                default:
                    // No implementation needed.
                    break;
            }
            (void)osPoolFree(ble_evt_pool, p_ble_evt);
        }
    }
}

/*---------------------------------------------------------------------------*
 * Routine:  sys_arch_mbox_fetch
 *---------------------------------------------------------------------------*
 * Description:
 *      Blocks the thread until a message arrives in the mailbox, but does
 *      not block the thread longer than "timeout" milliseconds (similar to
 *      the sys_arch_sem_wait() function). The "msg" argument is a result
 *      parameter that is set by the function (i.e., by doing "*msg =
 *      ptr"). The "msg" parameter maybe NULL to indicate that the message
 *      should be dropped.
 *
 *      The return values are the same as for the sys_arch_sem_wait() function:
 *      Number of milliseconds spent waiting or SYS_ARCH_TIMEOUT if there was a
 *      timeout.
 *
 *      Note that a function with a similar name, sys_mbox_fetch(), is
 *      implemented by lwIP.
 * Inputs:
 *      sys_mbox_t mbox         -- Handle of mailbox
 *      void **msg              -- Pointer to pointer to msg received
 *      u32_t timeout           -- Number of milliseconds until timeout
 * Outputs:
 *      u32_t                   -- SYS_ARCH_TIMEOUT if timeout, else number
 *                                  of milliseconds until received.
 *---------------------------------------------------------------------------*/
u32_t sys_arch_mbox_fetch(sys_mbox_t *mbox, void **msg, u32_t timeout) {
    u32_t start = us_ticker_read();
    
    osEvent event = osMessageGet(mbox->id, (timeout != 0)?(timeout):(osWaitForever));
    if (event.status != osEventMessage)
        return SYS_ARCH_TIMEOUT;
    
    *msg = (void *)event.value.v;
    
    return (us_ticker_read() - start) / 1000;
}

// Receive the 'data' in the Message 'queue'. Returns 1 if a message was received, 0 otherwise.
int receive_message(float * data, osMessageQId queue){
	osEvent evt = osMessageGet(queue, 0); 
	if (evt.status == osEventMessage) {
		Message* msg = ((Message *)evt.value.p);
		*data = msg->data;			
		osPoolFree(mem_pool, msg);
		
		return 1;
	}
	
	return 0;
}

void blinkThread(void const *argument) {
  uint16_t delay = 500; /* Default delay */
  osEvent evt;

  while(1) {
    evt = osMessageGet(MsgBox, 1);
    if(evt.status == osEventMessage)
      delay = evt.value.v;

    HAL_GPIO_TogglePin(LD2_GPIO_Port, LD2_Pin);
    osDelay(delay);
  }
}

static void demo_task1(void *arg)
{
    osEvent event;

    while (1) {
        event = osMessageGet(p_msgqueue, osWaitForever);
        if (event.status == osEventMessage) {
            printf("demo_task1 get msg %d\n", event.value.v);
        } else {
            printf("demo_task1 get msg failed\n");
        }
    };
}

/**
  * @brief  USB Host Thread task
  * @param  pvParameters not used
  * @retval None
  */
static void USBH_Process_OS(void const * argument)
{
  osEvent event;
  
  for(;;)
  {
    event = osMessageGet(((USBH_HandleTypeDef *)argument)->os_event, osWaitForever );
    
    if( event.status == osEventMessage )
    {
      USBH_Process((USBH_HandleTypeDef *)argument);
    }
   }
}

/// Get a mail from a queue
osEvent osMailGet(osMailQId queue_id, uint32_t millisec)
{
  osEvent ret;

  if (queue_id == NULL) {
    ret.status = osErrorParameter;
    return ret;
  }

  ret = osMessageGet(*((void **)queue_id), millisec);
  if (ret.status == osEventMessage) ret.status = osEventMail;

  return ret;
}

/**
  * @brief  USBH_TEMPLATE_Init 
  *         The function Initialize the TEMPLATE function
  * @param  phost: Host handle
  * @retval USBH Status
  */
USBH_StatusTypeDef USBH_TEMPLATE_Init (USBH_HandleTypeDef *phost)
{
  USBH_StatusTypeDef Status = USBH_BUSY;
#if (USBH_USE_OS == 1)
  osEvent event;
  
  event = osMessageGet( phost->class_ready_event, osWaitForever );
  
  if( event.status == osEventMessage )      
  {
    if(event.value.v == USBH_CLASS_EVENT)
    {
#else 
      
  while ((Status == USBH_BUSY) || (Status == USBH_FAIL))
  {
    /* Host background process */
    USBH_Process(phost);
    if(phost->gState == HOST_CLASS)
    {
#endif        
      Status = USBH_OK;
    }
  }
  return Status;   
}

/**
  * @brief  USBH_TEMPLATE_IOProcess 
  *         TEMPLATE TEMPLATE process
  * @param  phost: Host handle
  * @retval USBH Status
  */
USBH_StatusTypeDef USBH_TEMPLATE_IOProcess (USBH_HandleTypeDef *phost)
{
  if (phost->device.is_connected == 1)
  {
    if(phost->gState == HOST_CLASS)
    {
      USBH_TEMPLATE_Process(phost);
    }
  }
  
  return USBH_OK;
}

/*----------------------------------------------------------------------------
 *  Thread 2: Receive thread
 *---------------------------------------------------------------------------*/
void recv_thread (void const *argument) {
  T_MEAS  *rptr;
  osEvent  evt;

  for (;;) {
    evt = osMessageGet(MsgBox, osWaitForever);  /* wait for message          */
    if (evt.status == osEventMessage) {
      rptr = evt.value.p;
      printf ("\nVoltage: %.2f V\n",rptr->voltage);
      printf ("Current: %.2f A\n",rptr->current);
      printf ("Number of cycles: %d\n",(int)rptr->counter);
      #ifdef __USE_FFLUSH
      fflush (stdout);
      #endif
      osPoolFree(mpool,rptr);         /* free memory allocated for message   */
    }
  }
}

/**************************************************************************//**
 * @brief
 *   Thread 1: Print LCD thread - modified to not use LCD
 *****************************************************************************/
void PrintLcdThread(void const *argument)
{
  lcdText_t *rptr;
  osEvent   evt;
  (void) argument;                 /* Unused parameter. */

  while (1)
  {
    /* Wait for message */
    evt = osMessageGet(msgBox, osWaitForever);
    if (evt.status == osEventMessage)
    {
      rptr = evt.value.p;
      /* Free memory allocated for message */
      osPoolFree(mpool, rptr);
    }
  }
}

/**
  * @brief  Start task
  * @param  pvParameters not used
  * @retval None
  */
static void StartThread(void const * argument)
{
  osEvent event;
  
  /* Init MSC Application */
  MSC_InitApplication();
  
  /* Init Host Library */
  USBH_Init(&hUSBHost, USBH_UserProcess, 0);
  
  /* Add Supported Class */
  USBH_RegisterClass(&hUSBHost, USBH_MSC_CLASS);
  
  /* Start Host Process */
  USBH_Start(&hUSBHost);
  
  /* Register the file system object to the FatFs module */
  if(f_mount(&USBH_fatfs, "", 0) != FR_OK)
  {  
    LCD_ErrLog("ERROR : Cannot Initialize FatFs! \n");
  }
  
  for( ;; )
  {
    event = osMessageGet(AppliEvent, osWaitForever);
    
    if(event.status == osEventMessage)
    {
      switch(event.value.v)
      {  
      case APPLICATION_DISCONNECT:
        Appli_state = APPLICATION_DISCONNECT;
        osSemaphoreRelease(MenuEvent);
        break;
        
      case APPLICATION_READY:
        Appli_state = APPLICATION_READY;
        
      default:
        break;
      }
    }
  }
}

static void LogCollector_Process_OS(void const * argument)
{
  osEvent event;

  for(;;)
  {
    event = osMessageGet(((EventLogger_Handle_TypeDef *)argument)->os_event, osWaitForever );

    //if(((EventLogger_Handle_TypeDef *)argument)-> LogStatus == LOG_BUFFER_FULL)

    if( event.status == osEventMessage  )
    {
    	((EventLogger_Handle_TypeDef *)argument)->pLogData = &Log_List[event.value.v];
    	((EventLogger_Handle_TypeDef*) argument)->Interface->WriteLog((EventLogger_Handle_TypeDef*) argument);
    	((EventLogger_Handle_TypeDef *)argument)->pLogData = 0;
    	LogIndex_List[event.value.v] = 0;
    }

    //osDelay(0);
   }
}