// Libraries
#include <AccelStepper.h>

// Constants
#define NUM_MOTORS 3      // Jumlah motor stepper
#define PIN_STEP_X 3      // Pin untuk STEP motor X
#define PIN_DIR_X 2       // Pin untuk DIR motor X
#define PIN_STEP_Y 5      // Pin untuk STEP motor Y
#define PIN_DIR_Y 4       // Pin untuk DIR motor Y
#define PIN_STEP_Z 6      // Pin untuk STEP motor Z
#define PIN_DIR_Z 7       // Pin untuk DIR motor Z
#define PIN_BUTTON 13     // Pin untuk tombol
#define PIN_LED 12        // Pin untuk LED

// Variables
AccelStepper motors[NUM_MOTORS] = {
  AccelStepper(1, PIN_STEP_X, PIN_DIR_X),
  AccelStepper(1, PIN_STEP_Y, PIN_DIR_Y),
  AccelStepper(1, PIN_STEP_Z, PIN_DIR_Z)
};
bool isMoving = false;    // Flag untuk menunjukkan apakah motor sedang bergerak

void setup() {
  pinMode(PIN_BUTTON, INPUT_PULLUP);
  pinMode(PIN_LED, OUTPUT);

  for (int i = 0; i < NUM_MOTORS; i++) {
    motors[i].setMaxSpeed(2000);    // Set kecepatan maksimum motor (disesuaikan dengan kebutuhan)
    motors[i].setAcceleration(10);  // Set percepatan motor (disesuaikan dengan kebutuhan)
  }
}

void loop() {
  // Baca status tombol
  bool buttonState = digitalRead(PIN_BUTTON);

  // Jika tombol ditekan dan motor tidak bergerak, mulai gerakan
  if (buttonState == LOW && !isMoving) {
    digitalWrite(PIN_LED, HIGH);  // Nyalakan LED

    // Hitung langkah-langkah untuk mencapai jarak 10 cm pada setiap sumbu
    long stepsX = 10 * 200;   // Hitung langkah-langkah untuk sumbu X (disesuaikan dengan perbandingan langkah per cm pada sumbu X)
    long stepsY = 10 * 200;   // Hitung langkah-langkah untuk sumbu Y (disesuaikan dengan perbandingan langkah per cm pada sumbu Y)
    long stepsZ = -10 * 200;  // Hitung langkah-langkah untuk sumbu Z (berlawanan arah)

    // Gerakan setiap motor ke langkah yang sesuai
    motors[0].move(stepsX);   // Motor X bergerak menuju 10 cm
    motors[1].move(stepsY);   // Motor Y bergerak menuju 10 cm
    motors[2].move(stepsZ);   // Motor Z bergerak menuju 10 cm

    // Set kecepatan relatif untuk setiap sumbu (disesuaikan dengan kebutuhan)
    motors[0].setSpeed(100);  // Set kecepatan motor X
    motors[1].setSpeed(200);  // Set kecepatan motor Y
    motors[2].setSpeed(300);  // Set kecepatan motor Z

    isMoving = true;
  }

  // Update setiap motor stepper
  for (int i = 0; i < NUM_MOTORS; i++) {
    motors[i].runSpeedToPosition();
  }

  // Cek apakah gerakan motor selesai
  if (isMoving) {
    bool motorsMoving = false;
    for (int i = 0; i < NUM_MOTORS; i++) {
      if (motors[i].distanceToGo() != 0) {
        motorsMoving = true;
        break;
      }
    }
    if (!motorsMoving) {
      digitalWrite(PIN_LED, LOW);  // Matikan LED
      isMoving = false;
    }
  }
}