#include "EEPROM.h"
#include <Wire.h>
#include <MsTimer2.h>
#include "controlpanel.h"
#include "cycleProperties.h"
#include "serviceMode.h"
#include "programFunctions.h"
#include "errorDetector.h"

/*
  STATE 0: program selection possible
  STATE 1: delay timer counting down
  STATE 2: cycle started and running
  STATE 3: cycle paused
  STATE 4: cycle canceled
  STATE 5: cycle ended
  STATE 6: set salt
  STATE 7: set rinse aid
  STATE 8: service mode
  STATE 9: programming mode
  STATE 10: blocking error mode (eg. faulty drain pump)
*/


bool isDispenserOpened = false;
unsigned long timeDispenserWasOpened;
unsigned long lastBlink;
bool countDownDone = false;
bool doneWriting = false;
long elapsedTimeBeforePowerFailure;
bool outputStateBeforePowerFailure[7] = {
  0, 0, 0, 0, 0, 0, 0
};
byte cyclesRunBetweenRegenerations = 0;
bool regenDone = false;
long prevVCC = 0;


void updateOutputStatesBeforePowerFailure() {
  for (byte m = 0; m <= regenSolenoid - drainPump; m++) {
    if (digitalRead(m + drainPump) != outputStateBeforePowerFailure[m]) {
      outputStateBeforePowerFailure[m] = digitalRead(m + drainPump);
    }
  }
}

void setup() {
  Wire.begin(); // (SDA=A4,SCL=A5)
  Serial.begin(9600);
  MsTimer2::set(50, powerFailure);
  MsTimer2::start();
  display.begin();            // initializes the display

  display.setBacklight(100);  // set the brightness to 100 %
  currentMacroState = PROGSELECTION;
  attachInterrupt(digitalPinToInterrupt(2), pulsesCounter, FALLING);
  for (int i = 4; i <= 11; i++) {
    pinMode(i, OUTPUT);
  }
  pinMode(flowMeter, INPUT_PULLUP);
  pinMode(flowSwitch, INPUT_PULLUP);
  pinMode(antiOverflow, INPUT_PULLUP);
  pinMode(saltSensor, INPUT_PULLUP);
  pinMode(rinseAidSensor , INPUT_PULLUP);
  pinMode(doorSwitch, INPUT_PULLUP);

  if (readButtons() == SERVICE_MODE_BTN) {
    display.clear();
    display.print(F("TEST MODE"));
    currentMacroState = SERVICEMODE;
  } else if (readButtons() == SETTINGS_BTN) {
    display.clear();
    display.home();
    currentMacroState = PROGRAMMINGMODE;
  }
  initTimeArray(); // do it here as it might be that the data from eeprom must initialize them
  assignParameters();
  stageStartTime = millis();

  if (EEPROM.read(0) == true) { // for next iteration
    EEPROM.write(0, false);
    progIndex = EEPROM.read(1);
    currentCycleState = EEPROM.read(2);
    currentMacroState = EEPROM.read(3);
    EEPROM.get(4, flowMeterPulses);
    EEPROM.get(6, elapsedTimeBeforePowerFailure);

    for (byte j = 0; j < 7; j++) {
      outputStateBeforePowerFailure[j] = EEPROM.get(6 + sizeof(elapsedTimeBeforePowerFailure) + j, outputStateBeforePowerFailure[j]);
    }
    for (byte i = 4; i <= 11; i++) {
      digitalWrite(i, outputStateBeforePowerFailure[i - drainPump]);//rewrite pins to the state they originally were before pausing the cycle
    }
    initTimeArray(); // do it here as it might be that the data from eeprom must initialize them
    assignParameters();
    isItPossibleToStart = true;
    stageStartTime = millis() - elapsedTimeBeforePowerFailure;
  }
  cyclesRunBetweenRegenerations = EEPROM.read(65);
  display.clear();
}

