#include <AccelStepper.h>
#include <Encoder.h>

#define STEP_PIN 2
#define DIR_PIN 3

#define STEPS_PER_REVOLUTION 200

#define MAX_SPEED 1000
#define ACCELERATION 100

#define MS1_PIN 10
#define MS2_PIN 9
#define MS3_PIN 8

AccelStepper stepper(AccelStepper::DRIVER, STEP_PIN, DIR_PIN);

int targetPosition = 2;
bool start = false;

void setup() {
  
  stepper.setMaxSpeed(MAX_SPEED);
  stepper.setAcceleration(ACCELERATION);

  stepper.moveTo(targetPosition);
  stepper.runToPosition();

  Serial.begin(9600);
}

void loop() {

  int currentPosition = stepper.currentPosition();

  if(start){
    
    stepper.runSpeed();

    displayPositionAndVelocity(currentPosition-2, stepper.speed());

    
    if (stepper.distanceToGo() == 0) {
      stepper.stop();
      start = false;
    }
  }
  else {

    if (Serial.available() > 0) {

      targetPosition = Serial.parseInt() + 2;
      Serial.println(targetPosition);

      Serial.flush();

      if (targetPosition > currentPosition) {
        stepper.setSpeed(MAX_SPEED);
        stepper.moveTo(targetPosition);
      } 
      else if (targetPosition < currentPosition) {
        stepper.setSpeed(-MAX_SPEED); 
        stepper.moveTo(targetPosition);
      }

      start = true;

      while (Serial.available() > 0) {
        Serial.read();
      }
    }
  }
    
}

void displayPositionAndVelocity(int position, float velocity) {
  Serial.print("Position: ");
  Serial.println(position);
  Serial.print("Velocity: ");
  Serial.println(velocity);
}