/**
\brief Test case: TC_MutexNestedAcquire
\details
- Create a mutex object
- Obtain a mutex object
- Create a high priority thread that waits for the same mutex
- Recursively acquire and release a mutex object
- Release a mutex
- Verify that every subsequent call released the mutex
- Delete a mutex object
- Mutex object must be released after each acquisition
*/
void TC_MutexNestedAcquire (void) {
  osStatus stat;
  
  /* - Create a mutex object */
  G_MutexId = osMutexCreate (osMutex (Mutex_Nest));
  ASSERT_TRUE (G_MutexId != NULL);

  if (G_MutexId  != NULL) {
    /* - Obtain a mutex object */
    stat = osMutexWait (G_MutexId, 0);
    ASSERT_TRUE (stat == osOK);
    
    if (stat == osOK) {
      /* - Create a high priority thread that will wait for the same mutex */
      G_Mutex_ThreadId = osThreadCreate (osThread (Th_MutexWait), NULL);
      ASSERT_TRUE (G_Mutex_ThreadId != NULL);
    
      /* - Recursively acquire and release a mutex object */
      RecursiveMutexAcquire (5, 5);

      /* - Release a mutex */
      stat = osMutexRelease (G_MutexId);
      ASSERT_TRUE (stat == osOK);

      /* - Verify that every subsequent call released the mutex */
      ASSERT_TRUE (osMutexRelease (G_MutexId) == osErrorResource);
    }
    /* - Delete a mutex object */
    ASSERT_TRUE (osMutexDelete (G_MutexId) == osOK);
  }
}

error_t udpInvokeRxCallback(NetInterface *interface, const IpPseudoHeader *pseudoHeader,
   const UdpHeader *header, const ChunkedBuffer *buffer, size_t offset)
{
   uint_t i;
   void *params;
   UdpRxCallbackDesc *entry;

   //This flag tells whether a matching entry has been found
   bool_t found = FALSE;

   //Acquire exclusive access to the callback table
   osMutexAcquire(udpCallbackMutex);

   //Loop through the table
   for(i = 0; i < UDP_CALLBACK_TABLE_SIZE; i++)
   {
      //Point to the current entry
      entry = &udpCallbackTable[i];

      //Check whether the entry is currently in used
      if(entry->callback != NULL)
      {
         //Bound to a particular interface?
         if(entry->interface == NULL || entry->interface == interface)
         {
            //Does the specified port number match the current entry?
            if(entry->port == ntohs(header->destPort))
            {
               //Retrieve callback parameters
               params = entry->params;

               //Release mutex to prevent any deadlock
               if(params == NULL)
                  osMutexRelease(udpCallbackMutex);

               //Invoke user callback function
               entry->callback(interface, pseudoHeader,
                  header, buffer, offset, params);

               //Acquire mutex
               if(params == NULL)
                  osMutexAcquire(udpCallbackMutex);

               //A matching entry was found
               found = TRUE;
            }
         }
      }
   }

   //Release exclusive access to the callback table
   osMutexRelease(udpCallbackMutex);

   //Return status code
   return found ? NO_ERROR : ERROR_PORT_UNREACHABLE;
}

/**
  * @brief  Function that receives data and performs Triple Modular Redundancy. This is a blocking call.
  * @param  *receiver: pointer to a Receiver structure. 
  * @retval None.
  */
void wireless_RX(struct Receiver *receiver) {
	uint8_t i=0;
	uint8_t temp_data=0;
	
	osMutexWait(receiver->mutexID, osWaitForever);	
	uint8_t raw_data[sizeof(receiver->data)/sizeof(receiver->data[0]) * 3];
	CC2500_StrobeSend(SRX_R,&(receiver->state),&(receiver->buffer_space));
	osMutexRelease(receiver->mutexID);
	
	osDelay(STROBE_DELAY);
	
	while (i<(sizeof(receiver->data)/sizeof(receiver->data[0]) * 3)) {
		osMutexWait(receiver->mutexID, osWaitForever);
		CC2500_StrobeSend(SNOP_R,&(receiver->state),&(receiver->buffer_space));	
		
		if (receiver->buffer_space>0) {
			CC2500_Read(&temp_data, 0x3F, 1);
			if ((temp_data&0xF0)==0xF0) {
				raw_data[0]=temp_data&0x0F;
				i=1;
			} else if (i>0) {
				if ((temp_data&0xF0)==i<<4) {
					raw_data[i]=temp_data&0x0F;
					i++;
				} else {
					i=0;
				}
			}
		}
		
		osMutexRelease(receiver->mutexID);
		
		osDelay(STROBE_DELAY);
	}
	
	osMutexWait(receiver->mutexID, osWaitForever);
	for(uint32_t j=0;j<sizeof(receiver->data)/sizeof(receiver->data[0]);j++){
		receiver->data[j] = ((raw_data[3*j]&raw_data[3*j+1]) | (raw_data[3*j]&raw_data[3*j+2]) | (raw_data[3*j+2]&raw_data[3*j+1]));
	}
	
	CC2500_StrobeSend(SIDLE_R,&(receiver->state),&(receiver->buffer_space));
	osMutexRelease(receiver->mutexID);
	osDelay(STROBE_DELAY);
	
	osMutexWait(receiver->mutexID, osWaitForever);
	CC2500_StrobeSend(SNOP_R,&(receiver->state),&(receiver->buffer_space));
	osMutexRelease(receiver->mutexID);
	osDelay(STROBE_DELAY);
}