void powerFailure() {
  long VCC = readVcc();
  if (VCC >= 4250) {
    updateOutputStatesBeforePowerFailure();
  } else if (VCC < 4250 && prevVCC > VCC && doneWriting == false && (currentMacroState == DELAYCOUNT || currentMacroState == CYCLERUNNING || currentMacroState == CYCLEPAUSED)) {
    elapsedTimeBeforePowerFailure += millis() - stageStartTime;
    EEPROM.write(0, true);
    EEPROM.write(1, progIndex); //save selected program
    EEPROM.write(2, currentCycleState); // save current cycle state
    EEPROM.write(3, currentMacroState); // save current macro state (eg. cycle running or others)
    EEPROM.put(4, flowMeterPulses);

    EEPROM.put(6, elapsedTimeBeforePowerFailure);

    EEPROM.put(6 + sizeof(elapsedTimeBeforePowerFailure), outputStateBeforePowerFailure);

    doneWriting = true;
  }
  prevVCC = VCC;
}

void transitionTo(byte state) {
  currentCycleState = state;
  elapsedTimeBeforePowerFailure = 0;
  //flowMeterPulses = 0;
  stageStartTime = millis();
  fillFunctionState = 0;
  heatFunctionState = 0;
  drainFunctionState = 0;
  regenState = 0;
  filterFlushState = 0;
  resinWashingState = 0;
}

void progSelection() {
  if (!isItPossibleToStart) {
    macroStateManager(); //if start button is pressed the machine will enter CYCLERUNNING state
  } else if (isItPossibleToStart == true && digitalRead(doorSwitch) == LOW) {
    display.setColonOn(false);
    if (delayTime > 0) {
      currentMacroState = DELAYCOUNT;
    } else {
      currentMacroState = CYCLERUNNING;
    }

    Serial.println(F("STARTING CYCLE"));
    currentCycleState = cycleArray[progIndex].startPhase;
    drainFunctionState = 0;
    stageStartTime = millis();
    display.clear();
    display.setColonOn(false);
  } else if (isItPossibleToStart == true && digitalRead(doorSwitch) == HIGH) {
    if (millis() - lastBlink <= 500) {
      display.setColonOn(false);
      display.on();
      display.home();
      display.print(F("door"));
    } else if (millis() - lastBlink <= 1000) {
      display.off();
    } else {
      lastBlink = millis();
    }
  }

}

void delayCount() {
  if (millis() - stageStartTime >= delayTime * 60 * K) { // delayTime * 60 * K
    countDownDone = true;
    if (digitalRead(doorSwitch) == HIGH) {
      if (millis() - lastBlink <= 500) {
        display.setColonOn(false);
        display.on();
        display.home();
        display.print(F("door"));
      } else if (millis() - lastBlink <= 1000) {
        display.off();
      } else {
        lastBlink = millis();
      }
    }
  } else {
    display.setColonOn(true);

    unsigned long totalDelayTime = delayTime * 60 * K;
    unsigned long delayTimeToDisplay = totalDelayTime - (millis() - stageStartTime);

    //Serial.println(timeToDisplay);

    unsigned long delayHoursRemaining = (delayTimeToDisplay / K) / 60;
    unsigned long delayMinutesRemaining = (delayTimeToDisplay / K) - delayHoursRemaining * 60;

    if (delayMinutesRemaining == 0) {
      if (delayHoursRemaining < 10) {
        display.setCursor(0, 1);
        display.print(F("h"));
        display.setCursor(0, 2);
        display.print(0);
        display.setCursor(0, 3);
        display.print(delayHoursRemaining);
      } else {
        display.setCursor(0, 1);
        display.print(F("h"));
        display.setCursor(0, 2);
        display.print(delayHoursRemaining);
      }
    } else {
      if (delayHoursRemaining + 1 < 10) {
        display.setCursor(0, 1);
        display.print(F("h"));
        display.setCursor(0, 2);
        display.print(0);
        display.setCursor(0, 3);
        display.print(delayHoursRemaining + 1);
      } else {
        display.setCursor(0, 1);
        display.print(F("h"));
        display.setCursor(0, 2);
        display.print(delayHoursRemaining + 1);
      }
    }
  }


  if (countDownDone == true && digitalRead(doorSwitch) == LOW) {
    display.setColonOn(true);
    stageStartTime = millis();
    currentMacroState = CYCLERUNNING;
  }

  if (millis() - lastButtonReadingTime > 100) {
    buttonReading = readButtons();
    lastButtonReadingTime = millis();
  }
  //  setButtons();
}

