#include <Wire.h>
#include <Keypad.h>
#include <LiquidCrystal_I2C.h>
#include <elapsedMillis.h>

LiquidCrystal_I2C lcd(0x27, 20, 4); // set the LCD address to 0x27 for a 16 chars and 2 line display

// LCD Connection:
// GND - GND
// VCC - 5V
// SDA - A4
// SCL - A5

const int ROW_NUM = 4; //four rows
const int COLUMN_NUM = 4; //four columns

char keys[ROW_NUM][COLUMN_NUM] = {
  {'1', '2', '3', 'A'},
  {'4', '5', '6', 'B'},
  {'7', '8', '9', 'C'},
  {'*', '0', '#', 'D'}
};

byte pin_rows[ROW_NUM] = {52, 50, 48, 46}; //connect to the row pinouts of the keypad
byte pin_column[COLUMN_NUM] = {44, 42, 40, 38}; //connect to the column pinouts of the keypad
// Keypad Arduino
// 1      2
// 2      3
// 3      4
// 4      5
// 5      6
// 6      7
// 7      8
// 8      9

Keypad keypad = Keypad( makeKeymap(keys), pin_rows, pin_column, ROW_NUM, COLUMN_NUM );

String input_value;
long input_int;
unsigned long totalCycle = 48;
int mode = 0;

////////////////////////////////new variables start/////////////////////////////////////
elapsedMillis timeElapsed;  //declare global if you don't want it reset every time loop runs
elapsedMillis output1_timer;
elapsedMillis cycleElapsed;
elapsedMillis buzElapsed;
elapsedMillis ftime;
elapsedMillis rtime;
//-----------------------------------------

int f_pwm[6] = {8, 9, 10, 11, 12, 13};
int r_pwm[6] = {2, 3, 4, 5, 6, 7};
int cycle_count = 0;
int buzzer = A0;


//////////////door timing variable's /////////////////////
float doorOpening = 2000;
int opening = doorOpening ;//* 1000;

int doorHoldOnOpen = 6;
int openHold = doorHoldOnOpen * 1000;

float doorClosing = 2000;
int closing = doorClosing;// * 1000;

int first_door_timing = opening + openHold + closing;
int sub_door_timing = openHold + closing;
//////////////////////////////////////////////////////

//
unsigned long delayAfterCycle = 10;
unsigned long cycleDelay = (delayAfterCycle * 60) * 1000;

int motorDone = 0;
int motorSel = 2;
int speed = 100;
float speedPersentage = 2.55 * speed;

int brake = 0;

int between_cycle_delay = 24;
int mid_stop = 0;





void Buzzer() {
  ///////
  int b = 250;
  if (buzElapsed >= b) {
    analogWrite(buzzer, 200);
  } if (buzElapsed >= b + b) {
    analogWrite(buzzer, 0);
    buzElapsed = 0;
  }
  //////////////
}

void calibration() {
  lcd.print("Calibrating");
  /*for (int i = 0; i < 6; i++) {
    analogWrite(r_pwm[i], brake);
    analogWrite(f_pwm[i], 255);
  }*/
  delay(1000);
  for (int i = 0; i < 6; i++) {
    analogWrite(r_pwm[i], 255);
    analogWrite(f_pwm[i], brake);
  }
  delay(4000);
  for (int i = 0; i < 6; i++) {
    analogWrite(r_pwm[i], brake);
    analogWrite(f_pwm[i], brake);
  }
  lcd.clear();

}
//////////////////////////////New variables end/////////////////////////////////////////


