#include <arduino-timer.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 {
  unsigned int PinUpTime = 1000;
  unsigned int PinDelay = 0;

  byte On = false;
  byte CurrPinID = 0;
  byte CurrMode = 0;
  byte CurrModeLength = 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
  };
};
Heating Heating;

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

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

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

    for(byte outputPin : OutputPins) {
      pinMode(outputPin, OUTPUT);
      digitalWrite(outputPin, LOW);
    }

    Serial.begin(9600);

    startHeating(5, 50, 0);

    // MainTimer.every(10 * 1000, [](void*) -> bool {
    //   if(!Heating.On) {
    //     startHeating(5);
    //   } else {
    //     stopHeating();
    //   }
    //   return true;
    // });

    MainTimer.every(1000, [](void*) -> bool {
      Serial.print("Timers: ");
      Serial.print(MainTimer.size());

      Serial.print(" | Free RAM: ");
      Serial.println(freeMemory(), DEC);
      return true;
    });
}

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

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

    if(UpTime == 0)
      return false;

    Heating.PinUpTime = UpTime;
    Heating.PinDelay = Delay;
  
    Heating.CurrMode = Mode;
    Heating.CurrModeLength = 0;

    // Find the total number of active pins in array
    int ArrayLength = sizeof(Heating.Modes[0]);

    // For each element in the array (existing and non-existing)
    for(int i = 0; i < ArrayLength; i++) {

      // if the element has a value (meaning is bigger than 0)
      if(Heating.Modes[Heating.CurrMode][i] > 0)

        // Increase the length
        Heating.CurrModeLength++;
    }

    // Mark the heating as on
    Heating.On = true;

    // Set the start pin to the last Pin (total length - 1)
    Heating.CurrPinID = Heating.CurrModeLength - 1;

    // Start the Heating Handler
    Heating_Handler((void *) false);
}

void stopHeating() {
  Heating.On = false;
}


bool Heating_Handler(void *) {
    // set the previous pin to LOW
    digitalWrite(Heating.Modes[Heating.CurrMode][Heating.CurrPinID], LOW);

    // If Heating.On is false, it means we have to stop the heating process
    if(!Heating.On)
      return false;

      // If we've set a delay...
      if(Heating.PinDelay > 0) {

        // Execute the next instruction in that delay
        MainTimer.in(Heating.PinDelay, Heating_CheckPinStatus);
      } else {

        // Otherwise, call it directly
        Heating_CheckPinStatus((void *) false);
      }

    return false;
}


bool Heating_CheckPinStatus(void *) {

  // We check that the Pin we've set to low, is not high (still)
  if(digitalRead(Heating.Modes[Heating.CurrMode][Heating.CurrPinID]) == HIGH) {

    // If it is, call the Heating_WaitForPinLow() and wait until Pin gets low
    // NOTE: This should (almost) never happen...
    MainTimer.every(1, Heating_WaitForPinLow);
  } else {

    // Otherwise, if we confirm that the PIN is low, proceed to the next Pin
    Heating_TurnOnNextPin();
  }
}

bool Heating_WaitForPinLow(void *) {

  // If the Pin is still HIGH...
  if(digitalRead(Heating.Modes[Heating.CurrMode][Heating.CurrPinID]) == HIGH) {

      // We set it to LOW once again
      digitalWrite(Heating.Modes[Heating.CurrMode][Heating.CurrPinID], LOW);

      // And return true, so the every() loop can continue
      return true;
    } else {

      // But if the Pin is LOW already, we proceed to the next pin...
      Heating_TurnOnNextPin();

      // And return false, so we stop the every() loop
      return false;
    }
}

void Heating_TurnOnNextPin() {

  // We increase the Pin ID to the next one in the array
  Heating.CurrPinID++;

  // If we've reached the last one (meaning end of array)
  if(Heating.CurrPinID == Heating.CurrModeLength) {

    // Set the Pin position to 0
    Heating.CurrPinID = 0;
  }

  // Set the increased pin ID to HIGH
  digitalWrite(Heating.Modes[Heating.CurrMode][Heating.CurrPinID], HIGH);

  // And, take the process on and on once again...
  MainTimer.in(Heating.PinUpTime, Heating_Handler);
}