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

// Defined pins - just to show the code, real pins will differ

# define rightButtonPin 2
# define leftButtonPin 3

# define rightEndstopPin 4
# define leftEndstopPin 5
# define middleEndstopPin 6

# define leftCar  0
# define rightCar 1
# define left (-1)
# define right 1

bool fork;  // if the mechanical simulation is asked to do the impossible this goes true.

# include <ezButton.h>

ezButton button[5] = {2, leftButtonPin, 4, 5, 6};

enum buttonNames {RIGHT = 0, LEFT, R_STOP, M_STOP, L_STOP};
char * buttonTags[] = {"Right", "Left", "Right Limit", "Middle Limit", "Left Limit"};

bool limitLeft ;
bool limitMiddle;
bool limitRight;

void setup() {
  // Serial print for debugging
  Serial.begin(115200);

  // Initialize pins - for now all.
  pinMode(rightButtonPin, INPUT_PULLUP);
  pinMode(leftButtonPin, INPUT_PULLUP);
  pinMode(rightEndstopPin, INPUT_PULLUP);
  pinMode(leftEndstopPin, INPUT_PULLUP);
  pinMode(middleEndstopPin, INPUT_PULLUP);

  // for now, they all ezButtons
  for (int ii = 0; ii < 5; ii++)
    button[ii].setDebounceTime(20);

  setupMech();
}

// simple loop steps a cabinet if it can
void loop() {

  // if the high level logic doesn't break things, this should never happen
  if (fork) {
    Serial.println("logic error");
    for (; ;);
  }


  // INPUT the user pushbuttons and the limit switches
  button[RIGHT].loop();
  button[LEFT].loop();

  limitLeft = digitalRead(6) == HIGH;
  limitMiddle = digitalRead(5) == HIGH;
  limitRight = digitalRead(4) == HIGH;

  if (0) {
    // button presses for now:
    if (button[LEFT].isPressed()) {
      if (!limitLeft) move(leftCar, left);
      else if (!limitMiddle) move(rightCar, left);
    }

    if (button[RIGHT].isPressed()) {
      if (!limitRight) move(rightCar, right);
      else if (!limitMiddle) move(leftCar, right);
    }
  }
  process();

  updateMech();
  renderMech();

  return;
}

void run(int theCar, int theStep) {
  static uint32_t last;
  uint32_t now = millis();
  if (now - last > 377) {
    last = now;
    move(theCar, theStep);
  }
}

void process(void) {
  enum STATES {IDLE, LEFT_LEFTWARDS, RIGHT_LEFTWARDS, RIGHT_RIGHTWARDS, LEFT_RIGHTWARDS};
  static STATES state = IDLE;
  switch (state) {
    case IDLE: {
        if (button[RIGHT].isPressed()) {
          if (!limitRight) state = RIGHT_RIGHTWARDS; // move(rightCar, right);
          else if (!limitMiddle) state = LEFT_RIGHTWARDS; // move(leftCar, right);
        } else {
          if (button[LEFT].isPressed()) {
            if (!limitLeft) state = LEFT_LEFTWARDS;//move(leftCar, left);
            else if (!limitMiddle) state = RIGHT_LEFTWARDS; //move(rightCar, left);
          }
        }
      }
      break;
    case RIGHT_RIGHTWARDS: {
        if (button[RIGHT].isPressed() || button[LEFT].isPressed()) {
          state = IDLE;
        }
        else {
          if (limitRight) state = LEFT_RIGHTWARDS;
          else run(rightCar, right);
        }
      }
      break;
    case LEFT_RIGHTWARDS: {
        if (button[RIGHT].isPressed() || button[LEFT].isPressed()) {
          state = IDLE;
        }
        else {
          if (limitMiddle) state = IDLE; // move(rightCar, right);
          else run(leftCar, right);
        }
      }
      break;
    case LEFT_LEFTWARDS: {
        if (button[RIGHT].isPressed() || button[LEFT].isPressed()) {
          state = IDLE;
        }
        else {
          if (limitLeft) state = RIGHT_LEFTWARDS;
          else run(leftCar, left);
        }
      }
      break;
    case RIGHT_LEFTWARDS: {
        if (button[RIGHT].isPressed() || button[LEFT].isPressed()) {
          state = IDLE;
        }
        else {
          if (limitMiddle) state = IDLE;
          else run(rightCar, left);
        }
      }
      break;
  }
}

// mostly the simulation of the mechanism
// draw the "cars", set the limit signals

# define N_REAL 30
# define PIN_NEOPIXEL 45

