#include <AccelStepper.h>

#define FORWARD_BUTTON_PIN 3
#define REVERSE_BUTTON_PIN 4
#define SET_POSITION_BUTTON_PIN 2
#define POTENTIOMETER_PIN A0


float speed;

//AccelStepper stepper(AccelStepper::FULL4WIRE, 8, 9, 10, 11);

AccelStepper stepper(AccelStepper::DRIVER, 8,9);

int position1 = 0;
int position2 = 0;
int currentPosition = 0;
int stepsBetweenPositions = 0;
int SH = 1000;
int SL = 100 ;
int maxAcc = 10000;

void setup() {
  int speed = map(analogRead(POTENTIOMETER_PIN), 0, 1023, SL, SH);
  stepper.setMaxSpeed(speed);
  stepper.setMaxSpeed(15000); // Set your desired max speed here
  stepper.setAcceleration(maxAcc); // 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, SL, SH);
  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, SL, SH);
      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, SL, SH);
      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, SL, SH);
    stepper.setMaxSpeed(speed);
    delay(100);
  }

  // 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, SL, SH);
    stepper.setMaxSpeed(speed);
    delay(100);
  }
}