// 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 openButton  2
#define closeButton  3

#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 openDoor = false;
bool closeDoor = false;


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

  pinMode(homingBtn, INPUT_PULLUP);
  pinMode(openButton, INPUT_PULLUP);
  pinMode(closeButton, 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 (!openDoor) {
    openDoor = digitalRead(openButton);
  }

  if (!closeDoor) {
    closeDoor = digitalRead(closeButton);
  }


  if (openDoor) {
    closeDoor = false;

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