# include <Adafruit_NeoPixel.h>
Adafruit_NeoPixel track(N_REAL, PIN_NEOPIXEL, NEO_RGB + NEO_KHZ800);

int carL, carR;

const byte lLimitPin = 51;
const byte mLimitPin = 49;
const byte rLimitPin = 47;

bool isLeft, isMiddle, isRight;

void setupMech()
{
  pinMode(lLimitPin, OUTPUT);
  pinMode(mLimitPin, OUTPUT);
  pinMode(rLimitPin, OUTPUT);

  track.begin();
  track.setPixelColor(1, 0xffffff);
  track.show(); delay(777);

  carL = 0; carR = 20;
  renderCars();

  track.show();
  delay(777);
}

void renderCars() {
  for (int ii = 0; ii < 10; ii++) {
    int p = carL + ii;
    if (p >= 0 && p < 30) track.setPixelColor(p, track.getPixelColor(p) | 0xff0000);

    p = carR + ii;
    if (p >= 0 && p < 30) track.setPixelColor(p, track.getPixelColor(p) | 0x0000ff);
  }
}

// move the L/R wardrobe L or R
// 0 is left car, 1 is right.  -1 is to the left, 1 is to the right
// should never try to do something impossible
// fork is a global denoting that a logic error occurred.
void move(int theCar, int theStep)
{
  Serial.print(theCar == leftCar ? "left cabinet " : "right cabinet ");
  Serial.println(theStep == -1 ? "left" : "right");

  if (theCar == rightCar) { // right hand car
    carR += theStep; if (carR > 20) fork = true;
  }
  else { // left hand car
    carL += theStep; if (carL < 0) fork = true;
  }

  // various other forks here or before the move - limit inhibits.
}

void updateMech() {}

void renderMech()
{
  track.clear();
  renderCars();

  track.show();   // doh!

  //... will update sensors here

  isLeft = carL <= 0;
  isMiddle = carR == carL + 10;
  isRight = carR >= 20;

  //  runProxies

  digitalWrite(lLimitPin, isLeft ? HIGH : LOW);
  digitalWrite(mLimitPin, isMiddle ? HIGH : LOW);
  digitalWrite(rLimitPin, isRight ? HIGH : LOW);
}



/*
  /*
  // Defineed states from FSM
  enum State {
  IDLE,
  RIGHTWARDS,
  LEFTWARDS,
  MOVE_R_RIGHTWARDS,
  MOVE_L_RIGHTWARDS,
  MOVE_L_LEFTWARDS,
  MOVE_R_LEFTWARDS,
  FINISHED_MOVEMENT,
  NO_STATE
  };

  char *stateTags[] = {
  "IDLE",
  "RIGHTWARDS",
  "LEFTWARDS",
  "MOVE_R_RIGHTWARDS",
  "MOVE_L_RIGHTWARDS",
  "MOVE_L_LEFTWARDS",
  "MOVE_R_LEFTWARDS",
  "FINISHED_MOVEMENT",
  "NO_STATE"
  };
  State currentState = IDLE; // Initial state for FSM
  // State previousState = IDLE; // Previous state to track changes

  // Function to check button and end stop states
  bool isButtonPressed(int pin) {
  return digitalRead(pin) == LOW;
  }

  bool isEndstopTriggered(int pin) {
  return digitalRead(pin) == LOW;
  }
*/




//// junk from development and testing

/*

  if (0) {
  Serial.print(limitLeft ? "LEFT_LIMIT!" : "     ");
  Serial.print(limitMiddle ? "MID_LIMIT!" : "     ");
  Serial.print(limitRight ? "RIGHT_LIMIT!" : "     ");
  Serial.println("");

  delay(100);
  }
*/

// test mech buttons and cars
void updateMech0()
{
  for (int ii = 0; ii < 5; ii++)
    button[ii].loop();

  //  curiously pin 3 is an output for some reason.
  //  Serial.println((char *) (button[1].getState() ? "hello" : "goodbye")); delay(100);

  // three tests with the main buttons
  /*
    if (button[0].isPressed()) { Serial.print("r+ "); carR++; Serial.println(carR); }
    if (button[1].isPressed()) { Serial.println("r- "); carR--; Serial.println(carR); }
    if (button[3].isPressed()) { Serial.println("l+"); carL++;}
    if (button[4].isPressed()) { Serial.println("l-"); carL--;}
  */
  /*
    if (button[0].isPressed()) carR++;
    if (button[1].isPressed()) carR--;
    if (button[3].isPressed()) carL++;
    if (button[4].isPressed()) carL--;
  */

  static unsigned long lastStep;
  unsigned long now = millis();
  if (now - lastStep < 333) return;

  lastStep = now;

  if (!button[0].getState()) carR++;
  if (!button[1].getState()) carR--;

  if (!button[3].getState()) carL++;
  if (!button[4].getState()) carL--;
}




