#include <LiquidCrystal_I2C.h>
#include <Button.h>

// Define connections to Relay
const int R1 = 23;
const int R2 = 25;
const int R3 = 27;
const int R4 = 29;
const int R5 = 31;
const int R6 = 33;
const int R7 = 35;
const int R8 = 37;

//define connections to Optocoupler
const int entrySensor = 22;
const int meatSensor = 24;
const int exitSensor = 26;
const int extendReed = 28;
const int retractReed = 30;

//define connections to HMI Wiring
//const int upBtnIn = 7;//39;
//const int okBtnIn = 6;//41;
//const int dnBtnIn = 5;//43;
// Set the LCD address to 0x27 for a 16 chars and 2 line display
LiquidCrystal_I2C dispOut(0x27, 20, 4);
Button upBtn(7);
  bool upBtnRead;
Button okBtn(6);
  bool okBtnRead;
Button dnBtn(5);
  bool dnBtnRead;

// Define connections to Ultrasonic Sensor
const int TRIGPIN = 8;
const int ECHOPIN = 9;

//For buttonInput function requirement
const int totalBtnsAttached = 3;

//values to store for JogMode & AutoMode Values
long mainMenuCntr;                    //for storing values for screenswitching
String sequenceState;                 //for storing status of the sequence
bool ifJogMode = false;               //default to false
bool ifAutoMode = false;              //default to false
String conveyorState = "stop";        //default to stop       --->  forward/reverse
String injectionState = "retracted";  //default to retracted  --->  extending/extended/retracting/retracted
String spicePumpState = "pumpOff";    //default to pumpOff    --->  pumpOn/pumpDone
bool entrySensorState, meatSensorState, exitSensorState;

//Values to store for settings
unsigned long injectionDelayVal = 1500;   //defaults to 1.5 seconds
unsigned long meatDetectionDelay = 2000;  //defaults to 2 seconds
int spiceWarnLevel = 25;                  //defaults to 25%

//Values for container volume
unsigned long containerHeight = 500;
unsigned long containerRadius = 100;

void setup() {
  //For Serial Monitor
  Serial.begin(9600);

  //for LCD Setup
  dispOut.begin(20, 4);
  dispOut.backlight();
  dispOut.setCursor(0, 0);
  dispOut.print("  AUTO. MEAT SPICE  ");
  dispOut.setCursor(0, 1);
  dispOut.print(" INJECTION  MACHINE ");
  dispOut.setCursor(0, 3);
  dispOut.print("   BSMX 3-A NIGHT   ");

  //declaring inputs
  upBtn.begin();
  okBtn.begin();
  dnBtn.begin();
  pinMode(entrySensor,INPUT_PULLUP);
  pinMode(meatSensor,INPUT_PULLUP);
  pinMode(exitSensor,INPUT_PULLUP);
  pinMode(extendReed,INPUT_PULLUP);
  pinMode(retractReed,INPUT_PULLUP);

  //for Ultrasonic pins
  pinMode(TRIGPIN, OUTPUT);
  pinMode(ECHOPIN, INPUT);

  //for outputs
  pinMode(R1, OUTPUT);
  pinMode(R2, OUTPUT);
  pinMode(R3, OUTPUT);
  pinMode(R4, OUTPUT);
  pinMode(R5, OUTPUT);
  pinMode(R6, OUTPUT);
  pinMode(R7, OUTPUT);
  pinMode(R8, OUTPUT);

  //turns off the Relay
  digitalWrite(R1, HIGH);
  digitalWrite(R2, HIGH);
  digitalWrite(R3, HIGH);
  digitalWrite(R4, HIGH);
  digitalWrite(R5, HIGH);
  digitalWrite(R6, HIGH);
  digitalWrite(R7, HIGH);
  digitalWrite(R8, HIGH);
  //commence that sequence is ready to run
  sequenceState = "ready";
}

void loop() {
  //runs the entire function in the loop
  buttonRead();
  menuSwitching();
  HMIControl(mainMenuCntr);
  
  //creating a bool flag using the mainMenu Code Value
  ifAutoMode = mainMenuCntr == 1000 ? true: false;
  ifJogMode = mainMenuCntr == 2000 || (mainMenuCntr >= 20000 && mainMenuCntr <= 29999) ? true: false;

  //to avoid Auto & Jog Mode from overlapping
  int runAuto = ifAutoMode && !ifJogMode ? true: false;
  //runs the autoSequence mode using the runAuto Signal
  autoSequence(mainMenuCntr, runAuto); 

  Serial.println(twinTimer(1000, 1000));
}

