#include <Arduino.h>

#define MAX_TASKS 5

typedef void (*TaskFunction)(); // Task function type

typedef struct {
  TaskFunction taskFunction; // Function to execute the task
  unsigned long interval;    // Timing interval (ms)
  unsigned long lastRun;     // Timestamp of the last execution
  bool isActive;             // Task active state
} Task;

Task taskList[MAX_TASKS]; // Array to store tasks
int taskCount = 0;

// Add a new task to the scheduler
void addTask(TaskFunction func, unsigned long interval) {
  if (taskCount < MAX_TASKS) {
    taskList[taskCount].taskFunction = func;
    taskList[taskCount].interval = interval;
    taskList[taskCount].lastRun = 0;
    taskList[taskCount].isActive = true;
    taskCount++;
  }
}

// Scheduler
void runScheduler() {
  unsigned long currentTime = micros();
  for (int i = 0; i < taskCount; i++) {
    if (taskList[i].isActive && (currentTime - taskList[i].lastRun >= taskList[i].interval)) {
      taskList[i].taskFunction(); // Execute the task
      taskList[i].lastRun = currentTime; // Update timestamp
    }
  }
}

// vTaskDelay: Non-blocking delay in ticks (ms-based)
void vTaskDelay(unsigned long delayTicks) {
  unsigned long startTime = millis();
  while (millis() - startTime < delayTicks) {
    runScheduler(); // Allow the scheduler to execute other tasks
  }
}

// vTaskDelayMs: Non-blocking delay in milliseconds
void vTaskDelayMs(unsigned long delayMs) {
  vTaskDelay(delayMs); // Reuse vTaskDelay for milliseconds
}

// vTaskDelayUs: Non-blocking delay in microseconds
void vTaskDelayUs(unsigned long delayUs) {
  unsigned long startMicros = micros();
  while (micros() - startMicros < delayUs) {
    runScheduler(); // Allow the scheduler to execute other tasks
  }
}

// Example tasks
void task1() {
  Serial.println("[Task1] Running...");
  vTaskDelay(1000); // Simulate task workload with a 1000ms non-blocking delay
}

void task2() {
  Serial.println("[Task2] Running...");
  vTaskDelayMs(2000); // Simulate task workload with a 200ms non-blocking delay
}

void task3() {
  Serial.println("[Task3] Running...");
  vTaskDelayUs(50000); // Simulate task workload with a 50,000us (50ms) non-blocking delay
}

void setup() {
  Serial.begin(9600);
  // Register tasks
  addTask(task1, 1000000); // Runs every 1 second
  addTask(task2, 2000000); // Runs every 2 seconds
  addTask(task3, 50000);  // Runs every 0.05 seconds
}

void loop() {
  runScheduler(); // Execute the scheduler
}