/*//Output previous time
unsigned long previousOutput1Time = 0;
unsigned long previousOutput2Time = 0;
unsigned long previousOutput3Time = 0;
unsigned long previousOutput4Time = 0;
//Output stay on time 1000ms(1s)
float outputOnTime1 = 1000;
float outputOnTime2 = 1000;
//delay after 1 cycle
unsigned long delayInOut1Out2 = 4;



//Cycle previous time
unsigned long previousCycleTime = 0;
//Timing for auto mode on and off
unsigned long outElapsed = 0;
unsigned long outStartTime = 0;
unsigned long outPreviousTime = 0;





int outputRelays = 0;*/
unsigned long elapsed = 0;
unsigned long currentTime = 0;
unsigned long startTime = 0;

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

  lcd.init();
  lcd.backlight();
  Serial.begin(9600);
  startTime = millis();

  for (int i = 0; i < 5; i++) {
    pinMode(f_pwm[i], OUTPUT);
    analogWrite(f_pwm[i], brake);
    pinMode(r_pwm[i], OUTPUT);
    analogWrite(r_pwm[i], brake);
  }

}

void loop() {
  // put your main code here, to run repeatedly:
  if (mode == 0) {


    lcd.setCursor(0, 0); lcd.print("Cycle: "); lcd.print(totalCycle); lcd.print(" Delay:"); lcd.print(delayAfterCycle); lcd.print("m");
    lcd.setCursor(0, 3); lcd.print("Run:A/B");
    lcd.setCursor(0, 1); lcd.print("Door: "); lcd.print(doorOpening/1000); lcd.print("s,");
    lcd.print(doorHoldOnOpen); lcd.print("s,");
    lcd.print(doorClosing/1000); lcd.print("s");
    lcd.setCursor(0, 2); lcd.print("SelDoor:"); lcd.print(motorSel); lcd.print(",");
    lcd.print("Speed:"); lcd.print(speed); lcd.print("%");
    //lcd.setCursor(0,3);lcd.print("Next Page:C");
    lcd.setCursor(15, 3); lcd.print("Set:D");

    char key = keypad.getKey();
    if (key) {
    } if (key == 'C') {
      lcd.clear();
      mode = 0;
      startTime = millis();
      elapsed = 0;

    } else if (key == 'D') {
      currentTime = millis();
      elapsed = 0;
      mode = 4;
      lcd.clear();

    } else if (key == 'B') {
      lcd.clear();
      mode = 2;


    } else if (key == 'A') {
      lcd.clear();
      mode = 1;
      startTime = millis();
      elapsed = 0;
      
    }


    //Auto mode pre start function
  } else if (mode == 1) {
    lcd.setCursor(0, 0); lcd.print(" [Press 1 to start] ");
    lcd.setCursor(0, 1); lcd.print("Clear any blockages");
    lcd.setCursor(0, 2); lcd.print("Press 2 celebration");
    lcd.setCursor(14, 3); lcd.print("Exit:C");

    first_door_timing = opening + openHold + closing;
    sub_door_timing = openHold + closing;
    opening = doorOpening ;//* 1000;
    openHold = doorHoldOnOpen * 1000;
    closing = doorClosing;// * 1000;
    cycleDelay = (delayAfterCycle * 60) * 1000;



    char key = keypad.getKey();
    if (key) {
    } if (key == 'C') {
      lcd.clear();
      currentTime = millis();
      elapsed = 0;
      mode = 0;

    } else if (key == '1') {
      lcd.clear();
      timeElapsed = 0;
      output1_timer = 0;
      cycleElapsed = 0;
      startTime = millis();
      elapsed = 0;

      mode = 3;

    } else if (key == '2') {
      lcd.clear();

      mode = 12;

    } else if (key == '3') {
      lcd.clear();
      timeElapsed = 0;
      output1_timer = 0;
      cycleElapsed = 0;
      startTime = millis();
      mode = 12;
    }




  } else if (mode == 2) {       //Manual runing mode to drive output solenoids

    first_door_timing = opening + openHold + closing;
    sub_door_timing = openHold + closing;
    opening = doorOpening ;//* 1000;
    openHold = doorHoldOnOpen * 1000;
    closing = doorClosing;// * 1000;
    cycleDelay = (delayAfterCycle * 60) * 1000;



    elapsed = (millis() - startTime);
    //lcd.setCursor(0, 3); lcd.print(elapsed / 1000);

    //lcd.setCursor(5, 0); lcd.print("Manual Run"); //lcd.print(mode);
    lcd.setCursor(0, 0); lcd.print("Press:1 for Forward");
    lcd.setCursor(0, 1); lcd.print("Press:2 for Reverse");
    lcd.setCursor(0, 2); lcd.print("Time:");
    lcd.print(doorOpening/1000); lcd.print("s");
    lcd.print(" Speed:");
    lcd.print(speed); lcd.print("%");
    lcd.setCursor(14, 3); lcd.print("Exit:C");
    //lcd.setCursor(0, 3); lcd.print(output1_timer);



    if (ftime <= opening) {
      analogWrite(r_pwm[0], brake);
      analogWrite(f_pwm[0], speedPersentage);
    } else {
    analogWrite(f_pwm[0], brake);
    }

    if (rtime <= closing) {
      analogWrite(f_pwm[0], brake);
      analogWrite(r_pwm[0], speedPersentage);
    } 



    char key = keypad.getKey();
    if (key) {
      if (key == 'C') {
        lcd.clear();

        currentTime = millis();
        elapsed = 0;
        mode = 0;

      } else if (key == '1') {

        ftime = 0;
      } else if (key == '2') {

        rtime = 0;
      } else if (key == '3') {

        
      } else if (key == '4') {

        
      }
    }


  } else if (mode == 3) {

    
    lcd.setCursor(14, 3); lcd.print("Exit:C");
    char cycleCount[20];
    sprintf(cycleCount, "Cycle: %d/%d ", cycle_count, totalCycle);
    lcd.setCursor(0, 0); lcd.print(cycleCount);
    char doors[20];
    sprintf(doors, "Doors: %d/%d", motorDone, motorSel);
    lcd.setCursor(0, 1); lcd.print(doors);

    //lcd.setCursor(12, 0); lcd.print("S:"); lcd.print(speedPersentage);
    //lcd.setCursor(12, 1); lcd.print("D"); lcd.print(cycleDelay);
////////////////////First door opening logic//////////////////////////////
    if (output1_timer <= opening && motorDone == 0) {
      analogWrite(r_pwm[motorDone], brake);
      analogWrite(f_pwm[motorDone+1], brake);
      analogWrite(f_pwm[motorDone], speedPersentage);
      lcd.setCursor(0, 2); lcd.print("Door opening");
      Buzzer();
    }
  ////////////////////First door Stop logic//////////////////////////////
    if (output1_timer >= opening && output1_timer <= (openHold + opening) && motorDone == 0) {
      analogWrite(f_pwm[motorDone], brake);
      lcd.setCursor(0, 2); lcd.print("Door Closed  ");
      analogWrite(buzzer, 0);
      //digitalWrite(r_pwm[motorDone], speedPersentage);
    }

////////////////////First door closing with second door opening logic//////////////////////////////
    if (output1_timer >= (openHold + opening) && output1_timer <= ((openHold + opening) + closing) && motorDone == 0) {
      analogWrite(f_pwm[motorDone], brake);
      analogWrite(r_pwm[motorDone+1], brake);
      analogWrite(r_pwm[motorDone], speedPersentage);
      analogWrite(f_pwm[motorDone + 1], speedPersentage);
      lcd.setCursor(0, 2); lcd.print("Door opening");
      Buzzer();
      //second door start opening when first start closing
    }
  ////////////////////2nd door closing with next door opening logic//////////////////////////////
    if (output1_timer >= openHold && output1_timer <= (openHold + closing) && motorDone >= 1) {
      analogWrite(f_pwm[motorDone], brake);
      analogWrite(r_pwm[motorDone+1], brake);
      analogWrite(r_pwm[motorDone], speedPersentage);
      analogWrite(f_pwm[motorDone + 1], speedPersentage);
      lcd.setCursor(0, 2); lcd.print("Door opening");
      Buzzer();
      //second door start opening when first start closing
    }
   ////////////////////First door closed with second door opened hold time logic//////////////////////////////
    if (output1_timer >= first_door_timing) {
      //analogWrite(r_pwm[motorDone], brake);
      analogWrite(f_pwm[motorDone + 1], brake);
      lcd.setCursor(0, 2); lcd.print("Door Closed  ");
      analogWrite(buzzer, 0);
      //door 1 and 2 stop, door 1 is closed position, door 2 is on open position
      motorDone++;
      output1_timer = 0;
    }
    ///////////////
    if (output1_timer >= openHold + closing  && motorDone >= 1) {
      //analogWrite(r_pwm[motorDone], brake);
      analogWrite(f_pwm[motorDone + 1], brake);
      analogWrite(buzzer, 0);
      lcd.setCursor(0, 2); lcd.print("Door Closed  ");

      //door 1 and 2 stop, door 1 is closed position, door 2 is on open position
      motorDone++;
      output1_timer = 0;
    }
    //////////////////////////
    if (motorDone >= motorSel) {
      analogWrite(buzzer, 0);
      output1_timer = 0;

    }
    ////////////////////After finish all door 1mint hold or cycle delay logic//////////////////////////////
    int i;
    i = motorSel - 1;
    if (cycleElapsed >= (first_door_timing + (sub_door_timing * i)) + cycleDelay) {
      
      analogWrite(buzzer, 0);
      motorDone = 0;
      output1_timer = 0;
      cycleElapsed = 0;
      cycle_count++;
    }
    ////////////////////cycle counter for stop testing logic//////////////////////////////
    if (cycle_count >= totalCycle ) {
      analogWrite(buzzer, 0);
      timeElapsed = 0;
      output1_timer = 0;
      cycleElapsed = 0;
      lcd.clear();
      mode = 11;
      for (int i = 0; i < 6; i++) {
        analogWrite(r_pwm[i], brake);
        analogWrite(f_pwm[i], brake);
      }
    }





    char key = keypad.getKey();
    if (key) {
      if (key == 'C') {
        analogWrite(buzzer, 0);
        for (int i = 0; i < 6; i++) {
          analogWrite(r_pwm[i], brake);
          analogWrite(f_pwm[i], brake);
        }
        lcd.clear();
        motorDone = 0;
        output1_timer = 0;
        cycleElapsed = 0;
        cycle_count = 0;
        currentTime = millis();
        elapsed = 0;
        mode = 0;

      } else if (key == 'D') {
        analogWrite(buzzer, 0);
        currentTime = millis();
        elapsed = 0;
        mode = 4;
        lcd.clear();

      }
    }


  } else if (mode == 4) {       // Program Cycle Time

    lcd.setCursor(0, 0);
    lcd.print("Enter Cycle");
    lcd.setCursor(0, 1);
    lcd.print(totalCycle); lcd.print(" ");
    lcd.setCursor(0, 2);
    lcd.print(input_value); lcd.print(" ");
    lcd.setCursor(0, 3);
    lcd.print("*Clr  # Entr  D Next");

    char key = keypad.getKey();
    if (key) {
      if (key == '*') {
        input_value = "";  // clear input value
        lcd.clear();
      } else if ((key == '#') && (input_value.length() > 0)) {
        input_int = input_value.toInt();
        totalCycle = input_int;
      } else if (key == 'D') {
        mode = 5;
        lcd.clear();
        input_value = "";
      } else if (key >= '0' && key <= '9') {
        input_value += key; // append new character to input value string
      }
    }

  } else if (mode == 5) {       // Program Cycle Time

    lcd.setCursor(0, 0);
    lcd.print("Enter Cycle Delay");
    lcd.setCursor(0, 1);
    lcd.print(delayAfterCycle); lcd.print(" min");
    lcd.setCursor(0, 2);
    lcd.print(input_value); lcd.print(" min");
    lcd.setCursor(0, 3);
    lcd.print("*Clr  # Entr  D Next");

    char key = keypad.getKey();
    if (key) {
      if (key == '*') {
        input_value = "";  // clear input value
        lcd.clear();
      } else if ((key == '#') && (input_value.length() > 0)) {
        input_int = input_value.toInt();
        delayAfterCycle = input_int;
        cycleDelay = (delayAfterCycle * 60) * 1000;
      } else if (key == 'D') {
        mode = 6;
        lcd.clear();
        input_value = "";
      } else if (key >= '0' && key <= '9') {
        input_value += key; // append new character to input value string
      }
    }

  } else if (mode == 6) {       // Program Cycle Time

    lcd.setCursor(0, 0);
    lcd.print("Enter Door Opening");
    lcd.setCursor(0, 1);
    lcd.print(doorOpening); lcd.print(" ms");
    lcd.setCursor(0, 2);
    lcd.print(input_value); lcd.print(" ms");
    lcd.setCursor(0, 3);
    lcd.print("*Clr  # Entr  D Next");

    char key = keypad.getKey();
    if (key) {
      if (key == '*') {
        input_value = "";  // clear input value
        lcd.clear();
      } else if ((key == '#') && (input_value.length() > 0)) {
        input_int = input_value.toInt();
        doorOpening = input_int;
      } else if (key == 'D') {
        mode = 7;
        lcd.clear();
        input_value = "";
      } else if (key >= '0' && key <= '9') {
        input_value += key; // append new character to input value string
      }
    }
  } else if (mode == 7) {       // Program Cycle Time

    lcd.setCursor(0, 0);
    lcd.print("Enter Door Hold Time");
    lcd.setCursor(0, 1);
    lcd.print(doorHoldOnOpen); lcd.print(" s");
    lcd.setCursor(0, 2);
    lcd.print(input_value); lcd.print(" s");
    lcd.setCursor(0, 3);
    lcd.print("*Clr  # Entr  D Next");

    char key = keypad.getKey();
    if (key) {
      if (key == '*') {
        input_value = "";  // clear input value
        lcd.clear();
      } else if ((key == '#') && (input_value.length() > 0)) {
        input_int = input_value.toInt();
        doorHoldOnOpen = input_int;
      } else if (key == 'D') {
        mode = 8;
        lcd.clear();
        input_value = "";
      } else if (key >= '0' && key <= '9') {
        input_value += key; // append new character to input value string
      }
    }
  } else if (mode == 8) {       // Program Cycle Time

    lcd.setCursor(0, 0);
    lcd.print("Enter Door Closing T");
    lcd.setCursor(0, 1);
    lcd.print(doorClosing); lcd.print(" ms");
    lcd.setCursor(0, 2);
    lcd.print(input_value); lcd.print(" ms");
    lcd.setCursor(0, 3);
    lcd.print("*Clr  # Entr  D Next");

    char key = keypad.getKey();
    if (key) {
      if (key == '*') {
        input_value = "";  // clear input value
        lcd.clear();
      } else if ((key == '#') && (input_value.length() > 0)) {
        input_int = input_value.toInt();
        doorClosing = input_int ;
      } else if (key == 'D') {
        mode = 9;
        lcd.clear();
        input_value = "";
      } else if (key >= '0' && key <= '9') {
        input_value += key; // append new character to input value string
      }
    }

  } else if (mode == 9) {       // Program Cycle Time

    lcd.setCursor(0, 0);
    lcd.print("Enter How many Motor");
    lcd.setCursor(0, 1);
    lcd.print(motorSel); lcd.print(" ");
    lcd.setCursor(0, 2);
    lcd.print(input_value); lcd.print(" ");
    lcd.setCursor(0, 3);
    lcd.print("*Clr  # Entr  D Next");

    char key = keypad.getKey();
    if (key) {
      if (key == '*') {
        input_value = "";  // clear input value
        lcd.clear();
      } else if ((key == '#') && (input_value.length() > 0)) {
        input_int = input_value.toInt();
        motorSel = input_int ;
      } else if (key == 'D') {
        mode = 10;
        lcd.clear();
        input_value = "";
      } else if (key >= '0' && key <= '9') {
        input_value += key; // append new character to input value string
      }
    }

  } else if (mode == 10) {       // Program Cycle Time

    lcd.setCursor(0, 0);
    lcd.print("Enter Speed");
    lcd.setCursor(0, 1);
    lcd.print(speed); lcd.print(" ");
    lcd.setCursor(0, 2);
    lcd.print(input_value); lcd.print(" ");
    lcd.setCursor(0, 3);
    lcd.print("*Clr  # Entr  D Next");

    char key = keypad.getKey();
    if (key) {
      if (key == '*') {
        input_value = "";  // clear input value
        lcd.clear();
      } else if ((key == '#') && (input_value.length() > 0)) {
        input_int = input_value.toInt();
        speed = input_int ;
        speedPersentage = 2.55 * speed;
      } else if (key == 'D') {
        mode = 0;
        lcd.clear();
        input_value = "";
      } else if (key >= '0' && key <= '9') {
        input_value += key; // append new character to input value string
      }
    }

  } else if (mode == 11) {       // Program Cycle Time

    lcd.setCursor(0, 0);
    lcd.print(totalCycle); lcd.print(" Cycle completed");

    char key = keypad.getKey();
    if (key) {
      if (key == 'C') {
        lcd.clear();
        motorDone = 0;
        output1_timer = 0;
        cycleElapsed = 0;
        cycle_count = 0;
        currentTime = millis();
        elapsed = 0;
        mode = 0;
      } else if (key == 'D') {
        currentTime = millis();
        elapsed = 0;
        mode = 4;
        lcd.clear();

      }
    }



  } else if (mode == 12) {       // Program Cycle Time


    calibration();
    mode = 1;
    char key = keypad.getKey();
    if (key) {
      if (key == 'C') {
        lcd.clear();
        motorDone = 0;
        output1_timer = 0;
        cycleElapsed = 0;
        cycle_count = 0;
        currentTime = millis();
        elapsed = 0;
        mode = 0;
      } else if (key == 'D') {
        currentTime = millis();
        elapsed = 0;
        mode = 4;
        lcd.clear();

      }
    }



  }else if (mode == 13) {

    
    lcd.setCursor(14, 3); lcd.print("Exit:C");
    char cycleCount[20];
    sprintf(cycleCount, "Cycle: %d/%d ", cycle_count, totalCycle);
    lcd.setCursor(0, 0); lcd.print(cycleCount);
    char doors[20];
    sprintf(doors, "Doors: %d/%d", motorDone, motorSel);
    lcd.setCursor(0, 1); lcd.print(doors);

    //lcd.setCursor(12, 0); lcd.print("S:"); lcd.print(speedPersentage);
    //lcd.setCursor(12, 1); lcd.print("D"); lcd.print(cycleDelay);
////////////////////First door opening logic//////////////////////////////
    if (output1_timer <= opening && motorDone == 0) {
      analogWrite(r_pwm[motorDone], brake);
      analogWrite(f_pwm[motorDone+1], brake);
      analogWrite(f_pwm[motorDone], speedPersentage);
      lcd.setCursor(0, 2); lcd.print("Door opening");
      Buzzer();
    }
  ////////////////////First door Stop logic//////////////////////////////
    if (output1_timer >= opening && output1_timer <= (openHold + opening) && motorDone == 0) {
      analogWrite(f_pwm[motorDone], brake);
      lcd.setCursor(0, 2); lcd.print("Door Closed  ");
      analogWrite(buzzer, 0);
      //digitalWrite(r_pwm[motorDone], speedPersentage);
    }

////////////////////First door closing with second door opening logic//////////////////////////////
    if (output1_timer >= (openHold + opening) && output1_timer <= ((openHold + opening) + closing) && motorDone == 0) {
      analogWrite(f_pwm[motorDone], brake);
      analogWrite(r_pwm[motorDone+1], brake);
      analogWrite(r_pwm[motorDone], speedPersentage);
      analogWrite(f_pwm[motorDone + 1], speedPersentage);
      lcd.setCursor(0, 2); lcd.print("Door opening");
      Buzzer();
      //second door start opening when first start closing
    }
  ////////////////////2nd door closing with next door opening logic//////////////////////////////
    if (output1_timer >= openHold && output1_timer <= (openHold + closing) && motorDone >= 1) {
      analogWrite(f_pwm[motorDone], brake);
      analogWrite(r_pwm[motorDone+1], brake);
      analogWrite(r_pwm[motorDone], speedPersentage);
      analogWrite(f_pwm[motorDone + 1], speedPersentage);
      lcd.setCursor(0, 2); lcd.print("Door opening");
      Buzzer();
      //second door start opening when first start closing
    }
   ////////////////////First door closed with second door opened hold time logic//////////////////////////////
    if (output1_timer >= first_door_timing) {
      //analogWrite(r_pwm[motorDone], brake);
      analogWrite(f_pwm[motorDone + 1], brake);
      lcd.setCursor(0, 2); lcd.print("Door Closed  ");
      analogWrite(buzzer, 0);
      //door 1 and 2 stop, door 1 is closed position, door 2 is on open position
      motorDone++;
      output1_timer = 0;
    }
    ///////////////
    if (output1_timer >= openHold + closing  && motorDone >= 1) {
      //analogWrite(r_pwm[motorDone], brake);
      analogWrite(f_pwm[motorDone + 1], brake);
      analogWrite(buzzer, 0);
      lcd.setCursor(0, 2); lcd.print("Door Closed  ");

      //door 1 and 2 stop, door 1 is closed position, door 2 is on open position
      motorDone++;
      output1_timer = 0;
    }
    //////////////////////////
    if (motorDone >= motorSel) {
      analogWrite(buzzer, 0);
      output1_timer = 0;

    }
    ////////////////////After finish all door 1mint hold or cycle delay logic//////////////////////////////
    int i;
    i = motorSel - 1;
    if (cycleElapsed >= (first_door_timing + (sub_door_timing * i)) + cycleDelay) {
      
      analogWrite(buzzer, 0);
      motorDone = 0;
      output1_timer = 0;
      cycleElapsed = 0;
      cycle_count++;
    }
    ////////////////////cycle counter with middle stop for 4 hours//////////////////////////////
    if (cycle_count >= between_cycle_delay &&  mid_stop == 0 ) {
      
      analogWrite(buzzer, 0);
      
      output1_timer = 0;
      cycleElapsed = 0;
      lcd.clear();
      
      for (int i = 0; i < 6; i++) {
        analogWrite(r_pwm[i], brake);
        analogWrite(f_pwm[i], brake);
      }
    }
    ////////////////////cycle counter for stop testing logic//////////////////////////////
    if (cycle_count >= totalCycle ) {
      analogWrite(buzzer, 0);
      timeElapsed = 0;
      output1_timer = 0;
      cycleElapsed = 0;
      lcd.clear();
      mode = 11;
      for (int i = 0; i < 6; i++) {
        analogWrite(r_pwm[i], brake);
        analogWrite(f_pwm[i], brake);
      }
    }





    char key = keypad.getKey();
    if (key) {
      if (key == 'C') {
        analogWrite(buzzer, 0);
        for (int i = 0; i < 6; i++) {
          analogWrite(r_pwm[i], brake);
          analogWrite(f_pwm[i], brake);
        }
        lcd.clear();
        motorDone = 0;
        output1_timer = 0;
        cycleElapsed = 0;
        cycle_count = 0;
        currentTime = millis();
        elapsed = 0;
        mode = 0;

      } else if (key == 'D') {
        analogWrite(buzzer, 0);
        currentTime = millis();
        elapsed = 0;
        mode = 4;
        lcd.clear();

      }
    }


  }
}