#include <arduino-timer.h>
#include "DHT.h"

// ################ freeMemory() ##############
#ifdef __arm__
// should use uinstd.h to define sbrk but Due causes a conflict
extern "C" char* sbrk(int incr);
#else  // __ARM__
extern char *__brkval;
#endif  // __arm__

int freeMemory() {
    char top;
#ifdef __arm__
    return &top - reinterpret_cast<char*>(sbrk(0));
#elif defined(CORE_TEENSY) || (ARDUINO > 103 && ARDUINO != 151)
    return &top - __brkval;
#else  // __arm__
    return __brkval ? &top - __brkval : &top - __malloc_heap_start;
#endif  // __arm__
}
// ############ EO: freeMemoy() ##############

struct PINs {
  const byte HE_BD = 6;  // Heating Element (Bottom Door)
  const byte HE_BC = 7;  // Heating Element (Bottom Center)
  const byte HE_BS = 8;  // Heating Element (Bottom Side)
  const byte HE_WS = 11; // Heating Element (Wall Side)
  const byte HE_WC = 12; // Heating Element (Wall Center)
};
PINs PINs;

struct Heating {

  /* Change this according to dev/prod */
  bool PIN_ON = HIGH;
  bool PIN_OFF = LOW;

  unsigned int PinUpTime = 1000;
  unsigned int PinDelay = 1;
  byte Mode = 2;

  unsigned int CurrPinDelay = 0;
  byte CurrPinID = 0;

  byte Modes[6][5] = {
          {}, // 0
          {}, // 1
          {PINs.HE_WC, PINs.HE_WS}, // Mode-2
          {PINs.HE_BD, PINs.HE_BC, PINs.HE_BS}, // Mode-3
          {}, // 4
          {PINs.HE_BD, PINs.HE_BC, PINs.HE_BS, PINs.HE_WC, PINs.HE_WS}, // Mode-5
  };

  byte Monitor[15];
  byte MonitorInterval = 100;
};
Heating Heating;

Timer<48> MainTimer; // Timer with 48 task slots

const byte OutputPins[] = { PINs.HE_BD, PINs.HE_BC, PINs.HE_BS, PINs.HE_WS, PINs.HE_WC};

/* For Serial Handling */
bool Serial_HasData = false;
char Serial_Params[4][10];

struct Serial_Values {
    unsigned int MID;
    unsigned long P1;
    unsigned long P2;
    unsigned long P3;
    unsigned int  Prev_MID;
    unsigned long Prev_P1;
    unsigned long Prev_P2;
    unsigned long Prev_P3;
};
Serial_Values Serial_Values;

struct Timers {
    Timer<>::Task Heating;
    Timer<>::Task TestTimer_10;
    Timer<>::Task TestTimer_11;
    Timer<>::Task TestTimer_12;
    Timer<>::Task TestTimer_13;
    Timer<>::Task TestTimer_14;
    Timer<>::Task TestTimer_15;
    Timer<>::Task TestTimer_16;
    Timer<>::Task TestTimer_17;
    Timer<>::Task TestTimer_18;
    Timer<>::Task TestTimer_19;
    Timer<>::Task TestTimer_20;
    Timer<>::Task TestTimer_21;
    Timer<>::Task TestTimer_22;
    Timer<>::Task TestTimer_23;
    Timer<>::Task TestTimer_24;
    Timer<>::Task TestTimer_25;
    Timer<>::Task TestTimer_26;
    Timer<>::Task TestTimer_27;
    Timer<>::Task TestTimer_28;
    Timer<>::Task TestTimer_29;
};
Timers Timers;

/* Stored Temperature Data */
struct TemperatureData {
    float Interior_Temp = 0;
    float Interior_Hum = 0;
    float LM35_BTM_Curr_Temp = 0;
    float LM35_WALL_Curr_Temp = 0;
    long  TemperatureAge;
    long  HumidityAge;

    float Temp_Old_1 = -127.00;
    float Temp_Old_2 = -127.00;
    float Temp_Old_3 = -127.00;
    float Temp_Old_4 = -127.00;
    float Temp_Old_5 = -127.00;