//for reading the buttons efficiently using Button.h library
//Comments will be updated soon
void buttonRead() {
  //for Up button
  if (upBtn.pressed()) {
    upBtnRead = true;
  }
  else if (upBtn.released()){
    upBtnRead = false;
  }

  //for Dn button
  if (dnBtn.pressed()) {
    dnBtnRead = true;
  }
  else if (dnBtn.released()){
    dnBtnRead = false;
  }

  //for Ok button
  if (okBtn.pressed()) {
    okBtnRead = true;
  }
  else if (okBtn.released()){
    okBtnRead = false;
  }
}

//for running the auto sequences using the value codes
//Comments will be updated soon
void autoSequence(long value, bool run) {
  //Insert Sequential control of the process
  bool entryState, meatState, exitState;

  //for storing lastSensors states
  static bool lastEntryState = false, lastMeatState = false, lastExitState = false, lastRunState = false;
  //for storing total Meat fed & spiceLevel
  static int meatCount, spiceLevel;
  //for storing last state of all the sequences
  static String lastSequenceState = "", lastConveyorState ="", lastInjectionState ="", lastSpicePumpState ="";

  //reads the spice level
  spiceLevel = spiceLevelPercentage(containerHeight, containerRadius, ultrasonicRead(ECHOPIN, TRIGPIN, 50, "mm"));

  entryState = digitalRead(entrySensor) ? false: true;
  meatState = digitalRead(meatSensor) ? false: true;
  exitState = digitalRead(exitSensor) ? false: true;

  if (conveyorState != lastConveyorState
    || injectionState != lastInjectionState
    || spicePumpState != lastSpicePumpState
    || sequenceState != lastSequenceState) {
    //Serial.println(conveyorState);
    //Serial.println(injectionState);
    //Serial.println(spicePumpState); 
    //Serial.println(sequenceState);

    lastConveyorState =conveyorState;
    lastInjectionState =injectionState;
    lastSpicePumpState =spicePumpState;
    lastSequenceState = sequenceState;
  }

  //if run is true:
  if(run && (spiceLevel >= spiceWarnLevel)){
    //sets the state to true
    lastRunState = run;

    digitalWrite(R6, HIGH);
    digitalWrite(R7, twinTimer(500, 500));
    digitalWrite(R8, LOW);

    //checks if meatCounter has value after being reset due to error
    if(meatCount > 0 && sequenceState == "ready") {
      sequenceState = "running";
    }

    //if entry sensor detected meat
    if(entryState) {
      //check if all other devices are at home positions
      if(injectionState == "retracted" && spicePumpState == "pumpOff") {
        //checks if sequenceState says ready
        if(sequenceState == "running") {
          //runs the conveyor
          motorControl(R1, R2, true, false, conveyorState);
        }

        //adds one if entryState changes
        if(entryState!=lastEntryState) {
          //increments to the meatCounter
          meatCount+=1;

          Serial.print("Meat Count: ");
          Serial.println(meatCount);

          sequenceState = "running";
    
          lastEntryState = entryState;
        }
      }
    }
    else if(!entryState) {
      lastEntryState = entryState;
    }

    //if meat sensor detected
    if(meatState) {
      //checks if sequence is running to avoid any unnecessary readings
      if(sequenceState == "running") {
        //stops the motor
        motorControl(R1, R2, false, false, conveyorState);

        //checks if motor has stopped successfully
        if(conveyorState == "stop") {
          //changes the sequenceState to injecting
          sequenceState = "injecting";
        }
      }

      //if injecting sequence is received, performs the injection sequence
      if(sequenceState == "injecting") {

        cylinderControl(R5, true, false, extendReed, retractReed, injectionState);

        //doublechecks if cylinder has retracted
        if(injectionState == "extended") {
          motorPump(R3, true, extendReed, injectionDelayVal, spicePumpState);
        }

        //if pump is done, retracts the cylinder
        if(spicePumpState == "pumpDone") {
          //sets a new sequenceState
          sequenceState = "injected";
        }
      }

      //If done injecting,
      if(sequenceState == "injected") {
        //stops the motor pump
        motorPump(R3, false, extendReed, injectionDelayVal, spicePumpState);

        //if cylinder has retracted, turns off the pump
        if(spicePumpState == "pumpOff") {
          //retracts back the cylinder
          cylinderControl(R5, false, true, extendReed, retractReed, injectionState);

          if(injectionState == "retracted") {
            //sets the status to checking if still is detected
            sequenceState = "checking";
          }
        }
      }
    }

    //checking state will run the conveyor for a given period of time using the twinTimer
    if (sequenceState == "checking") {
      static unsigned long previousMillis = millis();  // Initialize previousMillis
      unsigned long checkTime = meatDetectionDelay;

      // Check if the check time has elapsed
      if (millis() - previousMillis < checkTime) {
        // Start the conveyor motor
        motorControl(R1, R2, true, false, conveyorState);
      } 
      else {
        if (meatState && conveyorState == "forward") {
          // Set the sequenceState back to injecting
          sequenceState = "injecting";
          
          // Stop the conveyor motor
          motorControl(R1, R2, false, false, conveyorState);
        } 
        else if (!meatState && conveyorState == "forward") {
          sequenceState = "running";
          Serial.println(sequenceState);
        }

        //resets the time
        previousMillis = millis();
      }
    }

    //if exit sensor detected meat
    if(exitState) {
      //subtracts one if exitState changes
      if(exitState!=lastExitState) {
        //decrements to the meatCounter
        if(meatCount > 0) {
          meatCount-=1;
        }

        Serial.print("Meat Count: ");
        Serial.println(meatCount);
  
        lastExitState = exitState;
      }
    }
    else if(!exitState) {
      lastExitState = exitState;
    }

    //if meatCount is equals to 0, stops the conveyor:
    if(sequenceState == "running" && meatCount <= 0) {
      //make sures the exit sensor becomes low
      if(!exitState) {
        //runs the conveyor
        motorControl(R1, R2, false, false, conveyorState);

        //sets the status to ready
        sequenceState = "ready";
      }
    }
  }
  //if spice level is below the warn level, stops the sequence
  else if(spiceLevel < spiceWarnLevel) {
    //throws an error lamp
    digitalWrite(R6, twinTimer(500, 500));
    digitalWrite(R7, HIGH);
    digitalWrite(R8, HIGH);

    //throws an error to the screen
    sequenceState = "SpLvlLow";

    //stops the conveyor
    motorControl(R1, R2, false, false, conveyorState);
    //stops the motor pump
    motorPump(R3, false, extendReed, injectionDelayVal, spicePumpState);
    //Retracts the Cylinder
    cylinderControl(R5, false, true, extendReed, retractReed, injectionState);
  }
  else if (run == false) {
    //throws an error lamp
    digitalWrite(R6, HIGH);
    digitalWrite(R7, HIGH);
    digitalWrite(R8, LOW);
    
    if(lastRunState != run) {
      //stops the conveyor
      motorControl(R1, R2, false, false, conveyorState);
      //stops the motor pump
      motorPump(R3, false, extendReed, injectionDelayVal, spicePumpState);
      //Retracts the Cylinder
      cylinderControl(R5, false, true, extendReed, retractReed, injectionState);

      run = lastRunState;
    }
  }
}

