/*!
 @brief Thread to display temperature
 @param argument Unused
*/
void temperature_DispFlag_thread(void const * argument)
{
	// Extracts the semaphore object from the argument.
	// This semaphore object was created in the main
	osSemaphoreId* semaphore;
	semaphore = (osSemaphoreId*)argument;
	
	uint8_t ResourceLocked = 1;  // Flag gets turned on if semaphore_lock achieved
	uint32_t tokentemp;          // Return of semwait
	
	// Wait for the semaphore with 1ms timeout
  osEvent event = osSignalWait(SIGNAL_DISP_TEMPERATURE, 1);

	while(1)
	{
		tokentemp=osSemaphoreWait (*semaphore, osWaitForever);
    ResourceLocked=1; // turn flag on if resource_locked		
		
    while (ResourceLocked)
		{
			// wait for a signal with 1ms timeout
			event = osSignalWait(SIGNAL_DISP_TEMPERATURE, 1);
			if (event.status == osEventTimeout)
			{
				// in case of timeout display
				LCD_DisplayTemperature(temp);
			}
			else
			{
				// signal received which is not timeout
				// clear the signal flag for the tilt display thread
				osSignalClear (tilt_TurnDispFlag_thread, SIGNAL_DISP_TILT);
				
				//clear the screen
				LCD_clear_display();
				
				// turn the resource_locked flag off
				ResourceLocked = 0;
				tokentemp=0;
				
				// turn the flag to display the string "temperature" once
				GLB_Temperature_display_flag=1;
				
				// release the semaphore 
				osSemaphoreRelease(*semaphore);
				
				//wait 5ms for the other thread to grab the semaphore
			  osDelay(5);
			}
		}
		
	}
	
}

void blinkLED(void const *argument) {
  while (1) {
    __GPIO_WRITE(GPIOA, 5, GPIO_PIN_SET);
    osSignalWait(0x0001, osWaitForever);
    __GPIO_WRITE(GPIOA, 5, GPIO_PIN_RESET);
    osSignalWait(0x0001, osWaitForever);
    
    //sprintf(Buf, "%x\r\n", serbuff);
    //HAL_UART_Transmit(&huart2, (uint8_t *) Buf, strlen(Buf), 0xffffffff);
  }
}

/*----------------------------------------------------------------------------
 *      Thread  'Accelerometer': Reads Accelerometer
 *---------------------------------------------------------------------------*/
void Thread_ACCELEROMETER (void const *argument) 
{		
	/* These are preserved between function calls (static) */
	KalmanState kstate_pitch = {0.001, 0.0032, 0.0, 0.0, 0.0};	/* Filter parameters obtained by experiment and variance calculations */
	KalmanState kstate_roll  = {0.001, 0.0032, 0.0, 0.0, 0.0};	/* Filter parameters obtained by experiment and variance calculations */
	
	float ax, ay, az;
	float roll, pitch;

	osTimerId doubletap_timer_id, accel_dataready_timer_id;
	
	doubletap_timer_id = osTimerCreate(osTimer(doubletap_timer), osTimerOnce, NULL);
	accel_dataready_timer_id = osTimerCreate(osTimer(accel_dataready_timer), osTimerOnce, NULL);
	
	while(1)
	{
		/* Wait for accelerometer new data signal or Nucleo board SPI signal for read request.
		   No need to send data all the time. */
		osSignalWait(ACCELEROMETER_SIGNAL, osWaitForever);

		Accelerometer_ReadAccel(&ax, &ay, &az);
		Accelerometer_Calibrate(&ax, &ay, &az);
					
		Accelerometer_GetRollPitch(ax, ay, az, &pitch, &roll);		
		Kalmanfilter_asm(&pitch, &filtered_pitch, 1, &kstate_pitch);	/* filter the pitch angle */
		Kalmanfilter_asm(&roll, &filtered_roll, 1, &kstate_roll);			/* filter the roll angle 	*/
				
		if (Accelerometer_DetectDoubletap(sqrtf(ax * ax + ay * ay + az * az)))
			NucleoSPI_SetDoubletap(doubletap_timer_id, DOUBLETAP_TIMEOUT_MS);

		NucleoSPI_SetAccelDataready(accel_dataready_timer_id, ACCEL_DATAREADY_TIMEOUT_MS);
	}
}

/*----------------------------------------------------------------------------
 *      Main: Initialise and start RTX Kernel
 *---------------------------------------------------------------------------*/
