/*
 Basic Menu

 https://lcdmenu.forntoh.dev/examples/basic

*/
#include <ItemSubMenu.h>
#include <ItemToggle.h>
#include <ItemCommand.h>
#include <ItemInput.h>
#include <LiquidCrystal_I2C.h>
#include <LcdMenu.h>
#include <utils/commandProccesors.h>

#define LCD_ROWS 2
#define LCD_COLS 16
#define I2C_ADDR 0x27
#define CHARSET_SIZE 6
// Create your charset
char charset[] = {'0', '1', '2', '3', '5'};
// Active index of the charset
int8_t charsetPosition = -1;


extern MenuItem* extPrimeMenu[];
extern MenuItem* fillTankMenu[];
extern MenuItem* litresMenu[];
extern MenuItem* presWashMenu[];

void fillTank();
void normalOp();
void extPrime();
void presWash();
void valvesMoving();
void returnState();
void relayControl();
void inputCallback(char* value);


bool pumpSwitch, interrupted;
int currentMode;
int prevMode;
int count;

unsigned long time;
unsigned long opTime;
unsigned long valvesTime;
unsigned long valvesMovedT;


// Configure keyboard keys (ASCII)
int UP = 7;  // NUMPAD 8
int DOWN = 8;   // NUMPAD 2
int ENTER = 9;  // NUMPAD 5
int BACK = 2;  // NUMPAD 7

int tankV = 6;
int extV = 5;
int vanV = 4;
int loopV = 3;
int wasteV = 12;
int freshV = 11;
int pumpP = 13;


// Initialize the main menu items
MAIN_MENU(
    ITEM_COMMAND("Normal Op", normalOp),
    ITEM_SUBMENU("Fill From EXT", fillTankMenu),
    ITEM_SUBMENU("Use EXT Tank", extPrimeMenu),
    ITEM_SUBMENU("Pressure Wash", presWashMenu)
);

/**
 * Create the submenu and precise its parent
 */
SUB_MENU(extPrimeMenu, mainMenu,
    ITEM_COMMAND("Tank ~ EXT", extPrime),
    ITEM_COMMAND("Then Click", extPrime)
);

SUB_MENU(litresMenu, fillTankMenu,
    ITEM_INPUT("Litres?", inputCallback),
    ITEM_COMMAND("Go!", fillTank) // add fill amount
);

SUB_MENU(fillTankMenu, mainMenu,
    ITEM_SUBMENU("Tank ~ EXT", litresMenu),
    ITEM_SUBMENU("Then Click", litresMenu)
);

SUB_MENU(presWashMenu, mainMenu,
    ITEM_COMMAND("Pres Wash ~ EXT", presWash),
    ITEM_COMMAND("Then Click", presWash)
);

// Construct the LcdMenu
LcdMenu menu(LCD_ROWS, LCD_COLS);
LiquidCrystal_I2C lcd(I2C_ADDR, LCD_COLS, LCD_ROWS); // construct lcd text

void setup() {

  pinMode(UP, INPUT_PULLUP);
  pinMode(DOWN, INPUT_PULLUP);
  pinMode(ENTER, INPUT_PULLUP);
  pinMode(BACK, INPUT_PULLUP);
  pinMode(13, OUTPUT);
  pinMode(tankV, OUTPUT);
  pinMode(extV, OUTPUT);
  pinMode(vanV, OUTPUT);
  pinMode(loopV, OUTPUT);
  pinMode(wasteV, OUTPUT);
  pinMode(freshV, OUTPUT);
  pinMode(pumpP, OUTPUT);
  digitalWrite(pumpP, HIGH);

  Serial.begin(9600);
  pumpSwitch = 0;
  currentMode = 1;
  prevMode = 11;
  interrupted = 0;
  relayControl();
  lcd.init(); //initialise lcd without using menu
  lcd.setCursor(0, 0); // set lcd cursor pos
    // Initialize LcdMenu with the menu items
  menu.setupLcdWithMenu(0x27, mainMenu);
     // turn on lcd backlight to stop flickering on startup.
  lcd.backlight();
}

void loop() {

  time = millis();

  if (!digitalRead(UP)) {
      if (menu.isInEditMode()){  // Update the position only in edit mode
        charsetPosition = (charsetPosition + 1) % CHARSET_SIZE;
        menu.drawChar(charset[charsetPosition]);  // Works only in edit mode
        menu.up();
    }
    menu.up();
    while(millis() < time+200);
    while (!digitalRead(UP));
  }
  if (!digitalRead(DOWN)) {
      if (menu.isInEditMode()){  // Update the position only in edit mode
        charsetPosition =
            constrain(charsetPosition - 1, 0, CHARSET_SIZE);
        menu.drawChar(charset[charsetPosition]);  // Works only in edit mode
        menu.down();
    }
    menu.down();
    while(millis() < time+200);
    while (!digitalRead(DOWN));
  }
  if (!digitalRead(ENTER)) {
    if (menu.isInEditMode()) {
      menu.type(charset[charsetPosition]);  // Works only in edit mode
      menu.enter();
    }
    menu.enter();
    while(millis() < time+200);
    while (!digitalRead(ENTER));
  }
  if (!digitalRead(BACK)) {
    menu.back();
    while(millis() < time+200);
    while (!digitalRead(BACK));
  }

}


