bool_t osWaitForEvent(OsEvent *event, systime_t timeout)
{
   int32_t ret;

   //Wait until the specified event is in the signaled
   //state or the timeout interval elapses
   if(timeout == INFINITE_DELAY)
   {
      //Infinite timeout period
      ret = osSemaphoreWait(event->id, osWaitForever);
   }
   else
   {
#if defined(osCMSIS_RTX)
      systime_t n;

      //Loop until the assigned time period has elapsed
      do
      {
         //Limit the timeout value
         n = MIN(timeout, 10000);
         //Wait for the specified time interval
         ret = osSemaphoreWait(event->id, n);
         //Decrement timeout value
         timeout -= n;

         //Check timeout value
      } while(ret == 0 && timeout > 0);
#else
      //Wait for the specified time interval
      ret = osSemaphoreWait(event->id, timeout);
#endif
   }

#if defined(osCMSIS_RTX)
   //Check return value
   if(ret > 0)
   {
      //Force the event back to the nonsignaled state
      while(osSemaphoreWait(event->id, 0) > 0);

      //The specified event is in the signaled state
      return TRUE;
   }
   else
   {
      //The timeout interval elapsed
      return FALSE;
   }
#else
   //Check return value
   if(ret == osOK)
      return TRUE;
   else
      return FALSE;
#endif
}

void Start_input_buttonTask(void const * argument)
{
  GPIO_InitTypeDef GPIO_InitStruct;
  fsm_event_f button_fsm_event;

  /* Take both semaphores for the first time */
  osSemaphoreWait(sem_input_button_short_pressHandle, osWaitForever);
  osSemaphoreWait(sem_input_button_long_pressHandle, osWaitForever);

  /* Infinite loop */
  for(;;)
  {
    /* If I/O button is pressed */
    if (osSemaphoreWait(sem_input_button_short_pressHandle, osWaitForever) == osOK)
    {
      /* Make falling edge sensitive */
      GPIO_InitStruct.Pin = WAKEUP_Pin;
      GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING;
      GPIO_InitStruct.Pull = GPIO_NOPULL;
      HAL_GPIO_Init(WAKEUP_GPIO_Port, &GPIO_InitStruct);

      // TEST
      osSemaphoreRelease(sem_ecg_keygenHandle);

      /* Wait for falling edge */
      if (osSemaphoreWait(sem_input_button_long_pressHandle, 2000) == osErrorOS)
      {
        /* If falling edge is NOT detected before timeout
         * we have a long press
         */
        button_fsm_event = fsm_button_long;
        while(osMailPut(queue_fsm_eventsHandle, (void *) &button_fsm_event) != osOK)
        {
          osDelay(1);
        }
      }
      else
      {
        /* Make rising edge sensitive again */
        GPIO_InitStruct.Pin = WAKEUP_Pin;
        GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
        GPIO_InitStruct.Pull = GPIO_NOPULL;
        HAL_GPIO_Init(WAKEUP_GPIO_Port, &GPIO_InitStruct);

        /* If falling edge is detected before timeout
         * we have a short press
         */
        button_fsm_event = fsm_button_short;
        while(osMailPut(queue_fsm_eventsHandle, (void *) &button_fsm_event) != osOK)
        {
          osDelay(1);
        }
      }
    }
  }
}

void osResetEvent(OsEvent *event)
{
#if defined(osCMSIS_RTX)
   //Force the specified event to the nonsignaled state
   while(osSemaphoreWait(event->id, 0) > 0);
#else
   //Force the specified event to the nonsignaled state
   osSemaphoreWait(event->id, 0);
#endif
}

bool_t osCreateEvent(OsEvent *event)
{
   osSemaphoreDef_t semaphoreDef;

#if defined(osCMSIS_RTX)
   semaphoreDef.semaphore = event->cb;
#else
   semaphoreDef.dummy = 0;
#endif

   //Create a binary semaphore object
   event->id = osSemaphoreCreate(&semaphoreDef, 1);

   //Check whether the returned semaphore ID is valid
   if(event->id != NULL)
   {
      //Force the specified event to the nonsignaled state
      osSemaphoreWait(event->id, 0);
      //Event successfully created
      return TRUE;
   }
   else
   {
      //Failed to create event object
      return FALSE;
   }
}

