#include <Arduino.h>

struct Task {
    void (*taskFunction)();
    unsigned long interval;
    unsigned long lastRun;
    int priority;
    unsigned long delayUntil;
};

Task tasks[2];
int taskCount = 0;

void addTask(void (*taskFunction)(), unsigned long interval, int priority) {
    tasks[taskCount].taskFunction = taskFunction;
    tasks[taskCount].interval = interval;
    tasks[taskCount].lastRun = 0;
    tasks[taskCount].priority = priority;
    tasks[taskCount].delayUntil = 0;
    taskCount++;
}

void vTaskDelay(unsigned long delayTime, Task &task) {
    task.delayUntil = micros() + delayTime;
}

void scheduler() {
  while(1) {
    unsigned long now = micros();
    for (int i = 0; i < taskCount; i++) {
        if (now >= tasks[i].delayUntil && (now - tasks[i].lastRun >= tasks[i].interval)) {
            tasks[i].taskFunction();
            tasks[i].lastRun = now;
        }
    }
  }
}

// Feladatok példái
void task1() {
    Serial.println("Task 1 RUN");
    digitalWrite(12,!digitalRead(12));
    vTaskDelay(2000000, tasks[0]); // Wait 2 sec
}

void task2() {
    Serial.println("Task 2 RUN");
     digitalWrite(14,!digitalRead(14));
    vTaskDelay(5000000, tasks[1]); // Wait 5 sec
}

void setup() {
    Serial.begin(9600);

    pinMode(12, OUTPUT);
    pinMode(14, OUTPUT);
    addTask(task1, 1000000, 1); // Task 1 every 1 sec
    addTask(task2, 1000000, 2); // Task 2 every 1 sec
    scheduler();
}

void loop() {
    
}