void normalOp() { // Normal Van Operation
  if(currentMode != 1){ 
    prevMode = currentMode;
    currentMode = 1;  
    valvesMoving();
    if(interrupted == 0){
      Serial.println("Normal complete");
      opTime = millis();
      lcd.setCursor(0, 1);
      lcd.print("Can Be Resumed");
      lcd.setCursor(0, 0);
      lcd.print("Normal Operation");
      while(millis() < opTime + 3000){
      }
    }
    interrupted = 0;
    lcd.clear();
    menu.show();
  }
  else {  
    lcd.clear();
    lcd.print("State Already");
    lcd.setCursor(0, 1);
    lcd.print("Selected");
    delay(2000);
    lcd.clear();
    menu.show();
  }
}

void fillTank() { // Fill onboard tank from external tank
  prevMode = currentMode;
  Serial.println(prevMode);
  currentMode = 2;
  Serial.println(currentMode);
  valvesMoving();
  if(interrupted == 0){
    time = millis();
    lcd.clear();
    lcd.print("Filling ~10.6L");
    lcd.setCursor(0, 1);
    lcd.print("1 Min Wait");
    while(millis() < time + 60000){ //During 60 Second fill time
      if(!digitalRead(BACK)){ // If back button is pressed
        break; // Break loop
        }   
        digitalWrite(pumpP, LOW); //Keep the pump relay low (on)
      }
      digitalWrite(pumpP, HIGH);
  }
  interrupted = 0;
  lcd.clear();
  menu.show();
}

void extPrime() {
  prevMode = currentMode;
  Serial.println(prevMode);
  currentMode = 3;
  Serial.println(currentMode);
  valvesMoving();
  if(interrupted == 0){
    time = millis();
    lcd.clear();
    lcd.print("Running on");
    lcd.setCursor(0, 1);
    lcd.print("Ext Tank");
    digitalWrite(pumpP, LOW);
    while(millis() < time + 3000){ 

      }
  }
  interrupted = 0;
  lcd.clear();
  menu.show();
}

void presWash() {
  prevMode = currentMode;
  Serial.println(prevMode);
  currentMode = 4;
  Serial.println(currentMode);
  valvesMoving();
  if(interrupted == 0){
    time = millis();
    lcd.clear();
    lcd.print("Running on");
    lcd.setCursor(0, 1);
    lcd.print("Ext Tank");
    digitalWrite(pumpP, LOW);
    while(millis() < time + 3000){ 

      }
  }
  interrupted = 0;
  lcd.clear();
  menu.show();
}

void returnState(){ // Return to previous state on Back button pressed during valve move
  currentMode = prevMode;
  relayControl();
  Serial.println("Returning State");
  lcd.clear();
  lcd.print("Please Wait...");
  lcd.setCursor(0, 1);
  lcd.print("Valves Returning");
  valvesMovedT = (millis() - valvesTime) + millis(); // Move the valves back for the same amount of time why were opened for
  while(millis() < valvesMovedT){

  }
  interrupted = 1;
  lcd.clear();
}

void valvesMoving(){ // Valve control function. point to relay control and handle back button press
  if(prevMode != currentMode){
    relayControl();
    menu.hide();
    valvesTime = millis();
    lcd.print("Please Wait...");
    lcd.setCursor(0, 1);
    lcd.print("Valves Moving");
    while(millis() < valvesTime + 20000){ // 20 Second valve move time. Set how long it takes for valves to open
      if(!digitalRead(BACK)){
        Serial.println("Interrupted Move");
        returnState();
        break;
      }   
    }
  }

}

void relayControl(){ // Select which relays to open for each mode
  switch (currentMode) {
    case 1: //Normal Op
      Serial.println("Case 1");
      digitalWrite(tankV, LOW); //Open
      digitalWrite(extV, LOW); // Close
      digitalWrite(vanV, LOW); // Open
      digitalWrite(loopV, LOW); // Close
      digitalWrite(wasteV, LOW); // Close
      digitalWrite(freshV, LOW); // Close
      break;
    
    case 2: // Fill Onboard tank from Ext
      Serial.println("Case 2");
      digitalWrite(tankV, HIGH); //Close Valve (NO)
      digitalWrite(extV, HIGH); //Open Valve (NC)
      digitalWrite(vanV, HIGH); // Close Valve (NO)
      digitalWrite(loopV, HIGH); // Open Valve (NC)
      break;

    case 3: //Draw From EXT Tank to van.
      Serial.println("Case 3");
      digitalWrite(tankV, HIGH); //Close Valve (NO)
      digitalWrite(extV, HIGH); //Open Valve (NC)
      digitalWrite(vanV, LOW); // Open Valve (NO)
      digitalWrite(loopV, LOW); // Close Valve (NC)
      break; 

    case 4: //Auto Pressure Wash
      Serial.println("Case 4");
      digitalWrite(tankV, LOW); //Open Valve (NO)
      digitalWrite(extV, HIGH); //Open Valve (NC)
      digitalWrite(vanV, HIGH); // Close Valve (NO)
      digitalWrite(loopV, LOW); // Close Valve (NC) 
      break;   
  }
}

void inputCallback(char* value) {
    // Do stuff with value
    Serial.print(F("# "));
    Serial.println(value);
}