int main(void) {

   // Get main thread ID
   mainFunctionId = osThreadGetId();

   // Create thread
   threadFunctionId = osThreadCreate(osThread(threadFunction), NULL);
   if (threadFunctionId == NULL) {
      __asm__("bkpt");
   }

// Need suitable features for the stdio & leds
//   printf("Main thread ID = %d\n", (int)mainFunctionId);
//   printf("Thread ID      = %d\n", (int)threadFunctionId);

//   led_initialise();

   for (;;) {
      // Wait for signal from thread
      osSignalWait(SIGNAL_MASK, osWaitForever);

//      printf("Thread signaled\n");
//      greenLedToggle();
   }

}

 /*----------------------------------------------------------------------------
*      Thread  'SEGMENT': Display values on 7-segment display
 *---------------------------------------------------------------------------*/
void Thread_SEGMENT (void const *argument) 
{
	int counter = 0;
	DisplayMode mode;
	
	while(1)
	{
		osSignalWait(SEGMENT_SIGNAL, osWaitForever);
		
		if (counter % FLASH_PERIOD == 0)
		{
			if (SevenSegment_GetFlashing()) {
				osMutexWait(segment_mutex, osWaitForever);
				activated = !activated;
				osMutexRelease(segment_mutex);
			}
		}
		
		mode = SevenSegment_GetDisplayMode();
		
		if (mode == TEMP_MODE) {
			SevenSegment_ToggleDisplayedDigit_Angle();
		} else if (mode == ANGLE_MODE) {
			SevenSegment_ToggleDisplayedDigit_Temp();
		}

		counter++;
	}
}

/*----------------------------------------------------------------------------
  Task 2 'ledOff': switches the LED off
 *---------------------------------------------------------------------------*/
 void ledOff (void const *argument) {
  for (;;) {
    osSignalWait (0x0001, osWaitForever); /* wait for an event flag 0x0001   */
    osDelay(800);                         /* delay 800ms                     */
    LED_Off(0);                           /* Turn LED Off                    */
  }
}

/**
* @brief Main loop for the adc
* @param None
* @retval None 
*/
void Thread_ADC (void const *argument) {
		while(1){
			osSignalWait (ADC_FLAG,osWaitForever); 
			osSignalClear(tid_Thread_ADC,ADC_FLAG); 
			poll();
		}
}

/**
\brief Test case: TC_MutexTimeout
\details
- Create and initialize a mutex object
- Create a thread that acquires a mutex but never release it
- Wait for mutex release until timeout
*/
void TC_MutexTimeout (void) {
  osThreadId ctrl_id, lock_id;
  osEvent    evt;

  /* Get control thread id */
  ctrl_id = osThreadGetId ();
  ASSERT_TRUE (ctrl_id != NULL);
  
  if (ctrl_id != NULL) {
    /* - Create and initialize a mutex object */
    G_MutexId = osMutexCreate (osMutex (MutexTout));
    ASSERT_TRUE (G_MutexId != NULL);
    
    if (G_MutexId != NULL) {
      /* - Create a thread that acquires a mutex but never release it */
      lock_id = osThreadCreate (osThread (Th_MutexLock), &ctrl_id);
      ASSERT_TRUE (lock_id != NULL);
      
      if (lock_id != NULL) {
        /* - Wait for mutex release until timeout */
        ASSERT_TRUE (osMutexWait (G_MutexId, 10) == osErrorTimeoutResource);
        /* - Release a mutex */
        osSignalSet (lock_id, 0x01);
        evt = osSignalWait (0x01, 100);
        ASSERT_TRUE (evt.status == osEventSignal);
        /* - Terminate locking thread */
        ASSERT_TRUE (osThreadTerminate (lock_id)  == osOK);
      }
      /* Delete mutex object */
      ASSERT_TRUE (osMutexDelete (G_MutexId)  == osOK);
    }
  }
}

/*----------------------------------------------------------------------------
  Thread 3 'lights': executes if current time is between start & end time
 *---------------------------------------------------------------------------*/
