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

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

// --- Pin Mapping ---
#define STEP_PIN 2
#define DIR_PIN 3
#define LIMIT_0 A2
#define LIMIT_1 A3
#define POT_PIN A6
#define START_BTN 6
#define PNEU_BAWAH 4
#define PNEU_KUAS 5

// --- Objek ---
AccelStepper stepper(AccelStepper::DRIVER, STEP_PIN, DIR_PIN);
LiquidCrystal_I2C lcd(0x27, 16, 2);

// --- Variabel ---
long stepperSpeed = 500;
unsigned long lastDebounceTime = 0;
const unsigned long debounceDelay = 200; // Debounce 200ms

void homing() {
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("Homing...");

  stepper.setMaxSpeed(400);
  stepper.setSpeed(-200);

  // Gerak mundur sampai Limit 0
  while (digitalRead(LIMIT_0) == HIGH) {
    stepper.runSpeed();
  }

  stepper.setCurrentPosition(0);  // posisi nol
  stepper.stop();
  lcd.setCursor(0,1);
  lcd.print("OK Pos=0");
  delay(1000);
}

void setup() {
  // Serial untuk debugging
  Serial.begin(9600);
  
  pinMode(LIMIT_0, INPUT_PULLUP);
  pinMode(LIMIT_1, INPUT_PULLUP);
  pinMode(START_BTN, INPUT_PULLUP); // Tombol aktif LOW ketika ditekan

  pinMode(PNEU_BAWAH, OUTPUT);
  pinMode(PNEU_KUAS, OUTPUT);
  digitalWrite(PNEU_BAWAH, LOW);
  digitalWrite(PNEU_KUAS, LOW);

  lcd.init();
  lcd.backlight();

  stepper.setMaxSpeed(5000);
  stepper.setAcceleration(1000);

  // Homing sekali
  homing();
  
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("System Ready");
  lcd.setCursor(0,1);
  lcd.print("Press START...");
}

void loop() {
  // --- DEBUG: Tampilkan status tombol di Serial Monitor ---
  Serial.print("START_BTN: ");
  Serial.println(digitalRead(START_BTN));
  
  // --- Standby Mode ---
  lcd.setCursor(0,0);
  lcd.print("Press START...");
  
  // Update speed dari potentiometer
  int potValue = analogRead(POT_PIN);
  stepperSpeed = map(potValue, 0, 1023, 1000, 4000);
  stepper.setMaxSpeed(stepperSpeed);

  lcd.setCursor(0,1);
  lcd.print("Speed:");
  lcd.print(stepperSpeed);
  lcd.print("    ");

  // --- Deteksi Tombol START dengan Debounce Sederhana ---
  int btnState = digitalRead(START_BTN);
  
  if (btnState == LOW) { // Tombol ditekan (karena INPUT_PULLUP)
    Serial.println("Tombol ditekan terdeteksi!");
    delay(50); // Debounce sederhana
    
    // Konfirmasi tombol masih ditekan setelah debounce
    if (digitalRead(START_BTN) == LOW) {
      // Tunggu sampai tombol dilepas
      while (digitalRead(START_BTN) == LOW) {
        delay(10);
      }
      
      // Jalankan siklus setelah tombol dilepas
      runCycle();
    }
  }
  
  delay(100); // Delay kecil untuk mengurangi beban CPU
}

void runCycle() {
  Serial.println("Memulai siklus...");
  
  // --- Jalankan siklus ---
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("Cycle Run");

  // 1. Pneumatik bawah ON
  digitalWrite(PNEU_BAWAH, HIGH);
  delay(1000);

  // 2. Kuas ON
  digitalWrite(PNEU_KUAS, HIGH);
  delay(1000);

  // 3. Stepper maju sampai LIMIT_1
  lcd.setCursor(0,1);
  lcd.print("Moving FWD...");
  stepper.setSpeed(stepperSpeed);
  
  unsigned long startTime = millis();
  while (digitalRead(LIMIT_1) == HIGH) {
    stepper.runSpeed();
    
    // Timeout safety - jika lebih dari 30 detik, berhenti
    if (millis() - startTime > 30000) {
      lcd.clear();
      lcd.print("ERROR: Timeout!");
      break;
    }
  }
  stepper.stop();
  Serial.println("Sampai LIMIT_1");

  // 4. Kuas OFF
  digitalWrite(PNEU_KUAS, LOW);
  delay(500);

  // 5. Stepper mundur kembali ke 0 (LIMIT_0)
  lcd.setCursor(0,1);
  lcd.print("Moving REV...");
  stepper.setSpeed(-stepperSpeed);
  
  startTime = millis();
  while (digitalRead(LIMIT_0) == HIGH) {
    stepper.runSpeed();
    
    // Timeout safety
    if (millis() - startTime > 30000) {
      lcd.clear();
      lcd.print("ERROR: Timeout!");
      break;
    }
  }
  stepper.stop();
  stepper.setCurrentPosition(0); // Reset posisi setelah sampai
  Serial.println("Sampai LIMIT_0");

  // 6. Pneumatik bawah OFF
  digitalWrite(PNEU_BAWAH, LOW);

  // --- Selesai ---
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("Cycle Complete");
  lcd.setCursor(0,1);
  lcd.print("Ready...");
  delay(2000);
  
  // Kembali ke standby
  lcd.clear();
}