// Design by Eko Andriyanto
// Sebaik-baik manusia adalah yang paling bermanfaat bagi orang lain
// Dilengkapi fitur homing, sensor proximity, pencegahan tombol dobel
// Maju Teknik Art 0878-5063-1600

#include <AccelStepper.h>

// Mapping pin ESP32
#define DIR_A 18
#define STEP_A 19
#define DIR_B 21
#define STEP_B 22

#define START_BUTTON 23
#define POT_PIN 34      // ADC input (0–4095 di ESP32)
#define RELAY_PIN_1 26
#define RELAY_PIN_2 27

#define HOME_SWITCH_A 32
#define HOME_SWITCH_B 33

#define STEPS_PER_REV 800
#define DEBOUNCE_DELAY 100

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

// Flag & debounce
bool programRunning = false;
int lastButtonState = HIGH;
unsigned long lastButtonCheck = 0;

// === Forward Declaration (supaya bisa dipanggil di setup) ===
void homingMotor(AccelStepper &motor, int homePin, int direction, int backoffSteps = 200);

// === Setup ===
void setup() {
  pinMode(START_BUTTON, INPUT_PULLUP);
  pinMode(RELAY_PIN_1, OUTPUT);
  pinMode(RELAY_PIN_2, OUTPUT);
  pinMode(HOME_SWITCH_A, INPUT_PULLUP);
  pinMode(HOME_SWITCH_B, INPUT_PULLUP);

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

  motorA.setMaxSpeed(4000);
  motorA.setAcceleration(8000);
  motorB.setMaxSpeed(10000);
  motorB.setAcceleration(10000);

  // === HOMING di awal startup ===
  homingMotor(motorA, HOME_SWITCH_A, -1);  
  homingMotor(motorB, HOME_SWITCH_B, -1);  
}

// === Loop ===
void loop() {
  int currentButtonState = digitalRead(START_BUTTON);

  // Deteksi tombol dilepas (LOW -> HIGH)
  if (lastButtonState == LOW && currentButtonState == HIGH) {
    unsigned long now = millis();
    if (now - lastButtonCheck > DEBOUNCE_DELAY && !programRunning) {
      programRunning = true;
      runSequence();
      programRunning = false;
    }
    lastButtonCheck = now;
  }

  lastButtonState = currentButtonState;
}

// === Fungsi Homing Motor Presisi ===
void homingMotor(AccelStepper &motor, int homePin, int direction, int backoffSteps) {
  unsigned long startTime = millis();

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

  // 2. Mundur keluar
  motor.move(backoffSteps * -direction);
  while (motor.distanceToGo() != 0) {
    motor.run();
  }

  delay(50);

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

  // 4. Set posisi nol
  motor.setCurrentPosition(0);
  delay(200);
}

// === Sequence Gerakan ===
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);
  }

  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, 4095, 1000, 8000);
  motorA.setSpeed(speed);
  motorB.setSpeed(speed);
}