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

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

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

  setupSteppers();

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

// limit switches normal/wiring
# define ASSERTED HIGH

// simple loop steps a cabinet if it can
void loop()
{
// 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[RIGHT].loop();
  button[LEFT].loop();

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

  process();

  stepperLoop();    // run both steppers

  updateMech();
  renderMech();

  return;
}

//           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:
const 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 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;

      stopCars();
    }

  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, 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);
  }
  else {  // the right car
    if (!theStop) rightStepper.moveTo(0);
    else rightStepper.moveTo(1300);
  }

  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() / 100;
  int carR = N_REAL - rightStepper.currentPosition() / 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 = ((3299 - rightP) - leftP) < 2005;
  bool isRight = rightP < 5;

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


// testing and idea factory
void testSteppersLoop()
{
  static bool running = true;

  int analog_in = analogRead(PositionPot);

if (running) {
  leftStepper.setAcceleration(analogRead(AccelerationPot));
  leftStepper.moveTo(analog_in);
  leftStepper.run();

  rightStepper.setAcceleration(analogRead(AccelerationPot));
  rightStepper.moveTo(1023 - analog_in);
  rightStepper.run();
}  

  if (digitalRead(rightButtonPin) == LOW) {
    running = false;
    rightStepper.moveTo(rightStepper.currentPosition());
  }

  rightStepper.run();
  leftStepper.run();
}