#include <AccelStepper.h>

#define DIR_A 11 
#define STEP_A 12 
#define DIR_B 8 
#define STEP_B 9

#define START_BUTTON A1
#define POT_PIN A0
#define RELAY_PIN_1 10
#define RELAY_PIN_2 6

#define STEPS_PER_REV 1600
#define DEBOUNCE_DELAY 100

AccelStepper motorA(AccelStepper::DRIVER, STEP_A, DIR_A);
AccelStepper motorB(AccelStepper::DRIVER, STEP_B, DIR_B);

bool isRunning = false;
unsigned long lastButtonCheck = 0;
int lastButtonState = HIGH;

void setup() {
  pinMode(START_BUTTON, INPUT_PULLUP);
  pinMode(RELAY_PIN_1, OUTPUT);
  pinMode(RELAY_PIN_2, OUTPUT);

  digitalWrite(RELAY_PIN_1, LOW);
  digitalWrite(RELAY_PIN_2, LOW);

  motorA.setMaxSpeed(15000);
  motorA.setAcceleration(40000);
  
  // Motor B akan dikontrol kecepatannya secara manual
  motorB.setMaxSpeed(4000);
  motorB.setAcceleration(4000); // Akselerasi sangat tinggi
  
  motorA.setCurrentPosition(0);
  motorB.setCurrentPosition(0);
}

void loop() {
  int currentButtonState = digitalRead(START_BUTTON);
  
  if (currentButtonState == LOW && lastButtonState == HIGH) {
    lastButtonCheck = millis();
  }
  
  if (currentButtonState == LOW && 
      (millis() - lastButtonCheck >= DEBOUNCE_DELAY) && 
      !isRunning) {
    isRunning = true;
    runSequence();
    isRunning = false;
    lastButtonCheck = millis();
  }
  
  lastButtonState = currentButtonState;
}

void runSequence() {
  // Reset posisi motor sebelum memulai sequence baru
  motorA.setCurrentPosition(0);
  motorB.setCurrentPosition(0);
  
  digitalWrite(RELAY_PIN_1, HIGH);
  delay(500);
  digitalWrite(RELAY_PIN_2, HIGH);

  struct Step {
    float angle;
    int stepB;
  };

  // Sequence steps tanpa pergerakan mundur
  Step steps[] = {
    { 0, 800 }, 
    { 22.5, 800 }, 
    { 45, 1600 }, 
    { 67.5, 1600 },
    { 90, 1600 }, 
    { 112.5, 1600 }, 
    { 135, 1600 }, 
    { 157.5, 1600 },
    { 180, 1600 }, 
    { 202.5, 1600 }, 
    { 225, 1600 }, 
    { 247.5, 1600 },
    { 270, 1600 }, 
    { 292.5, 1600 }, 
    { 315, 1600 }, 
    { 337.5, 1600 },
    { 360, 800 },    // Motor B maju 800 steps
    { 382.5, 800 },  // Motor B maju 800 steps
    { 360, 800 },    // Kembali ke 360°, Motor B maju 800 steps
    { 382.5, 800 }   // Kembali ke 382.5°, Motor B maju 800 steps
  };

  int totalSteps = sizeof(steps) / sizeof(steps[0]);
  
  for (int i = 0; i < totalSteps; i++) {
    moveMotorA(steps[i].angle);
    moveMotorB(steps[i].stepB);
  }

  // Hanya mematikan relay tanpa mengembalikan motor ke posisi 0
  digitalWrite(RELAY_PIN_1, LOW);
  digitalWrite(RELAY_PIN_2, LOW);
}

void moveMotorA(float angle) {
  int targetSteps = (angle / 360.0) * STEPS_PER_REV;
  motorA.moveTo(targetSteps);
  
  while (motorA.distanceToGo() != 0) {
    updateSpeedMotorA();
    motorA.run();
  }
}

void moveMotorB(int stepsToMove) {
  // Dapatkan kecepatan dari potensiometer
  int speedB = getMotorBSpeed();
  
  // Atur kecepatan motor B
  motorB.setMaxSpeed(speedB);
  motorB.setSpeed(speedB);
  
  // Hitung target position
  long targetPosition = motorB.currentPosition() + stepsToMove;
  motorB.moveTo(targetPosition);
  
  // Jalankan motor B dengan kecepatan konstan tanpa akselerasi
  unsigned long startTime = millis();
  while (motorB.distanceToGo() != 0) {
    // Update kecepatan secara real-time dari potensiometer
    speedB = getMotorBSpeed();
    motorB.setMaxSpeed(speedB);
    motorB.setSpeed(speedB);
    
    motorB.runSpeedToPosition();
    
    // Tambahkan delay kecil untuk menghindari pembacaan potensiometer yang terlalu cepat
    delayMicroseconds(100);
  }
}

int getMotorBSpeed() {
  int potValue = analogRead(POT_PIN);
  // Mapping nilai potensiometer ke kecepatan motor B (300-2000 steps/detik)
  return map(potValue, 0, 1023, 1000, 4000);
}

void updateSpeedMotorA() {
  int potValue = analogRead(POT_PIN);
  int speedA = map(potValue, 0, 1023, 8000, 20000);
  motorA.setMaxSpeed(speedA);
}