

#ifndef TG_TIMEDFUN_H
#define TG_TIMEDFUN_H

#include <vector>
#include <stdint.h>
#include <Arduino.h>

typedef void (*timedfun_ptr)();

class TimedFun {
public:
    TimedFun() {};

    void registerFun(timedfun_ptr p, uint64_t s) {
        funmap.push_back(std::pair<timedfun_ptr,uint64_t>(p,s));
    };

    void registerFunCond(timedfun_ptr p, uint64_t s) {
        funmap.push_back(std::pair<timedfun_ptr,uint64_t>(p,s));
    };

    void tick() {
        ++counter;
    }

    void tick(int64_t m) {
        counter += m;
    }

    void setTick(int64_t m) {
        counter = m;
        old_counter = m;
    }

    void update() {
        uint64_t ccounter = old_counter;
        old_counter = counter;
        for (const auto &e : funmap) {
            if( ((counter % e.second == 0) && (counter != ccounter)) ||
                    ((counter-ccounter) >= e.second) || ((counter%e.second) < (ccounter%e.second))) {
                e.first();
            }
        }
    };

    void updateForce() {
        Serial.println("TF: Force updating all");
        for (const auto &e : funmap) {
            e.first();
        }
        Serial.println("TF: OK");
    }

private:
    uint64_t counter = 0;
    uint64_t old_counter = 0;
    std::vector<std::pair<timedfun_ptr,uint64_t>> funmap;

};

#endif