// MACRO-START * MACRO-START * MACRO-START * MACRO-START * MACRO-START * MACRO-START *
// Take it for granted at the moment scroll down to void setup
// start of macros dbg and dbgi
#define dbg(myFixedText, variableName) \
  Serial.print( F(#myFixedText " "  #variableName"=") ); \
  Serial.println(variableName);
// usage: dbg("1:my fixed text",myVariable);
// myVariable can be any variable or expression that is defined in scope

#define dbgi(myFixedText, variableName,timeInterval) \
  do { \
    static unsigned long intervalStartTime; \
    if ( millis() - intervalStartTime >= timeInterval ){ \
      intervalStartTime = millis(); \
      Serial.print( F(#myFixedText " "  #variableName"=") ); \
      Serial.println(variableName); \
    } \
  } while (false);
// usage: dbgi("2:my fixed text",myVariable,1000);
// myVariable can be any variable or expression that is defined in scope
// third parameter is the time in milliseconds that must pass by until the next time a
// Serial.print is executed
// end of macros dbg and dbgi
// print only once when value has changed
#define dbgc(myFixedText, variableName) \
  do { \
    static long lastState; \
    if ( lastState != variableName ){ \
      Serial.print( F(#myFixedText " "  #variableName" changed from ") ); \
      Serial.print(lastState); \
      Serial.print( F(" to ") ); \
      Serial.println(variableName); \
      lastState = variableName; \
    } \
  } while (false);
// MACRO-END * MACRO-END * MACRO-END * MACRO-END * MACRO-END * MACRO-END * MACRO-END *


#include <MobaTools.h>
const byte dirPin  = 2;
const byte stepPin = 3;

const byte sensorPin = 4;
const byte buttonPin = 5;

const int STEPS_REVOLUTION = 200;

// modes of operation = states of the state-machine
const byte sm_idling            = 0;
const byte sm_RotFwd1Rev        = 1;
const byte sm_wait1Rev          = 2;
const byte sm_Pausing           = 3;
const byte sm_CheckForBeamBreak = 4;
const byte sm_WaitRotBack1Rev   = 5;

// 2D-array of chars for indicating the state 
// the state-machine is in with letters instead of a number
// which makes it easier to understand by just reading
const char stateName[][20] = {"sm_idling",
                              "sm_RotFwd1Rev",
                              "sm_wait1Rev",
                              "sm_Pausing",
                              "sm_CheckForBeamBreak",
                              "sm_WaitRotBack1Rev"
                            };


const byte pressed   = LOW;
const byte unPressed = HIGH;

byte myState = sm_idling;
const int oneRevolution = 200;

unsigned long waitTimer;

MoToStepper myStepMotor(STEPS_REVOLUTION, STEPDIR); // HALFSTEP ist default

void setup() {
  Serial.begin(115200);
  Serial.println("Setup-Start");
  pinMode(sensorPin, INPUT_PULLUP);
  pinMode(buttonPin, INPUT_PULLUP);

  myStepMotor.attach(stepPin,dirPin); 
  myStepMotor.setSpeed( 240 );       // = 24 rotations/Min
  myStepMotor.setRampLen( oneRevolution/4 );     // = 1/4 Umdrehung
  myStepMotor.setZero();            // Referenzpunkt für Motor 1 setzen
    
  myState = sm_idling;
  Serial.println("press button for start" );
   
  printStateIfChanged(myState);
}

void loop() {
  // everything happens inside the function myStepperStateMachine
  myStepperStateMachine();
}


void myStepperStateMachine() {

  printStateIfChanged(myState);

  switch (myState) {
    
    // through the break;-statement each case is mutually exclusive
    case sm_idling:
       // once every 1000 millisecondsprint valueofIO-pin sensorPin
      dbgi("idling:",digitalRead(sensorPin),1000);
      // check if button is pressed
      if ( digitalRead(buttonPin) == pressed) {
        // if button is pressed
        myState = sm_RotFwd1Rev; // change to other state 
      }  
      break; // immidiately jump down to END-OF-SWITCH

    case sm_RotFwd1Rev:
      // give command to rotate the stepper-motor number of steps
      // the step-pulsecreation is done in the backround
      // which means the code will go on executing the lines below
      myStepMotor.doSteps(oneRevolution); // set new targetposition 

      // step-pulses are created in the backround
      // code executes these line BEFORE stepper-motor has reached the new target-position
      myState = sm_wait1Rev;              
      break; // immidiately jump down to END-OF-SWITCH

    // wait for stepper-motor to reach target position
    case sm_wait1Rev:
      if ( !myStepMotor.moving() ) { // if motor HAS reached target-position
        waitTimer = millis();        // store snapshot of time as time-reference for waiting
        myState = sm_Pausing;
      }
      break; // immidiately jump down to END-OF-SWITCH

    // wait until pausing time is over
    case sm_Pausing:
      // check if 2000 milliseconds have passed by (since waitTimer = millis(); )
      if ( TimePeriodIsOver(waitTimer,2000)  ) {
        // if REALLY 2000 milliseconds have passed by
        myStepMotor.rotate(-1);  // start continous rotating // opposite direction is .rotate(1);
        Serial.println("starting continous rotating");
        myState = sm_CheckForBeamBreak; 
      }
      break; // immidiately jump down to END-OF-SWITCH

    case sm_CheckForBeamBreak:
      dbgi("Check:",digitalRead(sensorPin),500);
      if ( digitalRead(sensorPin) == pressed) {
        myStepMotor.stop();                  // stop continous rotation
        Serial.print("Beam interrupted at step-position ");
        Serial.println(myStepMotor.readSteps() );
        // start rotation one rev backwards
        myStepMotor.doSteps(-oneRevolution); 
        Serial.println("rotating back to step-position " );
        // as the step-pulses are created in the backround
        // code executes the lines below
        myState = sm_WaitRotBack1Rev;
      }
      break; // immidiately jump down to END-OF-SWITCH

    case sm_WaitRotBack1Rev:
      // each time value of .distanceToGo() changes print 
      dbgc("Back1Rev",myStepMotor.distanceToGo());
      //if ( !myStepMotor.moving() ) { // if motor has reached target-position
      if ( myStepMotor.distanceToGo() == 0 ) {
        Serial.println("1 revolution CCW finished");
        Serial.println(myStepMotor.readSteps() );
        myState = sm_idling;
        Serial.println("press button for new start" );
      }  
      //myState = sm_idling;
      break; // immidiately jump down to END-OF-SWITCH
  } //END-OF-SWITCH

}

// easy to use helper-function for non-blocking timing
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 printStateIfChanged(byte p_state) {
  static byte lastState;

  // check if value of p_state is different than last time calling the function
  if (lastState != p_state) {
    // if it IS different 
    Serial.print("state changed from ");
    Serial.print(stateName[lastState]);
    Serial.print(" to ");
    Serial.println(stateName[p_state]);
    lastState = p_state; // update lastState to new value
  }
}