void cycleRunning() {
  display.setBacklight(100);
  runCycle();
  countDownFunction();
  pauseCycleFunction();
  detectErrors();
  if (millis() - lastButtonReadingTime > 100) {
    buttonReading = readButtons();
    lastButtonReadingTime = millis();
  }
  setButtons();

}

void cyclePaused() {
  resumeCycleFunction();
  if (millis() - lastButtonReadingTime > 100) {
    buttonReading = readButtons();
    lastButtonReadingTime = millis();
  }
  //  setButtons();
  if (buttonReading == PROG_SEL_BTN) {
    currentMacroState = PROGSELECTION;
  }

  if (millis() - lastBlink <= 500) {
    display.on();
    if (minutesRemaining >= 10) {
      display.setCursor(0, 1);
      display.print(hoursRemaining);
      display.setCursor(0, 2);
      display.print(minutesRemaining);
    } else {
      display.setCursor(0, 1);
      display.print(hoursRemaining);
      display.setCursor(0, 2);
      display.print(0);
      display.setCursor(0, 2);
      display.print(minutesRemaining);
    }
  } else if (millis() - lastBlink <= 1000) {
    display.off();
  } else {
    lastBlink = millis();
  }
}

void cycleCanceled() {
  display.setColonOn(true);
  display.setCursor(0, 1);
  display.print(F("---"));
  EEPROM.update(0, false);
  if (millis() - lastButtonReadingTime > 100) {
    buttonReading = readButtons();
    lastButtonReadingTime = millis();
  }
  //  setButtons();
  if (buttonReading == PROG_SEL_BTN) {
    display.clear();
    isItPossibleToStart = false;
    currentMacroState = PROGSELECTION;
  }

  EEPROM.update(0, false);
  EEPROM.update(1, 0); //save selected program
  EEPROM.update(1 + sizeof(progIndex), 0); // save current cycle state
  EEPROM.update(1 + sizeof(progIndex) + sizeof(currentCycleState), 4); // save current macro state (eg. cycle running or others)
  EEPROM.update(6, 0);
}

void manageError() {
  display.setColonOn(false);

  if (millis() - lastBlink <= 500) {
    display.on();
    display.setCursor(0, 1);
    display.print(F("F"));
    display.setCursor(0, 2);
    display.print(faultCode);
  } else if (millis() - lastBlink <= 1000) {
    display.off();
  } else {
    lastBlink = millis();
  }

  if (faultCode == 1 || faultCode == 2) {
    if (readButtons() == START_BTN) {
      stageStartTime = millis();
      display.setColonOn(true);
      display.clear();
      drainAttemptsCounter = 0;
      drainFunctionState = 0;
      fillFunctionState = 0;

      currentMacroState = CYCLERUNNING;
    }
  }
}

