#include <ezButton.h>
#include <AccelStepper.h>

// Define number of steps per revolution:
const int stepsPerRevolution = 200;

// Give the motor control pins names:   for MotorShield Rev3
// #define pwmA 3
// #define pwmB 11
// #define brakeA 9
// #define brakeB 8
// #define dirA 12
// #define dirB 13

// Simulation wise for A4988
const int dirPin = 2;
const int stepPin = 3;

// Define the AccelStepper interface type:
#define MotorInterfaceType 1

#define DIRECTION_CCW -1
#define DIRECTION_CW 1

#define STATE_CCW 1
#define STATE_CW 2
#define STATE_HOMING 3

#define MAX_POSITION 0x7FFFFFFF  // maximum of position we can set (long type)

ezButton limitSwitch_1(5);  // create ezButton object that attach to pin 2;
ezButton limitSwitch_2(6);  // create ezButton object that attach to pin 3;

// Create a new instance of the AccelStepper class:
AccelStepper stepper = AccelStepper(MotorInterfaceType, stepPin, dirPin);

int stepperState = STATE_HOMING;  // Initial state: Homing
int direction = DIRECTION_CW;
long targetPos = 0;

void setup() {
  Serial.begin(115200);

  // Set the PWM and brake pins so that the direction pins can be used to control the motor:   for MotorShield Rev3
  // pinMode(pwmA, OUTPUT);
  // pinMode(pwmB, OUTPUT);
  // pinMode(brakeA, OUTPUT);
  // pinMode(brakeB, OUTPUT);

  // digitalWrite(pwmA, HIGH);
  // digitalWrite(pwmB, HIGH);
  // digitalWrite(brakeA, LOW);
  // digitalWrite(brakeB, LOW);

  limitSwitch_1.setDebounceTime(50);  // set debounce time to 50 milliseconds
  limitSwitch_2.setDebounceTime(50);  // set debounce time to 50 milliseconds

  stepper.setMaxSpeed(80.0);      // set the maximum speed
  stepper.setAcceleration(10.0);  // set acceleration
  stepper.setSpeed(80);           // set initial speed
}

void loop() {
  limitSwitch_1.loop();       // MUST call the loop() function first
  limitSwitch_2.loop();       // MUST call the loop() function first
  Serial.println(targetPos);  //Flag for debugging

  switch (stepperState) {
    case STATE_HOMING:
      Serial.println("HOMING");
      if (limitSwitch_1.isPressed()) {
        stepper.setCurrentPosition(0);  // Set current position to 0 when limit switch 1 is pressed
        targetPos = (DIRECTION_CCW * MAX_POSITION);
        stepper.moveTo(targetPos);
        direction = DIRECTION_CCW;  // Set direction to counterclockwise
        stepperState = STATE_CCW;   // Move to the next state: Move
      } else {
        stepper.setSpeed(40);                         // Set speed for homing
        stepper.setAcceleration(5.0);                 // Set acceleration for homing
        stepper.moveTo(DIRECTION_CW * MAX_POSITION);  // Move stepper motor counter-clockwise until limit switch 1 is pressed
        stepper.run();
      }
      break;

    case STATE_CCW:
      // Serial.print("CCW");
      // Serial.println(DIRECTION_CCW);
      if (limitSwitch_2.isPressed()) {
        targetPos = (DIRECTION_CW * MAX_POSITION);
        direction = DIRECTION_CW;  // Set direction to clockwise
        stepperState = STATE_CW;   // Move to the next state: Move
      } else {
        stepper.moveTo(targetPos);
        stepper.run();
      }
      break;

    case STATE_CW:
      // Serial.print("CW");
      // Serial.println(DIRECTION_CW);
      if (limitSwitch_1.isPressed()) {
        targetPos = (DIRECTION_CCW * MAX_POSITION);
        direction = DIRECTION_CCW;  // Set direction to counterclockwise
        stepperState = STATE_CCW;   // Move to the next state: Move
      } else {
        stepper.moveTo(targetPos);
        stepper.run();
      }
      break;
  }
}