/*
Programmed by : AJLM
Version       : 1.2
Last Update   : April 17, 2023
*/

#include <AccelStepper.h>
#include <MultiStepper.h>
#include <Servo.h>
#include <LiquidCrystal_I2C.h>
#include <Button.h>


//*-------Paper Pick Up Roller Devices-----------*//
//Stepper Motors
AccelStepper P_Tray1(AccelStepper::DRIVER, 13, 22);
AccelStepper P_Tray2(AccelStepper::DRIVER, 12, 24);
AccelStepper P_Tray3(AccelStepper::DRIVER, 11, 26);
//Stepper Motors EnablePin
#define P_Tray1_EN 23
#define P_Tray2_EN 25
#define P_Tray3_EN 27

//Servo Motors
Servo P_Tray1_Servo;
Servo P_Tray2_Servo;
Servo P_Tray3_Servo;
int anglestep_P_Tray1 = 5;    //Variable for storing angle per millisecond
int anglestep_P_Tray2 = 5;    //Variable for storing angle per millisecond
int anglestep_P_Tray3 = 5;    //Variable for storing angle per millisecond
int lowerDownSpeed = 75;
int retractDistance = 12;

//For Timing
unsigned long currentMillis_P_Tray1, currentMillis_P_Tray2, currentMillis_P_Tray3;
unsigned long previousMillis_P_Tray1, previousMillis_P_Tray2, previousMillis_P_Tray3;
unsigned long interval = 50;

//For Paper Jam Timing
unsigned long currentMillis_PJam, previousMillis_PJam;
unsigned long interval_PJam = 5000;    //Equivalent for 10 seconds

//For Punching Timing
unsigned long currentMillis_Punching, previousMillis_Punching;
unsigned long interval_Punching = 50;

//For Stepper Speed Calculation
float rpm = 20.0;             //you can change this from 5 ~ 22(max)
float speed;
float stepsperrev = 2038.0;

//For Sensors attached
#define P_Tray1_IR 30
#define P_Tray2_IR 32
#define P_Tray3_IR 34

//*----End ofPaper Pick Up Roller Devices--------*//

//*-------------Output Path Devices--------------*//
//Setting up the P_Out stepper motors
AccelStepper P_Out(AccelStepper::DRIVER, 10, 28);

//Setting up the enable pin of the stepper driver
#define P_Out_EN 29

//For the Infrared Sensors attached
#define P_Path_IR 36
#define P_Out_IR 38

//Servo for Punch and Paper Out
Servo P_Punch_Servo;
Servo P_Door_Servo;
Servo P_Out_Servo;
  //For Punching Servo variables
  int anglePunch;               //Storage for the calculated angle
  int targetAnglePunch = 90;    //The targetAngle to be compared with
  int anglePunchInc = 5;        //The increment every interval set
  //FOr Output Roller Servo Variables
  int angleOut = 0;             //0 deg is the actuated angle (if set up is not raised, move the arm upward)
  int angleHome = 15;           //Home position of the servo
  //For Output door that opens/closes
  int doorOpen = 90;            //90deg is the open position based on the actual set up
  int doorClose = 180;          //180deg is the close position based on the actual set up

//*----------End of Output Path Devices----------*//

//*-----------Human-Machine Interface-----------*//
// Set the LCD address to 0x27 for a 16 chars and 2 line display
LiquidCrystal_I2C dispOut(0x27, 16, 2);

//For Button Press Timing
unsigned long currentMillis_BTN_UP, currentMillis_BTN_DN, currentMillis_BTN_OK;
unsigned long previousMillis_BTN_UP, previousMillis_BTN_DN, previousMillis_BTN_OK;
unsigned long interval_BTN = 50;

//for attached button switches
//#define BTN_UP 31
//#define BTN_OK 33
//#define BTN_DN 35
Button BTN_UP(31);
Button BTN_OK(33);
Button BTN_DN(35);

//For Screen Status Switching
int screenNo = 1;
int old_screenNo;
//For Number Paper to be picked
int noPaperPick = 1;
int old_noPaperPick;

//For Number of Trays to be used
int noTray = 2;     //atleast 2 trays to use. default
int old_noTray;
//*--------End ofHuman-Machine Interface--------*//

