scheduler.h

// scheduler.h - the muwerk simple scheduler
 
#pragma once
 
#include "ustd_platform.h"
#include "ustd_array.h"
#include "ustd_queue.h"
#include "ustd_functional.h"
#include "muwerk.h"
 
#include <stdio.h>
 
#if defined(__ESP__) || defined(__ESP32__) || defined(__UNIXOID__) || defined(__RP_PICO__)
#include <functional>
#endif
 
namespace ustd {
 
#define SCHEDULER_MAIN 0
 
enum T_PRIO {
    PRIO_SYSTEMCRITICAL = 0,  
    PRIO_TIMECRITICAL = 1,    
    PRIO_HIGH = 2,            
    PRIO_NORMAL = 3,          
    PRIO_LOW = 4,             
    PRIO_LOWEST = 5           
};
 
enum T_MSGTYPE {
    MSG_NONE = 0,
    MSG_DIRECT = 1,
    MSG_SUBSCRIBE = 2,
    MSG_UNSUBSCRIBE = 3,
    MSG_PUBLISH = 4,
    MSG_PUBLISHRAW = 5
};
 
#if defined(__ESP__) || defined(__ESP32__) || defined(__UNIXOID__) || defined(__RP_PICO__)
typedef std::function<void()> T_TASK;
#elif defined(__ATTINY__)
typedef void (*T_TASK)();
#else
typedef ustd::function<void()> T_TASK;
#endif
 
typedef struct {
    char *originator;
    char *topic;
    char *msg;
} T_MSG;
 
#if defined(__ESP__) || defined(__ESP32__) || defined(__UNIXOID__) || defined(__RP_PICO__)
typedef std::function<void(String topic, String msg, String originator)> T_SUBS;
#elif defined(__ATTINY__)
typedef void (*T_SUBS)(String topic, String msg, String originator);
#else
typedef ustd::function<void(String topic, String msg, String originator)> T_SUBS;
#endif
 
typedef struct {
    int subscriptionHandle;
    int taskID;
    char *originator;
    char *topic;
    T_SUBS subs;
} T_SUBSCRIPTION;
 
typedef struct {
    int taskID;
    char *szName;
    T_TASK task;
    T_PRIO prio;
    unsigned long minMicros;
    unsigned long lastCall;
#if USTD_FEATURE_MEMORY > USTD_FEATURE_MEM_512B
    unsigned long lateTime;
    unsigned long cpuTime;
    unsigned long callCount;
#endif
} T_TASKENTRY;
 
// forward declaration
class Console;
 
class Scheduler {
  private:
    friend class Console;
    ustd::array<T_TASKENTRY> taskList;
    ustd::queue<T_MSG *> msgqueue;
    ustd::array<T_SUBSCRIPTION> subscriptionList;
    int subscriptionHandle;
    int taskID;
    bool bSingleTaskMode = false;
    int singleTaskID = -1;
#if USTD_FEATURE_MEMORY > USTD_FEATURE_MEM_512B
    bool bGenStats = false;
    unsigned long statIntervallMs = 0;
    unsigned long statTimer;
    unsigned long systemTimer;
    unsigned long systemTime = 0;
    unsigned long appTimer;
    unsigned long appTime = 0;
    unsigned long mainTime = 0;  // Time spent with SCHEDULER_MAIN id.
#endif
    unsigned long upTime = 0;  // Seconds system is running
    unsigned long upTimeTicker = 0;
    int currentTaskID = -2;  // TaskID that is currently been executed
 
  public:
    Scheduler(int nTaskListSize = 2, int queueSize = 2, int nSubscriptionListSize = 2)
        : taskList(nTaskListSize), msgqueue(queueSize), subscriptionList(nSubscriptionListSize) {
        subscriptionHandle = 0;
        taskID = 0;  // 0 is SCHEDULER_MAIN
        upTime = 0;
        upTimeTicker = micros();
#if USTD_FEATURE_MEMORY > USTD_FEATURE_MEM_512B
        resetStats(true);
#endif
 
#if defined(__ESP__) && !defined(__ESP32__) && !defined(__ESP32_RISC__)
        ESP.wdtDisable();
        ESP.wdtEnable(WDTO_8S);
#endif
    }
 
#ifndef __ATTINY__
    virtual ~Scheduler() {
        for (unsigned int i = 0; i < taskList.length(); i++) {
            if (taskList[i].szName != nullptr)
                free(taskList[i].szName);
        }
        int l = msgqueue.length();
        for (int i = 0; i < l; i++) {
            msgqueue.pop();
        }
    }
#endif
 