int main(void)
{
  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();


  /* Initialize all configured peripherals */
  MX_GPIO_Init();

  /* Create the threads and semaphore */
  osThreadDef(defaultTask, StartDefaultTask, osPriorityNormal, 0, 128);
  defaultTaskHandle = osThreadCreate(osThread(defaultTask), NULL);
  osThreadDef(blinkTask, BlinkTask, osPriorityNormal, 0, 128);
  blinkTaskHandle = osThreadCreate(osThread(blinkTask), NULL);
  osSemaphoreDef(sem);
  semHandle = osSemaphoreCreate(osSemaphore(sem), 1);
  osSemaphoreWait(semHandle, osWaitForever);

  /* Start scheduler */
  osKernelStart();

  /* We should never get here as control is now taken by the scheduler */

  /* Infinite loop */
  while (1);

}

void getWirelessAngles(int8_t* angles)
{
	osSemaphoreWait(wirelessAccId, osWaitForever);
	angles[0] =  wirelessAngles[0];
	angles[1] =  wirelessAngles[1];
	osSemaphoreRelease(wirelessAccId);
}

CAN_ERROR CAN_hw_tx_empty (U32 ctrl)  
{
	HAL_CAN_StateTypeDef state;
  CAN_HandleTypeDef *hCAN;
	
	int av = osSemaphoreWait(wr_sem[ctrl-1], 0);
	
  if (av >0)
	{
		switch (ctrl)
		{
	#if RTE_CAN1 == 1
			case __CTRL1:
				hCAN = &hCAN1;
				break;
	#endif
	#if RTE_CAN2 == 1
			case __CTRL2:
				hCAN= &hCan2;
				break;
	#endif
			default:
				return CAN_UNEXIST_CTRL_ERROR;
		}
		state = HAL_CAN_GetState(hCAN);
		if((state == HAL_CAN_STATE_READY) || (state == HAL_CAN_STATE_BUSY_RX))
			return CAN_OK;
		else 
			osSemaphoreRelease(wr_sem[ctrl-1]);    /* Return a token back to semaphore    */
	}

  return CAN_TX_BUSY_ERROR;
}

bool_t osWaitForSemaphore(OsSemaphore *semaphore, systime_t timeout)
{
   int32_t ret;

   //Wait until the semaphore is available or the timeout interval elapses
   if(timeout == INFINITE_DELAY)
   {
      //Infinite timeout period
      ret = osSemaphoreWait(semaphore->id, osWaitForever);
   }
   else
   {
#if defined(osCMSIS_RTX)
      systime_t n;

      //Loop until the assigned time period has elapsed
      do
      {
         //Limit the timeout value
         n = MIN(timeout, 10000);
         //Wait for the specified time interval
         ret = osSemaphoreWait(semaphore->id, n);
         //Decrement timeout value
         timeout -= n;

         //Check timeout value
      } while(ret == 0 && timeout > 0);
#else
      //Wait for the specified time interval
      ret = osSemaphoreWait(semaphore->id, timeout);
#endif
   }

#if defined(osCMSIS_RTX)
   //Check return value
   if(ret > 0)
      return TRUE;
   else
      return FALSE;
#else
   //Check return value
   if(ret == osOK)
      return TRUE;
   else
      return FALSE;
#endif
}

void test_thread(void const *name) {
    while (true) {
        osSemaphoreWait(two_slots, osWaitForever);
        printf("%s\n\r", (const char*)name);
        osDelay(1000);
        osSemaphoreRelease(two_slots);
    }
}

