#include <AccelStepper.h>

#define HALFSTEP 8

#define motorPin1  8     // IN1 on ULN2003 ==> Blue   on 28BYJ-48
#define motorPin2  9     // IN2 on ULN2004 ==> Pink   on 28BYJ-48
#define motorPin3  10    // IN3 on ULN2003 ==> Yellow on 28BYJ-48
#define motorPin4  11    // IN4 on ULN2003 ==> Orange on 28BYJ-48

AccelStepper stepper(HALFSTEP, motorPin1, motorPin2, motorPin3, motorPin4);

int distance = 0;

const uint8_t startPin = 5;
const uint8_t stopPin = 6;
const uint8_t replayPin = 7;

enum State {IDLE, LEARN, REPLAY} state;

bool learning;   // hack instead of doing proper buttons. Sry Mom!

void initStepper() {
  stepper.setMaxSpeed(50);
  stepper.setSpeed(50);
  stepper.setAcceleration(35);  // Set Acceleration
}
//runToNewPosition()

void runFSM() {
  switch (state) {
    case IDLE :
      if (digitalRead(startPin) == LOW) {
        state = LEARN;
        distance = 0;  //...
        Serial.println(F("LEARN"));
      }
      if (digitalRead(replayPin) == LOW) {
        state = REPLAY;
        Serial.println(F("REPLAY"));
      }
      if (digitalRead(stopPin) == LOW) {
        stepper.runToNewPosition(0);
        Serial.println(F("go home."));
      }
      break;
    case LEARN:
      if (digitalRead(stopPin) == LOW) {
Serial.println("why does it not ");
        stepper.stop();
        distance = stepper.currentPosition();
// figure the distance for the replay
        state = IDLE;
        learning = false;
        Serial.println(F("IDLE"));

        break;
      }
      if (learning)
        break;

      learning = true;

Serial.println(" gonna learn ");  
/*
      stepper.move(1);
      distance++;
      Serial.println(distance);
      delay(100); // just a dirty delay to slow down the movement
*/
      stepper.setSpeed(50);
      stepper.setAcceleration(35.0);
    
      stepper.moveTo(1000);
//      stepper.runToPosition();

      break;
  
    case REPLAY:
          stepper.setSpeed(100);
      stepper.setAcceleration(70.0);


      Serial.print(F("steps to go ")); Serial.println(distance);
      stepper.move(distance);
      state = IDLE;
      Serial.println(F("IDLE"));
      break;
  }
}

void setup() {
  Serial.begin(115200);
  pinMode(startPin, INPUT_PULLUP);  // Button to initiate steps to move
  pinMode(stopPin, INPUT_PULLUP);   // Button to freeze steps to be moved by stepper
  pinMode(replayPin, INPUT_PULLUP); // Button to Start the motor to run

  initStepper();
}

void loop() {
//  delay(100);

  stepper.run();
  runFSM();

}


/*

#include <AccelStepper.h>

#define HALFSTEP 8

#define motorPin1  8     // IN1 on ULN2003 ==> Blue   on 28BYJ-48
#define motorPin2  9     // IN2 on ULN2004 ==> Pink   on 28BYJ-48
#define motorPin3  10    // IN3 on ULN2003 ==> Yellow on 28BYJ-48
#define motorPin4  11    // IN4 on ULN2003 ==> Orange on 28BYJ-48

AccelStepper stepper(HALFSTEP, motorPin1, motorPin3, motorPin2, motorPin4);

int Distance = 0;
long initial_homing = 1;

void setup() {
  Serial.begin(9600);
  pinMode(5, INPUT_PULLUP); // Button to initiate steps to move
  pinMode(6, INPUT_PULLUP); // Button to freeze steps to be moved by stepper
  pinMode(7, INPUT_PULLUP); // Button to Start the motor to run
}

void loop() {
  distanceTomove();
  Run_motor();
}

void distanceTomove() {
  stepper.setMaxSpeed(1000);
  stepper.setSpeed(100);
  stepper.setAcceleration(1000);  // Set Acceleration

  if (digitalRead(5) == LOW) {
    stepper.moveTo(initial_homing);
    initial_homing++;
    delay(5);
    stepper.runSpeedToPosition();
  }
  if (digitalRead(6) == LOW) {
    Distance = initial_homing;
  }
}


void Run_motor() {
  stepper.setMaxSpeed(1000);
  stepper.setSpeed(500);
  stepper.setAcceleration(1000);  // Set Acceleration

  if (digitalRead(7) == LOW) {
    stepper.moveTo(-Distance);
    stepper.runSpeedToPosition();
  }
}

*/