//For Signal Acquisition
String signal = "NULL";

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);

  dispOut.begin(16, 2);
  dispOut.backlight();
  dispOut.setCursor(0, 0);
  dispOut.print("AUTO. PAPER PICK");
  dispOut.setCursor(0, 1);
  dispOut.print("& PUNCHING MACH.");

  //For Paper Tray's Servo Initiliazation
  P_Tray1_Servo.attach(44);      //Servo Initialization
  P_Tray2_Servo.attach(45);      //Servo Initialization
  P_Tray3_Servo.attach(46);      //Servo Initialization

  //For Paper Punch Servo Initialization
  P_Punch_Servo.attach(7);      //Servo Initialization

  //For Paper Door Out Servo Initialization
  P_Door_Servo.attach(6);
  //For Paper Out Servo Initialization
  P_Out_Servo.attach(5);      //Servo Initialization

  //initializing Servo's home position
  P_Tray1_Servo.write(0);
  P_Tray2_Servo.write(0);
  P_Tray3_Servo.write(0);
  P_Punch_Servo.write(0);
  P_Door_Servo.write(doorClose);
  P_Out_Servo.write(angleHome);

  //For Stepper's Enable Pin
  pinMode(P_Tray1_EN, OUTPUT);
    digitalWrite(P_Tray1_EN, HIGH); //Disables the Stepper Motor at initial

  pinMode(P_Tray2_EN, OUTPUT);
    digitalWrite(P_Tray2_EN, HIGH); //Disables the Stepper Motor at initial
    
  pinMode(P_Tray3_EN, OUTPUT);
    digitalWrite(P_Tray3_EN, HIGH); //Disables the Stepper Motor at initial
  
  pinMode(P_Out_EN, OUTPUT);
    digitalWrite(P_Out_EN, HIGH); //Disables the Stepper Motor at initial

  //For Paper Tray's Sensors Initialization
  pinMode(P_Tray1_IR, INPUT_PULLUP);   //Sensor Initialization
  pinMode(P_Tray2_IR, INPUT_PULLUP);   //Sensor Initialization
  pinMode(P_Tray3_IR, INPUT_PULLUP);   //Sensor Initialization
  pinMode(P_Path_IR, INPUT_PULLUP);   //Sensor Initialization
  pinMode(P_Out_IR, INPUT_PULLUP);   //Sensor Initialization

  //For HMI Initialization
  //pinMode(BTN_UP, INPUT_PULLUP);   //Sensor Initialization
  //pinMode(BTN_OK, INPUT_PULLUP);   //Sensor Initialization
  //pinMode(BTN_DN, INPUT_PULLUP);   //Sensor Initialization
  BTN_UP.begin();
  BTN_OK.begin();
  BTN_DN.begin();

  //REMOVED: For Further development
  //For Speed Adjust PinMode
  //pinMode(A0, INPUT);                   //Potentiometer Initialization

  //Sample Signal to be removed after simulation
  //signal = "OUTPUT GO";

  //Wait for arduino to boot up
  delay(1000);
}

void speedCalculation() {
  //Reading the potentiometers valuea
  //rpm = map(analogRead(A0), 0, 1023, 1, 15);      //Removed for further research
  
  //Calculation for Speed based on RPM
  speed = (rpm * stepsperrev) / 60.0;
  
  //Setting the max speed & Acceleration
  P_Tray1.setMaxSpeed(speed);
  P_Tray1.setAcceleration(speed);
  P_Tray2.setMaxSpeed(speed);
  P_Tray2.setAcceleration(speed);
  P_Tray3.setMaxSpeed(speed);
  P_Tray3.setAcceleration(speed);
  P_Out.setMaxSpeed(speed);
  P_Out.setAcceleration(speed);
}

void loop() {
  speedCalculation();
  fullProcess();            //Main Process flow happens here
  humanMachineInterface();  //Process for the LCD Display
  P_PathJamCheck();         //For detecting path jams and error
  P_Tray1_Run();            //Per Tray run sequence
  P_Tray2_Run();            //Per Tray run sequence 
  P_Tray3_Run();            //Per Tray run sequence
  P_Punching_Run();         //Punching run sequence
  P_Out_Run();              //Paper Out run sequence
  
  //For debugging purposes
  //Serial.println(signal);
}

