// https://forum.arduino.cc/t/running-2-steppers-for-robot/1123391
// https://wokwi.com/projects/363858299712048129

/*

  If L1 is LOW, run M1 for - 50 steps. Set Home Position
  if L2 is LOW, initiate HOMING function
  IF L1 and L2 are LOW, initiate HOMING function.

  HOMING:
  Run M1 and M2.
  If L1 is HIGH then STOP M1 and Run M1 for -50 Steps. Set M1 Home Position.
  If L3 is HIGH then STOP M2 and set M2 Home Position.

  MAIN SEQUENCE:
    Run M1.
    When M1 DISTANCE = 800 then run M1 reverse direction to Home Position and run M2 for 100 steps.
    Run M2 until L3 is LOW
    When M1 is at M1 Home Position. Loop to 1.
    If L1 or L2 are LOW during MAIN SEQUENCE. STOP M1 and M2.

*/

// GOD OF MOUTH

#include <AccelStepper.h>

// Define stepper motors and limit switches
AccelStepper stepperM1(1, 2, 5); // (driver, STEP, DIR)
AccelStepper stepperM2(1, 3, 6);
const int L1 = 9;
const int L2 = 10;
const int L3 = 11;

void setup() {
  // Set initial speed and acceleration for both motors
  stepperM1.setMaxSpeed(1000);
  stepperM1.setAcceleration(100);
  stepperM2.setMaxSpeed(1000);
  stepperM2.setAcceleration(100);

  // Check limit switches for initialization
  if (digitalRead(L1) == HIGH) {
    stepperM1.moveTo(-50);
    while (stepperM1.distanceToGo() != 0) {
      stepperM1.run();
      simLimitSwitchStates();
    }
    stepperM1.setCurrentPosition(0);

  } else if (digitalRead(L2) == HIGH || (digitalRead(L1) == LOW && digitalRead(L2) == LOW)) {
    // Homing function

    stepperM1.moveTo(-2000); // Move M1 towards L1 and L2
    stepperM2.moveTo(-2000);

    while (digitalRead(L1) == LOW || digitalRead(L2) == LOW) {
      stepperM1.run();
      stepperM2.run();
      simLimitSwitchStates();
    }
    if (digitalRead(L1) == HIGH) {
      stepperM1.moveTo(-50);
      while (stepperM1.distanceToGo() != 0) {
        stepperM1.run();
        simLimitSwitchStates();
      }
      stepperM1.setCurrentPosition(0);
    }
    if (digitalRead(L3) == HIGH) {
      stepperM2.setCurrentPosition(0);
    }
    stepperM1.setCurrentPosition(0);
    stepperM2.setCurrentPosition(0);
  }
}

void loop() {
  // Main sequence
  stepperM1.run();

  if (stepperM1.currentPosition() == 800) {
    stepperM1.moveTo(-800);
    stepperM2.moveTo(100);
  }

  if (stepperM2.distanceToGo() != 0) {
    stepperM2.run();
  } else {
    if (digitalRead(L3) == LOW) {
      stepperM2.moveTo(-50);
    } else {
      stepperM2.setCurrentPosition(0);
    }
  }

  simLimitSwitchStates();

  // Check for limit switch during main sequence
  if (digitalRead(L1) == HIGH || digitalRead(L2) == HIGH) {
    stepperM1.stop();
    stepperM2.stop();
  }
}

void simLimitSwitchStates(void) {
  static bool init = false;
  static bool homed = false;
  unsigned long interval = 10;
  static unsigned long last = -interval;
  unsigned long now = millis();
  if (now - last > interval) {
    last = now;
    if (!init) {
      Serial.begin(115200);
      init = true;
      pinMode(A3, OUTPUT);
      pinMode(A4, OUTPUT);
      pinMode(A5, OUTPUT);
      digitalWrite(A3, HIGH);
      digitalWrite(A4, HIGH);
      digitalWrite(A5, HIGH);
    }
    // L1 LOW when M1 < 0
    // L2 LOW when M2 < 0
    // L3 LOW when M2 >1000

    if (homed) {
      if (stepperM1.currentPosition() < -1000 ) {
        digitalWrite(A3, LOW);
      } else {
        digitalWrite(A3, HIGH);
      }
      if (stepperM2.currentPosition() < 0) {
        digitalWrite(A4, LOW);
      } else {
        digitalWrite(A4, HIGH);
      }
      if (stepperM2.currentPosition() > 1000) {
        digitalWrite(A5, LOW);
      } else {
        digitalWrite(A5, HIGH );
      };
    } else { // !homed
      if (stepperM1.currentPosition() <= 0) {
        digitalWrite(A3, LOW);
      } else {
        digitalWrite(A3, HIGH);
      }
      if (stepperM2.currentPosition() < 0) {
        digitalWrite(A4, LOW);
      } else {
        digitalWrite(A4, HIGH);
      }
      if (stepperM2.currentPosition() > 1000) {
        digitalWrite(A5, LOW);
      } else {
        digitalWrite(A5, HIGH );
      };
    }
  }

}