// 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

// 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 > 177) {
    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 WIDTH  10
# define N_REAL (3 * WIDTH)
# 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 = 2 * WIDTH;
  renderCars();

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

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

    p = carR + ii;
    if (p >= 0 && p < (3 * WIDTH)) 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 > 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!

  isLeft = carL <= 0;
  isMiddle = carR == carL + WIDTH;
  isRight = carR >= 2 * WIDTH;

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