void fullProcess() {
  //For starting the system
  if (signal == "NULL" && noPaperPick != 0 && screenNo == 20) {
    signal = "PT_3 GO";  //Starts the Running
  }
  else if (signal == "P_3 RECEIVED") {
    signal = "PT_2 GO"; 
  }
  else if (signal == "P_2 RECEIVED") {
    if (noTray == 3) {
      signal = "PT_1 GO";
    }
    else {
      delay(1000);          //gives a second to settle in the papers
      signal = "PUNCH GO";
    }
  }
  else if (signal == "P_1 RECEIVED") {
    delay(1000);          //gives a second to settle in the papers
    signal = "PUNCH GO";
  }
  else if (signal == "PUNCH DONE") {
    signal = "OUTPUT GO";
    //signal = "OUTPUT DONE"; //bypassed for testing purpose
  }
  else if (signal == "OUTPUT DONE") {
    if (noPaperPick != 0) {
      noPaperPick-=1;
      delay(50);      
      //Checks if after deduction is equal to 0
      if (noPaperPick == 0) {
        screenNo = 2;       //Go backs to homescreen
        noPaperPick = 1;    //resets the value
        signal = "NULL";    //resets the signal to NULL
      }
      else {
        signal = "PT_3 GO";
      }
    }
  }
  else  if (signal == "NULL") {
    digitalWrite(P_Tray1_EN, HIGH);
    digitalWrite(P_Tray2_EN, HIGH);
    digitalWrite(P_Tray3_EN, HIGH);
    digitalWrite(P_Out_EN, HIGH);
    
    anglestep_P_Tray1 = 5;
    P_Tray1_Servo.write(anglestep_P_Tray1);    
    anglestep_P_Tray2 = 5;
    P_Tray2_Servo.write(anglestep_P_Tray2);
    anglestep_P_Tray3 = 5;
    P_Tray3_Servo.write(anglestep_P_Tray3);
    P_Door_Servo.write(doorClose);
    P_Out_Servo.write(angleHome);
  }
}

void menu(int val) {
  String msg;

  //menu
  switch (val) {
    case 1: 
      dispOut.setCursor(0,0);
      dispOut.print("    SETUP >>    ");
      dispOut.setCursor(0,1);
      dispOut.print("                ");
      break;
    case 2: 
      dispOut.setCursor(0,0);
      dispOut.print("    << RUN      ");
      dispOut.setCursor(0,1);
      dispOut.print("                ");

      signal = "NULL";  //Stops the Running
      break;
    case 10:
      dispOut.setCursor(0,0);
      dispOut.print("  >> SETUP <<   ");
      dispOut.setCursor(0,1);
      dispOut.print(" # of Paper/Tray");
      break;
    case 11:
      dispOut.setCursor(0,0);
      dispOut.print("  >> SETUP <<   ");
      dispOut.setCursor(0,1);
      dispOut.print("# of Tray To Use");
      break;
    case 12:
      dispOut.setCursor(0,0);
      dispOut.print("  >> SETUP <<   ");
      dispOut.setCursor(0,1);
      dispOut.print("   EXIT SETUP   ");
      break;
    case 100:
      dispOut.setCursor(0,0);
      dispOut.print(" # of Paper/Tray");
      dispOut.setCursor(0,1);
      dispOut.print("   Paper: ");
      dispOut.setCursor(10,1);
      dispOut.print(noPaperPick);
      if (noPaperPick < 10) {
        dispOut.setCursor(11,1);
      }
      else if (noPaperPick >= 10) {
        dispOut.setCursor(12,1);
      }
      dispOut.print("        ");
      break;
    case 110:
      dispOut.setCursor(0,0);
      dispOut.print("# of Tray To Use");
      dispOut.setCursor(0,1);
      dispOut.print("   Tray: ");
      dispOut.setCursor(9,1);
      dispOut.print(noTray);
      dispOut.setCursor(10,1);
      dispOut.print("      ");
      break;
    case 20: 
      dispOut.setCursor(0,0);
      dispOut.print(" >> RUNNING! << ");
      dispOut.setCursor(0,1);
      dispOut.print("# Sets Left: ");
      dispOut.setCursor(14,1);
      dispOut.print(noPaperPick);
      break;
    
    //For Displaying Errors:
    case 1000:
      dispOut.setCursor(0,0);
      dispOut.print("EMERGENCY STOP!!");
      dispOut.setCursor(0,1);
      dispOut.print("P_1 PaperJammed!");
      break;
    case 1001:
      dispOut.setCursor(0,0);
      dispOut.print("EMERGENCY STOP!!");
      dispOut.setCursor(0,1);
      dispOut.print("P_2 PaperJammed!");
      break;
    case 1002:
      dispOut.setCursor(0,0);
      dispOut.print("EMERGENCY STOP!!");
      dispOut.setCursor(0,1);
      dispOut.print("P_3 PaperJammed!");
      break;

    //Confirming error
    case 2000:
      dispOut.setCursor(0,0);
      dispOut.print("Are you sure to ");
      dispOut.setCursor(0,1);
      dispOut.print(" RST the system?");
      break;
    case 2001:
      dispOut.setCursor(0,0);
      dispOut.print("Are you sure to ");
      dispOut.setCursor(0,1);
      dispOut.print(" RST the system?");
      break;
    case 2002:
      dispOut.setCursor(0,0);
      dispOut.print("Are you sure to ");
      dispOut.setCursor(0,1);
      dispOut.print(" RST the system?");
      break;
    case 0:
      dispOut.setCursor(0,0);
      dispOut.print("                ");
      dispOut.setCursor(0,1);
      dispOut.print("                ");
    break;
  }
}