void Thread_Mutex(void const *argument) {
  osStatus status;

  while(1) {
    ; // Insert thread code here...

    status = osMutexWait(mid_Thread_Mutex, NULL);
    switch(status) {
      case osOK:
        ; // Use protected code here...
        osMutexRelease(mid_Thread_Mutex);
        break;
      case osErrorTimeoutResource:
        break;
      case osErrorResource:
        break;
      case osErrorParameter:
        break;
      case osErrorISR:
        break;
      default:
        break;
    }

    osThreadYield();        // suspend thread
  }
}

//Send data in specified format. Same syntax as printf
util_ErrTd xb_SendF(char *format, ...){
	util_ErrTd Status = util_ErrTd_Ok;
	va_list va;
	int32_t i;


	if( osMutexWait(xb_MutexId, 100) == osOK ){																//Wait for shared resource (tx buffer access)
		HAL_NVIC_DisableIRQ(XB_DMA_TX_IRQN);																//Disable DMA interrupts (TXC could acces TxBuffer in the middle of writing data to it

		va_start(va, format);																				//Start reading of parameters
		vsnprintf(xb_TmpStr, sizeof(xb_TmpStr), format, va);												//Format new string and save its length
		va_end(va);																							//End of reading parameters

	//	if( Cnt <= 0 || Cnt >= sizeof(com_TmpStr) ){						//If formatted string doesnt fit into buffer
	//		snprintf(com_TmpStr, sizeof(com_TmpStr), "<erre %d %d>\r\n", (int)Cnt, (int)sizeof(com_TmpStr));	//Format error message instead
	//		Status = util_ErrTd_Overflow;
	//	}

		for( i=0; i<strlen(xb_TmpStr); i++ ){																//Copy byte by byte into tx buffer
			xb_TxBuffer[ xb_TxHead++ ] = xb_TmpStr[i];
			xb_TxHead &= XB_TXBUFHEADMASK;
		}

		HAL_NVIC_EnableIRQ(XB_DMA_TX_IRQN);																	//Enable DMA interrupts again
		xb_TransmitTxBuffer();																				//Transmit TX buffer

		osMutexRelease(xb_MutexId);																			//Release shared resource
	}
	return Status;
}

//-------------------------------------------------------------------
//read date time
void RTC_Read_datetime(uint8_t * data,uint8_t flag)
{
	uint8_t temp[3];
	//first read tiem ,then read date, or not time is not run;
	RTC_DateTypeDef sdatestructureget;
  RTC_TimeTypeDef stimestructureget;
	HAL_RTCStateTypeDef status;

	if(data!=NULL)
	{	
		osMutexWait(rtc_mutex, osWaitForever);
		/* Get the RTC current Time */
		HAL_RTC_GetTime(&hrtc, &stimestructureget, RTC_FORMAT_BIN);
		temp[0]=stimestructureget.Hours;
		temp[1]=stimestructureget.Minutes;
		temp[2]=stimestructureget.Seconds;
		
		memcpy(&current_datetime[3],temp,3);
		
		/* Get the RTC current Date */
		HAL_RTC_GetDate(&hrtc, &sdatestructureget, RTC_FORMAT_BIN);
		data[0]=sdatestructureget.Year;
		data[1]=sdatestructureget.Month;
		data[2]=sdatestructureget.Date;
		current_datetime[6]=sdatestructureget.WeekDay;

		memcpy(&current_datetime[0],data,3);

		if(flag==1)
		{
			memcpy(data,temp,3);
		}
		osMutexRelease(rtc_mutex);
	}
}