void runCycle() {
  switch (currentCycleState) {
    /* INITIAL DRAIN */
    case 0:
      if (drainFunction() == true) {
        transitionTo(1);
      }
      break;
    /* REGENERATION */
    case 1:
      //implementare rigenerazione quando necessario
      if (EEPROM.read(65) == EEPROM.read(64) - 1) { // if cycles run between regenerations are enough to make another regeneration happen
        if (regenFunction() == true) {
          regenDone = true;
          cyclesRunBetweenRegenerations = 0;
          EEPROM.update(65, cyclesRunBetweenRegenerations);
        }

        if (regenDone == true) {
          if (resinWashingFunction() == true) {
            Serial.println(F("prewash fill"));
            transitionTo(2);
            regenDone = false;
          }
        }
      } else { // if no regen is necessary skip to step 2
        Serial.println(F("prewash fill"));
        transitionTo(2);
      }
      break;
    /* PREWASH WATER FILL */
    case 2:
      if (fillFunction() == true) {
        transitionTo(3);
        Serial.println(F("prewash"));
        digitalWrite(washingPump, HIGH);
        digitalWrite(fillSolenoid, LOW);
        //Serial.println(cycleArray[progIndex].prewashDuration1);
      }
      break;
    /* PREWASH WITH COLD WATER */
    case 3:
      Serial.println(millis() - stageStartTime);
      if (millis() - stageStartTime >= cycleArray[progIndex].prewashDuration1 * K) { // prewash cold rinsing time over
        if (cycleArray[progIndex].heatedPrewash == true) {
          Serial.println(F("Heat prewash..."));
          transitionTo(4);
        } else {
          Serial.println(F("Drain prewash"));
          digitalWrite(washingPump, LOW);
          transitionTo(6);
        }
      }
      break;
    /* PREWASH WATER HEATING */
    case 4:
      if (heatFunction(cycleArray[progIndex].prewashTemp) == true) {
        transitionTo(5);
        Serial.println(F("DONE"));
      }
      break;
    /* PREWASH WITH HOT WATER */
    case 5:
      if (millis() - stageStartTime >= cycleArray[progIndex].prewashDuration2 * K) { // prewash hot rinsing time over
        digitalWrite(washingPump, LOW);
        transitionTo(6);
        Serial.println(F("Drain prewash"));
      }
      break;
    /* PREWASH DRAIN */
    case 6:
      if (drainFunction() == true) {
        if (cycleArray[progIndex].endPhase == 6) { // prewash only cycle
          cyclesRunBetweenRegenerations = EEPROM.read(65);
          cyclesRunBetweenRegenerations += 1;
          EEPROM.update(65, cyclesRunBetweenRegenerations);
          currentMacroState = CYCLEENDED;
        } else {
          transitionTo(7);
        }
      }
      break;
    /* WASH FILL */
    case 7:
      if (fillFunction() == true) {
        Serial.println(F("wash"));
        digitalWrite(washingPump, HIGH);
        digitalWrite(detDispenser, HIGH);
        timeDispenserWasOpened = millis();
        isDispenserOpened = true;
        transitionTo(8);
      }
      break;
    /* WASH WITH COLD WATER */
    case 8:
      if (millis() - timeDispenserWasOpened >= 3000 && isDispenserOpened == true) {
        digitalWrite(detDispenser, LOW);
        isDispenserOpened = false;
      }
      if (millis() - stageStartTime >= cycleArray[progIndex].washTime1 * K) {
        digitalWrite(detDispenser, LOW);
        transitionTo(9);
        Serial.println(F("Wash Heat 45..."));
      }
      break;
    /* HEATING TO 45°C */
    case 9:
      if (millis() - timeDispenserWasOpened >= 3000 && isDispenserOpened == true) {
        digitalWrite(detDispenser, LOW);
        isDispenserOpened = false;
      }
      if (heatFunction(45) == true) {
        Serial.println(F("DONE"));
        transitionTo(10);
      }
      break;
    /* WASHING AT 45°C */
    case 10:
      if (millis() - stageStartTime >= cycleArray[progIndex].washTime2 * K) {
        if (cycleArray[progIndex].secondHeating == true) {
          Serial.println(F("Wash Heat T2..."));
          transitionTo(11);
        } else {
          digitalWrite(washingPump, LOW);
          Serial.println(F("Skipping to drain"));
          transitionTo(15);
        }
      }
      break;
    /* HEATING TO T2 */
    case 11:
      if (heatFunction(cycleArray[progIndex].secondHeatingTemp) == true) {
        Serial.println(F("DONE"));
        transitionTo(12);
      }
      break;
    /* WASHING AT T2 */
    case 12:
      if (millis() - stageStartTime >= cycleArray[progIndex].washTime3 * K) {
        if (cycleArray[progIndex].thirdHeating == true) {
          transitionTo(13);
          Serial.println(F("Wash Heat T3..."));
        } else {
          digitalWrite(washingPump, LOW);
          Serial.println(F("Skipping to drain"));
          transitionTo(15);
        }
      }
      break;
    /* HEATING TO T3 */
    case 13:
      if (heatFunction(cycleArray[progIndex].thirdHeatingTemp) == true) {
        Serial.println(F("DONE"));
        transitionTo(14);
      }
      break;
    /* WASHING AT T3 */
    case 14:
      if (millis() - stageStartTime >= cycleArray[progIndex].washTime4 * K) {
        transitionTo(15);
        digitalWrite(washingPump, LOW);
        Serial.println(F("Wash Drain"));
      }
      break;
    /* WASH DRAIN AND FILTER FLUSH */
    case 15:
      if (cycleArray[progIndex].isWashDrainWithFilterFlush == true) {
        if (drainWithFilterFlushFunction() == true) {
          Serial.println(F("Flush DONE"));
          if (cycleArray[progIndex].withRinse1 == true) {
            transitionTo(16);
          } else {
            transitionTo(19); // skip to rinse 2
          }
        }
      } else {
        if (drainFunction() == true) {
          stageStartTime = millis();
          Serial.println(F("Drain DONE"));
          if (cycleArray[progIndex].withRinse1 == true) {
            transitionTo(16);
          } else {
            if (cycleArray[progIndex].withRinse2 == true) {
              transitionTo(19); // skip to rinse 2
            } else {
              transitionTo(26); // skip to drying
            }
          }
        }
      }
      break;
    /*   RINSE 1 FILL   */
    case 16:
      if (fillFunction() == true) {
        transitionTo(17);
        Serial.println(F("rinse 1"));
        digitalWrite(washingPump, HIGH);
      }
      break;
    /*   RINSE 1   */
    case 17:
      if (cycleArray[progIndex].heatedRinse1 == true) {
        heatFunction(60); // do not go over 60 degrees for heated rinses
      }
      if (millis() - stageStartTime >= cycleArray[progIndex].rinse1Duration * K) {
        digitalWrite(washingPump, LOW);
        digitalWrite(heater, LOW);
        transitionTo(18);
      }
      break;
    /*   RINSE 1 DRAIN */
    case 18:
      if (drainFunction() == true) {
        Serial.println(F("Drain r1 DONE"));
        if (cycleArray[progIndex].withRinse2 == true) {
          transitionTo(19);
        } else {
          transitionTo(22); // go to final rinse
        }
      }
      break;
    /*   RINSE 2 FILL   */
    case 19:
      if (fillFunction() == true) {
        Serial.println(F("rinse 2"));
        digitalWrite(washingPump, HIGH);
        transitionTo(20);
      }
      break;
    /*   RINSE 2   */
    case 20:
      if (cycleArray[progIndex].heatedRinse2 == true) {
        heatFunction(60); // do not go over 60 degrees for heated rinses
      }
      if (millis() - stageStartTime >= cycleArray[progIndex].rinse2Duration * K) {
        digitalWrite(washingPump, LOW);
        digitalWrite(heater, LOW);
        drainFunctionState = 0;
        transitionTo(21);
      }
      break;
    /*   RINSE 2 DRAIN */
    case 21:
      if (cycleArray[progIndex].isRinse2DrainWithFilterFlush == true) {
        if (drainWithFilterFlushFunction() == true) {
          Serial.println(F("Flush r2 DONE"));
          transitionTo(22);
        }
      } else {
        if (drainFunction() == true) {
          Serial.println(F("Drain r2 DONE"));
          transitionTo(22);
        }
      }
      break;
    /*   RINSE 3 FILL   */
    case 22:
      if (fillFunction() == true) {
        Serial.println(F("rinse 3"));
        digitalWrite(washingPump, HIGH);
        if (cycleArray[progIndex].heatedRinse3 == true) {
          transitionTo(23);
        } else {
          transitionTo(24); // skip the heating step
        }

      }
      break;
    /*   RINSE 3 HEAT   */
    case 23:
      if (isTempReached(45) == true && isDispenserOpened == false && EEPROM.read(66) == true) {// do not release rinse aid if not desired
        digitalWrite(detDispenser, HIGH);
        timeDispenserWasOpened = millis();
        isDispenserOpened = true;
      }
      if (millis() - timeDispenserWasOpened >= 3000) {
        digitalWrite(detDispenser, LOW);
      }
      if (heatFunction(cycleArray[progIndex].rinse3Temp) == true) {
        digitalWrite(detDispenser, LOW);
        Serial.println(F("rinse 3 heat OK"));
        isDispenserOpened = false; // reset for next time
        transitionTo(24);
      }
      break;
    /*   RINSE 3   */
    case 24:
      if (millis() - stageStartTime > cycleArray[progIndex].rinse3Duration * K) {
        digitalWrite(washingPump, LOW);
        transitionTo(25);
      }
      break;
    /* RINSE 3 DRAIN */
    case 25:
      if (drainFunction() == true) {
        Serial.println(F("Drain r3 DONE"));
        if (cycleArray[progIndex].endPhase == 25) {
          currentMacroState = CYCLEENDED;
        } else {
          transitionTo(26);
        }
      }
      break;
    /* DRYING */
    case 26:
      if ((millis() - stageStartTime) % (40 * K / 10) == 0) { // every four minutes
        digitalWrite(drainPump, HIGH);
      }

      if ((millis() - stageStartTime) % (42 * K / 10) == 0) { // every four minutes and twelve seconds
        digitalWrite(drainPump, LOW);
      }

      if (millis() - stageStartTime >= cycleArray[progIndex].dryingDuration * K) {
        digitalWrite(drainPump, LOW);
        transitionTo(27);
      }
      break;
    /* FINAL DRAIN */
    case 27:
      if (drainFunction() == true) {
        Serial.println(F("CYCLE FINISHED!"));
        cyclesRunBetweenRegenerations = EEPROM.read(65);
        cyclesRunBetweenRegenerations += 1;
        EEPROM.update(65, cyclesRunBetweenRegenerations);
        currentMacroState = CYCLEENDED;
      }
      break;
  }
}

