#include <FreeRTOS_v821.h>

//Pines Leds

const int ledRed = 42
const int ledYellow = 40;
const int ledGreen = 41;

void initLeds(){
    pinMode(ledRed, OUTPUT);
    pinMode(ledYellow, OUTPUT);
    pinMode(ledGreen, OUTPUT);
}

static void task_red(void* params );
static void task_green(void* params);

void setup(){
    Serial.begin(115200);
    initLeds();

    xTaskCreate(
        task_red,
        "idRed",
        256,
        NULL,
        2,
        NULL
    );

    xTaskCreate(
        task_yellow,
        "idYellow",
        256,
        NULL,
        1,
        NULL
    );

    vTaskStartScheduler();

    for (;;);
}

void loop(){
}

static void task_red(void* params){
    TickType_t xLastWakeTime; // Despertar a lo último.
    const TickType_t xPeriod = pdMS_TO_TICKS(1000); // Período
    const TickType_t xC = pdMS_TO_TICKS(2000); // WCET
    xLastWakeTime =  xTaskGetTickCount();
    long ticks;

    for(;;) {
        digitalWrite(ledRed, HIGH);
        // vTaskDelay(xPeriod);
        vTaskDelayUntil(&xLastWakeTime, xPeriod);
        digitalWrite(ledRed, LOW);
        vTaskDelayUntil(&xLastWakeTime, xC);
        Serial.print("Ticks RED:");
        ticks = xTaskGetTickCount();
        Serial.print(ticks);
    }
}
static void task_yellow(void* params){
    TickType_t xLastWakeTime;
    const TickType_t xPeriod = pdMS_TO_TICKS(3000); // Período
    const TickType_t xC = pdMS_TO_TICKS(1000); // WCET

    xLastWakeTime =  xTaskGetTickCount();

    for(;;){
        digitalWrite(ledGreen, HIGH);
        // vTaskDelay(xPeriod);
        vTaskDelayUntil(&xLastWakeTime, xC);
        digitalWrite(ledGreen, LOW);
        vTaskDelayUntil(&xLastWakeTime, xPeriod);
    }
}