#include <STM32FreeRTOS.h>
	#include <stdarg.h>

	SemaphoreHandle_t xSemaphore;

	void _printf(const char *format, ...) {
	    xSemaphoreTake(xSemaphore, portMAX_DELAY);
	    va_list va;
	    va_start(va, format);
	//    TickType_t _nowTick = xTaskGetTickCount();
	//    char * _taskName = pcTaskGetTaskName( NULL );
	//    printf("[%s:%d] ",_taskName, _nowTick);    
	    vprintf(format, va); 
	    va_end(va);
	    xSemaphoreGive(xSemaphore);
	}

	// 時間稼ぎ(Gain time)
	void ConsumptionTick(int delay) {
	    TickType_t startTick;
	    TickType_t endTick;
	    TickType_t nowTick;
	    startTick = xTaskGetTickCount();
	    endTick = startTick + delay;
	    //printf("startTick=%d endTick=%d\n",startTick,endTick);
	    while(1) {
	      nowTick = xTaskGetTickCount();
	      if (nowTick > endTick) break;
	    }
	}

	// Task Body
	void task(void *pvParameters)
	{
	    UBaseType_t prio;
	    TickType_t nowTick;

	    prio = uxTaskPriorityGet( NULL );
	    nowTick = xTaskGetTickCount();
	    printf("[%s:%d] start Priority=%d\n",pcTaskGetName(0),nowTick,(int)prio);
	//    _printf("[%s:%d] start Priority=%d\n",pcTaskGetName(0),nowTick,(int)prio);
	    ConsumptionTick(200);
	    nowTick = xTaskGetTickCount();
	    printf("[%s:%d] end\n",pcTaskGetName(0),nowTick);
	//    _printf("[%s:%d] end\n",pcTaskGetName(0),nowTick);
	    vTaskDelete( NULL );
	}


	//------------------------------------------------------------------------------
	void setup() {
	  portBASE_TYPE xTask1, xTask2, xTask3, xTask4;

	  Serial.begin(115200);
	  Serial.println("setup() start");
	  Serial.print("configTICK_RATE_HZ:");
	  Serial.println(configTICK_RATE_HZ);
	  Serial.print("portTICK_PERIOD_MS:");
	  Serial.println(portTICK_PERIOD_MS);
	  Serial.print("freeRTOS version:");
	  Serial.println(tskKERNEL_VERSION_NUMBER);

	#if 1
	// Task1 is started in priority=2
	// Task2 is started in priority=2
	// Task3 is started in priority=2
	// Task4 is started in priority=2
	  xTask1 = xTaskCreate(task, "Task1", configMINIMAL_STACK_SIZE, NULL, 2, NULL);
	  xTask2 = xTaskCreate(task, "Task2", configMINIMAL_STACK_SIZE, NULL, 2, NULL);
	  xTask3 = xTaskCreate(task, "Task3", configMINIMAL_STACK_SIZE, NULL, 2, NULL);
	  xTask4 = xTaskCreate(task, "Task4", configMINIMAL_STACK_SIZE, NULL, 2, NULL);
	#endif

	#if 0
	// Task1 is started in priority=2
	// Task2 is started in priority=3
	// Task3 is started in priority=4
	// Task4 is started in priority=5
	  xTask1 = xTaskCreate(task, "Task1", configMINIMAL_STACK_SIZE, NULL, 2, NULL);
	  xTask2 = xTaskCreate(task, "Task2", configMINIMAL_STACK_SIZE, NULL, 3, NULL);
	  xTask3 = xTaskCreate(task, "Task3", configMINIMAL_STACK_SIZE, NULL, 4, NULL);
	  xTask4 = xTaskCreate(task, "Task4", configMINIMAL_STACK_SIZE, NULL, 5, NULL);
	#endif

	  /* Check everything was created. */
	  configASSERT( xTask1 );
	  configASSERT( xTask2 );
	  configASSERT( xTask3 );
	  configASSERT( xTask4 );

	  /* Create Mutex for printf */
	  xSemaphore = xSemaphoreCreateMutex();
	  /* Check everything was created. */
	  configASSERT( xSemaphore );
	  
	  // start scheduler
	  Serial.println("Start Scheduler.....");
	  vTaskStartScheduler();

	#if 1
	  while (1) {
	    Serial.println("setup alive...");
	    delay(1000);
	  }
	#endif
	}

	//------------------------------------------------------------------------------
	// WARNING loop() called from vApplicationIdleHook(), so don't use this function.
	// loop must never block
	void loop() {
	  // Not used.
	}