#include <MobaTools.h>

const int FULLROT1 = 200;

const byte stepPin   = 5;
const byte dirPin    = 2;
const byte StartStopButtonPin = 7;

MoToStepper myStepper(FULLROT1, STEPDIR);

#define unPressed HIGH
#define pressed   LOW

const byte sm_waitForStart    = 0;
const byte sm_stepForward     = 1;
const byte sm_stepBackwards   = 2;
const byte sm_rotatingStopped = 3;

char myStateNames[][32] = {
  "sm_waitForStart",
  "sm_stepForward",
  "sm_stepBackwards",
  "sm_rotatingStopped"
};


byte myState;
byte resumeState;

bool rotating;


void setup() {
  Serial.begin(115200);
  Serial.println("Setup-Start");
  pinMode (LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, LOW);
  pinMode(StartStopButtonPin, INPUT_PULLUP);

  pinMode(dirPin, OUTPUT);
  pinMode(stepPin, OUTPUT);
  digitalWrite(dirPin, LOW);
  Serial.println("exiting Setup");

  myStepper.attach(stepPin, dirPin);
  myStepper.setSpeed( 1000 );              // 30 rev/min (if stepsPerRev is set correctly)
  myStepper.setRampLen( FULLROT1 / 2); // Ramp length is 1/2 revolution
  myStepper.setZero();
  rotating = false;
  myState = sm_waitForStart;
  resumeState = sm_waitForStart;
}


void myStartStopStateMachine() {

  printStateIfChanged();

  if (rotating) {
    if (digitalRead(StartStopButtonPin) == pressed) {
      digitalWrite(LED_BUILTIN, LOW);
      myStepper.stop();
      rotating = false;
      resumeState = myState;
      myState = sm_rotatingStopped;
      while (digitalRead(StartStopButtonPin) == pressed); // wait for button-release
    }
  }

  switch (myState) {

    case sm_waitForStart:
      if (digitalRead(StartStopButtonPin) == pressed) {
        rotating = true;
        myState = sm_stepForward;
        while (digitalRead(StartStopButtonPin) == pressed); // wait for button-release
        digitalWrite(LED_BUILTIN, HIGH);
        myStepper.writeSteps(1000); // drive to position 1000 steps away from zero-point
      }
      break; // IMMIDIATELY jump down to END-OF-SWITCH

    case sm_stepForward:
      if (myStepper.distanceToGo() == 0)  {
        myState = sm_stepBackwards;
        myStepper.writeSteps(0); // drive to position 0 steps away from zero-point
      }
      break; // IMMIDIATELY jump down to END-OF-SWITCH


    case sm_stepBackwards:
      if (myStepper.distanceToGo() == 0) {
        digitalWrite(LED_BUILTIN, LOW);
        myState = sm_waitForStart;
      }
      break; // IMMIDIATELY jump down to END-OF-SWITCH


    case sm_rotatingStopped:
      if (digitalRead(StartStopButtonPin) == pressed) {
        rotating = true;
        myState = resumeState;
        while (digitalRead(StartStopButtonPin) == pressed); // wait for button-release

        digitalWrite(LED_BUILTIN, HIGH);

        if (resumeState == sm_stepForward) {
          myStepper.writeSteps(1000); // drive to position 1000 steps away from zero-point
        }
        if (resumeState == sm_stepBackwards) {
          myStepper.writeSteps(0); // drive to position 1000 steps away from zero-point
        }
      }
      break; // IMMIDIATELY jump down to END-OF-SWITCH

  } // END-OF-SWITCH
}

void loop () {
  myStartStopStateMachine();
}


void printStateIfChanged() {
  static byte lastState;

  if (lastState != myState) {
    Serial.print("state changed from  ");
    Serial.print(myStateNames[lastState]);
    Serial.print("  to  ");
    Serial.println(myStateNames[myState]);
    lastState = myState;
  }
}