//for running the jog sequences using run string command
//Comments will be updated soon
void jogSequence(String run) {

  if(run == "motorFwd") {
    //forwards the motor:
    motorControl(R1, R2, true, false, conveyorState);
  }
  else if (run == "motorRev") {
    //reverses the motor:
    motorControl (R1, R2, false, true, conveyorState);
  }
  else if (run == "extend") {
    //Extends the Cylinder
    cylinderControl(R5, true, false, extendReed, retractReed, injectionState);
  }
  else if (run == "retract") {
    //Retracts the Cylinder
    cylinderControl(R5, false, true, extendReed, retractReed, injectionState);
  }
  else if (run == "exit") {
    //Resets all the set up value
      //stops the conveyor motor
      motorControl (R1, R2, false, false, conveyorState);

      //Retracts the Cylinder
      cylinderControl(R5, false, true, extendReed, retractReed, injectionState);
  }
}

//For Setting the LCD Display Viewing
//Comments will be updated soon
void HMIControl(long value) {
  static int previousValue = -1;  // Variable to store the previous value
  static byte sensorVal = 0;
  static byte previousSensorVal = -1;
  static long spiceLevel;
  static long prevSpiceLevel = -1;
  static String prevSequenceState = "";

  // Read the sensor values and update the sensorVal byte
  sensorVal = 0;
  sensorVal |= !digitalRead(entrySensor) << 0;     // Set the first bit
  sensorVal |= !digitalRead(meatSensor) << 1;      // Set the second bit
  sensorVal |= !digitalRead(exitSensor) << 2;      // Set the third bit
  sensorVal |= !digitalRead(extendReed) << 3;    // Set the fourth bit
  sensorVal |= !digitalRead(retractReed) << 4;     // Set the fifth bit

  //read the the spice level value
  spiceLevel = spiceLevelPercentage(containerHeight, containerRadius, ultrasonicRead(ECHOPIN, TRIGPIN, 50, "mm"));
  //Serial.println(spiceLevel);

  if (value != previousValue || sensorVal != previousSensorVal || spiceLevel != prevSpiceLevel || sequenceState != prevSequenceState) {
    // Value has changed, update the display
    switch(value) {
      case 0:
        dispOut.setCursor(0, 0);
        dispOut.print("  AUTO. MEAT SPICE  ");
        dispOut.setCursor(0, 1);
        dispOut.print(" INJECTION  MACHINE ");
        dispOut.setCursor(0, 2);
        dispOut.print("                    ");
        dispOut.setCursor(0, 3);
        dispOut.print("   BSMX 3-A NIGHT   ");
        break;
      //Automatic Mode
      case 100:
        dispOut.setCursor(0, 0);
        dispOut.print("--------------------");
        dispOut.setCursor(0, 1);
        dispOut.print("   AUTOMATIC MODE   ");
        dispOut.setCursor(0, 2);
        dispOut.print("    >>  RUN  <<     ");
        dispOut.setCursor(0, 3);
        dispOut.print("--------------------");
        break;
      case 1000:
        dispOut.setCursor(0, 0);
        dispOut.print("SpiceLvl: ");
        dispOut.setCursor(10, 0);
        dispOut.print(spiceLevel);
        if (spiceLevel > 9) {
          dispOut.setCursor(12, 0);
          dispOut.print(" %      ");
        }
        else if (spiceLevel < 10) {
          dispOut.setCursor(11, 0);
          dispOut.print("  %      ");
        }
        else if (spiceLevel < 99) {
          dispOut.setCursor(13, 0);
          dispOut.print("%      ");
        }
        
        
        dispOut.setCursor(0, 1);
        dispOut.print("Status  : ");
        dispOut.setCursor(10, 1);
        dispOut.print(sequenceState);
        // Print the spaces to cover the remaining characters
        dispOut.print(generateSpaces(10 - sequenceState.length()));

        dispOut.setCursor(0, 2);
        dispOut.print("--------------------");
        dispOut.setCursor(0, 3);
        if (sequenceState == "SpLvlLow") {
          dispOut.print("    >> RESET? <<    ");
        } else {
          dispOut.print("    >>  STOP  <<    ");
        }
        break;
      
      //Jog Mode
      case 200:
        dispOut.setCursor(0, 0);
        dispOut.print("--------------------");
        dispOut.setCursor(0, 1);
        dispOut.print("      JOG MODE      ");
        dispOut.setCursor(0, 2);
        dispOut.print("    >> Enter  <<    ");
        dispOut.setCursor(0, 3);
        dispOut.print("--------------------");
        break;
        case 2000:
          dispOut.setCursor(0, 0);
          dispOut.print("--------------------");
          dispOut.setCursor(0, 1);
          dispOut.print(" CONVEYOR  FWD/REV  ");
          dispOut.setCursor(0, 2);
          dispOut.print("    >> Enter  <<    ");
          dispOut.setCursor(0, 3);
          dispOut.print("--------------------");
          break;
          case 20000:
            dispOut.setCursor(0, 0);
            dispOut.print("--------------------");
            dispOut.setCursor(0, 1);
            dispOut.print(" CONVEYOR  FWD/REV  ");
            dispOut.setCursor(0, 2);
            dispOut.print("--------------------");
            dispOut.setCursor(0, 3);
            dispOut.print("REV     EXIT     FWD");
            break;
        
        case 2100:
          dispOut.setCursor(0, 0);
          dispOut.print("--------------------");
          dispOut.setCursor(0, 1);
          dispOut.print("  INJECTION UP/DN   ");
          dispOut.setCursor(0, 2);
          dispOut.print("    >> Enter  <<    ");
          dispOut.setCursor(0, 3);
          dispOut.print("--------------------");
          break;
          case 21000:
            dispOut.setCursor(0, 0);
            dispOut.print("--------------------");
            dispOut.setCursor(0, 1);
            dispOut.print("  INJECTION UP/DN   ");
            dispOut.setCursor(0, 3);
            dispOut.print("--------------------");
            dispOut.setCursor(0, 2);
            dispOut.print("RET     EXIT     EXT");
            break;
        case 2200:
          dispOut.setCursor(0, 0);
          dispOut.print("--------------------");
          dispOut.setCursor(0, 1);
          dispOut.print(" SENSOR INPUT TEST  ");
          dispOut.setCursor(0, 2);
          dispOut.print("    >> Enter  <<    ");
          dispOut.setCursor(0, 3);
          dispOut.print("--------------------");
          break;
          case 22000:
            dispOut.setCursor(0, 0);
            dispOut.print(" SENSOR INPUT TEST  ");

            //Entry Sensor
            dispOut.setCursor(0, 1);
            dispOut.print("Ent: ");
            dispOut.setCursor(5, 1);
            dispOut.print(bitRead(sensorVal, 0));
            
            //Meat Sensor
            dispOut.setCursor(6, 1);
            dispOut.print(" Meat: ");
            dispOut.setCursor(13, 1);
            dispOut.print(bitRead(sensorVal, 1));
            
            //Exit Sensor
            dispOut.setCursor(14, 1);
            dispOut.print(" Ex: ");
            dispOut.setCursor(19, 1);
            dispOut.print(bitRead(sensorVal, 2));
            
            //Reed Retract Sensor
            dispOut.setCursor(0, 2);
            dispOut.print("    Ret: ");
            dispOut.setCursor(9, 2);
            dispOut.print(bitRead(sensorVal, 4));
            
            //Reed Extend Sensor
            dispOut.setCursor(10, 2);
            dispOut.print(" Ext: ");
            dispOut.setCursor(15, 2);
            dispOut.print(bitRead(sensorVal, 3));

            //for population empty spaces
            dispOut.setCursor(16, 2);
            dispOut.print("    ");

            //exits
            dispOut.setCursor(0, 3);
            dispOut.print("        EXIT        ");
            break;
        //case 2300:
          //dispOut.setCursor(0, 0);
          //dispOut.print("--------------------");
          //dispOut.setCursor(0, 1);
          //dispOut.print("SET WARN LEVEL SPICE");
          //dispOut.setCursor(0, 2);
          //dispOut.print("    >> Enter  <<    ");
          //dispOut.setCursor(0, 3);
          //dispOut.print("--------------------");
          //break;
          //case 23000:
            //dispOut.setCursor(0, 0);
            //dispOut.print("--------------------");
            //dispOut.setCursor(0, 1);
            //dispOut.print("WARN VALUE: ");
            //dispOut.setCursor(12, 1);
            //dispOut.print("100");
            //dispOut.setCursor(15, 1);
            //dispOut.print(" %    ");

            //dispOut.setCursor(0, 2);
            //dispOut.print(" -      EXIT      + ");
            //dispOut.setCursor(0, 3);
            //dispOut.print("--------------------");
            break;
        //case 2400:
        case 2300:
          dispOut.setCursor(0, 0);
          dispOut.print("--------------------");
          dispOut.setCursor(0, 1);
          dispOut.print("   EXIT  JOG MODE   ");
          dispOut.setCursor(0, 2);
          dispOut.print("    >> Enter  <<    ");
          dispOut.setCursor(0, 3);
          dispOut.print("--------------------");
          break;

      //case 300:
      //  dispOut.setCursor(0, 0);
      //  dispOut.print("--------------------");
      //  dispOut.setCursor(0, 1);
      //  dispOut.print("      SETTINGS      ");
      //  dispOut.setCursor(0, 2);
      //  dispOut.print("    >> Enter  <<    ");
      //  dispOut.setCursor(0, 3);
      //  dispOut.print("--------------------");
      //  break;
    }

    // Update the previous value
    prevSequenceState = sequenceState;
    prevSpiceLevel = spiceLevel;
    previousSensorVal = sensorVal;
    previousValue = value; 
  }
}