void lights (void const *argument) {          /* traffic light operation     */
  SetLights (RED, 1);                         /* RED & STOP lights on        */
  SetLights (STOP, 1);
  SetLights (YELLOW, 0);
  SetLights (GREEN, 0);
  SetLights (WALK, 0);
  while (1) {                                 /* endless loop                */
    osDelay(500);                             /* wait for timeout: 500ms     */
    if (!signalon ()) {                       /* if traffic signal time over */
      tid_blinking = osThreadCreate(osThread(blinking), NULL);
                                              /* start blinking              */
      return;                                 /* STOP lights                 */
    }
    SetLights (YELLOW, 1);
    osDelay(500);                             /* wait for timeout: 500ms     */
    SetLights (RED, 0);                       /* GREEN light for cars        */
    SetLights (YELLOW, 0);
    SetLights (GREEN, 1);
    osSignalClear(tid_lights, 0x0010);
    osDelay(500);                             /* wait for timeout: 500ms     */
    osSignalWait(0x0010, 7500);               /* wait for event with timeout */
    SetLights (YELLOW, 1);                    /* timeout value: 7.5s         */
    SetLights (GREEN, 0);
    osDelay(500);                             /* wait for timeout: 500ms     */
    SetLights (RED, 1);                       /* RED light for cars          */
    SetLights (YELLOW, 0);
    osDelay(500);                             /* wait for timeout: 500ms     */
    SetLights (STOP, 0);                      /* GREEN light for WALKers     */    
    SetLights (WALK, 1);
    osDelay(2500);                            /* wait for timeout: 2.5s      */
    SetLights (STOP, 1);                      /* RED light for WALKers       */        
    SetLights (WALK, 0);
  }
}

void oModeTransThread(void const *args)
{
  while (1)
  {
    osSignalWait(SIG_START, osWaitForever);
    send_block_angles(totalNum, _rolls, _pitches, _deltas);
  }  
}

/*!
	Get pressed key after keypad interrupt occurs
 */
void Keypad(void const *argument){
	while (1){
		//wait until called
		osSignalWait(SIGNAL_KEYPAD, osWaitForever);
		get_press();
		osDelay(150);
	}
}

/*----------------------------------------------------------------------------
  Thread 5 'clock': Signal Clock
 *---------------------------------------------------------------------------*/
void clock (void const *argument) {
  for (;;) {
    osSignalWait(0x0100, osWaitForever);    /* wait for an event flag 0x0100 */
    LED_on (LED_CLK);
    osDelay(80);                            /* delay 80ms                    */
    LED_off(LED_CLK);
  }
}

/*----------------------------------------------------------------------------
 * blinkLED: blink LED and check button state
 *----------------------------------------------------------------------------*/
void blinkLED(void const *argument) {
  int32_t max_num = LED_GetCount();
  int32_t num = 0;

  for (;;) {
    LED_On(num);                                           // Turn specified LED on
    osSignalWait(0x0001, osWaitForever);
    LED_Off(num);                                          // Turn specified LED off
    osSignalWait(0x0001, osWaitForever);

    num++;                                                 // Change LED number
    if (num >= max_num) {
      num = 0;                                             // Restart with first LED
    }
  }

}

/*----------------------------------------------------------------------------
  Thread 1 'phaseA': Phase A output
 *---------------------------------------------------------------------------*/
void phaseA (void const *argument) {
  for (;;) {
    osSignalWait(0x0001, osWaitForever);    /* wait for an event flag 0x0001 */
    LED_on (LED_A);
    signal_func (tid_phaseB);               /* call common signal function   */
    LED_off(LED_A);
  }
}

/*----------------------------------------------------------------------------
 *      Thread 4 'phaseD': Phase D output
 *---------------------------------------------------------------------------*/
void phaseD (void  const *argument) {
  for (;;) {
    osSignalWait(0x0001, osWaitForever);    /* wait for an event flag 0x0001 */
    Switch_On (LED_D);
    signal_func(tid_phaseA);                /* call common signal function   */
    Switch_Off(LED_D);
  }
}

void wait_for_all_signals(thread::signal_set flags)
{
    WEOS_ASSERT(flags > 0 && flags <= thread::all_signals());
    osEvent result = osSignalWait(flags, osWaitForever);
    if (result.status != osEventSignal)
        WEOS_THROW_SYSTEM_ERROR(cmsis_error::cmsis_error_t(result.status),
                                "wait_for_signalflags failed");
}

/**
  * @brief  Accelerometer thread
  * @param  arguments
  * @retval None
  */
void Thread_Accelerometer(void const *argument){
	while(1){
	osSignalWait (data_ready_flag,osWaitForever); 
	osSignalClear(tid_Thread_Accelerometer,data_ready_flag); 
	calculateAngles();
	}
	
}

