#include <AccelStepper.h>


#define SET_POSITION_BUTTON_PIN 2 //Black Push button
#define FORWARD_BUTTON_PIN 3 // Blue push button
#define REVERSE_BUTTON_PIN 4 // red push button
#define POTENTIOMETER_PIN A0

// For A4988 driver board or similar
#define STEPPER_STEP_PIN 8
#define STEPPER_DIR_PIN 9
AccelStepper stepper(AccelStepper::DRIVER, STEPPER_STEP_PIN, STEPPER_DIR_PIN);


//For driver L298N or similar.
//#define STEPPER_PIN_1 8
//#define STEPPER_PIN_2 9
//#define STEPPER_PIN_3 10
//#define STEPPER_PIN_4 11
//AccelStepper stepper(AccelStepper::FULL4WIRE, STEPPER_PIN_1, STEPPER_PIN_2, STEPPER_PIN_3, STEPPER_PIN_4);

float speed;

int position1 = 0;
int position2 = 0;
int currentPosition = 0;
int stepsBetweenPositions = 0;

void setup() {
  //stepper.setMaxSpeed(1000); // Set your desired max speed here
  stepper.setAcceleration(100); // Set your desired acceleration here

  pinMode(FORWARD_BUTTON_PIN, INPUT_PULLUP);
  pinMode(REVERSE_BUTTON_PIN, INPUT_PULLUP);
  pinMode(SET_POSITION_BUTTON_PIN, INPUT_PULLUP);
}

void loop() {
  // Read potentiometer value and map it to speed range
  int speed = map(analogRead(POTENTIOMETER_PIN), 0, 1023, 0.1, 80);
  stepper.setMaxSpeed(speed);
  

  // Check button states
  bool forwardButtonPressed = digitalRead(FORWARD_BUTTON_PIN) == LOW;
  bool reverseButtonPressed = digitalRead(REVERSE_BUTTON_PIN) == LOW;
  bool setPositionButtonPressed = digitalRead(SET_POSITION_BUTTON_PIN) == LOW;

  // Move stepper forward if forward button is pressed
  if (forwardButtonPressed) {
    stepper.move(1); // Move one step forward
  }
  
  // Move stepper reverse if reverse button is pressed
  if (reverseButtonPressed) {
    stepper.move(-1); // Move one step backward
  }

  // Set position if set position button is pressed
  if (setPositionButtonPressed) {
    if (currentPosition == 0) {
      position1 = stepper.currentPosition();
      currentPosition = 1;
      int speed = map(analogRead(POTENTIOMETER_PIN), 0, 1023, 0.1, 80);
      stepper.setMaxSpeed(speed);
      
    } else if (currentPosition == 1) {
      position2 = stepper.currentPosition();
      stepsBetweenPositions = position2 - position1;
      currentPosition = 2;
      stepper.moveTo(position1); // Move back to position1
      int speed = map(analogRead(POTENTIOMETER_PIN), 0, 1023, 0.1, 80);
      stepper.setMaxSpeed(speed);
      
    }
    delay(500); // Add a small delay to debounce the button
  }

  // Update stepper
  stepper.run();

  // If both positions are set, move to the second position
  if (currentPosition == 2 && stepper.distanceToGo() == 0) {
    stepper.run();
    stepper.moveTo(position2);
    currentPosition = 3;
    stepper.run();
    int speed = map(analogRead(POTENTIOMETER_PIN), 0, 1023, 0.1, 80);
    stepper.setMaxSpeed(speed);
  }

  // If both positions are set and stepper reaches second position, loop back
  if (currentPosition == 3 && stepper.distanceToGo() == 0) {
    stepper.run();
    stepper.moveTo(position1);
    currentPosition = 2;
    stepper.run();
    int speed = map(analogRead(POTENTIOMETER_PIN), 0, 1023, 1, 80);
    stepper.setMaxSpeed(speed);
  }
}