// Simple demo of three threads
// LED blink thread, print thread, and idle loop
#include <STM32FreeRTOS.h>

const uint8_t LED_PIN = D7;

volatile uint32_t count = 0;

// handle for blink task
TaskHandle_t blink;

//------------------------------------------------------------------------------
// high priority for blinking LED
static void Task1(void *pvParameters) {

  UNUSED(pvParameters);
  pinMode(LED_PIN, OUTPUT);

  // Flash led every 200 ms.
  for (;;) {
    // Turn LED on.
    digitalWrite(LED_PIN, HIGH);
     Serial.println("Task1 ON");

    // Sleep for 50 milliseconds.
    //vTaskDelay((50L * configTICK_RATE_HZ) / 1000L);
vTaskDelay( 1000);

    // Turn LED off.
    digitalWrite(LED_PIN, LOW);
        Serial.println("Task1 OFF");

vTaskDelay(1000);

    // Sleep for 150 milliseconds.
    //vTaskDelay((150L * configTICK_RATE_HZ) / 1000L);
  }
}
//------------------------------------------------------------------------------
static void Task2(void *pvParameters) {



  UNUSED(pvParameters);
  pinMode(D8, OUTPUT);

  // Flash led every 200 ms.
  for (;;) {
    // Turn LED on.
    digitalWrite(D8, HIGH);
Serial.println("Task2 ON");
    // Sleep for 50 milliseconds.
    //vTaskDelay((50L * configTICK_RATE_HZ) / 1000L);
vTaskDelay(1000);

    // Turn LED off.
    digitalWrite(D8, LOW);
    Serial.println("Task2 OFF");

vTaskDelay(1000);

    // Sleep for 150 milliseconds.
    //vTaskDelay((150L * configTICK_RATE_HZ) / 1000L);
  }
}

static void Task3(void *pvParameters) {

  UNUSED(pvParameters);
  pinMode(D4, OUTPUT);

  // Flash led every 200 ms.
  for (;;) {
    // Turn LED on.
         Serial.println("Task3 ON");

    digitalWrite(D4, HIGH);
vTaskDelay(1000);
    // Sleep for 50 milliseconds.
    //vTaskDelay((50L * configTICK_RATE_HZ) / 1000L);
    
    // Turn LED off.
         Serial.println("Task3 OFF");

    digitalWrite(D4, LOW);
  vTaskDelay(1000);
    // Sleep for 150 milliseconds.
    //vTaskDelay((150L * configTICK_RATE_HZ) / 1000L);
  }

}

//------------------------------------------------------------------------------
void setup() {
  Serial.begin(9600);
  // wait for Leonardo
  while(!Serial) {}
  xTaskCreate(Task1,
    "Task1",
    128,
    NULL,
    1,
    NULL);

  // create print task
  xTaskCreate(Task2,
    "Task2",
    128,
    NULL,
    9,
    NULL);

    xTaskCreate(Task3,
    "Task3",
    128,
    NULL,
    3,
    NULL);
/*
  // create blink task
  xTaskCreate(Task1,
    "Task1",
    configMINIMAL_STACK_SIZE + 50,
    NULL,
    tskIDLE_PRIORITY + 2,
    &blink);

  // create print task
  xTaskCreate(Task2,
    "Task2",
    configMINIMAL_STACK_SIZE + 100,
    NULL,
    tskIDLE_PRIORITY + 1,
    NULL);

    xTaskCreate(Task3,
    "Task3",
    configMINIMAL_STACK_SIZE + 50,
    NULL,
    tskIDLE_PRIORITY + 2,
    &blink);
*/
  // start FreeRTOS
  vTaskStartScheduler();

}
//------------------------------------------------------------------------------
// WARNING idle loop has a very small stack (configMINIMAL_STACK_SIZE)
// loop must never block
void loop() {
  while(1) {
    // must insure increment is atomic
    // in case of context switch for print
    noInterrupts();
    count++;
    interrupts();
  }
}