/*                      Menu Switching Function
---------------------------- Parameters -----------------------------
upBtn       --->  return value of the buttonInput Up
dnBtn       --->  return value of the buttonInput Dn
okBtn       --->  return value of the buttonInput Ok
---------------------------------------------------------------------
-------------------------- Screen Assignement -----------------------
  0 ->  Welcome Screen
  100 ->  Auto Mode
    1000  ->  Start
    1100  ->  Stop
  200 ->  Jog Mode
    2000 ->  Conveyor FWD/REV
      20000 ->  send this signal to use forward or reverse
    2100 ->  Injection UP/DN
      21000 ->  send this signal to use up or dn
    2200 ->  Sensor Test
      22000 -> send this signal to show all values of the sensor
    2300 ->  Spice Level Percentage
      23000 -> send this signal to show values of the sensor
    2400 ->  Exit
  300 ->  Settings
    3000 ->  Injection Delay
      30000 ->  Injection Delay Value
    3100 ->  Meat Detection Delay
      30100 ->  Meat Detect Delay Value
    3200 ->  Spice Level Detection
      30200 ->  Spice Level Warning Value
    3300 ->  Exit
--------------------------------------------------------------------
*/
void menuSwitching() {
  //for storing the state of the button being pressed
   static boolean upBtnPressed = false;
   static boolean dnBtnPressed = false;
   static boolean okBtnPressed = false;

  // if Up button is pressed
  if (upBtnRead) {
    if (!upBtnPressed) {
      // Toggle the button pressed flag
      upBtnPressed = true;

      // to avoid exceeding the main menu items
      if (mainMenuCntr == 200 || mainMenuCntr == 1000 || mainMenuCntr == 2300 || mainMenuCntr == 3300) {
        mainMenuCntr += 0;
      }
      else if (mainMenuCntr <= 9999) {
        mainMenuCntr += 100;
      }
    }

    if (mainMenuCntr == 20000) {
      jogSequence("motorFwd");
    }
    else if (mainMenuCntr == 21000) {
      jogSequence("extend");
    }
  }
  //if up buttons is released
  else if (!upBtnRead && upBtnPressed) {
    // Reset the button pressed flag when the button is released
    upBtnPressed = false;
    jogSequence("exit");
  }

  // if dn button is pressed
  if (dnBtnRead) {
    if (!dnBtnPressed) {
      // Toggle the button pressed flag
      dnBtnPressed = true;

      // Add your logic here for when the button is pressed
      if (mainMenuCntr == 0 || mainMenuCntr == 1000 || mainMenuCntr == 2000 || mainMenuCntr == 3000 || mainMenuCntr >= 10000) {
        mainMenuCntr -= 0;
      } 
      else {
        mainMenuCntr -= 100;
      }
    }
    if (mainMenuCntr == 20000) {
      jogSequence("motorRev");
    }
    else if (mainMenuCntr == 21000) {
      jogSequence("retract");
    }
  }
  //if dn buttons is released
  else if (!dnBtnRead && dnBtnPressed) {
    // Reset the button pressed flag when the button is released
    dnBtnPressed = false;
    jogSequence("exit");
  }

  // if ok button is pressed
  if (okBtnRead) {
    if (!okBtnPressed) {
      // Toggle the button pressed flag
      okBtnPressed = true;

      //exits in Auto Mode
      if(mainMenuCntr == 1000) {
        if (sequenceState == "SpLvlLow") {
          //checks back if spiceLevel is already above the warn level
          if(spiceLevelPercentage(containerHeight, containerRadius, ultrasonicRead(ECHOPIN, TRIGPIN, 50, "mm")) >= spiceWarnLevel){
            sequenceState = "ready";
          }
        }
        else {
          //goes back to Auto Mode
          mainMenuCntr=100;
        }
      }
      //exits in Jog Mode
      else if(mainMenuCntr == 2300) {
        mainMenuCntr=200;
      }
      //exits in Settings
      else if(mainMenuCntr == 3300) {
        mainMenuCntr=300;
      }
      else if(mainMenuCntr >= 20000) {
        mainMenuCntr/=10;

        jogSequence("exit");
      }
      else {
        mainMenuCntr*=10;
      }
    }
  }
  //if up buttons is released
  else if (!okBtnRead && okBtnPressed) {
    // Reset the button pressed flag when the button is released
    okBtnPressed = false;
  }
}

