#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 work_period;

};

LedTaskParams redParams{13, 12, 30};
LedTaskParams greenParams{11, 10, 80};



// 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);

  
xTaskCreate(Task_FlashLed, "red-led", 128, (void *)&redParams, 2, NULL);
xTaskCreate(Task_FlashLed, "green-led", 128, (void *)&greenParams, 2, NULL); //create with 1024 Stack - Fails without notice - only first Task created. 
  
  // 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 xWorkTime = pdMS_TO_TICKS(data.work_period);

    const TickType_t xRepeatTime = pdMS_TO_TICKS(200); //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




   while(true)

    {

      for(int i=0;i<3;i++){
      digitalWrite(led_pin, !digitalRead(led_pin));

        vTaskDelay(xWorkTime);
        //delay(data.work_period);
    }
        

       vTaskDelayUntil(&lastRun, xRepeatTime);
       //delay(200);
    }

}