void loop() {
  switch (currentMacroState) {//program selection stage, selection possible as long as start is not pressed
    case PROGSELECTION:
      progSelection();
      break;
    case DELAYCOUNT:
      delayCount();
      break;
    case CYCLERUNNING:
      cycleRunning();
      //countdown time function
      break;
    case CYCLEPAUSED:
      cyclePaused();
      break;
    case CYCLECANCELED:
      cycleCanceled();
      break;
    case CYCLEENDED:
      if (millis() - lastBlink <= 750) {
        display.setColonOn(false);
        display.on();
        display.home();
        display.print(F(" End"));
      } else if (millis() - lastBlink <= 1500) {
        display.off();
      } else {
        lastBlink = millis();
      }
      controlLED();
      EEPROM.update(0, false);
      EEPROM.update(1, 0);
      EEPROM.update(2, 0);
      EEPROM.update(1 + sizeof(progIndex) + sizeof(currentCycleState), 0);
      EEPROM.update(6, 0); // elapsedTimeBeforePowerFailure
      break;
    case SERVICEMODE:
      serviceMode();
      break;
    case SETSALT:
      setSalt();
      break;
    case SETRINSEAID:
      setRinseAid();
      break;
    case PROGRAMMINGMODE:
      settingsMenu();
      break;
    case BLOCKINGERRORMODE:
      manageError();
      break;
  }
}