void humanMachineInterface() {
  
  //add refresh only if data is changed
  if(screenNo != old_screenNo || old_screenNo == 0) {
    menu(screenNo);
    old_screenNo = screenNo;  
  }
  else if (noPaperPick != old_noPaperPick || old_noPaperPick == 0) {
    old_noPaperPick = noPaperPick;
    menu(screenNo);
  }
  else if (noTray != old_noTray || old_noTray == 0) {
    old_noTray = noTray;
    menu(screenNo);
  }



  //button up scenarios
  if(screenNo > 2 && screenNo < 10) {
      screenNo = 1;
  }
  else if (BTN_UP.pressed()) {
    //if subMenu: # of Paper/Tray is being selected
    if (screenNo == 100) {
      noPaperPick += 1;
      if (noPaperPick > 99) {
        noPaperPick = 99;
      }
    }
    //if subMenu: # of Tray To Use is being selected
    else if (screenNo == 110) {
      noTray += 1;
      if (noTray >= 3) {
        noTray = 3;
      }
    }
    else if (screenNo == 20) {
      screenNo = 20;
    }
    else {
      screenNo+=1;
    }
  }
  //button down scenarios
  if(screenNo < 1) {
      screenNo = 2;
  }
  else if (BTN_DN.pressed()) {
    //if subMenu: # of Paper/Tray is being selected
    if (screenNo == 100) {
      noPaperPick -= 1;
      if (noPaperPick < 2) {
        noPaperPick = 1;
      }
    }
    //if subMenu: # of Tray To Use is being selected
    else if (screenNo == 110) {
      noTray -= 1;
      if (noTray < 2) {
        noTray = 1;
      }
    }
    else if (screenNo == 20) {
      screenNo = 20;
    }
    else {
      screenNo-=1;
    }
  }

  //entering subMenu
  //This will avoid exceeding to the numbers of menu being set up
  if(screenNo > 12 && screenNo < 20) {
    screenNo = 10;
  }
  else if(screenNo == 9) {
    screenNo = 12;
  }
  else if (BTN_OK.pressed()) {
    //If pressed again, this means that the set up is being made already, EXITS
    if (screenNo == 100 || screenNo == 110) { 
      screenNo/=10;
    }
    //Exits from the Setup
    else if (screenNo == 12) {
      screenNo = 1;
    }
    else if (screenNo == 20) {
      screenNo/=10;
    }
    else if (screenNo <= 999) {
      screenNo*=10;
    }

    //If error occured
    if (screenNo == 1000) {
      screenNo = 2000;
    }
    else if (screenNo == 2000) {
      screenNo = 2;
    }
    else if (screenNo == 1001) {
      screenNo = 2001;
    }
    else if (screenNo == 2001) {
      screenNo = 2;
    }
    else if (screenNo == 1002) {
      screenNo = 2002;
    }
    else if (screenNo == 2002) {
      screenNo = 2;
    }
  }
}