/*---------------------------------------------------------------------------*
 * Routine:  sys_arch_sem_wait
 *---------------------------------------------------------------------------*
 * Description:
 *      Blocks the thread while waiting for the semaphore to be
 *      signaled. If the "timeout" argument is non-zero, the thread should
 *      only be blocked for the specified time (measured in
 *      milliseconds).
 *
 *      If the timeout argument is non-zero, the return value is the number of
 *      milliseconds spent waiting for the semaphore to be signaled. If the
 *      semaphore wasn't signaled within the specified time, the return value is
 *      SYS_ARCH_TIMEOUT. If the thread didn't have to wait for the semaphore
 *      (i.e., it was already signaled), the function may return zero.
 *
 *      Notice that lwIP implements a function with a similar name,
 *      sys_sem_wait(), that uses the sys_arch_sem_wait() function.
 * Inputs:
 *      sys_sem_t sem           -- Semaphore to wait on
 *      u32_t timeout           -- Number of milliseconds until timeout
 * Outputs:
 *      u32_t                   -- Time elapsed or SYS_ARCH_TIMEOUT.
 *---------------------------------------------------------------------------*/
u32_t sys_arch_sem_wait(sys_sem_t *sem, u32_t timeout) {
    u32_t start = us_ticker_read();
    
    if (osSemaphoreWait(sem->id, (timeout != 0)?(timeout):(osWaitForever)) < 1)
        return SYS_ARCH_TIMEOUT;
    
    return (us_ticker_read() - start) / 1000;
}

/*-----------------------------------------------------------------------------
  Routine to Move the Balls
 *----------------------------------------------------------------------------*/
void S_MoveBall (bool newBall){
	uint8_t i;
	uint8_t j;
	
	if (newBall & (actualBallNumber < maxNumberBalls)){
		ballArray[actualBallNumber][0] = plane_x+9;
		ballArray[actualBallNumber][1] = plane_y+5;
		actualBallNumber++;
	}

	for (i = 0; i < actualBallNumber; i++){
		osSemaphoreWait (glcd_semaph_id, osWaitForever);
			GLCD_SetForegroundColor (GLCD_COLOR_BLACK);
			GLCD_DrawPixel	(ballArray[i][0], ballArray[i][1]);
		osSemaphoreRelease(glcd_semaph_id);
		ballArray[i][0]+=2;
		// If the new position exceed the limits, it will be erased from the array and it wont be drawn
		if(ballArray[i][0] > GLCD_WIDTH ){
			actualBallNumber--;
			ballArray[i][0] = ballArray[actualBallNumber][0];
			ballArray[i][1] = ballArray[actualBallNumber][1];
		} else {
			osSemaphoreWait (glcd_semaph_id, osWaitForever);
				GLCD_SetForegroundColor (GLCD_COLOR_RED);
				GLCD_DrawPixel	(ballArray[i][0]+2, ballArray[i][1]);
			osSemaphoreRelease(glcd_semaph_id);
		}
		// Object is deleted if hit by a ball
		osMutexWait(objectVar_mutex_id, osWaitForever);
		for (j = 0; j < objectNumb; j++){
			if ( (ballArray[i][0] > objectArray[j][0]) & (ballArray[i][0] < objectArray[j][0]+9) & (ballArray[i][1] > objectArray[j][1]) & (ballArray[i][1] < objectArray[j][1]+9) ){
			osSemaphoreWait (glcd_semaph_id, osWaitForever);
				GLCD_SetForegroundColor (GLCD_COLOR_BLACK);
				S_DrawStar (objectArray[j][0], objectArray[j][1]);
				S_IncPlayerScore();
			osSemaphoreRelease(glcd_semaph_id);
				objectNumb--;
				objectArray[j][0] = objectArray[objectNumb][0];
				objectArray[j][1] = objectArray[objectNumb][1];
			}
		}
		osMutexRelease(objectVar_mutex_id);
	}

}

void BlinkTask(void const *argument)
{
	if(osSemaphoreWait(semHandle, osWaitForever) == osOK) {
		while(1) {
			HAL_GPIO_TogglePin(GPIOD, GPIO_PIN_12);
			osDelay(500);
		}
	}
}

/**
*@brief A function that gets the information recieved over wireless to be processed
*@param[inout] rxBuffer is the buffer to which the data is copied to
*@param[in] bufferSize The size of the passed in buffer
*@warning bufferSize must be of the same size as WIRELESS_BUFFER_SIZE, see wireless.h for details
*@retval None
*/
void getRecieved(uint8_t* rxBuffer, uint8_t bufferSize){
	
	uint8_t i = 0;
	for( i =0; i< bufferSize; i++){		
		osSemaphoreWait(rxId, osWaitForever);
		rxBuffer[i] = rxWireless[i]; //Critical access portion
		osSemaphoreRelease(rxId);
	}
}

