// https://wokwi.com/projects/406405729223101441
// https://forum.arduino.cc/t/advice-on-finite-state-machine-code-to-move-two-motors-with-two-buttons-and-3-endstops/1284857

# include <AccelStepper.h>
# include <ezButton.h>
# include <LiquidCrystal_I2C.h>

unsigned long now;    //... current value of millis() for the loop pass

// Pin Definitions...
# define BTN_R 2 // Right button
# define BTN_L 3 // Left button

# define END_R 4 // Right endstop
# define END_M 5 // Middle endstop
# define END_L 6 // Left endstop

# define SSR A3 // SSR to power powersupply for drivers

// Stepper motor pins for the left stepper
# define STEP_L 7 // Step pin for the left stepper motor
# define DIR_L 8 // Direction pin for the left stepper motor
# define ENA_L 9 // Enable pin for the left stepper motor

// Stepper motor pins for the right stepper
# define STEP_R 10 // Step pin for the right stepper motor
# define DIR_R 11 // Direction pin for the right stepper motor
# define ENA_R 12 // Enable pin for the right stepper motor

// Alarm pin on internal interrupt to stop movement if something happens.
# define ALM 2 // Alarm from both stepper drivers into one internal interrupt

// Stepper Initialization
AccelStepper stepperL(AccelStepper::DRIVER, STEP_L, DIR_L);
AccelStepper stepperR(AccelStepper::DRIVER, STEP_R, DIR_R);

// LCD panel
# define I2C_ADDR    0x27
# define LCD_COLUMNS 16
# define LCD_LINES   2

LiquidCrystal_I2C lcd(I2C_ADDR, LCD_COLUMNS, LCD_LINES);

// yeah, just two ezButtons
ezButton button[2] = {BTN_R, BTN_L}; // 5, END_R, END_M, END_L};
const byte NEZBUTTONS = sizeof button / sizeof *button;

# define PRESST LOW   // ezButton state for a pressed button

// button "nicknames" for better readability
enum buttonNames {RIGHT = 0, LEFT};
char *buttonTags[] = {"Right", "Left",};

bool fork = false; // To indicate logic errors

// FSM states
enum State {
  IDLE,
  RIGHT_RIGHTWARDS,
  LEFT_LEFTWARDS,
  LEFT_RIGHTWARDS,
  RIGHT_LEFTWARDS,
  NONE,
};

char *stateTags[] = {"Idle", "R2R", "L2L", "L2R", "R2L", "NONE",};
State state = IDLE;

// Timers and Debounce
const unsigned long SSR_DEACTIVATION_TIME = 5 * 60 * 1000; // 5 minutes - sleep mode for SSR to save SSR itself and PS
unsigned long lastButtonPressTime = 0;

void setup() {
  Serial.begin(115200);

  setupLCD();

  pinMode(25, OUTPUT);
  pinMode(27, OUTPUT);

  // Initialize the buttons, set the debounce
  for (int ii = 0; ii < NEZBUTTONS; ii++)
    button[ii].setDebounceTime(20);

  // Initialize the endstops
  pinMode(END_R, INPUT_PULLUP);
  pinMode(END_M, INPUT_PULLUP);
  pinMode(END_L, INPUT_PULLUP);

  // Initialize the left stepper motor
  stepperL.setMaxSpeed(200);
  stepperL.setSpeed(200);
  stepperL.setAcceleration(100);
  stepperL.setEnablePin(ENA_L);
  stepperL.setPinsInverted(false, false, true);
  stepperL.disableOutputs();

  // Initialize the right stepper motor
  stepperR.setMaxSpeed(200);
  stepperR.setSpeed(200);
  stepperR.setAcceleration(100);
  stepperR.setEnablePin(ENA_R);
  stepperR.setPinsInverted(false, false, true);
  stepperR.disableOutputs();

  // SSR
  pinMode(SSR, OUTPUT);
  digitalWrite(SSR, HIGH); // Activate SSR at startup
}

void setupLCD()
{
  lcd.init();
  lcd.backlight();

  // Print something
  lcd.setCursor(0, 0);
  lcd.print("Hello");
  lcd.setCursor(0, 1);
  lcd.print("World!");

  delay(333);
}

void reportLCD()
{
  static unsigned long lastTime;

  if (now - lastTime < 100) return;

  lastTime = now;
  
  lcd.setCursor(0, 0);
  lcd.print("          ");
  lcd.setCursor(0, 0); 
  lcd.print(stepperL.currentPosition());

  lcd.setCursor(0, 1);
  lcd.print("          ");
  lcd.setCursor(0, 1);
  lcd.print(stepperR.currentPosition());
}

# define ASSERTED LOW

