/*
  Forum: https://forum.arduino.cc/t/weird-result-from-a-running-code/1201365
  Commented version
  Wokwi: https://wokwi.com/projects/384655323134971905

*/

// Stepper Motor connected to Arduino that drives a conveyor belt
#include <AccelStepper.h>
constexpr byte motorPin1 {2};
constexpr byte motorPin2 {3};
constexpr byte motorPin3 {4};
constexpr byte motorPin4 {5};
constexpr byte MotorInterfaceType {4};
AccelStepper stepper = AccelStepper(MotorInterfaceType, motorPin1, motorPin3, motorPin2, motorPin4);

// Variable to signalize that the stepper shall run (or not)
boolean stepperRun = false;

// Servos to move the object 
#include <Servo.h>;
// servoOne moves the object from the belt to a  location L1
Servo servoOne;
// servoTwo moves the object from th eL1 to a location L2
Servo servoTwo;
// The pins to control the servos
constexpr byte servoOnePin {11};
constexpr byte servoTwoPin {6};
// servoOne home and push position
constexpr byte servoOneHome {0};
constexpr byte servoOnePush {160};
// servoTwo home and push position
constexpr byte servoTwoHome { 0};
constexpr byte servoTwoPush {160};

// Pins for the Ultrasonic Distance Sensor
const int TRIG_PIN  = 9;
const int ECHO_PIN  = 10;

// "first" is used to identify if a change of state has been performed
// "first" is set in newState() and reset in isFirst()
// "startTime" holds the time in [ms] when a change of state takes place
// It is set in newState() and used in the function expired()
boolean first = true;
unsigned long startTime = 0;

// The enumeration machineStates list all states that the state machine can enter
// "state" is the global variable of type machineStates. It is initialized with "START"
// and changed in newState()
// The function stateMachine() switches accordingly to routines that apply for the appropriate state
enum machineStates {START, TRANSPORT, WAITFORFIRSTPUSH, FIRSTPUSH, SERVOONEHOME, SECONDPUSH, SERVOTWOHOME};
machineStates state = START;


void setup() {
  // Serial is configured for 115200 Bd
  Serial.begin(115200);
  // Both servos are attached to the appropriate pins
  servoOne.attach(servoOnePin);
  servoTwo.attach(servoTwoPin);
  // The stepper parameters are set
  stepper.setMaxSpeed(1000);
  stepper.setSpeed(500);
  // The Ultrasonic Distance Sensor pins are set as Output and Input
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  // The built-in led is used to signalize whether the conveyor is running or has been stopped
  pinMode(LED_BUILTIN, OUTPUT);
  // The servos are commanded to go to their home position 
  servoOne.write(servoOneHome);
  servoTwo.write(servoTwoHome);
  // Give the servos some time to get there .... (adjust to the appropriate delay ...)
  delay(1000); 
}

void loop() {
  // stateMachine controls the whole machine step by step
  stateMachine();
  // This function controls the conveyor belt in loop() and
  // makes sure that it moves if the belt shall run 
  handleConveyor();
}

// It does not do a lot but the function name explains better
// what it is good for than "stepper.runSpeed();"
void handleConveyor() {
  if (stepperRun) {
    stepper.runSpeed();
  }
}

// Everytime we change the state it might be helpful to
// set first and startTime also
// To avoid multiple lines with the same function
// setting of state, first and startTime is bundled 
// in one call
// Even if it would not be used all the time it is less verbose
// and more safe then coding it a several place multiple times.
// 
// In case we encounter further data that had to be set or reset
// with a state change, this would be the right place for it

void newState(machineStates aState) {
  state = aState;
  first = true;
  startTime = millis();
}

//The function isFirst() also makes live easier 
// It does set "first" to false after the first call 
// and avoids to code this several times in the different
// states
boolean isFirst() {
  if (first) {
    first = false;
    return true;
  }
  return false;
}

// Instead of writing the millis() function muliple times in the state machine
// we can use the expired() function to "delay" other functions in the state machine
// for a certain time without blocking the code
//
// This function is used to give some time for the servos to do their job in this sketch.
// If we had sensors that tell us when a servo has reached its home or push position
// we could use the sensor feedback to trigger the next state. That would be better as
// the machine would stop if e.g. a servo is out of order ...
boolean expired(unsigned long interval) {
  return (millis() - startTime >= interval);
}

