// Design by Eko Andriyanto
// Sebaik-baik manusia adalah yang paling bermanfaat bagi orang lain
// Maju Teknik Art 0878-5063-1600

#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 LIMIT_SWITCH_A A2   // Proximity sensor pin Motor A
#define LIMIT_SWITCH_B A3   // Proximity sensor pin Motor B

#define STEPS_PER_REV 1600
#define DEBOUNCE_DELAY 100  // 100ms debounce time
#define HOMING_SPEED 500   // Speed for homing
#define BACKOFF_STEPS 300   // Steps to back off after homing

// step/rev motorA adalah 1600
// step/rev motorB adalah 800

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() {
  Serial.begin(9600);

  pinMode(START_BUTTON, INPUT_PULLUP);
  pinMode(RELAY_PIN_1, OUTPUT);
  pinMode(RELAY_PIN_2, OUTPUT);
  pinMode(LIMIT_SWITCH_A, INPUT_PULLUP);
  pinMode(LIMIT_SWITCH_B, INPUT_PULLUP);

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

  motorA.setMaxSpeed(2000);
  motorA.setAcceleration(4000);
  motorB.setMaxSpeed(4000);
  motorB.setAcceleration(4000);
  
  // --- Homing hanya sekali di startup ---
  homeMotorA();
  homeMotorB();
}

// === Fungsi Homing Motor Presisi ===
void homingMotor(AccelStepper &motor, int homePin, int direction, int backoffSteps) {
  unsigned long startTime = millis();
  Serial.println("Starting precision homing...");

  // 1. Cari switch (cepat)
  motor.setSpeed(500 * direction);
  while (digitalRead(homePin) == HIGH) {
    motor.runSpeed();
    if (millis() - startTime > 5000) { // timeout 5 detik
      Serial.println("Homing timeout - switch not found!");
      motor.stop();
      motor.setCurrentPosition(0);
      return; // gagal homing
    }
  }

  // 2. Mundur keluar
  Serial.println("Switch found, backing off...");
  motor.move(backoffSteps * -direction);
  while (motor.distanceToGo() != 0) {
    motor.run();
  }

  delay(50);

  // 3. Cari switch lagi (pelan)
  Serial.println("Fine homing...");
  startTime = millis();
  motor.setSpeed(100 * direction);
  while (digitalRead(homePin) == HIGH) {
    motor.runSpeed();
    if (millis() - startTime > 5000) { // timeout 5 detik
      Serial.println("Fine homing timeout!");
      motor.stop();
      motor.setCurrentPosition(0);
      return; // gagal homing
    }
  }

  // 4. Set posisi nol
  motor.setCurrentPosition(0);
  Serial.println("Homing completed successfully");
  delay(200);
}

void homeMotorA() {
  Serial.println("Homing Motor A...");
  homingMotor(motorA, LIMIT_SWITCH_A, -1, BACKOFF_STEPS);
  Serial.println("Motor A homing done");
}

void homeMotorB() {
  Serial.println("Homing Motor B...");
  homingMotor(motorB, LIMIT_SWITCH_B, -1, BACKOFF_STEPS);
  Serial.println("Motor B homing done");
}

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

    // Reset posisi setelah sequence selesai
    motorA.setCurrentPosition(0);
    motorB.setCurrentPosition(0);

    isRunning = false;
    lastButtonCheck = millis();
  }
  
  lastButtonState = currentButtonState;
}

void runSequence() {
  digitalWrite(RELAY_PIN_1, HIGH);
  delay(500);
  digitalWrite(RELAY_PIN_2, HIGH);

  struct Step {
    float angle;
    int stepB;
  };

  Step steps[] = {
    { 0, 800 }, { 22.5, 1600 }, { 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, 1600 }, { 382.5, 800 }
  };

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

  // Pastikan posisi direset ke nol
  motorA.setCurrentPosition(0);
  motorB.setCurrentPosition(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) {
    updateSpeed();
    motorA.run();
  }
}

void moveMotorB(int stepsToMove) {
  long targetPosition = motorB.currentPosition() + stepsToMove;
  motorB.moveTo(targetPosition);
  
  while (motorB.distanceToGo() != 0) {
    updateSpeed();
    motorB.runSpeed();
  }
}

void updateSpeed() {
  int potValue = analogRead(POT_PIN);
  int speed = map(potValue, 0, 1023, 1000, 4000);
  motorA.setSpeed(speed);
  motorB.setSpeed(speed);
}