#include <Stepper.h>
const int stepPin = 6;  //PUL -Pulse
const int dirPin = 7; //DIR -Direction
const int enPin = 8;  //ENA -Enable

//Global Variables

double trayPosition = 0; //Counts number of Motor Spins completed
// double trayEnd = 10;  //Motor spins needed for 'out' position

double pusherPosition = 0;
int trayStepsRev = 200;
int trayFast = 200; // Adjust for our motor later
int traySlow = 50;

Stepper pushMotor(2048, 13, 14, 15, 16); //Initialize push motor

bool StartButton = digitalRead(3); //Start button

bool ResetButton = digitalRead(4); //Reset Button

bool E_STOP = digitalRead(5); //E_STOP Button

bool LimitSwitch = digitalRead(6); //Tray Limit Switch
bool pusherLimitSwitch = digitalRead(7); //Pusher Limit Switch

bool trayHome, pusherHome;

unsigned long trayStartTime, trayStopTime, trayRunTime;  //Initialize trayRunTime once time value is known

unsigned long pusherStartTime, pusherStopTime, pusherRunTime;

void pushIn();
void pushOut();
void trayIn();
void trayOut();
void reset();
void emergency();



void setup ()
{
  //Input Pins
  pinMode(3, INPUT); //Start button

  pinMode(4, INPUT); //Reset Button

  pinMode(5, INPUT); //E_STOP

  pinMode(6, INPUT); //Tray Limit Switch

  pinMode(7, INPUT); //Pusher Limit Switch

  //Output Pins
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  pinMode(enPin, OUTPUT);
  digitalWrite(enPin, LOW);

  trayStartTime = 0;
  trayStopTime = 0;
  trayRunTime = 5000; //5 seconds of tray run time

  pushMotor.setSpeed(60); //Set speed to 60 rpm

  Serial.begin(9600);

}

void trayIn()
{
  if (!pusherHome) {
    pushIn();
  }
  while (LimitSwitch != HIGH)
  {

    while (ResetButton != HIGH || E_STOP != HIGH) {

      if (ResetButton == HIGH)
      {
        reset();
        break;
      }
      if (E_STOP == HIGH) {
        emergency();
        break;
      }
      else if (trayPosition <= 20)
      {
        for (int x = 0; x < 20 * trayStepsRev; x++) {
          if (trayPosition == 0) {
            trayHome = true;
            break;
          }
          digitalWrite(dirPin, LOW); //Motor spinning in reverse dir
          digitalWrite(stepPin, HIGH);
          delayMicroseconds(traySlow);;
          trayPosition--;
        }
      }

      else
      {
        if (trayPosition == 0 || LimitSwitch == HIGH) {
          trayHome = true;
          break;
        }
        digitalWrite(dirPin, LOW); //Motor spinning in reverse dir
        digitalWrite(stepPin, HIGH);
        delayMicroseconds(trayFast);;
        trayPosition--;

      }
    }//end while loop
  }//end limit switch loop
} //end trayIn


void trayOut()
{ while (trayPosition != 120) {
    while (E_STOP != HIGH || ResetButton != HIGH) {
      if (E_STOP == HIGH) {
        emergency();
        break;
      }
      else if (ResetButton == HIGH) {
        reset();
      }
      else {
        digitalWrite(dirPin, HIGH); //Forward Movement
        digitalWrite(stepPin, HIGH);
        delayMicroseconds(trayFast);
        trayPosition++;

      }
    }//end while
    delay(5000);
    pushOut();

  } //end trayOut

  void pushIn(){
    while (pusherLimitSwitch != HIGH)
    {
      while (ResetButton != HIGH || E_STOP != HIGH) {
        if (ResetButton == HIGH)
        {
          reset();
        }
        else if (E_STOP == HIGH) {
          emergency();
        }
        else if (pusherPosition == 5)
        {
          for (int x = 0; x < 5; x++) {
            pushMotor.step(-slowDown);
            pusherPosition--;
          }
        }
        else
        {
          if (pusherPosition == 0 || pusherLimitSwitch == HIGH) {
            pusherHome = true;
            break;
          }
          pushMotor.step(-stepsPerRev);  //Moving backwards
          pusherPosition --;

        }
      }//end while
    }
  }// end function


  void pushOut() {
    while (pusherPosition != 20) {
      while (E_STOP != HIGH || ResetButton != HIGH) {
        if (E_STOP == HIGH) {
          emergency();
          break;
        }
        else if (ResetButton == HIGH) {
          reset();
        }
        else {
          pushMotor.step(stepsPerRev); //forward movement
          pusherPosition++;

        }

      }

      void reset()
      {
        pushMotor.step(0);
        trayMotor.step(0);
        if (!pusherHome) {
          pushIn();
        }
        if (!trayHome) {
          trayIn();
        }
      }

      void emergency() {
        while (ResetButton != HIGH || StartButton != HIGH) {
          pushMotor.step(0);
          trayMotor.step(0);

          if (ResetButton == HIGH) {
            reset();
          }
          else if (StartButton != HIGH) {

            if (pusherHome != true || pusherPosition != 20)
            {
              pushOut();
            }

            if (trayHome != true || trayPosition != 120) {
              if (digitalRead(dirPin) == LOW) //Means tray Motor was moving back
              {
                trayIn();
              }
              else {
                trayOut();
              }
            }

          }
        }//end while
      }//end function


      void loop ()
      {
        if (trayPosition == 0) {
          trayHome = true;
        }
        if (pusherPosition == 0) {
          pusherHome = true;
        }

        if (((StartButton == HIGH ) && (Home == HIGH)) || ((StartButton == HIGH) && (ResetButton == HIGH)) || ((Home == HIGH) && (ResetButton == HIGH)) || (StartButton == HIGH && Home == HIGH && ResetButton == HIGH) ) //Multiple button press
        {
          reset();  //Home everything
        }

        if (Start == HIGH) {
          trayOut();
        }
        else if (Home == HIGH) {
          trayIn();
        }

      }//end loop