//function for selectively running the function P_Tray
void stepperMove(int stepper, bool run) {
  //For Paper Tray 1
  if(stepper == 1 && run == true) {
    //Enabling the stepper motor
    digitalWrite(P_Tray1_EN, LOW);

    P_Tray1.setSpeed(speed);          //sets the speed of the stepper motor

    //removed the acceleration as it conflicts when multiple steppers are attached
    P_Tray1.setAcceleration(speed);   //sets the acceleration

    P_Tray1.move(99999);              //run continously
    P_Tray1.runSpeedToPosition();       //need to use this function to achieve the set speed
  }
  else if (stepper == 1 && run == false) {
    //Stopping the Stepper Motor
    P_Tray1.stop();

    //Disabling the stepper motor
    digitalWrite(P_Tray1_EN, HIGH);
  }

  //For Paper Tray 2
  if(stepper == 2 && run == true) {
    //Enabling the stepper motor
    digitalWrite(P_Tray2_EN, LOW);
    
    P_Tray2.setSpeed(speed);          //sets the speed of the stepper motor

    //removed the acceleration as it conflicts when multiple steppers are attached
    P_Tray2.setAcceleration(speed);   //sets the acceleration

    P_Tray2.move(99999);              //run continously
    P_Tray2.runSpeedToPosition();     //need to use this function to achieve the set speed
  }
  else if (stepper == 2 && run == false) {
    //Stopping the Stepper Motor
    P_Tray2.stop();

    //Disabling the stepper motor
    digitalWrite(P_Tray2_EN, HIGH);
  }

  //For Paper Tray 3
  if(stepper == 3 && run == true) {
    //Enabling the stepper motor
    digitalWrite(P_Tray3_EN, LOW);
    
    P_Tray3.setSpeed(speed);          //sets the speed of the stepper motor

    //removed the acceleration as it conflicts when multiple steppers are attached
    P_Tray3.setAcceleration(speed);   //sets the acceleration

    P_Tray3.move(99999);              //run continously
    P_Tray3.runSpeedToPosition();     //need to use this function to achieve the set speed
  }
  else if (stepper == 3 && run == false) {
    //Stopping the Stepper Motor
    P_Tray3.stop();

    //Disabling the stepper motor
    digitalWrite(P_Tray3_EN, HIGH);
  }

  //For OUTPUT
  if(stepper == 4 && run == true) {
    //Enabling the stepper motor
    digitalWrite(P_Out_EN, LOW);
    
    P_Out.setSpeed(speed);          //sets the speed of the stepper motor

    //removed the acceleration as it conflicts when multiple steppers are attached
    P_Out.setAcceleration(speed);   //sets the acceleration

    P_Out.move(99999);              //run continously
    P_Out.runSpeedToPosition();     //need to use this function to achieve the set speed
  }
  else if (stepper == 4 && run == false) {
    //Stopping the Stepper Motor
    P_Out.stop();

    //Disabling the stepper motor
    digitalWrite(P_Out_EN, HIGH);
  }
}