void memPoolFree(void *p)
{
//Use fixed-size blocks allocation?
#if (MEM_POOL_SUPPORT == ENABLED)
   uint_t i;

   //Acquire exclusive access to the memory pool
   osMutexAcquire(memPoolMutex);

   //Loop through allocation table
   for(i = 0; i < MEM_POOL_BUFFER_COUNT; i++)
   {
      if(memPool[i] == p)
      {
         //Mark the current block as free
         memPoolAllocTable[i] = FALSE;
         //Exit immediately
         break;
      }
   }

   //Release exclusive access to the memory pool
   osMutexRelease(memPoolMutex);
#else
   //Release memory block
   osMemFree(p);
#endif
}

/**
   * @brief Set value to be displayed on the seven-segment display.
   * @param float angle: angle to be displayed
   * @retval None
   */
void SevenSegment_SetDisplayValue_Angle(float angle)
{
	/* We have race conditions. Need to use mutexes to protect these global flags. */
	osMutexWait(segment_mutex, osWaitForever);
	displayed_angle = angle;
	osMutexRelease(segment_mutex);
}

/**
   * @brief Set value to be displayed on the seven-segment display.
   * @param float angle: angle to be displayed
   * @retval None
   */
void SevenSegment_SetDisplayValue_Temp(float temp)
{
	/* We have race conditions. Need to use mutexes to protect these global flags. */
	osMutexWait(segment_mutex, osWaitForever);
	displayed_temp = temp;
	osMutexRelease(segment_mutex);
}

 /*----------------------------------------------------------------------------
*      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++;
	}
}

/**
   * @brief Sets seven-segment display mode (ANGLE_MODE, TEMP_MODE).
   * @param None
   * @retval None
   */
void SevenSegment_SetDisplayMode(DisplayMode mode)
{
	/* We have race conditions. Need to use mutexes to protect these global flags. */
	osMutexWait(segment_mutex, osWaitForever);
	display_mode = mode;
	osMutexRelease(segment_mutex);
}

/**
   * @brief Starts flashing the display.
   * @param None
   * @retval None
   */
void SevenSegment_StartFlashing(void)
{
	/* We have race conditions. Need to use mutexes to protect these global flags. */
	osMutexWait(segment_mutex, osWaitForever);
	flashing = 1;
	osMutexRelease(segment_mutex);
}

/**
   * @brief Deactivates 7-segment display and turn off the LEDs.
   * @param None
   * @retval None
   */
void SevenSegment_TurnOff(void)
{
	osMutexWait(segment_mutex, osWaitForever);
	GPIOE->ODR &= 0x000F;	/* Clear the bits corresponding to GPIO_PIN_4 to GPIO_PIN_15 */
	activated = 0;
	osMutexRelease(segment_mutex);
}

/**
   * @brief Activates 7-segment display.
   * @param None
   * @retval None
   */
void SevenSegment_TurnOn(void)
{
	/* We have race conditions. Need to use mutexes to protect these global flags. */
	osMutexWait(segment_mutex, osWaitForever);
	activated = 1;
	osMutexRelease(segment_mutex);
}

/*-----------------------------------------------------------------------------
 * Low priority job used for priority inversion test
 *----------------------------------------------------------------------------*/
void Th_LowPrioJob (void const *arg) {
  osThreadId *ctrl_id = (osThreadId *)arg;
  osStatus    stat;
  uint32_t    i;

  /* Obtain a mutex object */
  stat = osMutexWait (G_MutexId, 0);
  ASSERT_TRUE (stat == osOK);

  if (stat == osOK) {
    /* Mutex acquired, inform control thread */
    osSignalSet (*ctrl_id, (1 << 0));
    
    /* Set mark into execution array */
    for (i = 0; i < 3; i++) {
      if (G_ExecArr[i] == 0) {
        G_ExecArr[i] = 'L';             /* L as Low priority job              */

        /* Inform control thread */
        osSignalSet (*ctrl_id, (1 << 1));
        break;
      }
    }
    ASSERT_TRUE (osMutexRelease (G_MutexId) == osOK);
  }
}

/* State behaviour */
void behaviour_welcome(state_ptr state)
{
  /* Set events to react to */

  /* Do state actions */

  /* Set menu */
  osMutexWait(mutex_menuHandle, osWaitForever);
  menu_copy(&menu_welcome, &current_menu);
  osMutexRelease(mutex_menuHandle);

  /* Display menu */
  uint32_t i;
  for (i = 0; i < menu_welcome.item_num; i++)
  {
    while (osMailPut(queue_lcdHandle, (void *) &menu_welcome.items[i]) != osOK)
    {
      osDelay(1);
    }
  }

  /* Do state actions */
  bluetooth_init();
  osDelay(2500);
  entry_to_running(state);
}

