// https://wokwi.com/projects/405980379522078721
// from
// 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  car to the Direction right car Right
# define rightRightPin 2
# define leftRightPin 7

# define rightLeftPin 3
# define leftLeftPin 8

# define fullStopPin  39
# define statsPin     37

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

// mech simulation
# define leftCar  0
# define rightCar 1
# define endStop    0
# define middleStop 1
# define left (-1)
# define right 1

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

//// now the higher level code

# include <ezButton.h>
//                           2            3             7             8
ezButton button[4] = {rightRightPin, leftRightPin, rightLeftPin, leftLeftPin};

enum buttonNames {bR2R, bL2R, bR2L, bL2L};

enum oldButtonNames {RIGHT = 0, LEFT};

char * buttonTags[] = {"R>>", "L>>", "<<R", "<<L"};

bool limitLeft;
bool limitMiddle;
bool limitRight;

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

  setupSteppers();

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

  // four ezButtons - time to think again later
  for (int ii = 0; ii < 4; ii++)
    button[ii].setDebounceTime(20);

  pinMode(fullStopPin, INPUT_PULLUP);   // true sense for fullStop
  pinMode(statsPin, INPUT_PULLUP);   // print some data...

  setupMech();
}

// limit switches normal/wiring
# define ASSERTED HIGH

// simple loop steps a cabinet if it can
void loop()
{
  if (digitalRead(statsPin) == LOW) report();
/* just the steppers ma'am
  if (0) {
    testSteppersLoop();
    return;
  }
*/
  // 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[bR2R].loop();
  button[bL2R].loop();
  button[bR2L].loop();
  button[bL2L].loop();

  limitLeft = digitalRead(leftEndstopPin) == ASSERTED;
  limitMiddle = digitalRead(middleEndstopPin) == ASSERTED;
  limitRight = digitalRead(rightEndstopPin) == ASSERTED;

  process();

  stepperLoop();    // run both steppers

  updateMech();
  renderMech();

  return;
}

//void process4() { process(); }; // so sue me

void process4()
{
  static unsigned int count;
  for (unsigned char ii = 0; ii < 4; ii++)
    if (button[ii].isPressed()) {
      Serial.print(count);
      Serial.print(" ");
      Serial.println(buttonTags[ii]);
      count++;
    }

  if (button[bL2L].isPressed()) run(leftCar, endStop);
  if (button[bL2R].isPressed()) run(leftCar, middleStop);
  
}

//           Idle  LEFT_LEFTWARDS   RIGHT_LEFTWARDS   RIGHT_RIGHTWARDS   LEFT_RIGHTWARDS
enum STATES {IDLE, L2L,             R2L,              R2R,               L2R,              NONE};

char *stateTags[] = {
  "Idle.", 
  "L2L LEFT_LEFTWARDS",
  "R2L RIGHT_LEFTWARDS",
  "RLR RIGHT_RIGHTWARDS",
  "L2R LEFT_RIGHTWARDS",
};

// if true stop between cars, now on slide switch at fullStopPin
bool fullStop = true;