void loop() {
  now = millis();

  stepperL.run();
  stepperR.run();
  reportLCD();

  // INPUT the user pushbuttons and the limit switches
  for (int ii = 0; ii < NEZBUTTONS; ii++) {
    button[ii].loop();
  }

  bool limitRight = digitalRead(END_R) == ASSERTED;
  bool limitMiddle = digitalRead(END_M) == ASSERTED;
  bool limitLeft = digitalRead(END_L) == ASSERTED;


//... not yet. lastButtonPressTime is the last _loop_ time. SSR will never be deactivated
  lastButtonPressTime = now;
  digitalWrite(SSR, HIGH); // Activate SSR


  reportState();

  static bool once = false;  // hack to provide a one-shot operation in a repeated case


//...
static bool needJamLeft;
static long jamThisLeft;


  // FSM State Machine
  switch (state) {
    case IDLE :
    if (!stepperL.isRunning() && needJamLeft) {
      stepperL.setCurrentPosition(jamThisLeft);
      needJamLeft = false;
    }
  
    if (button[RIGHT].isPressed()) {
        if (!limitRight) {
            state = RIGHT_RIGHTWARDS; once = true;
        } else { // Right button pressed AND right limit reached
            state = LEFT_RIGHTWARDS; once = true;
        }
    } else if (button[LEFT].isPressed()) {
        if (!limitLeft) {
            state = LEFT_LEFTWARDS; once = true;
        } else { // Left button pressed AND left limit reached
            state = RIGHT_LEFTWARDS; once = true;
        }
    }

    break;
    
    case RIGHT_RIGHTWARDS :
      if (button[RIGHT].isPressed()) {
        stepperR.stop();
        state = IDLE;
      }
      else if (limitRight) {
        stepperR.stop();
        stepperR.disableOutputs();  // stop. stepping. now.
        state = IDLE;
        if (!limitMiddle && button[RIGHT].getState() == PRESST) {
          state = LEFT_RIGHTWARDS; once = true;
        }
      }
      else if (once) {
        stepperR.enableOutputs();
        stepperR.moveTo(20000);
        once = false;
      }

      break;
      
// stop the left car if it is moving and the button gets pressed
// stop the left car at the left limit; if the button is down, poke the right car      
// otherwise start up the left car moving to the left
      
    case LEFT_LEFTWARDS :
      if (button[LEFT].isPressed()) {
        stepperL.stop();
        state = IDLE;
      }
      else if (limitLeft) {
        stepperL.stop();
        Serial.print(" left limit HALT ");
        Serial.println(stepperL.currentPosition());
        jamThisLeft = stepperL.currentPosition();
        needJamLeft = true;
        stepperL.disableOutputs();  // stop. stepping. now.
        state = IDLE;
        if (!limitMiddle && button[LEFT].getState() == PRESST) {
          state = RIGHT_LEFTWARDS; once = true;
        }
      }
      else if (once) {
        stepperL.enableOutputs();
        stepperL.moveTo(-20000);
        once = false;
      }

      break;

    case LEFT_RIGHTWARDS :
      if (button[RIGHT].isPressed() || limitMiddle) {
        stepperL.stop();
        state = IDLE;
      }
      else if (once) { stepperL.enableOutputs(); stepperL.moveTo(20000); once = false;}


      break;

    case RIGHT_LEFTWARDS :
      if (button[LEFT].isPressed() || limitMiddle) {
        stepperR.stop();
        state = IDLE;
      }
      else if (once) { stepperR.enableOutputs(); stepperR.moveTo(-20000); once = false;}

      break;
  }

  // Disable SSR after timeout
  if (now - lastButtonPressTime >= SSR_DEACTIVATION_TIME) {
    digitalWrite(SSR, LOW); // Deactivate SSR
  }

// Disable stepper outputs when idle
  if (state == IDLE) {
    stepperL.disableOutputs();
    stepperR.disableOutputs();
  }
}









// various status reportings

void reportState()
{
  static State lastState = NONE;
  if (state != lastState) {
    Serial.print("-> ");
    Serial.println(stateTags[state]);
    lastState = state;

    if (state == IDLE) {
      Serial.print(stepperL.currentPosition());
      Serial.print(" <<left: car position :right>> ");
      Serial.println(stepperR.currentPosition());
    }
  }

  motorLEDs();
}

// just some LEDs showing if the steppers are moving left or right
void motorLEDs()
{
  static int lastLeft = 32767;
  static int lastRight = 32767;

  static unsigned long lastLEDLeft;
  static unsigned long lastLEDRight;

  int leftNow = stepperL.currentPosition();

  if (lastLeft < leftNow) {
    digitalWrite(27, HIGH);
    digitalWrite(25, LOW);
    lastLEDLeft = now;
  }
  if (lastLeft > leftNow) {
    digitalWrite(25, HIGH);
    digitalWrite(27, LOW); 
    lastLEDLeft = now;    
  }
  if (lastLeft == leftNow)
    if (now - lastLEDLeft > 222) {
      digitalWrite(25, LOW); digitalWrite(27, LOW);
    }

  lastLeft = leftNow;

// second verse, much like the first...

  int rightNow = stepperR.currentPosition();
//  Serial.print(lastRight); Serial.print(" now "); Serial.println(rightNow);

  if (lastRight < rightNow) {
//Serial.println("                          lastRight < rightNow");
    digitalWrite(31, HIGH);
    digitalWrite(29, LOW);
    lastLEDRight = now;
  }
  if (lastRight > rightNow) {
//Serial.println("                                          lastRight > rightNow");
    digitalWrite(29, HIGH);
    digitalWrite(31, LOW); 
    lastLEDRight = now;    
  }
  if (lastRight == rightNow)
    if (now - lastLEDRight > 222) {
      digitalWrite(29, LOW); digitalWrite(31, LOW);
    }

  lastRight = rightNow;
}