#include <Arduino_FreeRTOS.h>
#include <task.h>
#include <semphr.h>  // add the FreeRTOS functions for Semaphores (or Flags).
#include <FreeRTOSConfig.h>


/* Define the traceTASK_SWITCHED_IN() and *_OUT() macros become HIGH when the tasks is switched in, and low w
when switched out. This has to be done in the RTOS Library. */

//#define traceTASK_SWITCHED_IN() digitalWrite( (int)pxCurrentTCB->pxTaskTag, HIGH )
//#define traceTASK_SWITCHED_OUT() digitalWrite((int)pxCurrentTCB->pxTaskTag, LOW )

/* Define the pin on which the tasks is being monitored (to be put in each task) */ /*modified by Markus Pfeil*/
/* vTaskSetApplicationTaskTag( NULL, ( void * )data.debug_pin); */ 

struct LedTaskParams {

    int pin;
    int debug_pin;
    int flashPeriod;

};

LedTaskParams redParams{13, 12, 200};
LedTaskParams greenParams{11, 10, 300};
LedTaskParams blueParams{9,8, 100};


// the setup function runs once when you press reset or power the board
void setup() {
  pinMode(redParams.pin, OUTPUT);
  pinMode(redParams.debug_pin, OUTPUT);
  pinMode(greenParams.pin, OUTPUT);
  pinMode(greenParams.debug_pin, OUTPUT);
  pinMode(blueParams.pin, OUTPUT);
  pinMode(blueParams.debug_pin, OUTPUT);
  
xTaskCreate(Task_FlashLed, "red-led", 128, (void *)&redParams, 1, NULL);
xTaskCreate(Task_FlashLed, "green-led", 128, (void *)&greenParams, 1, NULL); //create with 1024 Stack - Fails without notice - only first Task created. 
xTaskCreate(Task_FlashLed, "blue-led", 128, (void *)&blueParams, 1, NULL);
  
  // Now the Task scheduler, which takes over control of scheduling individual Tasks, is automatically started.
  vTaskStartScheduler();

  
}

void loop()
{
  // Empty. Things are done in Tasks.
}

/*--------------------------------------------------*/
/*---------------------- Tasks ---------------------*/
/*--------------------------------------------------*/

void Task_FlashLed(void * pvParameters)

{

    configASSERT((LedTaskParams*)pvParameters != nullptr);

    const LedTaskParams data = *((LedTaskParams*)pvParameters);

    const int led_pin = data.pin;

    vTaskSetApplicationTaskTag( NULL, ( void * )data.debug_pin); //as this is done on the first execution the initial switch in will not be shown. 

    const TickType_t xOuterFrequency = pdMS_TO_TICKS(data.flashPeriod);

    const TickType_t xInnerFrequency = pdMS_TO_TICKS(20); //if this is less than 15 ms the vtaskdelay will trigger, but with 0 ticks length, i.e. Priority will be passed, but no time delay taken. 


   TickType_t lastRun = xTaskGetTickCount();  // 1

   TickType_t flashMarker;



   while(true)

    {

      digitalWrite(led_pin, !digitalRead(led_pin));

      vTaskDelay(xInnerFrequency);
      

      digitalWrite(led_pin, !digitalRead(led_pin));

      //delay(data.flashPeriod);
      vTaskDelayUntil(&lastRun, xOuterFrequency);
      
    }

}