    static bool mqttmatch(const String pubstr, const String substr) {
        if (pubstr == substr)
            return true;
 
        // Attiny compile: core needs to be extended, add c_str():
        // In WString.h add:  const char *c_str() const { return buffer; }
        const char *pub = (const char *)pubstr.c_str();
        const char *sub = (const char *)substr.c_str();
 
        int lp = strlen(pub);
        int ls = strlen(sub);
 
        bool wPos = true;  // sub wildcard is legal now
        int ps = 0;
        for (int pp = 0; pp < lp; pp++) {
            if (pub[pp] == '+' || pub[pp] == '#') {
                return false;  // Illegal wildcards in pub
            }
            if (wPos) {
                wPos = false;
                if (sub[ps] == '#') {
                    if (ps == ls - 1) {
                        return true;
                    } else {
                        return false;  // In sub, # must not be followed by
                                       // anything else
                    }
                }
                if (sub[ps] == '+') {
                    while (pp < lp && pub[pp] != '/')
                        ++pp;
                    ++ps;
                    if (pp == lp) {
                        if (ps == ls) {
                            return true;
                        } else if (!strcmp(&sub[ps], "/#")) {
                            return true;
                        }
                    }
                }
            } else {
                if (sub[ps] == '+' || sub[ps] == '#') {
                    return false;  // Illegal wildcard-position
                }
            }
            if (pub[pp] != sub[ps] && strcmp(&sub[ps], "/#")) {
                return false;
            }
            if (pub[pp] == '/')
                wPos = true;
            if (pp == lp - 1) {
                if (ps == ls - 1) {
                    return true;
                }
                if (!strcmp(&sub[ps + 1], "/#") || !strcmp(&sub[ps + 1], "#") ||
                    !strcmp(&sub[ps + 1], "+")) {
                    return true;
                }
                return false;
            }
            ++ps;
        }
        if (ps == ls) {
            return true;
        } else {
            return false;
        }
    }
 
  private:
    int getIndexFromTaskID(int taskID) {
        for (unsigned int i = 0; i < taskList.length(); i++) {
            if (taskList[i].taskID == taskID) {
                return i;
            }
        }
        return -1;
    }
 
#if USTD_FEATURE_MEMORY > USTD_FEATURE_MEM_512B
    bool schedReceive(const char *topic, const char *msg) {
        const char *p0, *p1;
        p0 = topic ? topic : "";
        p1 = strchr(p0, '/');
        if (p1) {
            ++p1;
            if (!strcmp(p1, "stat/get")) {
                statIntervallMs = msg ? atoi(msg) : 0;
                if (statIntervallMs) {
                    bGenStats = true;
                    resetStats(true);
                } else
                    bGenStats = false;
                return true;
            }
        }
        return false;
    }
#endif
 
  public:
    bool publish(String topic, String msg = "", String originator = "") {
#if USTD_FEATURE_MEMORY > USTD_FEATURE_MEM_512B
        if (!strncmp(topic.c_str(), "$SYS", 4))
            if (schedReceive(topic.c_str(), msg.c_str()))
                return true;
#endif
        T_MSG *pMsg = (T_MSG *)malloc(sizeof(T_MSG) +
                                      (3 + originator.length() + topic.length() + msg.length()) *
                                          sizeof(char));
        if (pMsg) {
            pMsg->originator = (char *)(&pMsg[1]);
            pMsg->topic = pMsg->originator + ((originator.length() + 1) * sizeof(char));
            pMsg->msg = pMsg->topic + ((topic.length() + 1) * sizeof(char));
            strcpy(pMsg->originator, originator.c_str());
            strcpy(pMsg->topic, topic.c_str());
            strcpy(pMsg->msg, msg.c_str());
            return msgqueue.push(pMsg);
        }
        return false;
    }
 
    int subscribe(int taskID, String topic, T_SUBS subs, String originator = "") {
        T_SUBSCRIPTION sub = {};
        sub.taskID = taskID;
        sub.subs = subs;
        sub.subscriptionHandle = subscriptionHandle + 1;
        sub.topic = (char *)malloc((topic.length() + originator.length() + 2) * sizeof(char));
        if (sub.topic) {
            sub.originator = sub.topic + ((topic.length() + 1) * sizeof(char));
            strcpy(sub.topic, topic.c_str());
            strcpy(sub.originator, originator.c_str());
            if (subscriptionList.add(sub) != -1) {
                ++subscriptionHandle;
                return subscriptionHandle;
            }
            // free up memory
            free(sub.topic);
        }
        return -1;
    }
 