    float Temp_BTM_Old_1 = -127.00;
    float Temp_BTM_Old_2 = -127.00;
    float Temp_BTM_Old_3 = -127.00;
    float Temp_BTM_Old_4 = -127.00;
    float Temp_BTM_Old_5 = -127.00;

    float Temp_WALL_Old_1 = -127.00;
    float Temp_WALL_Old_2 = -127.00;
    float Temp_WALL_Old_3 = -127.00;
    float Temp_WALL_Old_4 = -127.00;
    float Temp_WALL_Old_5 = -127.00;
};
TemperatureData TemperatureData;

DHT dht_amb(4, DHT22);
DHT dht_btm(3, DHT22);
DHT dht_wall(2, DHT22);

struct Thermostat {
    float   SetTemp = 20.00;
    float   SafetyRelayActivationTemp = 30.00;
    float   WallMaxTemp = 35.00;
    float   BtmMaxTemp = 35.00;
    bool    Active = true;
    byte    CalculatedMode = 0;
};
Thermostat Thermostat;

void setup() {
  // put your setup code here, to run once:

    for(byte outputPin : OutputPins) {
      pinMode(outputPin, OUTPUT);
      digitalWrite(outputPin, Heating.PIN_OFF);
      Heating.Monitor[outputPin] = 0;
    }

    Serial.begin(9600);
    Serial.println("Started, Pam Pam!");

    dht_amb.begin();
    dht_btm.begin();
    dht_wall.begin();

    // Timers.Heating = MainTimer.every(Heating.PinUpTime, HandleHeating);
    // MainTimer.every(100, Heating_PinStatusMonitor);
    MainTimer.every(5000, SecondTrigger);

    
    Timers.TestTimer_10 = MainTimer.every(1000, TestFunc_10);
    Timers.TestTimer_11 = MainTimer.every(1000, TestFunc_11);
    Timers.TestTimer_12 = MainTimer.every(1000, TestFunc_12);
    Timers.TestTimer_13 = MainTimer.every(1000, TestFunc_13);
    Timers.TestTimer_14 = MainTimer.every(1000, TestFunc_14);
    Timers.TestTimer_15 = MainTimer.every(1000, TestFunc_15);

}

void loop() {
  MainTimer.tick<void>(); // avoids ticks() calculation
  HandleUSBSerial();
}