static void demo_task1(void *arg)
{
    int count = 0;

    while (1) {
        osSemaphoreWait(pSem, 0xffffffff);
        printf("demo_task1 get sem %d\n", count++);
    };
}

CAN001_MessageHandleType* myCAN1_Get()
{
	CAN001_MessageHandleType* pmsg;
	if(can1rx.SendPos==can1rx.SendPos) osSemaphoreWait(can1rx_semaphore_id,osWaitForever);
	pmsg=&can1rx.buf[can1rx.SendPos];
	can1rx.SendPos++;
	can1rx.SendPos&=(MAXLINE-1);
	return pmsg;
}

/**
*@brief A function that safely access's the other board's accelerometer readings
*@param[inout] accValues A pointer to the new location in memory that data is copied to
*@retval None
*/
void getWirelessACCValues(float* accValues)
{
	
	int i = 0;
	for(i = 0; i < 3; i++){
		osSemaphoreWait(wirelessAccId, osWaitForever);
		accValues[i] = wirelessAccValues[i]; //Critical access portion
		osSemaphoreRelease(wirelessAccId);
	}
}

/*----------------------------------------------------------------------------
 *   Thread 2 - Normal Priority - looks for a free semaphore and uses
 *                                the resource whenever it is available
 *---------------------------------------------------------------------------*/
void thread2 (void const *argument) {
  while (1) {
    /* Wait indefinetly for a free semaphore */
    osSemaphoreWait (semaphore, osWaitForever);
    /* OK, the serial interface is free now, use it. */
    printf ("Thread 2\n");
    /* Return a token back to a semaphore. */
    osSemaphoreRelease (semaphore);
  }
}

/**
*@brief A function that safely access's the corrected values of the accelerometer
*@param[inout] accValues A pointer to the new location in memory that data is copied to
*@retval None
*/
void getACCValues(float* accValues)
{
	
	int i = 0;
	for(i = 0; i < 3; i++){
		osSemaphoreWait(accId, osWaitForever);
		accValues[i] = accCorrectedValues[i]; //Critical access portion
		osSemaphoreRelease(accId);
	}
}

/*!
 @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 S_Thread_MovePlane (void const *arg){  
	uint16_t 	plane_x_last = 1;
	uint16_t 	plane_y_last = GLCD_HEIGHT/2;
	uint16_t 	plane_x_i;
	uint16_t 	plane_y_i;
	uint8_t		i;
	
	while(!stopThreads){
		osMutexWait(planeVar_mutex_id, osWaitForever);
		plane_x_i = plane_x;
		plane_y_i = plane_y;
		osMutexRelease(planeVar_mutex_id);
		
		if( (plane_x_last != plane_x_i) || (plane_y_last != plane_y_i) ){
		
			osSemaphoreWait (glcd_semaph_id, osWaitForever);
				S_DrawPlane(plane_x_last, plane_y_last, true);	// Delete last position plane
				S_DrawPlane(plane_x_i, plane_y_i, false);				// Draw new position plane
			osSemaphoreRelease(glcd_semaph_id);
			
			plane_x_last = plane_x_i;
			plane_y_last = plane_y_i;
		}
		
		osMutexWait(objectVar_mutex_id, osWaitForever);
		for (i = 0; i < objectNumb; i++){
			// If the square around the plane intersects with the square around the object: the plane is destroyed
			if ( ( (objectArray[i][0]>(plane_x_i-9)) & (objectArray[i][0]<(plane_x_i+9)) ) & 
				( (objectArray[i][1]>(plane_y_i-9)) & (objectArray[i][1]<(plane_y_i+9)) ) ){
				osSemaphoreWait (glcd_semaph_id, osWaitForever);
					GLCD_SetForegroundColor (GLCD_COLOR_YELLOW);
					S_DrawExplosion(plane_x_i, plane_y_i);
					S_IncMachineScore();
				osSemaphoreRelease(glcd_semaph_id);
			}
		}
		osMutexRelease(objectVar_mutex_id);
		
		osDelay(50);
	}
}