    bool unsubscribe(int subscriptionHandle) {
        for (unsigned int i = 0; i < subscriptionList.length(); i++) {
            if (subscriptionList[i].subscriptionHandle == subscriptionHandle) {
                free(subscriptionList[i].topic);
                subscriptionList.erase(i);
                return true;
            }
        }
        return false;
    }
 
  private:
    void checkMsgQueue() {
        T_MSG *pMsg;
        while ((pMsg = msgqueue.pop()) != nullptr) {
            for (unsigned int i = 0; i < subscriptionList.length(); i++) {
                if (mqttmatch(pMsg->topic, subscriptionList[i].topic)) {
                    if (*(pMsg->originator) != 0)
                        if (strcmp(subscriptionList[i].originator, pMsg->originator) == 0) {
                            continue;
                        }
                    subscriptionList[i].subs(pMsg->topic, pMsg->msg, pMsg->originator);
 
#if USTD_FEATURE_MEMORY > USTD_FEATURE_MEM_512B
                    unsigned long startTime = micros();
                    if (subscriptionList[i].taskID != SCHEDULER_MAIN) {
                        int ind = getIndexFromTaskID(subscriptionList[i].taskID);
                        if (ind != -1)
                            taskList[ind].cpuTime += timeDiff(startTime, micros());
                    } else {
                        mainTime += timeDiff(startTime, micros());
                    }
#endif
                }
            }
            free(pMsg);
        }
    }
 
  public:
    int add(T_TASK task, String name, unsigned long minMicroSecs = 100000L,
            T_PRIO prio = PRIO_NORMAL) {
        T_TASKENTRY taskEnt = {};
        taskEnt.taskID = taskID + 1;
        taskEnt.task = task;
        taskEnt.minMicros = minMicroSecs;
        taskEnt.prio = prio;
        if (name.length()) {
            taskEnt.szName = (char *)malloc(name.length() + 1);
            if (!taskEnt.szName) {
                return -1;
            }
            strcpy(taskEnt.szName, name.c_str());
        } else {
            taskEnt.szName = nullptr;
        }
        if (taskList.add(taskEnt) >= 0) {
            ++taskID;
            return taskID;
        }
        if (taskEnt.szName) {
            free(taskEnt.szName);
        }
        return -1;
    }
 
    bool remove(int taskID) {
        if (currentTaskID == taskID) {
            return false;  // A task can't delete itself.
        }
        for (unsigned int i = 0; i < taskList.length(); i++) {
            if (taskList[i].taskID == taskID) {
                if (taskList[i].szName != nullptr)
                    free(taskList[i].szName);
                taskList.erase(i);
                return true;
            }
        }
        return false;
    }
 
    bool reschedule(int taskID, unsigned long minMicroSecs = 100000L, T_PRIO prio = PRIO_NORMAL) {
        for (unsigned int i = 0; i < taskList.length(); i++) {
            if (taskList[i].taskID == taskID) {
                taskList[i].minMicros = minMicroSecs;
                return true;
            }
        }
        return false;
    }
 
    unsigned long getUptime() {
        return upTime;
    }
 
    void singleTaskMode(int _singleTaskID) {
        singleTaskID = _singleTaskID;
        if (_singleTaskID == -1) {
            bSingleTaskMode = false;
        } else {
            bSingleTaskMode = true;
        }
    }
 
  private:
    void runTask(T_TASKENTRY *pTaskEnt) {
        unsigned long startTime = micros();
        unsigned long tDelta = timeDiff(pTaskEnt->lastCall, startTime);
        if (tDelta >= pTaskEnt->minMicros && pTaskEnt->minMicros) {
            currentTaskID = pTaskEnt->taskID;  // prevent task() to delete itself.
            pTaskEnt->task();
            currentTaskID = -2;
            pTaskEnt->lastCall = startTime;
#if USTD_FEATURE_MEMORY > USTD_FEATURE_MEM_512B
            pTaskEnt->lateTime += tDelta - pTaskEnt->minMicros;
            pTaskEnt->cpuTime += timeDiff(startTime, micros());
            ++pTaskEnt->callCount;
#endif
        }
    }
 
#if USTD_FEATURE_MEMORY > USTD_FEATURE_MEM_512B
    void resetStats(bool bHard = false) {
        for (unsigned int i = 0; i < taskList.length(); i++) {
            taskList[i].cpuTime = 0;
            taskList[i].lateTime = 0;
            taskList[i].callCount = 0;
        }
        statTimer = micros();
        if (bHard)
            systemTimer = micros();
        systemTime = 0;
        appTime = 0;
        mainTime = 0;
    }
 