// first process to call on steppers
void process(void)
{
  static STATES state = IDLE;

{
  static STATES lastPrinted = NONE;
  if (lastPrinted != state) {
    Serial.print("x ");
    Serial.println(stateTags[state]);
    lastPrinted = state;
  }
}

  bool gR2R = button[bR2R].isPressed();
  bool gL2R = button[bL2R].isPressed();
  bool gR2L = button[bR2L].isPressed();
  bool gL2L = button[bL2L].isPressed();

  fullStop = digitalRead(fullStopPin) == HIGH;

// this line makes constant button pressing necessary
// choose fullStop thoughfully also:
// not yet implemented

// this stops motion in progress and eats the buttons
  if (state != IDLE)
    if (gR2R || gL2R || gR2L || gL2L) {
      state = IDLE;

      gR2R = false;
      gL2R = false;
      gR2L = false;
      gL2L = false;

      stopCars();
    }

  switch (state) {
  case IDLE :
    if (gR2R) state = R2R;
    if (gR2L) state = R2L;
    if (gL2R) state = L2R;
    if (gL2L) state = L2L;

    break;

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

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

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

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

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

// mechanical simulation now runs steppers

# include <AccelStepper.h>

# define stepPinLeft  25
# define directionPinLeft 27

# define stepPinRight  29
# define directionPinRight 31

AccelStepper leftStepper(AccelStepper::DRIVER, stepPinLeft, directionPinLeft);
AccelStepper rightStepper(AccelStepper::DRIVER, stepPinRight, directionPinRight);

const byte PositionPot = A0;
const byte AccelerationPot = A1;

//// mostly two steppers

void setupSteppers()
{
  leftStepper.setPinsInverted(false, false, true);
  leftStepper.enableOutputs();
  leftStepper.setMaxSpeed(777);
  leftStepper.setSpeed(200);

  leftStepper.setAcceleration(250);  // whatever. from the original non-motor-controller version

  rightStepper.setPinsInverted(true, false, true);
  rightStepper.enableOutputs();
  rightStepper.setMaxSpeed(777);
  rightStepper.setSpeed(200);

  rightStepper.setAcceleration(250);
}

bool isRunning;   // has been moveTo, may not be moving. NOT USED yet

void stepperLoop()
{
  rightStepper.run();
  leftStepper.run();
}

// 0 left car / 1 right car; 0 to the end stop / 1 to the middle stop
void run(int theCar, int theStop)
{

if (0) {
  static long printed = -1;
  long leftP = leftStepper.currentPosition();
  if (printed != leftP) {
    Serial.println(leftP);
    printed = leftP;
  }
}

// if (isRunning) return;

  if (!theCar) { // the left car will move
    if (!theStop) leftStepper.moveTo(0);
//    else leftStepper.moveTo(1300);
// the left car must move to meet the right car
    else {
      int posR = (3300 - rightStepper.currentPosition());
      int meeting = posR - 2000;
      leftStepper.moveTo(meeting);

//S//erial.print(" meeting "); Serial.println(meeting);

//while (leftStepper.currentPosition() != meeting) leftStepper.run();
//delay(100);


    }
  }
  else {  // the right car will move
    if (!theStop) rightStepper.moveTo(0);
//    else rightStepper.moveTo(1300);
// the right car must move to meet the left car
    else {
      int posL = leftStepper.currentPosition();
      int meeting = 3300 - (posL + 2000);
      rightStepper.moveTo(meeting);
    }
  }

  isRunning = true;
}

// did this break all hell loose? yes.
void stopCars()
{
//  if (!isRunning) return;
  isRunning = false;

// ??
//  leftStepper.moveTo(leftStepper.currentPosition());
//  rightStepper.moveTo(rightStepper.currentPosition());
// or ??
  leftStepper.stop();
  rightStepper.stop();
}

// neopixel linear graph

# define WIDTH  10    // Sry^3 might break easily
# 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() {
  int carL = (leftStepper.currentPosition() + 50) / 100;
  int carR = N_REAL - (rightStepper.currentPosition() + 50) / 100 - WIDTH;

  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);
  }
}

void updateMech() {}

// new render mechanism process for steppers
void renderMech()
{
  track.clear();
  renderCars();

  track.show();   // doh!

  int leftP = leftStepper.currentPosition();
  int rightP = rightStepper.currentPosition();

// width 10 of 33 @ 100/ ! yikes  
  bool isLeft = leftP < 5;
  bool isMiddle = ((3300 - rightP) - leftP) <= 2000;
  bool isRight = rightP < 5;

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

void report()
{
  Serial.print("  left car "); Serial.print(leftStepper.currentPosition());
  Serial.print("     right car "); Serial.print(rightStepper.currentPosition());
  Serial.println("");

  delay(777);
}