/*                      Twin Timer Function
---------------------------- Parameters -----------------------------
onTime      --->  total time the output will turn on  (in Millis)
offTime     --->  total time the output will turn off (in Millis)

twinTimer(onTimeVal, offTimeVal);
---------------------------------------------------------------------
*/
bool twinTimer(unsigned long onTime, unsigned long offTime) {
  static bool stat = false;
  static unsigned long previousMillis = 0;

  unsigned long currentMillis = millis();
  
  if (!stat && (currentMillis - previousMillis >= offTime)) {
    //Serial.println("Turned On");
    previousMillis = currentMillis;
    stat = true;
  }

  else if (stat && (currentMillis - previousMillis >= onTime)) {
    //Serial.println("Turned Off");
    previousMillis = currentMillis;
    stat = false;
  }

  //returns off and on
  return stat;
}

/*  CYLINDER CONTROL DOUBLE ACTING with 5/2 WAY SINGLE SOLENOID
---------------------------- Parameters -----------------------------
R4      --->  Relay 1 Input Pin
extend  --->  If user wants to extend
retract --->  If user wants to retract
extSensor --->  Sensor attached at the extend point
retSensor --->  Sensor attached at the retract point
---------------------------------------------------------------------
*/
void cylinderControl (int R5, bool extend, bool retract, int extSensor, int retSensor, String& status) {
   bool extState, retState;
   static String insideStatus = "";

  extState = digitalRead(extSensor) ? false: true;
  retState = digitalRead(retSensor) ? false: true;
  //If extend command is sent
  if (extend) {
    //make sures retract reed switch is switched
    if(retState) {
      //Triggers the relay assigned and latches
      digitalWrite(R5, LOW);

      insideStatus = "extending";
    }
    else if (!retState && insideStatus == "retracted") {
      //pass error Status
      status = "error_RetractReed";
    }
    else if(extState && insideStatus == "extending") {
      //pass status extended
      status = "extended";

      //for internal checking
      insideStatus = "extended";
    }
  }

  //if retract command is sent
  if(retract) {
    //make sures extend reed switch is switched
    if(extState) {
      //Releases the relay being latched.
      digitalWrite(R5, HIGH);

      insideStatus = "retracting";
    }
    else if (!extState && insideStatus == "extended") {
      //pass error Status
      status = "error_ExtendReed";
    }
    else if (retState && insideStatus == "retracting") {
      //pass status rectracted
      status = "retracted";

      //for internal checking
      insideStatus = "retracted";
    }
  }
}