    void checkStats() {
        if (!bGenStats || !statIntervallMs)
            return;
        unsigned long now = micros();
        unsigned long tDelta = timeDiff(statTimer, now);
#ifdef USTD_FEATURE_FREE_MEMORY
        unsigned long mem = (unsigned long)freeMemory();
#else
        unsigned long mem = 0;
#pragma message("freeMemory() is not implemented for this platform.")
#endif
        if (tDelta > statIntervallMs * 1000) {
            // local stats
            for (unsigned int i = 0; i < taskList.length(); i++) {
            }
            // mqtt stats
            const char *null_name = "<null>";
            const char *skeleton_head =
                "{\"dt\":%ld,\"syt\":%ld,\"apt\":%ld,"
                "\"mat\":%ld,\"upt\":%ld,\"mem\":%ld,\"tsks\":%ld,\"tdt\":[";
            const char *skeleton_tail = "]}";
            const char *bone = "[\"%s\",%ld,%ld,%ld,%ld,%ld],";
            unsigned long memreq =
                strlen(skeleton_head) + 7 * 7 + (strlen(bone) + 7 * 5) * taskList.length();
            for (unsigned int i = 0; i < taskList.length(); i++) {
                if (taskList[i].szName == nullptr)
                    memreq += strlen(null_name);
                else
                    memreq += strlen(taskList[i].szName);
            }
            memreq += 1;
            char *jsonstr = (char *)malloc(memreq);
            if (jsonstr != nullptr) {
                memset(jsonstr, 0, memreq);
                sprintf(jsonstr, skeleton_head, tDelta, systemTime, appTime, mainTime, upTime, mem,
                        (long)taskList.length());
                for (unsigned int i = 0; i < taskList.length(); i++) {
                    char *p = &jsonstr[strlen(jsonstr)];
                    if (taskList[i].szName == nullptr) {
                        sprintf(p, bone, null_name, (long)taskList[i].taskID, taskList[i].minMicros,
                                taskList[i].callCount, taskList[i].cpuTime, taskList[i].lateTime);
                    } else {
                        sprintf(p, bone, taskList[i].szName, (long)taskList[i].taskID,
                                taskList[i].minMicros, taskList[i].callCount, taskList[i].cpuTime,
                                taskList[i].lateTime);
                    }
                }
                char *p = &jsonstr[strlen(jsonstr)];
                if (taskList.length() > 0)
                    --p;  // no final ','
                strcpy(p, skeleton_tail);
 
                //#ifdef USE_SERIAL_DBG
                // Serial.print(memreq);
                // Serial.print(" ");
                // Serial.println(strlen(jsonstr));
                // Serial.println(jsonstr);
                // char brz[50];
                // sprintf(brz, "%ld,%ld", memreq, strlen(jsonstr));
                // String msg = "JSONMEM: " + String(brz);
                // publish("$SYS/stat", msg, "scheduler-dbg");
                //#endif
                publish("$SYS/stat", jsonstr, "scheduler");
                free(jsonstr);
            }
            resetStats(false);
        }
    }
#endif
 
  public:
    void loop() {
        unsigned long current = micros();
        if (timeDiff(upTimeTicker, current) > 1000000L) {
            upTime += 1;
            upTimeTicker += 1000000;
        }
#if USTD_FEATURE_MEMORY > USTD_FEATURE_MEM_512B
        systemTime += timeDiff(systemTimer, current);
        appTimer = current;
#endif
        if (!bSingleTaskMode) {
#if USTD_FEATURE_MEMORY > USTD_FEATURE_MEM_512B
            checkStats();
#endif
            checkMsgQueue();
        }
        for (unsigned int i = 0; i < taskList.length(); i++) {
            if (!bSingleTaskMode) {
                checkMsgQueue();
                runTask(&taskList[i]);
            } else {
                if (taskList[i].taskID == singleTaskID) {
                    runTask(&taskList[i]);
                }
            }
#if defined(__ESP__) && !defined(__ESP32__)
            appTime += timeDiff(appTimer, micros());
            systemTimer = micros();
            yield();
            systemTime += timeDiff(systemTimer, micros());
            appTimer = micros();
#endif
        }
#if USTD_FEATURE_MEMORY > USTD_FEATURE_MEM_512B
        appTime += timeDiff(appTimer, micros());
        systemTimer = micros();
#endif
#if defined(__ESP__) && !defined(__ESP32__) && !defined(__ESP32_RISC__)
        ESP.wdtFeed();
#endif
    }
};  // namespace ustd
}  // namespace ustd