bool Thermostat_Handler(void *) {
    
    Analog_UpdateTemperatureHistory();

    if(!Thermostat.Active) {
        if(Heating.Mode != 0)
            Heating_Set(0, 1000, 0);
        return true;
    }
    
    if(
            TemperatureData.Temp_Old_1 == -127.00
        ||  TemperatureData.Temp_Old_2 == -127.00
        ||  TemperatureData.Temp_Old_3 == -127.00
        ||  TemperatureData.Temp_Old_4 == -127.00
        ||  TemperatureData.Temp_Old_5 == -127.00
        ||  TemperatureData.Temp_BTM_Old_1 == -127.00
        ||  TemperatureData.Temp_BTM_Old_2 == -127.00
        ||  TemperatureData.Temp_BTM_Old_3 == -127.00
        ||  TemperatureData.Temp_BTM_Old_4 == -127.00
        ||  TemperatureData.Temp_WALL_Old_1 == -127.00
        ||  TemperatureData.Temp_WALL_Old_2 == -127.00
        ||  TemperatureData.Temp_WALL_Old_3 == -127.00
        ||  TemperatureData.Temp_WALL_Old_5 == -127.00
    ) {
        return true;
    }

    Thermostat.CalculatedMode = 0;

    if(
        (
                Heating.Mode == 0
            &&  (
                        TemperatureData.Temp_Old_1 <= Thermostat.SetTemp
                    &&  TemperatureData.Temp_Old_2 <= Thermostat.SetTemp
                    &&  TemperatureData.Temp_Old_3 <= Thermostat.SetTemp
                    &&  TemperatureData.Temp_Old_4 <= Thermostat.SetTemp
                    &&  TemperatureData.Temp_Old_5 <= Thermostat.SetTemp
            )
        )

        ||
        
        (
                Heating.Mode != 0
            &&  (
                        TemperatureData.Temp_Old_1 <= Thermostat.SetTemp
                    ||  TemperatureData.Temp_Old_2 <= Thermostat.SetTemp
                    ||  TemperatureData.Temp_Old_3 <= Thermostat.SetTemp
                    ||  TemperatureData.Temp_Old_4 <= Thermostat.SetTemp
                    ||  TemperatureData.Temp_Old_5 <= Thermostat.SetTemp
            )
        )
    ) {
        // Bottom handling...
        if(
            (
                    (Heating.Mode == 0 || Heating.Mode == 2)
                &&  (
                            TemperatureData.Temp_BTM_Old_1 <= Thermostat.BtmMaxTemp
                        &&  TemperatureData.Temp_BTM_Old_2 <= Thermostat.BtmMaxTemp
                        &&  TemperatureData.Temp_BTM_Old_3 <= Thermostat.BtmMaxTemp
                        &&  TemperatureData.Temp_BTM_Old_4 <= Thermostat.BtmMaxTemp
                        &&  TemperatureData.Temp_BTM_Old_5 <= Thermostat.BtmMaxTemp
                            
                )
            )
            ||
            (
                    (Heating.Mode == 5 || Heating.Mode == 3)
                &&  (
                            TemperatureData.Temp_BTM_Old_1 <= Thermostat.BtmMaxTemp
                        ||  TemperatureData.Temp_BTM_Old_2 <= Thermostat.BtmMaxTemp
                        ||  TemperatureData.Temp_BTM_Old_3 <= Thermostat.BtmMaxTemp
                        ||  TemperatureData.Temp_BTM_Old_4 <= Thermostat.BtmMaxTemp
                        ||  TemperatureData.Temp_BTM_Old_5 <= Thermostat.BtmMaxTemp
                )
            )
        ) {
            Thermostat.CalculatedMode += 3;
        }
        
        // Wall handling...
        if(
            (
                    (Heating.Mode == 0 || Heating.Mode == 3)
                &&  (
                        TemperatureData.Temp_WALL_Old_1 <= Thermostat.BtmMaxTemp
                    &&  TemperatureData.Temp_WALL_Old_2 <= Thermostat.BtmMaxTemp
                    &&  TemperatureData.Temp_WALL_Old_3 <= Thermostat.BtmMaxTemp
                    &&  TemperatureData.Temp_WALL_Old_4 <= Thermostat.BtmMaxTemp
                    &&  TemperatureData.Temp_WALL_Old_5 <= Thermostat.BtmMaxTemp
                )
            )
            ||
            (
                    (Heating.Mode == 5 || Heating.Mode == 2)
                &&  (
                        TemperatureData.Temp_WALL_Old_1 <= Thermostat.BtmMaxTemp
                    ||  TemperatureData.Temp_WALL_Old_2 <= Thermostat.BtmMaxTemp
                    ||  TemperatureData.Temp_WALL_Old_3 <= Thermostat.BtmMaxTemp
                    ||  TemperatureData.Temp_WALL_Old_4 <= Thermostat.BtmMaxTemp
                    ||  TemperatureData.Temp_WALL_Old_5 <= Thermostat.BtmMaxTemp
                )
            )
        ) {
            Thermostat.CalculatedMode += 2;
        }
    }

    if(Thermostat.CalculatedMode != Heating.Mode) {
        Serial.print("THERMO: Setting new heating mode: [");
        Serial.print(Thermostat.CalculatedMode);
        Serial.println("]");

        Heating_Set(/* Mode */ Thermostat.CalculatedMode, /* UpTime */ 1000, /* Delay */0);
    }
}

void Analog_UpdateTemperatureHistory() {
    TemperatureData.Temp_BTM_Old_5 = TemperatureData.Temp_BTM_Old_4;
    TemperatureData.Temp_BTM_Old_4 = TemperatureData.Temp_BTM_Old_3;
    TemperatureData.Temp_BTM_Old_3 = TemperatureData.Temp_BTM_Old_2;
    TemperatureData.Temp_BTM_Old_2 = TemperatureData.Temp_BTM_Old_1;
    TemperatureData.Temp_BTM_Old_1 = TemperatureData.LM35_BTM_Curr_Temp;

    TemperatureData.Temp_WALL_Old_5 = TemperatureData.Temp_WALL_Old_4;
    TemperatureData.Temp_WALL_Old_4 = TemperatureData.Temp_WALL_Old_3;
    TemperatureData.Temp_WALL_Old_3 = TemperatureData.Temp_WALL_Old_2;
    TemperatureData.Temp_WALL_Old_2 = TemperatureData.Temp_WALL_Old_1;
    TemperatureData.Temp_WALL_Old_1 = TemperatureData.LM35_WALL_Curr_Temp;
}