void motorPump(int R3, bool run, int extSensor, unsigned long runTime, String& status) {
  static unsigned long previousMillis = 0;  // Variable to store the previous time
  static bool pumpRunning = false;  // Flag to track pump state
  static bool extSensorState;

  extSensorState = digitalRead(extSensor) ? false: true;

  // Check if the pump should be running
  if (run && extSensorState) {
    if (!pumpRunning) {
      // Pump is not running, start it
      digitalWrite(R3, LOW);
      previousMillis = millis();  // Set the initial value of previousMillis
      pumpRunning = true;  // Set the pump state
    }

    // Check if the target time has been reached
    if (pumpRunning && (millis() - previousMillis > runTime)) {
      // Target time reached, stop the pump
      digitalWrite(R3, HIGH);

      status = "pumpDone";
    }
  } 
  else if (!run && extSensorState) {
    // Pump should not be running, check if it was running before
    if (pumpRunning) {
      // Reset everything
      digitalWrite(R3, HIGH);
      pumpRunning = false;  // Reset the pump state
      previousMillis = 0;  // Reset the previous time

      status = "pumpOff";
    }
  }

}



/*                         MOTOR CONTROL V1
---------------------------- Parameters -----------------------------
R1  --->  Relay 1 Input Pin
R2  --->  Relay 2 Input Pin
fwd --->  bool for FWD State
rev --->  bool for REV State
---------------------------------------------------------------------
----------------------------Truth Table:-----------------------------
-motorControl(R1, R2, true, false) --->  Makes the motor go forward
-motorControl(R1, R2, false, true) --->  Makes the motor go reverse
-motorControl(R1, R2, false, false)  ->  Makes the motor stop
---------------------------------------------------------------------
*/
void motorControl (int R1, int R2, bool fwd, bool rev, String& status) {
  /*
  -LOW    LOW   Motor Off   (-) (-)   NC  NC  --> 2 relays are not triggerd
  -HIGH   HIGH  Motor Off   (+) (+)   NO  NO  --> 2 relays are triggered
  -LOW    HIGH  Motor Fwd   (-) (+)   NC  NO  --> R1 is only triggered
  -HIGH   LOW   Motor Rev   (+) (-)   NO  NC  --> R2 is only Triggered

  Wiring:
  Common contacts are attached to each side of the motor either (-) or (+) side of the motor
  NCs of R1 and R2 are attached to the 0V of the power supply
  NOs of R1 and R2 are attached to the 24V of the power supply
  */

  //One liner if else:
  //condition + '?' + if true conditions separated by commas + ':' + if false conditions separated by commas;
  //If fwd, then forward sequence
  //fwd && !rev ? digitalWrite(R1, LOW), digitalWrite(R2, HIGH)
    //else rev, then reverse sequence
  //  :(!fwd && rev ? digitalWrite(R1, HIGH), digitalWrite(R2, LOW)
      //else, stops
  //    :digitalWrite(R1, LOW), digitalWrite(R2, LOW));

  //Normal if/else condition
  
  //usefool bool and string state
  static bool isDisplayed = true;
  static String state;

  if (fwd && !rev) {
    //forward seqeunce
    digitalWrite(R1, HIGH);
    digitalWrite(R2, LOW);

    status = "forward";
  
    //Insert your Serial communication here
    if(!isDisplayed && state == "fwd"){
      Serial.println("Motor Forward");
      isDisplayed = true;
    }
    else if(isDisplayed && (state == "rev" || state == "")){
      //sets the state to forwards
      state = "fwd";

      //sets the display to false
      isDisplayed = false;
    }
  }
  else if (!fwd && rev) {
    //reverse seqeunce
    digitalWrite(R1, LOW);
    digitalWrite(R2, HIGH);

    status = "reverse";
    
    //Insert your Serial communication here
    if(!isDisplayed && state == "rev"){
      Serial.println("Motor Reverse");
      isDisplayed = true;
    }
    else if(isDisplayed && state == "fwd"){
      //sets the state to reverse
      state = "rev";

      //sets the display to false
      isDisplayed = false;
    }
  }
  else if (!fwd && !rev) {
    //stop seqeunce
    digitalWrite(R1, HIGH);
    digitalWrite(R2, HIGH);

    status = "stop";
    
    //Insert your Serial communication here
    if(isDisplayed && (state == "rev" || state == "fwd")){
      Serial.println("Motor Stopped");
      isDisplayed = false;
    }
  }
   
}