//function for checking paper jam for each paper tray run
void P_PathJamCheck() {
  //Sensor is Detected
  if (digitalRead(P_Path_IR) == LOW) {
    //Timer starts now!
    currentMillis_PJam = millis();    //timer to check if paper is jammed

    //If paper passes and PT_1 DONE
    if (signal == "P_1 SLIDING") {
      signal = "P_1 PASSING"; //this means that paper is passing

      //Sets the time checker now; if 10 seconds is elapsed, meaning paper is jammed
      previousMillis_PJam = currentMillis_PJam;
    }
    //If paper passes and PT_2 DONE
    else if (signal == "P_2 SLIDING") {
      signal = "P_2 PASSING"; //this means that paper is passing

      //Sets the time checker now; if 10 seconds is elapsed, meaning paper is jammed
      previousMillis_PJam = currentMillis_PJam;
    }
    //If paper passes and PT_3 DONE
    else if (signal == "P_3 SLIDING") {
      signal = "P_3 PASSING"; //this means that paper is passing

      //Sets the time checker now; if 10 seconds is elapsed, meaning paper is jammed
      previousMillis_PJam = currentMillis_PJam;
    }

    //if paper is passing and it took for more than 10 seconds, it means paper jam
    else if(signal == "P_1 PASSING" && (currentMillis_PJam - previousMillis_PJam >= interval_PJam)) {
      signal = "P_1 Paper Jammed!";
      screenNo = 1000;
    }
    else if(signal == "P_2 PASSING" && (currentMillis_PJam - previousMillis_PJam >= interval_PJam)) {
      signal = "P_2 Paper Jammed!";
      screenNo = 1001;
    }
    else if(signal == "P_3 PASSING" && (currentMillis_PJam - previousMillis_PJam >= interval_PJam)) {
      signal = "P_3 Paper Jammed!";
      screenNo = 1002;
    }
  }

  else if (digitalRead(P_Path_IR) == HIGH) {
    //Timer starts now!
    currentMillis_PJam = millis();    //timer to check if paper is jammed

    //If passing is successful, sends received signal to proceed the 2nd paper tray
    if (signal == "P_1 PASSING") {
      signal = "P_1 RECEIVED";
    }
    //If passing is successful, sends received signal to proceed the 3rd paper tray
    else if (signal == "P_2 PASSING") {
      signal = "P_2 RECEIVED";
    }
    //If passing is successful, sends received signal to proceed Punching
    else if (signal == "P_3 PASSING") {
      signal = "P_3 RECEIVED";
    }

    //In case the paper didnt arrived at the passing sensor checked, this means, paper is jammed
    else if (signal == "PT_1 DONE") {
      signal = "P_1 SLIDING"; //this means paper is still goind down the paper path
      
      //Sets the time checker now; if 10 seconds is elapsed, meaning paper is jammed
      previousMillis_PJam = currentMillis_PJam;
    }
    else if (signal == "PT_2 DONE") {
      signal = "P_2 SLIDING"; //this means paper is still goind down the paper path
      
      //Sets the time checker now; if 10 seconds is elapsed, meaning paper is jammed
      previousMillis_PJam = currentMillis_PJam;
    }
    else if (signal == "PT_3 DONE") {
      signal = "P_3 SLIDING"; //this means paper is still goind down the paper path
      
      //Sets the time checker now; if 10 seconds is elapsed, meaning paper is jammed
      previousMillis_PJam = currentMillis_PJam;
    }

    //if paper is passing and it took for more than 10 seconds, it means paper jam
    else if(signal == "P_1 SLIDING" && (currentMillis_PJam - previousMillis_PJam >= interval_PJam)) {
      signal = "P_1 Paper Jammed!";
      screenNo = 1000;
    }
    else if(signal == "P_2 SLIDING" && (currentMillis_PJam - previousMillis_PJam >= interval_PJam)) {
      signal = "P_2 Paper Jammed!";
      screenNo = 1001;
    }
    else if(signal == "P_3 SLIDING" && (currentMillis_PJam - previousMillis_PJam >= interval_PJam)) {
      signal = "P_3 Paper Jammed!";
      screenNo = 1002;
    }

  }
}

//function for running the punching mechanism
void P_Punching_Run() {
  if(signal == "PUNCH GO") {
    //Timer starts now
    currentMillis_Punching = millis();

    //If the anglePunch is not more than the targetAnglePunch
    if (anglePunch <= targetAnglePunch) {
        //Checks if time interval is equals to the current time minus the previous time checkd
        if (currentMillis_Punching - previousMillis_Punching >= interval_Punching) {
          //Increments the anglePunch variable that will be used to run the servo
          anglePunch+=anglePunchInc;

          //Servo will moved depending on the anglePunch Value
          P_Punch_Servo.write(anglePunch);      //Punches the paper

          //Resets the time checking
          previousMillis_Punching = currentMillis_Punching;
        }
    }
    //if anglePunch is already at the targetAnglePunch, retracts the punching and sets the signal to DONE
    else {
      anglePunch *= 0;
      P_Punch_Servo.write(anglePunch);  //Retracts back the punching mechanism
      delay(150);                           //allows servo to rest for a while before retraction

      //Informs the system that punching is done
      signal = "PUNCH DONE";
    }
    
  }
}