/*

  // #include <CANSAME5x.h> //... we gonna fake this

  unsigned char synPacket[8];  //... the connection to the synthetic packet for now

  const byte xLED = 9;    // heart beat
  # include <ezButton.h>

  # define JAWS_PIN 12
  ezButton jawsControl(JAWS_PIN);

  // Pin configurations
  #define LEFT_PIN 10
  #define RIGHT_PIN 11

  # define NUM_LEDS 15     //... gonna make 5 segments of 2, later 5 segments of N
  # define N_REAL 115
  # define SEGMENT  3     //... length of segments

  # define PIN_NEOPIXEL 8

  #define CAN_BRAKE_MESSAGE_ID 0x46
  #define ONBOARD_NEOPIXEL PIN_NEOPIXEL
  //

  // Global variables for lights and CAN communication
*/

/*

  void loop0() {

  updateMech();
  renderMech();

  return;

  {
  static State printedState = NO_STATE;
  if (printedState != currentState) {
    Serial.print("state : ");
    Serial.println(stateTags[currentState]);
  }
  printedState = currentState;
  }

  switch (currentState) {
    case IDLE : // Check for button press, if both buttons or no buttons are pressed, we stay in IDLE
      if (isButtonPressed(rightButtonPin) && !isButtonPressed(leftButtonPin)) {
        currentState = RIGHTWARDS; // Right button pressed, proceed to Rightwards movement
        Serial.println("RIGHTWARDS");
      } else if (isButtonPressed(leftButtonPin) && !isButtonPressed(rightButtonPin)) {
        currentState = LEFTWARDS; // Left button pressed, proceed to Leftwards movement
        Serial.println("LEFTWARDS");
      }
      break;

    case RIGHTWARDS : // Determine, if corresponding endstop is triggered
      if (!isEndstopTriggered(rightEndstopPin)) {
        currentState = MOVE_R_RIGHTWARDS; // It is not = move right cabinet
        Serial.println("MOVE_R_RIGHTWARDS");

      } else {
        currentState = MOVE_L_RIGHTWARDS; // It is = cabinet is parked = move left cabinet
        Serial.println("MOVE_L_RIGHTWARDS");

      }
      break;

    case LEFTWARDS : // Determine, if corresponding endstop is triggered
      if (!isEndstopTriggered(leftEndstopPin)) {
        currentState = MOVE_L_LEFTWARDS; // It is not = move left cabinet
        Serial.println("MOVE_L_LEFTWARDS");

      } else {
        currentState = MOVE_R_LEFTWARDS; // It is = cabinet is parked = move right cabinet
        Serial.println("MOVE_R_LEFTWARDS");
      }
      break;

    case MOVE_R_RIGHTWARDS :
      // Code to move rightwards
      if (isEndstopTriggered(rightEndstopPin)) {
        currentState = RIGHTWARDS;
        Serial.println("RIGHTWARDS");

      }
      break;

    case MOVE_L_RIGHTWARDS :
      // Code to move rightwards
      if (isEndstopTriggered(rightEndstopPin)) {
        currentState = MOVE_L_RIGHTWARDS;
        Serial.println("MOVE_L_RIGHTWARDS");
      } else if (isEndstopTriggered(middleEndstopPin)) {
        currentState = FINISHED_MOVEMENT;
        Serial.println("FINISHED_MOVEMENT");
      }
      break;

    case MOVE_L_LEFTWARDS :
      // Code to move leftwards
      if (isEndstopTriggered(leftEndstopPin)) {
        currentState = LEFTWARDS;
        Serial.println("LEFTWARDS");
      }
      break;

    case MOVE_R_LEFTWARDS :
      // Code to move leftwards
      if (isEndstopTriggered(leftEndstopPin)) {
        currentState = MOVE_L_LEFTWARDS;
        Serial.println("MOVE_L_LEFTWARDS");
      } else if (isEndstopTriggered(middleEndstopPin)) {
        currentState = FINISHED_MOVEMENT;
        Serial.println("FINISHED_MOVEMENT");
      }
      break;

    case FINISHED_MOVEMENT :
      currentState = IDLE;
      Serial.println("IDLE");
      break;
  }

  // Check for button release to return to IDLE
  if (!isButtonPressed(rightButtonPin) && !isButtonPressed(leftButtonPin)) {
    currentState = IDLE;
  //    Serial.println("IDLE");
  }
  }
*/