/*                       Percentage for Spices
---------------------------- Parameters -----------------------------
containerHeight     --->  this will be the total height of the container
containerRadius     --->  the radiues of the container being used
reading             --->  ultrasonicRead return value
---------------------------------------------------------------------
*/
double spiceLevelPercentage (double containerHeight, double containerRadius, double reading) {
  //declaration of all variables needed
  static double cylinderVolume, emptyVolume, usedVolume, percentage;

  //starts the calculation once the reading already has a value
  if(reading > 0) {
    //gets the cylinderVolume
    cylinderVolume = (PI * (containerRadius * containerRadius)) * containerHeight;

    //after getting the cylinder volume, we get the emptyVolume the ultrasonic reads
    emptyVolume = (PI * (containerRadius * containerRadius) * reading);

    //deduct the cylinder volume with the emptyVolume to get the usedVolume
    usedVolume = cylinderVolume - emptyVolume;

    //get the percentage by dividing the usedVolume with the cylinder volume and then multiply by 100
    percentage = (usedVolume / cylinderVolume) * 100;
    if (percentage >= 0) {
      return percentage;
    }
    //if value exceeds negative, will make the value to 0%
    else if(percentage < 0) {
      return percentage = 0;
    }
  }
  //else, return an error
  else {
    return Serial.println("Error Reading the Ultrasonic! Try Again...");
  }  
}

