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

#include <AccelStepper.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

// ================== Mapping pin ESP32 ==================
#define DIR_A   18
#define STEP_A  19
#define DIR_B   14
#define STEP_B  12

#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    // Limit switch Motor A
#define HOME_SWITCH_B 33    // Limit switch Motor B

#define STEPS_PER_REV 800
#define DEBOUNCE_DELAY 100

// ================== Objek ==================
AccelStepper motorA(AccelStepper::DRIVER, STEP_A, DIR_A);
AccelStepper motorB(AccelStepper::DRIVER, STEP_B, DIR_B);
LiquidCrystal_I2C lcd(0x27, 16, 2);   // alamat I2C biasanya 0x27

// ================== Variabel kontrol ==================
bool isRunning = false;
bool programRunning = false;
int lastButtonState = HIGH;
unsigned long lastButtonCheck = 0;

// =======================================================
void setup() {
  // Pin 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);

  // Motor setup
  motorA.setMaxSpeed(4000);
  motorA.setAcceleration(8000);
  motorB.setMaxSpeed(10000);
  motorB.setAcceleration(10000);

  // LCD setup
  lcd.init();
  lcd.backlight();
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Maju Teknik Art");
  lcd.setCursor(0, 1);
  lcd.print("Init System...");
  delay(1500);

  // ========== HOMING =============
  // Jika tidak ingin homing, beri komentar (//) pada dua baris di bawah:
  homingMotor(motorA, HOME_SWITCH_A, -1, "Motor A");  // <-- KOMENTARI untuk nonaktifkan homing
  homingMotor(motorB, HOME_SWITCH_B, -1, "Motor B");  // <-- KOMENTARI untuk nonaktifkan homing
}

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

  // Tombol ditekan (debounce)
  if (currentButtonState == LOW && lastButtonState == HIGH) {
    lastButtonCheck = millis();
  }

  if (currentButtonState == LOW &&
      (millis() - lastButtonCheck >= DEBOUNCE_DELAY) &&
      !isRunning && !programRunning) {
    isRunning = true;
    programRunning = true;
    runSequence();
    isRunning = false;
    programRunning = false;
    lastButtonCheck = millis();
  }

  lastButtonState = currentButtonState;
}

// =======================================================
// Fungsi Homing
void homingMotor(AccelStepper &motor, int homePin, int direction, String nama) {
  unsigned long startTime = millis();
  bool found = false;

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Homing ");
  lcd.print(nama);

  // Jalan ke arah switch dengan batas waktu 5 detik
  while (digitalRead(homePin) == HIGH) {
    motor.setSpeed(500 * direction);
    motor.runSpeed();
    if (millis() - startTime > 5000) { // timeout 5 detik
      found = false;
      break;
    }
  }

  if (digitalRead(homePin) == LOW) {
    found = true;
  }

  if (found) {
    delay(100);
    // Backoff keluar 200 step
    motor.move(200 * (-direction));
    while (motor.distanceToGo() != 0) {
      motor.run();
    }
    motor.setCurrentPosition(0); // reset posisi setelah backoff
  } else {
    motor.stop();
    motor.setCurrentPosition(0); // jika gagal, jadikan posisi saat ini sebagai nol
  }

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(nama + " Ready");
  delay(500);
}

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

    // Tampilkan posisi di LCD
    lcd.setCursor(0, 0);
    lcd.print("MotorA: ");
    lcd.print(steps[i].angle, 1);
    lcd.print((char)223); // simbol derajat
    lcd.print("   ");

    lcd.setCursor(0, 1);
    lcd.print("MotorB: ");
    lcd.print(motorB.currentPosition());
    lcd.print("   ");
  }

  motorA.setCurrentPosition(0);
  motorB.setCurrentPosition(0);

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

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Sequence Done");
  delay(1000);
}

// =======================================================
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, 10000);
  motorA.setSpeed(speed);
  motorB.setSpeed(speed);
}