bool SecondTrigger(void *) {
    TemperatureUpdater();
    HTU21D_UpdateTemperatures();
    // Analog_UpdateTemperatureHistory();
    Thermostat_Handler((void *) false);
    // SerialPrinter();
    Serial.print("Timers: ");
    Serial.println(MainTimer.size());
    return true;
}

void SerialPrinter() {
    Serial.print("Int - Temp: ");
    Serial.print(TemperatureData.Interior_Temp);
    Serial.print("'C (");
    Serial.print((int) ((millis() - TemperatureData.TemperatureAge) / 1000));
    Serial.print("s ago) | Hum: ");
    Serial.print(TemperatureData.Interior_Hum);
    Serial.print("% (");
    Serial.print((int) ((millis() - TemperatureData.HumidityAge) / 1000));
    Serial.println("s ago)");

    Serial.print("Temp. History: ");
    Serial.print(TemperatureData.Temp_Old_1);
    Serial.print(" | ");
    Serial.print(TemperatureData.Temp_Old_2);
    Serial.print(" | ");
    Serial.print(TemperatureData.Temp_Old_3);
    Serial.print(" | ");
    Serial.print(TemperatureData.Temp_Old_4);
    Serial.print(" | ");
    Serial.println(TemperatureData.Temp_Old_5);

    Serial.print("BTM. History: ");
    Serial.print(TemperatureData.Temp_BTM_Old_1);
    Serial.print(" | ");
    Serial.print(TemperatureData.Temp_BTM_Old_2);
    Serial.print(" | ");
    Serial.print(TemperatureData.Temp_BTM_Old_3);
    Serial.print(" | ");
    Serial.print(TemperatureData.Temp_BTM_Old_4);
    Serial.print(" | ");
    Serial.println(TemperatureData.Temp_BTM_Old_5);

    Serial.print("WALL History: ");
    Serial.print(TemperatureData.Temp_WALL_Old_1);
    Serial.print(" | ");
    Serial.print(TemperatureData.Temp_WALL_Old_2);
    Serial.print(" | ");
    Serial.print(TemperatureData.Temp_WALL_Old_3);
    Serial.print(" | ");
    Serial.print(TemperatureData.Temp_WALL_Old_4);
    Serial.print(" | ");
    Serial.println(TemperatureData.Temp_WALL_Old_5);

    Serial.print("Thermostat Set [");
    Serial.print(Thermostat.Active);
    Serial.print("] | Temps - Int: [");
    Serial.print(Thermostat.SetTemp);
    Serial.print("] | Wall: [");
    Serial.print(Thermostat.WallMaxTemp);
    Serial.print("] | Btm: [");
    Serial.print(Thermostat.BtmMaxTemp);
    Serial.println("]");

    Serial.println();
}

void TemperatureUpdater() {
    TemperatureData.Interior_Temp = dht_amb.readTemperature();
    TemperatureData.Interior_Hum = dht_amb.readHumidity();
    TemperatureData.LM35_BTM_Curr_Temp = dht_btm.readTemperature();
    TemperatureData.LM35_WALL_Curr_Temp = dht_wall.readTemperature();
    TemperatureData.TemperatureAge = millis();
    TemperatureData.HumidityAge = millis();
}

void HTU21D_UpdateTemperatures() {
    TemperatureData.Temp_Old_5 = TemperatureData.Temp_Old_4;
    TemperatureData.Temp_Old_4 = TemperatureData.Temp_Old_3;
    TemperatureData.Temp_Old_3 = TemperatureData.Temp_Old_2;
    TemperatureData.Temp_Old_2 = TemperatureData.Temp_Old_1;
    TemperatureData.Temp_Old_1 = TemperatureData.Interior_Temp;
}