// Here comes the stateMachine ...
// Depending on the content of "state" the switch/case statement runs through the
// appropriate functions
// The if(isFirst()) parts are only performed once every entry to a certain state
// This enables to print a message and start activities which do not need to be called
// continouesly 

void stateMachine() {
  switch (state) {
    case START:
      if (isFirst()) {
        Serial.println("START");
        // Move the servos to home position (just to be sure ...)
        servoOne.write(servoOneHome);
        servoTwo.write(servoTwoHome);
        // We can of course reset the startTime outside of newState()!
        // This will "delay" the change to the next state
        startTime = millis();
      }
      // Depending on the last time when startTime was set
      // newState will be called to change to TRANSPORT
      if (expired(500)) {
        newState(TRANSPORT);
      }
      break;
    case TRANSPORT:
      if (isFirst()) {
        Serial.println("TRANSPORT");
        // We start the belt now (once everytime we enter this state from a different state)
        startConveyor();
      }
      // The boolean function objectIsClose() checks the distance to objects
      // and returns true if an object is closer than a given threshold
      if (objectIsClose()) {
        // If an object has been detected within the threshold distance
        // We stop the belt
        stopConveyor();
        // And change to the next state
        newState(WAITFORFIRSTPUSH);
      };
      break;
    case WAITFORFIRSTPUSH:
      if (isFirst()) {
        // Just used to print the state name
        Serial.println("WAITFORFIRSTPUSH");
      }
      // We wait for 2s after changing from TRANSPORT to WAITFORFIRSTPUSH
      // just to give the belt (and object) some time to stop stable
      if (expired(2000)) {
        newState(FIRSTPUSH);
      }
      break;
    case FIRSTPUSH:
      if (isFirst()) {
        Serial.println("FIRSTPUSH");
        // Command servo one to the push position
        servoOne.write(servoOnePush);
      }
      // And wait for 3s after changing to FIRSTPUSH
      // before we go to the next state as we do not have a 
      // sensor telling us that the servo and object have reached the 
      // push position L1
      if (expired(3000)) {
        newState(SERVOONEHOME);
      }
      break;
    case SERVOONEHOME:
      if (isFirst()) {
        Serial.println("SERVOONEHOME");
        // Command servo one to home position
        servoOne.write(servoOneHome);
      }
      // And wait for 3s after changing to SERVOONEHOME
      // before we go to the next state as we do not have a 
      // sensor telling us that the servo has reached the 
      // home position again
      if (expired(3000)) {
        newState(SECONDPUSH);
      }
      break;
    case SECONDPUSH:
      if (isFirst()) {
        Serial.println("SECONDPUSH");
        // After servo one (should have reached home position)
        // we command servo two to push the object to location L2
        servoTwo.write(servoTwoPush);
      }
      // And wait for 3s after changing to SECONDPUSH
      // before we go to the next state as we do not have a 
      // sensor telling us that the servo and object have reached the 
      // push position L2
      if (expired(3000)) {
        newState(SERVOTWOHOME);
      }
      break;
    case SERVOTWOHOME:
      if (isFirst()) {
        Serial.println("SERVOTWOHOME");
        // Command servo two to home position
        servoTwo.write(servoTwoHome);
      }
      // And wait for 3s after changing to SERVOTWOHOME
      // As we do not have a sensor telling us that the
      // servo has reached the home position again
      if (expired(3000)) {
        // If that time has expired we check the object distance
        // If there is still an object within the threshold distance
        // we wait until the belt is free again. If it is free
        // we start the cycle all over again from START
        if (!objectIsClose()) {
          newState(START);
        }
      }
      break;
  }

}

// This function is called where we want to know if an object is closer than
// the DISTANCE_THRESHOLD. It returns false if outside and true if closer than
// this given distance
boolean objectIsClose() {
  const int DISTANCE_THRESHOLD = 25.4; // centimeters
  float duration_us, distance_cm;
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  duration_us = pulseIn(ECHO_PIN, HIGH);
  distance_cm = 0.017 * duration_us;
  return (distance_cm < DISTANCE_THRESHOLD);
}

// startConveyor() just handles the built-in led and the boolean variable
// stepperRun
// We could write both lines in the stateMachine, but using an explicit function
// makes it easier to read (understand) the code and to implement future changes
// e.g. switching a separate external led etc.
void startConveyor() {
  digitalWrite(LED_BUILTIN, HIGH);
  stepperRun = true;
}

// The same applies to stopConveyor()
void stopConveyor() {
  digitalWrite(LED_BUILTIN, LOW);
  stepperRun = false;
}