bool spifs_unlink(const char *filename)
{
    int16_t fileId;
    char cleanname[SPIFS_FILENAME_LEN];
    SPIFlash handler;

    // Clean the filename of problem characters.
    if (!clean_filename(filename, cleanname)) return false;

    // Wait for exclusive access to the SPI master bus.
    osMutexWait(spiMasterMutex, osWaitForever);

    // Open the SPI flash device.
    spi_flash_open(SPI0, SPI_MODE0, false, Freq_1Mbps, &handler);

    // Find a file id associated with the filename.
    fileId = spifs_find_filename(&handler, cleanname);

    // Erase each fileblock associated with the file.
    if (SPIFS_FILEID_VALID(fileId)) spifs_erase_blocks(&handler, fileId);

    // Close the SPI flash device.
    spi_flash_close(&handler);

    // Release exclusive access to the SPI master bus.
    osMutexRelease(spiMasterMutex);

    return true;
}

/*-----------------------------------------------------------------------------
 * Recursive mutex acquisition
 *----------------------------------------------------------------------------*/
static void RecursiveMutexAcquire (uint32_t depth, uint32_t ctrl) {
  static uint32_t acq;                  /* Mutex acquisition counter          */
  osStatus stat;

  /* Acquire a mutex */
  stat = osMutexWait (G_MutexId, 100);
  ASSERT_TRUE (stat == osOK);

  if (stat == osOK) {
    if (ctrl == depth) {
      /* - Verify that mutex was aqcuired at count zero */
      ASSERT_TRUE (acq == 0);
    }
    acq++;

    if (depth) {
      RecursiveMutexAcquire (depth - 1, ctrl);
    }
    acq--;

    /* Release a mutex */
    stat = osMutexRelease (G_MutexId);
    ASSERT_TRUE (stat == osOK);
  }
}

/*---------------------------------------------------------------------------
     TITLE   : cmd_bluetooth_check
     WORK    :
     ARG     : void
     RET     : void
---------------------------------------------------------------------------*/
void cmd_bluetooth_check( void )
{
	uint32_t time_out;
	uint8_t  ch;
	uint8_t  ch_array[2];
	uint8_t  ch_i;
	osStatus ret;

	ret = osMutexWait( Mutex_Loop, 1000 );

	if( ret != osOK )
	{
		_menu_printf("Fail to osMutexWait\r\n");
		return;
	}

	core.blueport = uartOpen(USART2, NULL, 115200, MODE_RXTX);

	_menu_printf("\r\n");
	_menu_printf("AT -> ");

	serialPrint(core.blueport, "AT");


	ch_array[0] = 0;
	ch_array[1] = 0;
	ch_i  = 0;

	//-- 응답이 올때까지 기다림
	time_out = 1000;
	while(time_out--)
	{
		if( serialTotalBytesWaiting(core.blueport) )
		{
			ch = serialRead(core.blueport);
			_menu_putch(ch);
			ch_array[ch_i++] = ch;

			if( ch_i >= 2 ) break;
		}

		osDelay(1);
	}


	if( ch_array[0] == 'O' && ch_array[1] == 'K' )
	{
		_menu_printf("\r\nBluetooth OK");
	}
	else
	{
		_menu_printf("\r\nBluetooth Fail");
	}

	_menu_printf("\r\n");

	serialInit(mcfg.serial_baudrate);

	osMutexRelease( Mutex_Loop );
}

void Thread_DISP2(void const *argument){
	
	float roll_temp, pitch_temp;
	
	while(1){
		// osDelay(10);
		osMutexWait(mems_mutex_id, osWaitForever);
		roll_temp = roll;
		osMutexRelease(mems_mutex_id);
		// printf("mems: roll= %f\n", roll_temp);
		printf("mems: temp= %f\n", output);
				
		if(roll_temp < 100){
			Parse_Mems(parsed, roll_temp);								
			// printf("disp2: %d %d %d %d %d\n", parsed[3], parsed[2], parsed[1], parsed[0], parsed[4]);
		}
		
		else{
			parsed[0] = (int) roll_temp % 10;
			parsed[2] = ((int) roll_temp / 10) % 10;
			parsed[3] = ((int) roll_temp / 100) % 10;
			parsed[1] = 0;
			parsed[4] = 0;				
			// printf("disp2: %d %d %d %d %d\n", parsed[3], parsed[2], parsed[1], parsed[0], parsed[4]);
		}
		
		if(parsed[4] < 0 || parsed[3] < 0 || parsed[2] < 0 || parsed[1] < 0 || parsed[0] < 0) Show_Negative();
		else Show();
	}
}