// https://forum.arduino.cc/t/commanding-a-stepper-motor-to-make-a-series-of-movements-in-a-specific-order/1244098
// https://wokwi.com/projects/394336441594546177
/*
  if ( TimePeriodIsOver(MyTestTimer,1000) ) {

  }

*/

// Include AccelStepper Library
#include <AccelStepper.h>

//Define motor pin connections
const byte dirPin = 2;
const byte stepPin = 4;
const byte startButtonPin = 5;

const long targetPos1 = 10000;
const long targetPos2 = 5000;
const long targetPos3 = 2500;

long actualPos;
#define motorInterfaceType 1

//            123456789012345678
const byte sm_waitForButtonStart = 0;
const byte sm_stepToPositon      = 1;
const byte sm_returnToZero       = 2;
const byte sm_wait               = 3;

const char myStateName[][20] = { // the c_strings must be in the same order as the state-machine-constants
  "waitForButtonStart",
  "stepToPositon",
  "returnToZero",
  "sm_wait"
};


byte myState = sm_waitForButtonStart;

byte actualPosNum;

unsigned long MyTestTimer = 0;                   // Timer-variables MUST be of type unsigned long
const byte    OnBoard_LED = 13;   // onboard-LED uno, mega

const unsigned long myWaitTime = 2000;
unsigned long myWaitTimer;

// Creates an instance
AccelStepper myStepper(motorInterfaceType, stepPin, dirPin);

void setup() {
  Serial.begin(115200);
  Serial.println("Setup-Start");
  PrintFileNameDateTime();
  Serial.println("press button to start");

  pinMode(startButtonPin, INPUT_PULLUP);

  // set the maximum speed, acceleration factor,
  // initial speed and the target position
  myStepper.setMaxSpeed(10000);
  myStepper.setAcceleration(2000);
  //  myStepper.setSpeed(250);   // This is useless, as .run() only follows maxSpeed!
  myStepper.moveTo(5000);
}


void myStepperStateMachine() {

  printStateIfChanged();
  
  switch (myState) {

    case sm_waitForButtonStart: // mode of operation
      if (digitalRead(startButtonPin) == LOW) {
        actualPosNum = 1;
        actualPos = targetPos1;
        myStepper.moveTo(actualPos);
        myState = sm_stepToPositon;
      }
      break; // IMMIDIATELY jump down to END-OF-SWITCH

    case sm_stepToPositon: // mode of operation
      myStepper.run();
      if (myStepper.distanceToGo() == 0 ) {
        myStepper.moveTo(0);
        myState = sm_returnToZero;
      }
      break; // IMMIDIATELY jump down to END-OF-SWITCH

    case sm_returnToZero: // mode of operation
      myStepper.run();
      if (myStepper.distanceToGo() == 0 ) {
        // when zero-position is reached
        actualPosNum++; // increase position-number

        if (actualPosNum == 2) {
          actualPos = targetPos2;
          myStepper.moveTo(actualPos);
          myState = sm_stepToPositon;
        }

        if (actualPosNum == 3) {
          actualPos = targetPos2;
          myStepper.moveTo(actualPos);
          myState = sm_stepToPositon;
        }

        if (actualPosNum == 4) {
          myWaitTimer = millis();
          myState = sm_wait;
        }
      }
      break; // IMMIDIATELY jump down to END-OF-SWITCH

    case sm_wait: // mode of operation
      if ( TimePeriodIsOver(myWaitTimer, myWaitTime) ) {
        actualPosNum = 1;
        myStepper.moveTo(targetPos1);
        myState = sm_stepToPositon;
      }
      break; // IMMIDIATELY jump down to END-OF-SWITCH

  } // END-OF-SWITCH
}


void loop() {
  BlinkHeartBeatLED(OnBoard_LED, 250);

  myStepperStateMachine();
}


// helper-functions
void PrintFileNameDateTime() {
  Serial.println( F("Code running comes from file ") );
  Serial.println( F(__FILE__) );
  Serial.print( F("  compiled ") );
  Serial.print( F(__DATE__) );
  Serial.print( F(" ") );
  Serial.println( F(__TIME__) );
}


// easy to use helper-function for non-blocking timing
// explanation see here
// https://forum.arduino.cc/t/example-code-for-timing-based-on-millis-easier-to-understand-through-the-use-of-example-numbers-avoiding-delay/974017
boolean TimePeriodIsOver (unsigned long &startOfPeriod, unsigned long TimePeriod) {
  unsigned long currentMillis  = millis();
  if ( currentMillis - startOfPeriod >= TimePeriod ) {
    // more time than TimePeriod has elapsed since last time if-condition was true
    startOfPeriod = currentMillis; // a new period starts right here so set new starttime
    return true;
  }
  else return false;            // actual TimePeriod is NOT yet over
}



void BlinkHeartBeatLED(int IO_Pin, int BlinkPeriod) {
  static unsigned long MyBlinkTimer;
  pinMode(IO_Pin, OUTPUT);

  if ( TimePeriodIsOver(MyBlinkTimer, BlinkPeriod) ) {
    digitalWrite(IO_Pin, !digitalRead(IO_Pin) );
  }
}


void printStateIfChanged() {
  static byte lastState;

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