// Include the AccelStepper Library
#include <AccelStepper.h>

// Define step constants
#define FULLSTEP 4
#define HALFSTEP 8

// Define Motor Pins (2 Motors used)

#define motorPin1  8   // 28BYJ48 pin 1
#define motorPin2  9   // 28BYJ48 pin 2
#define motorPin3  10  // 28BYJ48 pin 3
#define motorPin4  11  // 28BYJ48 pin 4



AccelStepper stepperX(HALFSTEP, motorPin1, motorPin3, motorPin2, motorPin4);

// Define the Pins used

#define firstFloor  2
#define secondFloor  3
#define groundFloor  4
#define homingBtn 12

// Stepper Travel Variables
long TravelX;  // Used to store the X value entered in the Serial Monitor
int move_finished = 1; // Used to check if move is completed
long initial_homing = -1; // Used to Home Stepper at startup

long debouncing_time = 500; //Debouncing Time in Milliseconds
volatile unsigned long last_micros;

bool runToFirstFloor = false;
bool runToSecondFloor = false;
bool runToHome = false;


void setup() {
  Serial.begin(9600);  // Start the Serial monitor with speed of 9600 Bauds

  pinMode(homingBtn, INPUT_PULLUP);
  pinMode(firstFloor, INPUT_PULLUP);
  pinMode(secondFloor, INPUT_PULLUP);
  pinMode(groundFloor, INPUT_PULLUP);

  delay(5);  // Wait for EasyDriver wake up

  //  Set Max Speed and Acceleration of each Steppers at startup for homing
  stepperX.setMaxSpeed(1000.0);      // Set Max Speed of Stepper (Slower to get better accuracy)
  stepperX.setAcceleration(200);  // Set Acceleration of Stepper


  // Start Homing procedure of Stepper Motor at startup

  Serial.print("Stepper is Homing . . . . . . . . . . . ");

  stepperX.setCurrentPosition(0);  // Set the current position as zero for now
  stepperX.setMaxSpeed(1000.0);      // Set Max Speed of Stepper (Slower to get better accuracy)
  stepperX.setAcceleration(200);  // Set Acceleration of Stepper
  initial_homing = 1;

  while (!digitalRead(homingBtn)) { // Make the Stepper move CW until the switch is deactivated
    stepperX.moveTo(initial_homing);
    stepperX.run();
    initial_homing--;
    delay(5);
  }

  stepperX.setCurrentPosition(0);
  Serial.println("Homing Completed");
  Serial.println(stepperX.currentPosition());
  stepperX.setMaxSpeed(1000);      // Set Max Speed of Stepper (Faster for regular movements)
  stepperX.setAcceleration(200);  // Set Acceleration of Stepper
  delay(2000);
}

void loop() {
  if (!runToHome) {
    runToHome = digitalRead(groundFloor);
  }

  if (!runToFirstFloor) {
    runToFirstFloor = digitalRead(firstFloor);
  }

  if (!runToSecondFloor) {
    runToSecondFloor = digitalRead(secondFloor);
  }


  if (runToFirstFloor) {
    runToSecondFloor = false;
    runToHome = false;

    Serial.println(stepperX.currentPosition());
    stepperX.moveTo(2850);
    if ((stepperX.distanceToGo() != 0)) {
      stepperX.run();  // Move Stepper into position
    } else {
      runToFirstFloor = false;
    }
  } else if (runToSecondFloor) {
     runToFirstFloor = false;
    runToHome = false;
    Serial.println(stepperX.currentPosition());
    stepperX.moveTo(5700);
    if ((stepperX.distanceToGo() != 0)) {
      stepperX.run();  // Move Stepper into position
    } else {
      runToSecondFloor = false;
    }
  } else if (runToHome) {
     runToFirstFloor = false;
     runToSecondFloor = false;

    Serial.println(stepperX.currentPosition());
    stepperX.moveTo(0);
    if ((stepperX.distanceToGo() != 0)) {
      stepperX.run();  // Move Stepper into position
    } else {
      runToHome = false;
    }
  }
}