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

// user input
# define rightButtonPin 2
# define leftButtonPin 3

// limit switches
# define leftEndstopPin 6
# define middleEndstopPin 5
# define rightEndstopPin 4

# 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[2] = {rightButtonPin, leftButtonPin};

enum buttonNames {RIGHT = 0, LEFT};
char * buttonTags[] = {"Right", "Left"};

bool limitLeft ;
bool limitMiddle;
bool limitRight;

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

  pinMode(leftEndstopPin, INPUT_PULLUP);
  pinMode(middleEndstopPin, INPUT_PULLUP);
  pinMode(rightEndstopPin, INPUT_PULLUP); 

  // just two ezButtons
  for (int ii = 0; ii < 2; 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(leftEndstopPin) == HIGH;
  limitMiddle = digitalRead(middleEndstopPin) == HIGH;
  limitRight = digitalRead(rightEndstopPin) == HIGH;

  process();

  updateMech();
  renderMech();

  return;
}

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

enum STATES {IDLE, L2L, R2L, R2R, L2R, NONE};
char *stateTags[] = {
  "Idle.", 
  "LEFT_LEFTWARDS",
  "RIGHT_LEFTWARDS",
  "RIGHT_RIGHTWARDS",
  "LEFT_RIGHTWARDS",
};

// if true stop between cars:
const bool fullStop = true;

void process(void)
{
  static STATES state = IDLE;

{
  static STATES lastPrinted = NONE;
  if (lastPrinted != state) {
    Serial.print("x ");
    Serial.println(stateTags[state]);
    lastPrinted = state;
  }
}
  
  bool leftPress = button[LEFT].isPressed();
  bool rightPress = button[RIGHT].isPressed();
  
  bool xLeft = button[LEFT].getState() == LOW;
  bool xRight = button[RIGHT].getState() == LOW;

// this line makes constant button pressing necessary
// choose fullStop thoughfully also
  if (!xLeft && !xRight) state = IDLE;

  if (state != IDLE)
    if (leftPress || rightPress) {
      state = IDLE;
      leftPress = false;
      rightPress = false;
    }

  switch (state) {
  case IDLE :
    if (rightPress) {
      if (!limitRight) state = R2R;       // move(rightCar, right);
      else if (!limitMiddle) state = L2R; // move(leftCar, right);
    }
    if (leftPress) {
      if (!limitLeft) state = L2L;        // move(leftCar, left);
      else if (!limitMiddle) state = R2L; // move(rightCar, left);
    }
    break;

  case R2R :
    if (limitRight) state = fullStop ? IDLE : L2R;
    else run(rightCar, right);
    break;

  case L2R : 
    if (limitMiddle) state = IDLE;
    else run(leftCar, right);
    break;

  case L2L :
    if (limitLeft) state = fullStop ? IDLE : R2L;
    else run(leftCar, left);
    break;

  case R2L : 
    if (limitMiddle) state = IDLE;
    else run(rightCar, left);
    break;
  }
}

void process_dX(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 WIDTH  8
# define N_REAL (33)
# 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;

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

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

  carL = 0; carR = N_REAL - WIDTH;
  renderCars();

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

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

    p = carR + ii;
    if (p >= 0 && p < (N_REAL)) 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)
{
  if (0) {
    Serial.print(theCar == leftCar ? "left cabinet " : "right cabinet ");
    Serial.println(theStep == -1 ? "left" : "right");
  }

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

  // various other forks here or before the move - limits disrespected e.g.
}

void updateMech() {}

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

  track.show();   // doh!

  bool isLeft = carL <= 0;
  bool isMiddle = carR == carL + WIDTH;
  bool isRight = carR >= N_REAL - WIDTH;

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