/*                       Ultrasonic Reading
---------------------------- Parameters -----------------------------
echoPin     --->  Echo Pin of the Ultrasonic
trigPin     --->  Trig Pin of the Ultrasonic
refreshRate --->  refresh rate in millis Time/when will the ultrasonic reads
units       ---> the unit that the ultrasonic will output
---------------------------------------------------------------------
*/
float ultrasonicRead(int echoPin, int trigPin, unsigned long refreshRate, String units) {
  static float duration, distance;
  static unsigned long previousMillis = 0;

  unsigned long currentMillis = millis();

  if (currentMillis - previousMillis >= refreshRate) {
    // Set the trigger pin LOW for 2uS
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
   
    // Set the trigger pin HIGH for 20us to send pulse
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(20);
   
    // Return the trigger pin to LOW
    digitalWrite(trigPin, LOW);
   
    // Measure the width of the incoming pulse
    duration = pulseIn(echoPin, HIGH);
   
    // Determine distance from duration
    // Use 343 metres per second as speed of sound
    if(units == "mm") {
      // Divide by 1000 as we want millimeters
      distance = (duration / 2) * 0.343;
    }
    else if(units == "cm") {
      // Divide by 1000 as we want millimeters
      distance = (duration / 2) * 3.43;
    }
    else {
      //defaults to mm
      // Divide by 1000 as we want millimeters
      distance = (duration / 2) * 0.343;
    }
    

    //loops back to use it and compare back to the current Millis
    previousMillis = currentMillis;
  }
  
  return distance;
}

//for generating spaces for the lcd print
String generateSpaces(int numSpaces) {
  String spaces;
  for (int i = 0; i < numSpaces; i++) {
    spaces += " ";
  }
  return spaces;
}