// display thread handler
void segment_display(void const * arg){
	uint16_t LED_pins[4] = { GPIO_Pin_12, GPIO_Pin_13, GPIO_Pin_14, GPIO_Pin_15 };
	int count = 0;
	char key = DUMMY_KEY, led = '1';
	int mode = TEMP_MODE;
	float temperature = 50.0;
	float pitch = 0.0;
		
	display_init();
	
	float data; 
	
	while(1){
		osSignalWait(DISPLAY_READY, osWaitForever);
				
		// try to get the keypad message		
		if (receive_message(&data, keypad_queue)){
			key = (char)(((int)data) + '0');
			
			// figure out which type of key was pressed (i.e. mode or led) and adjust variables accordingly. 
			if (key == TEMP_MODE_KEY || key == MEMS_MODE_KEY){
				mode = key == TEMP_MODE_KEY ? TEMP_MODE : MEMS_MODE;
			} else {
				led = key;
			}			
		}
		
		// try to get the pitch message
		if (receive_message(&data, pitch_queue)){
			pitch = data;
		}
		
		// try to get the temperature message
		if (receive_message(&data, temp_queue)){
			temperature = data;
		}
				
		GPIO_ResetBits(GPIOD, GPIO_SEGMENT_PINS);
		GPIO_ResetBits(GPIOE, GPIO_DIGIT_SELECT_PINS);
	
		// run the display effect depending on whether or not an alarm is triggerd and the type of mode.
		if (temperature < ALARM_THRESHOLD || (count % (2 * TIM3_DESIRED_RATE)) < TIM3_DESIRED_RATE) {		
			if (mode == TEMP_MODE){
				GPIO_ResetBits(GPIOD, ALL_LED_PINS);
				display_value(temperature, count, TEMP_MODE); 
			} else {
				display_value(pitch, count, MEMS_MODE);
			}
		}
		
		if (mode == MEMS_MODE){
			display_LED(pitch, count, LED_pins[(led - '1')]);
		}
		
		count++;
	}
}

thread::signal_set wait_for_any_signal()
{
    osEvent result = osSignalWait(0, osWaitForever);
    if (result.status != osEventSignal)
        WEOS_THROW_SYSTEM_ERROR(cmsis_error::cmsis_error_t(result.status),
                                "wait_for_any_signal failed");

    return result.value.signals;
}

void StartTask_Uart1Reception (void const *argument) 
{
	extern osMessageQId Q_CmdReceptionHandle;
	
	uint32_t reclen;
	osStatus qretval;								/*!< The return value which indicates the osMessagePut() implementation result */
	uint8_t* ptrdata;								/*!< Pointer to any byte in the uart buffer */
	uint8_t (*ptr_bufhead)[1],			/*!< Pointer to the head of the uart buffer */
					(*ptr_buftail)[1];			/*!< Pointer to the tail of the uart buffer */
	
  while (1) 
	{
    osSignalWait (0x01, osWaitForever);
		
		reclen = MAX_DEPTH_UART1_BUF - huart1.hdmarx->Instance->NDTR;
		ptrdata = *uart_buf + reclen - 1;		// point to the last char received
		ptr_bufhead = (uint8_t(*)[1])uart1_buf[0];
		ptr_buftail = (uint8_t(*)[1])uart1_buf[MAX_COUNT_UART1_BUF - 2];
		
		if(*ptrdata == '\n')
		{
			/* Insert a terminal into the string */
			*(ptrdata + 1) = 0x0;
			
			if(*(--ptrdata) == '\r')	// A command has been received
			{
				/*
				* The current buffer has been used and post to the working thread
				* switch to the next uart1 queue buffer to recevie the furture data
				*/
				qretval = osMessagePut(Q_CmdReceptionHandle, (uint32_t)(uart_buf), 0);	// Put the pointer of the data container to the queue
				if(qretval != osOK)
				{
					__breakpoint(0);
						//printk(KERN_ERR "It's failed to put the command into the message queue!\r\n");
				}

				/* Move to the next row of the buffer */
				uart_buf++;
				if(uart_buf > (uint8_t(*)[50])ptr_buftail)
				{
						uart_buf = (uint8_t(*)[50])ptr_bufhead;
				}
			}
			/* Reset DMA_EN bit can result in the TCIF interrupt.
			The interrupt raises the HAL_UART_RxCpltCallback() event, the DMA_Rx will be restarted in it */
			HAL_DMA_Abort(huart1.hdmarx);
			USART_Start_Receive_DMA(&huart1);
		
		}
		else	/* Continue recepition if the last char is not '\n' */
		{
			__HAL_UART_ENABLE_IT(&huart1, UART_IT_IDLE);
		}
	}
}