#include <AccelStepper.h>

// Pin definisi
#define START_BUTTON A1
#define RESET_BUTTON A2
#define PWM_OUT_PIN 10
#define MOTOR1_PUL 12
#define MOTOR1_DIR 11
#define MOTOR2_PUL 9
#define MOTOR2_DIR 8
#define BUZZER_PIN 7

// Inisialisasi motor stepper
AccelStepper motorA(AccelStepper::DRIVER, MOTOR1_PUL, MOTOR1_DIR);
AccelStepper motorB(AccelStepper::DRIVER, MOTOR2_PUL, MOTOR2_DIR);

// Variabel status
bool startButtonPressed = false;
bool resetButtonPressed = false;

void setup() {
  pinMode(START_BUTTON, INPUT_PULLUP);
  pinMode(RESET_BUTTON, INPUT_PULLUP);
  pinMode(PWM_OUT_PIN, OUTPUT);
  pinMode(BUZZER_PIN, OUTPUT);

  digitalWrite(PWM_OUT_PIN, LOW);
  digitalWrite(BUZZER_PIN, LOW);

  motorA.setMaxSpeed(8000); // Kecepatan maksimum motor A
  motorA.setAcceleration(6000); // Akselerasi motor A
  motorB.setMaxSpeed(8000); // Kecepatan maksimum motor B
  motorB.setAcceleration(6000); // Akselerasi motor B

  Serial.begin(9600);
}

void loop() {
  if (digitalRead(START_BUTTON) == LOW && !startButtonPressed) {
    delay(200); // Debouncing tombol
    startButtonPressed = true;
    digitalWrite(PWM_OUT_PIN, HIGH); // Aktifkan PWM
    delay(3000); // Delay 3 detik sebelum motor berjalan
    runSequence();
    digitalWrite(PWM_OUT_PIN, LOW); // Matikan PWM setelah program selesai
    digitalWrite(BUZZER_PIN, HIGH); // Buzzer akan menyala terus
    startButtonPressed = false;
  }

  if (digitalRead(RESET_BUTTON) == LOW && !resetButtonPressed) {
    delay(200); // Debouncing tombol
    resetButtonPressed = true;
    resetMotors();
    digitalWrite(BUZZER_PIN, LOW); // Matikan buzzer saat reset ditekan
    resetButtonPressed = false;
  }
}

void runSequence() {
  // Gerakan awal motor B (1 kali dari 0 ke 180 derajat)
  moveMotorB(180);

  // Gerakan motor A dengan motor B bergerak 2 kali di setiap langkah
  float anglesA[] = {22.5, 45, 67.5, 90, 112.5, 135, 157.5, 180, 202.5, 225, 247.5, 270, 292.5, 315, 337.5, 360};
  for (int i = 0; i < 16; i++) {
    moveMotorA(anglesA[i]);

    // Jika sudut terakhir (360 derajat), motor B bergerak hanya 1 kali
    if (anglesA[i] == 360) {
      moveMotorB(180);
    } else {
      moveMotorB(180);
      moveMotorB(180);
    }
  }
}

void moveMotorA(float angle) {
  int stepsPerRev = 1600; // Langkah per revolusi untuk microstep 1
  int steps = (angle / 360.0) * stepsPerRev; // Konversi derajat ke langkah
  motorA.moveTo(steps);
  motorA.runToPosition();
}

void moveMotorB(float angle) {
  int stepsPerRev = 1600; // Langkah per revolusi untuk microstep 1
  int steps = (angle / 360.0) * stepsPerRev; // Konversi derajat ke langkah
  motorB.moveTo(steps);
  motorB.runToPosition();
  motorB.moveTo(0); // Kembali ke 0
  motorB.runToPosition();
}

void resetMotors() {
  motorA.moveTo(0);
  motorA.runToPosition();
  motorB.moveTo(0);
  motorB.runToPosition();
  Serial.println("Motors reset to position 0.");
}