#define LED 5

// Driver 1
#define Step1 3
#define Dir1 4
#define En1 2

// Driver 2
#define Step2 11
#define Dir2 12
#define En2 10

// Driver 3
#define Step3 15
#define Dir3 16
#define En3 13

#define STEP_DELAY 50  // microseconds

void setup() {
  // LED for status
  pinMode(LED, OUTPUT);

  // Motor 1
  pinMode(Step1, OUTPUT);
  pinMode(Dir1, OUTPUT);
  pinMode(En1, OUTPUT);

  // // Motor 2
  // pinMode(Step2, OUTPUT);
  // pinMode(Dir2, OUTPUT);
  // pinMode(En2, OUTPUT);

  // // Motor 3
  // pinMode(Step3, OUTPUT);
  // pinMode(Dir3, OUTPUT);
  // pinMode(En3, OUTPUT);

  // Enable all drivers (LOW = enabled for DRV8825)
  digitalWrite(En1, LOW);
  // digitalWrite(En2, LOW);
  // digitalWrite(En3, LOW);
}

void loop() {
  digitalWrite(LED, HIGH);  // Indicate forward motion

  // Set all directions to forward
  digitalWrite(Dir1, HIGH);
  // digitalWrite(Dir2, HIGH);
  // digitalWrite(Dir3, HIGH);

  // Move forward
  for (int i = 0; i < 7500; i++) {
    stepAllMotors();
  }

  delay(1000);

  digitalWrite(LED, LOW);  // Indicate reverse motion

  // Set all directions to reverse
  digitalWrite(Dir1, LOW);
  // digitalWrite(Dir2, LOW);
  // digitalWrite(Dir3, LOW);

  // Move backward
  for (int i = 0; i < 7500; i++) {
    stepAllMotors();
  }

  delay(1000);
}

// Function to step all motors together
void stepAllMotors() {
  digitalWrite(Step1, HIGH);
  // digitalWrite(Step2, HIGH);
  // digitalWrite(Step3, HIGH);
  delayMicroseconds(STEP_DELAY);

  digitalWrite(Step1, LOW);
  // digitalWrite(Step2, LOW);
  // digitalWrite(Step3, LOW);
  delayMicroseconds(STEP_DELAY);
}