//function for running the output rollers and door mechanism
void P_Out_Run() {
  
  if (signal == "OUTPUT GO") {
    //RUN THE OUTPUT PROCESS HERE
    //Insert Servo up command here
    P_Door_Servo.write(doorOpen);
    P_Out_Servo.write(angleOut);

    signal = "THROWING PAPER OUT";
  }
  else if (signal == "THROWING PAPER OUT") {
    stepperMove(4, true);
  }
  
  //Sensor Detection
  //If paper hits the sensor, continues to run until sensor detected nothing
  if (digitalRead(P_Out_IR) == LOW) {
    if (signal == "THROWING PAPER OUT") {
      signal = "P_OUT PASSING";
    }
    else if (signal == "P_OUT PASSING") {
      stepperMove(4, true);         //starts the motor
    }
  }
  

  //If paper went from passing to detected nothing, this means that the paper already passed the last roller
  if (digitalRead(P_Out_IR) == HIGH) {
    if (signal == "P_OUT PASSING") {
      P_Door_Servo.write(doorClose);
      P_Out_Servo.write(angleHome);
      signal = "OUTPUT DONE";
    }
    else if (signal == "OUTPUT DONE") {
      stepperMove(4, false);      //stops the motor
    }
  }
}

//functions for running the paper tray mechanism
void P_Tray1_Run() {
  //Time Starts Now
  currentMillis_P_Tray1 = millis();

  //If the sensor did not detect any paper
  if (digitalRead(P_Tray1_IR) == HIGH) {
    //If Paper is not yet picked up
    if (signal == "PT_1 GO") {
    
    //Sending 1 angle to servo every 100ms 
      if (currentMillis_P_Tray1 - previousMillis_P_Tray1 >= lowerDownSpeed) {
        if (anglestep_P_Tray1 <= 180) {
          anglestep_P_Tray1+= 1;                    //add 1 step to the angle
          previousMillis_P_Tray1 = currentMillis_P_Tray1;   //Saves the last time being read
        }

        P_Tray1_Servo.write(anglestep_P_Tray1);       //Writes the angle to the servo
      }
    
    //Continously Run the Stepper
    stepperMove(1, true);
  }

  //Once the paper passes the sensor, it will then retract the servo and stop the roller
    else if (signal == "PT_1 ACQUIRED") {
      anglestep_P_Tray1 = 5;
      P_Tray1_Servo.write(anglestep_P_Tray1);         //Returns the servo to home position
      
      stepperMove(1, false);                 //Stops the Stepper to rotate
      
      signal = "PT_1 DONE";      //Sends signal to the system that paper P_Tray 1 is finished
    }
    else if (signal == "NULL") {
      anglestep_P_Tray1 = 5;
      P_Tray3_Servo.write(anglestep_P_Tray3);
    }
  }

  //If the sensor detected a paper passes, 
  if (digitalRead(P_Tray1_IR) == LOW) {
    //changes the signal to Acquired
    if (signal == "PT_1 GO") {
      anglestep_P_Tray1-=retractDistance;                  //retracts set degrees after paper being picked to avoid double pick up
      P_Tray1_Servo.write(anglestep_P_Tray1);   //updates the servo with the updated angle
      delay(150);                     //add some delays to avoid inconsistent readings || must use millis, further improvement

      signal = "PT_1 ACQUIRED";      //Sets the signal paper acquired
    }
    //Once Paper is acquired, it will stop the servo from lowering down and servo roller will continue to run
    else if (signal == "PT_1 ACQUIRED") {
      stepperMove(1, true);
    }
  }
}
void P_Tray2_Run() {
  //Time Starts Now
  currentMillis_P_Tray2 = millis();

  //If the sensor did not detect any paper
  if (digitalRead(P_Tray2_IR) == HIGH) {
    //If Paper is not yet picked up
    if (signal == "PT_2 GO") {
    
    
    //Sending 1 angle to servo every 100ms 
      if (currentMillis_P_Tray2 - previousMillis_P_Tray2 >= lowerDownSpeed) {
        if (anglestep_P_Tray2 <= 180) {
          anglestep_P_Tray2+= 1;                    //add 1 step to the angle
          previousMillis_P_Tray2 = currentMillis_P_Tray2;   //Saves the last time being read
        }

        P_Tray2_Servo.write(anglestep_P_Tray2);       //Writes the angle to the servo
      }
    
    //Continously Run the Stepper
    stepperMove(2, true);
  }

  //Once the paper passes the sensor, it will then retract the servo and stop the roller
    else if (signal == "PT_2 ACQUIRED") {
      anglestep_P_Tray2 = 5;
      P_Tray2_Servo.write(anglestep_P_Tray2);         //Returns the servo to home position
      stepperMove(2, false);                 //Stops the Stepper to rotate
      
      signal = "PT_2 DONE";      //Sends signal to the system that paper P_Tray 1 is finished
    }
    else if (signal == "NULL") {
      anglestep_P_Tray1 = 5;
      P_Tray3_Servo.write(anglestep_P_Tray3);
    }
  }

  //If the sensor detected a paper passes, 
  if (digitalRead(P_Tray2_IR) == LOW) {
    //changes the signal to Acquired
    if (signal == "PT_2 GO") {
      anglestep_P_Tray2-=retractDistance;                  //retracts set degrees after paper being picked to avoid double pick up
      P_Tray2_Servo.write(anglestep_P_Tray2);   //updates the servo with the updated angle
      delay(150);                     //add some delays to avoid inconsistent readings || must use millis, further improvement

      signal = "PT_2 ACQUIRED";      //Sets the signal paper acquired
    }
    //Once Paper is acquired, it will stop the servo from lowering down and servo roller will continue to run
    else if (signal == "PT_2 ACQUIRED") {
      stepperMove(2, true);
    }
  }
}
void P_Tray3_Run() {
  //Time Starts Now
  currentMillis_P_Tray3 = millis();

  //If the sensor did not detect any paper
  if (digitalRead(P_Tray3_IR) == HIGH) {
    //If Paper is not yet picked up
    if (signal == "PT_3 GO") {
    
    
    //Sending 1 angle to servo every 100ms 
      if (currentMillis_P_Tray3 - previousMillis_P_Tray3 >= lowerDownSpeed) {
        if (anglestep_P_Tray3 <= 180) {
          anglestep_P_Tray3+= 1;                    //add 1 step to the angle
          previousMillis_P_Tray3 = currentMillis_P_Tray3;   //Saves the last time being read
        }

        P_Tray3_Servo.write(anglestep_P_Tray3);       //Writes the angle to the servo
      }
    
    //Continously Run the Stepper
    stepperMove(3, true);
  }

  //Once the paper passes the sensor, it will then retract the servo and stop the roller
    else if (signal == "PT_3 ACQUIRED") {
      anglestep_P_Tray3 = 5;
      P_Tray3_Servo.write(anglestep_P_Tray3);         //Returns the servo to home position
      stepperMove(3, false);                 //Stops the Stepper to rotate
      
      signal = "PT_3 DONE";      //Sends signal to the system that paper P_Tray 1 is finished
    }
    else if (signal == "NULL") {
      anglestep_P_Tray3 = 5;
      P_Tray3_Servo.write(anglestep_P_Tray3);
    }
  }

  //If the sensor detected a paper passes, 
  if (digitalRead(P_Tray3_IR) == LOW) {
    //changes the signal to Acquired
    if (signal == "PT_3 GO") {
      anglestep_P_Tray3-=retractDistance;                  //retracts set degrees after paper being picked to avoid double pick up
      P_Tray3_Servo.write(anglestep_P_Tray3);   //updates the servo with the updated angle
      delay(150);                     //add some delays to avoid inconsistent readings || must use millis, further improvement

      signal = "PT_3 ACQUIRED";      //Sets the signal paper acquired
    }
    //Once Paper is acquired, it will stop the servo from lowering down and servo roller will continue to run
    else if (signal == "PT_3 ACQUIRED") {
      stepperMove(3, true);
    }
  }
}