bool Heating_Handler(void *) {
    if(
            Heating.PinDelay > 0
            &&  Heating.CurrPinID == 0
            ) {
        Heating_PinsHandler(0);
        if(Heating.CurrPinDelay++ < Heating.PinDelay) {
            Serial.print("Delaying #");
            Serial.println(Heating.CurrPinDelay);
            return true;
        } else {
            Heating.CurrPinDelay = 0;
        }
    }

    Heating_PinsHandler(Heating.Modes[Heating.Mode][Heating.CurrPinID]);

    switch(Heating.Mode) {
        case 5: case 3: case 2:
            if(++Heating.CurrPinID >= Heating.Mode)
                Heating.CurrPinID = 0;
            return true;

        default:
            Serial.println("HE (TIMER_WARNING): STOPPED_DUE_TO_MODE-0");
            Heating_PinsHandler(0);
            return false;
    }
}

void Heating_PinsHandler(byte GPIO) {
    for(byte currGPIO : Heating.Modes[5]) {
        digitalWrite(currGPIO, Heating.PIN_OFF);
    }

    if(GPIO > 0) {
        /*
        Serial.print("HE_PH: Current PIN - ");
        Serial.println(GPIO);
        */
        digitalWrite(GPIO, Heating.PIN_ON);
    } else {
        /*
        Serial.println("HE_PH: ALL_OFF");
        */
    }
}

bool Heating_Set(byte Mode, unsigned int UpTime, unsigned int Delay) {

    if(
            Heating.Mode == Mode
            &&  Heating.PinDelay == Delay
            &&  Heating.PinUpTime == UpTime
            ) {
        Serial.println("HE: NOT_SETTING / TIMER_ACTIVE / SAME_SETTINGS");
        return true;
    }

    Heating.Mode = Mode;
    Heating.PinDelay = Delay;
    if(UpTime >= 250) {
        Heating.PinUpTime = UpTime;
    }

    switch(Mode) {
        case 0:
            MainTimer.cancel(Timers.Heating);
            Heating_PinsHandler(0);
            Serial.println("HE: STOPPED");
            return true;

        case 2: case 3: case 5:
            MainTimer.cancel(Timers.Heating);
            Heating_PinsHandler(0);
            Serial.println("HE: SETTING_NEW_TIMER");
            Heating.CurrPinDelay = 0;
            Heating.CurrPinID = 0;
            Timers.Heating = MainTimer.every(Heating.PinUpTime, Heating_Handler);
            return true;

        default:
            return false;
    }

}

bool Heating_PinStatusMonitor(void *) {
    for(byte currGPIO : Heating.Modes[5]) {
        if(digitalRead(currGPIO) == Heating.PIN_ON)
          Heating.Monitor[currGPIO]++;
        else
          Heating.Monitor[currGPIO] = 0;

        if(Heating.Monitor[currGPIO] > ((int) (Heating.PinUpTime / Heating.MonitorInterval) + 1)) {
          Serial.print("HE (WARNING): PIN ");
          Serial.print(currGPIO);
          Serial.println(" STATE -> STUCK_ON, TURNING OFF.");
          digitalWrite(currGPIO, Heating.PIN_OFF);
        }
    }

    return true;
}

void HandleUSBSerial() {
        Serial_GetData();
        Serial_HandleData();
}

void Serial_GetData() {
    static byte i = 0;
    static byte param_id = 0;
    static char rc;

    while (Serial.available() > 0 && Serial_HasData == false) {
        rc = Serial.read();

        if(rc == '\n') {
            Serial_HasData = true;
            i = 0;
            param_id = 0;
            continue;
        }

        if(param_id > 3) {
            continue;
        }

        if(rc == '_' || rc == ':') {
            Serial_Params[param_id][i] = '\0';
            i = 0;
            param_id++;
            continue;
        }

        Serial_Params[param_id][i++] = rc;
    }
}

