TaskManager

#include "TaskManager.h"
#include <Arduino.h>

// Copyright [2024] <Bipping>

#define MAX_TASKS 10
const uint32_t DEFAULT_MARGIN = 288;

uint32_t margin = DEFAULT_MARGIN;

// Utilisation des types spécifiques pour optimiser la mémoire
typedef struct {
    TaskFunction callback;
    uint32_t interval;
    uint32_t lastRun;
} Task;

Task tasks[MAX_TASKS];
uint8_t taskCount = 0;
volatile uint8_t isrCounter = 0;
volatile uint8_t remainingMicros = 0;

void addTask(TaskFunction callback, uint32_t interval) {
    if (taskCount < MAX_TASKS) {
        tasks[taskCount] = {callback, interval, micros()};
        taskCount++;
    }
}

void removeTask(TaskFunction callback) {
    for (int i = 0; i < taskCount; i++) {
        if (tasks[i].callback == callback) {
            for (int j = i; j < taskCount - 1; j++) {
                tasks[j] = tasks[j + 1];
            }
            taskCount--;
            break;
        }
    }
}

void changeTaskInterval(TaskFunction callback, uint32_t newInterval) {
    for (int i = 0; i < taskCount; i++) {
        if (tasks[i].callback == callback) {
            tasks[i].interval = newInterval;
            break;
        }
    }
}

void setSleepMode(SleepMode mode) {
    switch (mode) {
        case IDLE_MODE:
            set_sleep_mode(SLEEP_MODE_IDLE);
            break;
        case POWER_SAVE_MODE:
            set_sleep_mode(SLEEP_MODE_PWR_SAVE);
            break;
        case EXTENDED_STANDBY_MODE:
            set_sleep_mode(SLEEP_MODE_EXT_STANDBY);
            break;
        default:
            set_sleep_mode(SLEEP_MODE_IDLE);
            break;
    }
}

void updateTasks() {
    uint32_t currentTime = micros();
    uint32_t nextTime = UINT32_MAX;
    Task *taskQueue[MAX_TASKS];
    uint8_t taskQueueCount = 0;
    Task *indexFonction = nullptr;
    bool executeImmediately = false;

    for (int i = 0; i < taskCount; i++) {
        uint32_t scheduledTime = tasks[i].lastRun + tasks[i].interval;
        if (scheduledTime <= currentTime) {
            taskQueue[taskQueueCount++] = &tasks[i];
            executeImmediately = true;
        } else if (scheduledTime < nextTime && !executeImmediately) {
            nextTime = scheduledTime;
            indexFonction = &tasks[i];
        }
    }

    if (executeImmediately) {
        currentTime = micros();
        for (int i = 0; i < taskQueueCount; i++) {
            taskQueue[i]->callback();
            taskQueue[i]->lastRun = currentTime;
            // Fin de updateTask
        }
    } else {
        currentTime = micros();
        uint32_t timeToNextTask = nextTime > currentTime ? nextTime - currentTime : 0;

        if (timeToNextTask > margin) {
            uint16_t prescalerBits;
            uint32_t intervalMicros = timeToNextTask - margin;
            int8_t shifts;

            if (intervalMicros > 237856) {
                prescalerBits = 0x07;  // Prescaler 1024
                shifts = -6;
            } else if (intervalMicros > 113338) {
                prescalerBits = 0x06;  // Prescaler 256
                shifts = -4;
            } else if (intervalMicros > 51078) {
                prescalerBits = 0x05;  // Prescaler 128
                shifts = -3;
            } else if (intervalMicros > 19949) {
                prescalerBits = 0x04;  // Prescaler 64
                shifts = -2;
            } else if (intervalMicros > 4384) {
                prescalerBits = 0x03;  // Prescaler 32
                shifts = -1;
            } else if (intervalMicros > 288) {
                prescalerBits = 0x02;  // Prescaler 8
                shifts = 1;
            } else {
                prescalerBits = 0x01;  // Prescaler 1
                shifts = 4;
            }

            if (shifts < 0) {
                uint8_t mask = (1UL << -shifts) - 1;
                uint8_t lostBits = intervalMicros & mask;
            }

            uint32_t clockCycles = (shifts > 0) ? (intervalMicros << shifts) : (intervalMicros >> -shifts);
            isrCounter = clockCycles >> 8;
            uint32_t remainingCycles = clockCycles & 0xFF;

            if (remainingCycles > 0) {
                remainingMicros = remainingCycles - 1
                isrCounter++;
            } else {
                remainingMicros = 0;
            }

            OCR2A = isrCounter > 1 ? 0xFF : remainingMicros;
            TCCR2B = (TCCR2B & 0b11111000) | prescalerBits;

            TIMSK2 |= (1 << OCIE2A);  // Enable Timer2 Compare Match A interrupt
            TCCR2B |= (1 << CS22);  // Start Timer
            sleep_enable();
            sleep_cpu();
            // Au réveil

            TCCR2B &= ~(1 << CS22);  // Stop Timer
            TIMSK2 &= ~(1 << OCIE2A);  // Disable Timer2 Compare Match A interrupt
            sleep_disable();

            delayMicroseconds(lostBits);

        } else {
            delayMicroseconds(timeToNextTask);
        }

        if (indexFonction != nullptr) {
            indexFonction->callback();
            indexFonction->lastRun = micros();
            // Fin de updateTask
        }
    }
}

ISR(TIMER2_COMPA_vect) {
    isrCounter--;
    if (isrCounter == 1) {
        TCCR2B &= ~(1 << CS22);  // Stop Timer
        OCR2A = remainingMicros;
        TCCR2B |= (1 << CS22);  // Start Timer
        sleep_cpu();
    } else if (isrCounter == 0) {
        TCCR2B &= ~(1 << CS22);  // Stop Timer
        TIMSK2 &= ~(1 << OCIE2A);  // Disable Timer2 Compare Match A interrupt
        sleep_disable();
    } else {
        sleep_cpu();
    }
}