void Serial_HandleData() {
    if (Serial_HasData) {
        sscanf(Serial_Params[0], "%d", &Serial_Values.MID);
        memset(Serial_Params[0], 0, sizeof Serial_Params[0]);

        sscanf(Serial_Params[1], "%d", &Serial_Values.P1);
        memset(Serial_Params[1], 0, sizeof Serial_Params[1]);
        
        sscanf(Serial_Params[2], "%d", &Serial_Values.P2);
        memset(Serial_Params[2], 0, sizeof Serial_Params[2]);

        sscanf(Serial_Params[3], "%d", &Serial_Values.P3);
        memset(Serial_Params[3], 0, sizeof Serial_Params[3]);
        
        Serial.print("MID [");
        Serial.print(Serial_Values.MID);
        Serial.print("] -> P1 ");
        Serial.print(Serial_Values.P1);
        Serial.print(" | P2: [");
        Serial.print(Serial_Values.P2);
        Serial.print("] | P3: ");
        Serial.println(Serial_Values.P3);

        if(
                Serial_Values.MID == Serial_Values.Prev_MID
            &&  Serial_Values.P1 == Serial_Values.Prev_P1
            &&  Serial_Values.P2 == Serial_Values.Prev_P2
            &&  Serial_Values.P3 == Serial_Values.Prev_P3
        ) {
            Serial_SwitchActions();
            Serial_Values.MID == Serial_Values.Prev_MID;
            Serial_Values.Prev_P1 = 0;
            Serial_Values.Prev_P2 = 0;
            Serial_Values.Prev_P3 = 0;
        } else {
            Serial_Values.Prev_MID = Serial_Values.MID;
            Serial_Values.Prev_P1 = Serial_Values.P1;
            Serial_Values.Prev_P2 = Serial_Values.P2;
            Serial_Values.Prev_P3 = Serial_Values.P3;
            Serial.println("Please confirm.");
        }

        Serial_Values.MID = 0;
        Serial_Values.P1 = 0;
        Serial_Values.P2 = 0;
        Serial_Values.P3 = 0;
        Serial_HasData = false;
    }
}

bool Serial_SwitchActions() {
    switch(Serial_Values.MID) {

        case 1:
            Serial.println("SERIAL: Testing & Works!");
        break;

        /*
        case 15:
            if(Serial_Values.P2 > 255) {
                Serial_Values.P2 = 255;
            }
            Serial.print(F("SERIAL: analogWrite("));
            Serial.print(Serial_Values.P1);
            Serial.print(", ");
            Serial.print(Serial_Values.P2);
            Serial.println(");");
            analogWrite(Serial_Values.P1, Serial_Values.P2);
        break;

        case 22:
            if(!(Serial_Values.P2 == 0 || Serial_Values.P2 == 1)) {
                Serial.println("SERIAL: digitalWrite can only take P2 as 0 or 1");
                return false;
            }
            Serial.print("SERIAL: digitalWrite(");
            Serial.print(Serial_Values.P1);
            Serial.print(", ");
            Serial.print(Serial_Values.P2);
            Serial.println(");");
            digitalWrite(Serial_Values.P1, Serial_Values.P2);
        break;
        */

        case 50: // Heating Module Handling
            // startHeating(/* Mode */ Serial_Values.P1, /* UpTime */ Serial_Values.P2, /* Delay */Serial_Values.P3);
            // HeatingCommand(/* Mode */ Serial_Values.P1, /* UpTime */ Serial_Values.P2, /* Delay */Serial_Values.P3);
            Heating_Set(/* Mode */ Serial_Values.P1, /* UpTime */ Serial_Values.P2, /* Delay */Serial_Values.P3);
        break;

        case 55:
            Serial.print("Thermostat module ");
            switch(Serial_Values.P1) {
                case 1:
                    Thermostat.Active = true;
                    Serial.println("ON");
                break;

                case 0:
                    Thermostat.Active = false;
                    Serial.println("OFF");
                break;
            }
        break;

        case 60:
            if(Serial_Values.P1 > 0 && Serial_Values.P1 < 30) {
                Thermostat.SetTemp = Serial_Values.P1;
            }

            if(Serial_Values.P2 > 0 && Serial_Values.P2 <= 40) {
                Thermostat.BtmMaxTemp = Serial_Values.P2;
            }

            if(Serial_Values.P3 > 0 && Serial_Values.P3 <= 40) {
                Thermostat.WallMaxTemp = Serial_Values.P3;
            }

            Serial.print("Thermostat Temperatures: Ambient [");
            Serial.print(Thermostat.SetTemp);
            Serial.print("] - Bottom [");
            Serial.print(Thermostat.BtmMaxTemp);
            Serial.print("] - Wall [");
            Serial.print(Thermostat.WallMaxTemp);
            Serial.println("]");
        break;

        case 16:
            Timers.TestTimer_16 = MainTimer.every(1000, TestFunc_16);
        break;

        case 17:
            Timers.TestTimer_17 = MainTimer.every(1000, TestFunc_17);
        break;

        case 18:
            Timers.TestTimer_18 = MainTimer.every(1000, TestFunc_18);
        break;

        case 19:
            Timers.TestTimer_19 = MainTimer.every(1000, TestFunc_19);
        break;

        case 20:
            Timers.TestTimer_20 = MainTimer.every(1000, TestFunc_20);
        break;

        case 21:
            Timers.TestTimer_21 = MainTimer.every(1000, TestFunc_21);
        break;

        case 22:
            Timers.TestTimer_22 = MainTimer.every(1000, TestFunc_22);
        break;

        case 23:
            Timers.TestTimer_23 = MainTimer.every(1000, TestFunc_23);
        break;

        case 24:
            Timers.TestTimer_24 = MainTimer.every(1000, TestFunc_24);
        break;

        case 25:
            Timers.TestTimer_25 = MainTimer.every(1000, TestFunc_25);
        break;

        case 26:
            Timers.TestTimer_26 = MainTimer.every(1000, TestFunc_26);
        break;

        case 27:
            Timers.TestTimer_27 = MainTimer.every(1000, TestFunc_27);
        break;

        case 28:
            Timers.TestTimer_28 = MainTimer.every(1000, TestFunc_28);
        break;

        case 29:
            Timers.TestTimer_29 = MainTimer.every(1000, TestFunc_29);
        break;


        default:
            Serial.println("SERIAL: UNK_ACT");
    }
}

bool TestFunc_10(void *) {
    Serial.println("TestFunc_10 Triggered");
    return true;
}

bool TestFunc_11(void *) {
    Serial.println("TestFunc_11 Triggered");
    return true;
}

bool TestFunc_12(void *) {
    Serial.println("TestFunc_12 Triggered");
    return true;
}

bool TestFunc_13(void *) {
    Serial.println("TestFunc_13 Triggered");
    return true;
}

bool TestFunc_14(void *) {
    Serial.println("TestFunc_14 Triggered");
    return true;
}

bool TestFunc_15(void *) {
    Serial.println("TestFunc_15 Triggered");
    return true;
}

bool TestFunc_16(void *) {
    Serial.println("TestFunc_16 Triggered");
    return true;
}

bool TestFunc_17(void *) {
    Serial.println("TestFunc_17 Triggered");
    return true;
}

bool TestFunc_18(void *) {
    Serial.println("TestFunc_18 Triggered");
    return true;
}

bool TestFunc_19(void *) {
    Serial.println("TestFunc_19 Triggered");
    return true;
}

bool TestFunc_20(void *) {
    Serial.println("TestFunc_10 Triggered");
    return true;
}

bool TestFunc_21(void *) {
    Serial.println("TestFunc_21 Triggered");
    return true;
}

bool TestFunc_22(void *) {
    Serial.println("TestFunc_22 Triggered");
    return true;
}

bool TestFunc_23(void *) {
    Serial.println("TestFunc_23 Triggered");
    return true;
}

bool TestFunc_24(void *) {
    Serial.println("TestFunc_24 Triggered");
    return true;
}

bool TestFunc_25(void *) {
    Serial.println("TestFunc_25 Triggered");
    return true;
}

bool TestFunc_26(void *) {
    Serial.println("TestFunc_26 Triggered");
    return true;
}

bool TestFunc_27(void *) {
    Serial.println("TestFunc_27 Triggered");
    return true;
}

bool TestFunc_28(void *) {
    Serial.println("TestFunc_28 Triggered");
    return true;
}

bool TestFunc_29(void *) {
    Serial.println("TestFunc_29 Triggered");
    return true;
}