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

// ======================================================
// LCD I2C
// ======================================================
LiquidCrystal_I2C lcd(0x27, 16, 2);

// ======================================================
// TB6600 + NEMA17 (Guide Kiri-Kanan)
// ======================================================
#define STEP_PIN 26
#define DIR_PIN  27
#define EN_PIN   15

AccelStepper stepper(AccelStepper::DRIVER, STEP_PIN, DIR_PIN);

// ======================================================
// BTS7960 + PG45 (Motor Penarik Kabel) - SELALU KANAN/FORWARD
// ======================================================
#define RPWM 14
#define LPWM 12
#define PWM_FREQ 20000
#define PWM_RESOLUTION 8

int motorSpeed = 0;
unsigned long lastMotorRamp = 0;

// ======================================================
// BUZZER
// ======================================================
#define BUZZER_PIN 25

// ======================================================
// KY-040 ENCODER  [FIX: DT = GPIO 34 (input only OK)]
// ======================================================
#define ENC_CLK 32
#define ENC_DT  34

volatile unsigned long pulseCount = 0;
unsigned long lastPulseCount = 0;
unsigned long lastPulseDetected = 0;
volatile unsigned long lastPulseTime = 0;
const unsigned long DEBOUNCE_US = 3000;

// ======================================================
// ENCODER OFFSET - KABEL MELEWATI ENCODER SEBELUM SPOOL
// ======================================================
const float ENCODER_OFFSET_METER = 0.80; // 80 cm offset
bool encoderReset = false; // Flag untuk reset encoder di awal proses
unsigned long resetPulseCount = 0; // Pulse count saat reset

void IRAM_ATTR onEncoderPulse() {
  unsigned long now = micros();
  if ((now - lastPulseTime) > DEBOUNCE_US) {
    if (digitalRead(ENC_DT) == HIGH) {
      pulseCount++;
    }
    lastPulseTime = now;
  }
}

// ======================================================
// KEYPAD 4x4  [FIX: C4 = GPIO 33 (full I/O)]
// ======================================================
const byte ROWS = 4;
const byte COLS = 4;
char keys[ROWS][COLS] = {
  {'1','2','3','A'},
  {'4','5','6','B'},
  {'7','8','9','C'},
  {'*','0','#','D'}
};

byte rowPins[ROWS] = {4, 16, 17, 5};
byte colPins[COLS] = {18, 19, 23, 33};

Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

// ======================================================
// VARIABLE INPUT & PROSES
// ======================================================
char inputBuffer[8] = "";
byte inputIndex = 0;

float targetMeter = 0;
float sisaMeter = 0;
float totalMeter = 0;

// ======================================================
// KALIBRASI ENCODER
// ======================================================
float meterPerPulse = 0.01;
bool kalibrasiMode = false;
unsigned long kalibrasiPulseStart = 0;
unsigned long kalibrasiStartTime = 0;
const unsigned long KALIBRASI_TIMEOUT = 30000; // 30 detik timeout

bool autoMode = false;
bool prosesSelesai = false;

// ======================================================
// MODE MANUAL 20 PUTARAN
// ======================================================
enum ManualState {
  MANUAL_IDLE,
  MANUAL_MAJU_20,
  MANUAL_JEDA_1,
  MANUAL_MUNDUR_20,
  MANUAL_JEDA_2,
  MANUAL_SELESAI
};

ManualState manualState = MANUAL_IDLE;

const float STEPS_PER_REV = 3200.0;
const int   PUTARAN_20 = 20;
const long  TARGET_STEPS_20 = (long)(STEPS_PER_REV * PUTARAN_20);

const float MANUAL_MAX_SPEED = 3000;
const float MANUAL_ACCEL = 1500;
const unsigned long MANUAL_JEDA_MS = 500;

unsigned long manualJedaStart = 0;
float putaranSaatIni = 0;

// ======================================================
// LOGIKA STEPPER AUTO - 65 PUTARAN KANAN-KIRI LOOP
// ======================================================
const int   PUTARAN_65 = 65;
const long  STEPS_65_PUTARAN = (long)(STEPS_PER_REV * PUTARAN_65);

const float AUTO_MAX_SPEED = 15000;
const float AUTO_ACCEL = 8000;

bool arahKanan = true;

// ======================================================
// TOGGLE STEPPER KE KIRI (TOMBOL A)
// ======================================================
bool stepperKiriJalan = false;

// ======================================================
// EEPROM ADDRESSES
// ======================================================
#define EEPROM_ADDR_CALIBRATED  0
#define EEPROM_ADDR_METER_PP    4

// ======================================================
// TIMING VARIABLES
// ======================================================
unsigned long lastKey = 0;
unsigned long lastLCD = 0;
unsigned long lastSerialPrint = 0;

// ======================================================
// BUZZER FUNCTIONS
// ======================================================
void beep(int waktu) {
  digitalWrite(BUZZER_PIN, HIGH);
  delay(waktu);
  digitalWrite(BUZZER_PIN, LOW);
}

void selesaiBuzzer() {
  for (int i = 0; i < 5; i++) {
    beep(100);
    delay(100);
  }
}

void errorBuzzer() {
  for (int i = 0; i < 3; i++) {
    beep(300);
    delay(200);
  }
}

void kalibrasiBuzzer() {
  beep(500);
  delay(200);
  beep(500);
  delay(200);
  beep(500);
}

// ======================================================
// LCD FUNCTIONS
// ======================================================
void lcdInit() {
  Wire.begin(21, 22);
  delay(100);
  lcd.begin(16, 2);
  delay(100);
  lcd.backlight();
  delay(100);
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("M14 Spool 16cm");
  lcd.setCursor(0,1);
  lcd.print("Ready");
  delay(800);
  lcd.clear();
}

void tampilLCD() {
  static bool lastModeKalibrasi = false;
  static bool lastModeAuto = false;
  static bool lastModeSelesai = false;
  static bool lastModeInput = false;
  static bool lastKiriJalan = false;
  static ManualState lastManualState = MANUAL_IDLE;

  bool modeKalibrasi = kalibrasiMode;
  bool modeAuto = autoMode;
  bool modeSelesai = prosesSelesai;
  bool modeInput = !kalibrasiMode && !autoMode && !prosesSelesai && manualState == MANUAL_IDLE && !stepperKiriJalan;
  bool kiriJalan = stepperKiriJalan;

  if (modeKalibrasi != lastModeKalibrasi || 
      modeAuto != lastModeAuto || 
      modeSelesai != lastModeSelesai ||
      modeInput != lastModeInput ||
      manualState != lastManualState ||
      kiriJalan != lastKiriJalan) {
    lcd.clear();
    lastModeKalibrasi = modeKalibrasi;
    lastModeAuto = modeAuto;
    lastModeSelesai = modeSelesai;
    lastModeInput = modeInput;
    lastManualState = manualState;
    lastKiriJalan = kiriJalan;
  }

  if (stepperKiriJalan) {
    lcd.setCursor(0,0);
    lcd.print("KE KIRI...      ");
    lcd.setCursor(0,1);
    lcd.print("Pos:");
    lcd.print(stepper.currentPosition());
    lcd.print("    ");
  }
  else if (kalibrasiMode) {
    lcd.setCursor(0,0);
    lcd.print("KALIBRASI       ");
    lcd.setCursor(0,1);
    lcd.print("Pulse:");
    unsigned long currentPulse = pulseCount;
    unsigned long diff = currentPulse - kalibrasiPulseStart;
    lcd.print("      ");
    lcd.setCursor(6,1);
    lcd.print(diff);
    lcd.print("   ");
    
    // Tampilkan sisa waktu
    unsigned long elapsed = millis() - kalibrasiStartTime;
    unsigned long remaining = (KALIBRASI_TIMEOUT - elapsed) / 1000;
    lcd.setCursor(12,1);
    lcd.print(remaining);
    lcd.print("s ");
  }
  else if (autoMode) {
    lcd.setCursor(0,0);
    lcd.print("Sisa:");
    lcd.print(sisaMeter, 1);
    lcd.print("m  ");
    lcd.setCursor(0,1);
    lcd.print(arahKanan ? ">>" : "<<");
    lcd.print(" ");
    lcd.print(totalMeter, 1);
    lcd.print("m ");
    
    // Tambahkan indikator offset di pojok
    lcd.setCursor(13,0);
    lcd.print("+80");
  }
  else if (manualState != MANUAL_IDLE && manualState != MANUAL_SELESAI) {
    lcd.setCursor(0,0);
    lcd.print("M20:");
    lcd.print(abs(putaranSaatIni), 1);
    lcd.print("/20");
    lcd.setCursor(0,1);
    if (manualState == MANUAL_MAJU_20) {
      lcd.print("MAJU CW         ");
    } else if (manualState == MANUAL_JEDA_1) {
      lcd.print("JEDA...         ");
    } else if (manualState == MANUAL_MUNDUR_20) {
      lcd.print("MUNDUR CCW      ");
    } else if (manualState == MANUAL_JEDA_2) {
      lcd.print("JEDA...         ");
    }
  }
  else if (manualState == MANUAL_SELESAI) {
    lcd.setCursor(0,0);
    lcd.print("M20 SELESAI!    ");
    lcd.setCursor(0,1);
    lcd.print("20CW 20CCW Done");
  }
  else if (prosesSelesai) {
    lcd.setCursor(0,0);
    lcd.print("SELESAI         ");
    lcd.setCursor(0,1);
    lcd.print("Total:");
    lcd.print(totalMeter, 2);
    lcd.print("m");
  }
  else {
    lcd.setCursor(0,0);
    lcd.print("INPUT:");
    lcd.print(inputBuffer);
    for (int i = inputIndex; i < 6; i++) {
      lcd.print(" ");
    }
    lcd.setCursor(0,1);
    lcd.print("A=KIRI #=GO");
    lcd.print("   ");
    // Tampilkan status kalibrasi di pojok
    if (meterPerPulse > 0 && meterPerPulse < 1) {
      lcd.setCursor(13,1);
      lcd.print("CLB");
    }
  }
}

// ======================================================
// STOP STEPPER
// ======================================================
void stopStepperNow() {
  stepper.stop();
  unsigned long timeout = millis() + 3000;
  while (stepper.distanceToGo() != 0 && millis() < timeout) {
    stepper.run();
  }
  long currentPos = stepper.currentPosition();
  stepper.setCurrentPosition(currentPos);
  stepper.moveTo(currentPos);
  stepper.setSpeed(0);
  digitalWrite(EN_PIN, HIGH);
}

// ======================================================
// EEPROM
// ======================================================
void saveCalibration() {
  EEPROM.writeFloat(EEPROM_ADDR_METER_PP, meterPerPulse);
  EEPROM.writeInt(EEPROM_ADDR_CALIBRATED, 1);
  EEPROM.commit();
  Serial.println("Kalibrasi tersimpan ke EEPROM!");
}

void loadCalibration() {
  int calibrated = EEPROM.readInt(EEPROM_ADDR_CALIBRATED);
  if (calibrated == 1) {
    float saved = EEPROM.readFloat(EEPROM_ADDR_METER_PP);
    if (saved > 0.0 && saved < 1.0) {
      meterPerPulse = saved;
      Serial.print("Calibration loaded: ");
      Serial.println(meterPerPulse, 6);
    }
  }
}

// ======================================================
// KALIBRASI FUNCTIONS
// ======================================================
void toggleKalibrasi() {
  if (autoMode || manualState != MANUAL_IDLE || stepperKiriJalan) {
    Serial.println("ERROR: Sedang mode aktif!");
    errorBuzzer();
    return;
  }
  
  if (!kalibrasiMode) {
    // Start kalibrasi
    kalibrasiMode = true;
    kalibrasiPulseStart = pulseCount;
    kalibrasiStartTime = millis();
    
    // Matikan auto mode
    autoMode = false;
    prosesSelesai = false;
    
    // Hidupkan motor penarik dengan kecepatan rendah
    motorSpeed = 50;
    ledcWrite(RPWM, motorSpeed);
    ledcWrite(LPWM, 0);
    
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("KALIBRASI");
    lcd.setCursor(0,1);
    lcd.print("Gulung 1 meter");
    
    Serial.println("");
    Serial.println("========================================");
    Serial.println("        MULAI KALIBRASI");
    Serial.println("========================================");
    Serial.println("Gulung kabel tepat 1 meter");
    Serial.println("Tekan C lagi untuk selesai");
    Serial.println("Atau tunggu 30 detik timeout");
    Serial.println("========================================");
    
    kalibrasiBuzzer();
  } else {
    // Stop kalibrasi
    unsigned long currentPulse = pulseCount;
    unsigned long totalPulse = currentPulse - kalibrasiPulseStart;
    
    // Matikan motor
    motorSpeed = 0;
    ledcWrite(RPWM, 0);
    ledcWrite(LPWM, 0);
    
    kalibrasiMode = false;
    
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("Pulse:");
    lcd.print(totalPulse);
    
    Serial.println("");
    Serial.println("========================================");
    Serial.println("        SELESAI KALIBRASI");
    Serial.println("========================================");
    Serial.print("Total pulse untuk 1 meter: ");
    Serial.println(totalPulse);
    
    if (totalPulse > 0) {
      float newMeterPerPulse = 1.0 / (float)totalPulse;
      meterPerPulse = newMeterPerPulse;
      
      Serial.print("meterPerPulse = ");
      Serial.println(newMeterPerPulse, 6);
      
      saveCalibration();
      
      lcd.setCursor(0,1);
      lcd.print("Saved! ");
      lcd.print(meterPerPulse, 6);
      
      kalibrasiBuzzer();
    } else {
      Serial.println("ERROR: Tidak ada pulse terbaca!");
      lcd.setCursor(0,1);
      lcd.print("ERROR! No pulse");
      errorBuzzer();
    }
    
    delay(3000);
    tampilLCD();
  }
}

void checkKalibrasiTimeout() {
  if (kalibrasiMode) {
    if (millis() - kalibrasiStartTime > KALIBRASI_TIMEOUT) {
      Serial.println("Kalibrasi timeout!");
      
      // Matikan motor
      motorSpeed = 0;
      ledcWrite(RPWM, 0);
      ledcWrite(LPWM, 0);
      
      kalibrasiMode = false;
      
      lcd.clear();
      lcd.setCursor(0,0);
      lcd.print("TIMEOUT!");
      lcd.setCursor(0,1);
      lcd.print("Coba ulang");
      errorBuzzer();
      delay(2000);
      tampilLCD();
    }
  }
}

// ======================================================
// RESET ENCODER DI AWAL PROSES
// ======================================================
void resetEncoderForProcess() {
  if (!encoderReset) {
    noInterrupts();
    pulseCount = 0;
    lastPulseCount = 0;
    interrupts();
    encoderReset = true;
    resetPulseCount = 0;
    
    // Set total meter dengan offset 80cm
    totalMeter = ENCODER_OFFSET_METER;
    sisaMeter = targetMeter - ENCODER_OFFSET_METER;
    if (sisaMeter < 0) sisaMeter = 0;
    
    Serial.println("=== ENCODER RESET ===");
    Serial.print("Offset 80cm ditambahkan, totalMeter = ");
    Serial.println(totalMeter, 3);
    Serial.print("Sisa meter = ");
    Serial.println(sisaMeter, 3);
  }
}

// ======================================================
// SETUP
// ======================================================
void setup() {
  Serial.begin(115200);
  delay(1000);

  EEPROM.begin(512);
  loadCalibration();

  Serial.println("========================================");
  Serial.println("   M14 SPOOL 16cm - KABEL OTOMATIS");
  Serial.println("   [v5.5 - With Encoder Offset 80cm]");
  Serial.println("========================================");
  Serial.print("Keypad Row: ");
  Serial.print(rowPins[0]); Serial.print(", ");
  Serial.print(rowPins[1]); Serial.print(", ");
  Serial.print(rowPins[2]); Serial.print(", ");
  Serial.println(rowPins[3]);
  Serial.print("Keypad Col: ");
  Serial.print(colPins[0]); Serial.print(", ");
  Serial.print(colPins[1]); Serial.print(", ");
  Serial.print(colPins[2]); Serial.print(", ");
  Serial.println(colPins[3]);
  Serial.print("Encoder CLK: "); Serial.println(ENC_CLK);
  Serial.print("Encoder DT:  "); Serial.println(ENC_DT);
  Serial.print("Steps 20 putaran: ");
  Serial.println(TARGET_STEPS_20);
  Serial.print("Steps 65 putaran: ");
  Serial.println(STEPS_65_PUTARAN);
  Serial.print("Current meterPerPulse: ");
  Serial.println(meterPerPulse, 6);
  Serial.print("Encoder Offset: ");
  Serial.print(ENCODER_OFFSET_METER);
  Serial.println(" meter (80cm)");
  Serial.println("========================================");
  Serial.println("C = Kalibrasi (tekan 2x)");
  Serial.println("A = Ke Kiri");
  Serial.println("B = Manual 20 Putaran");
  Serial.println("* = Stop");
  Serial.println("# = GO (mulai proses)");
  Serial.println("========================================");

  lcdInit();

  pinMode(BUZZER_PIN, OUTPUT);
  digitalWrite(BUZZER_PIN, LOW);

  pinMode(ENC_CLK, INPUT_PULLUP);
  pinMode(ENC_DT, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(ENC_CLK), onEncoderPulse, FALLING);

  pinMode(EN_PIN, OUTPUT);
  digitalWrite(EN_PIN, HIGH);

  ledcAttach(RPWM, PWM_FREQ, PWM_RESOLUTION);
  ledcAttach(LPWM, PWM_FREQ, PWM_RESOLUTION);
  ledcWrite(RPWM, 0);
  ledcWrite(LPWM, 0);

  stepper.setMaxSpeed(AUTO_MAX_SPEED);
  stepper.setAcceleration(AUTO_ACCEL);
  stepper.setSpeed(0);

  tampilLCD();
  Serial.println("System Ready");
}

// ======================================================
// LOOP
// ======================================================
// ======================================================
// LOOP (LENGKAP)
// ======================================================
void loop() {
  stepper.run();

  bacaKeypad();
  bacaEncoder();
  handleManual20();
  checkKalibrasiTimeout();

  if (stepperKiriJalan && stepper.distanceToGo() == 0) {
    stepperKiriJalan = false;
    digitalWrite(EN_PIN, HIGH);
    Serial.println(">>> SUDAH SAMPAI KIRI <<<");
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("SUDAH KIRI");
    delay(800);
    tampilLCD();
  }

  if (autoMode) {
    if (millis() - lastMotorRamp > 30) {
      lastMotorRamp = millis();
      if (motorSpeed < 100) motorSpeed += 4;
    }
    ledcWrite(RPWM, motorSpeed);
    ledcWrite(LPWM, 0);
  } 
  else {
    if (millis() - lastMotorRamp > 30) {
      lastMotorRamp = millis();
      if (motorSpeed > 0) {
        motorSpeed -= 8;
        if (motorSpeed < 0) motorSpeed = 0;
      }
    }
    ledcWrite(RPWM, motorSpeed);
    ledcWrite(LPWM, 0);
  }

  if (autoMode && (millis() - lastPulseDetected > 15000)) {
    autoMode = false;
    stopStepperNow();
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("ENCODER ERROR");
    errorBuzzer();
  }

  if (millis() - lastLCD > 200) {
    tampilLCD();
    lastLCD = millis();
  }

  if (millis() - lastSerialPrint > 1000) {
    if (autoMode || kalibrasiMode || manualState != MANUAL_IDLE || stepperKiriJalan) {
      Serial.print("Pos: ");
      Serial.print(stepper.currentPosition());
      Serial.print(" | KiriJalan: ");
      Serial.print(stepperKiriJalan ? "YES" : "NO");
      Serial.print(" | Putaran: ");
      Serial.print(abs(putaranSaatIni), 2);
      if (kalibrasiMode) {
        Serial.print(" | Kalibrasi Pulse: ");
        Serial.print(pulseCount - kalibrasiPulseStart);
      }
      if (autoMode) {
        Serial.print(" | Total: ");
        Serial.print(totalMeter, 3);
        Serial.print("m | Sisa: ");
        Serial.print(sisaMeter, 3);
        Serial.print("m | Offset: 0.80m");
      }
      Serial.println("");
    }
    lastSerialPrint = millis();
  }
}

// ======================================================
// HANDLE MANUAL 20 PUTARAN
// ======================================================
void handleManual20() {
  if (manualState == MANUAL_IDLE || manualState == MANUAL_SELESAI) return;

  hitungPutaranManual();

  switch (manualState) {
    case MANUAL_MAJU_20:
      if (stepper.distanceToGo() == 0) {
        Serial.println(">>> 20 PUTARAN MAJU SELESAI!");
        Serial.print("Posisi: ");
        Serial.print(stepper.currentPosition());
        Serial.println(" steps");
        manualState = MANUAL_JEDA_1;
        manualJedaStart = millis();
      }
      break;

    case MANUAL_JEDA_1:
      if (millis() - manualJedaStart >= MANUAL_JEDA_MS) {
        Serial.println(">>> MULAI MUNDUR 20 PUTARAN");
        stepper.moveTo(0);
        manualState = MANUAL_MUNDUR_20;
        putaranSaatIni = 0;
      }
      break;

    case MANUAL_MUNDUR_20:
      if (stepper.distanceToGo() == 0) {
        Serial.println(">>> 20 PUTARAN MUNDUR SELESAI!");
        Serial.print("Posisi: ");
        Serial.print(stepper.currentPosition());
        Serial.println(" steps");
        manualState = MANUAL_JEDA_2;
        manualJedaStart = millis();
      }
      break;

    case MANUAL_JEDA_2:
      if (millis() - manualJedaStart >= MANUAL_JEDA_MS) {
        Serial.println("=================================");
        Serial.println("  MANUAL 20 PUTARAN SELESAI!");
        Serial.println("=================================");
        manualState = MANUAL_SELESAI;
        digitalWrite(EN_PIN, HIGH);
        for (int i = 0; i < 3; i++) {
          beep(200);
          delay(100);
        }
      }
      break;

    default:
      break;
  }
}

// ======================================================
// HITUNG PUTARAN REAL-TIME
// ======================================================
void hitungPutaranManual() {
  long posisi = stepper.currentPosition();
  putaranSaatIni = (float)posisi / STEPS_PER_REV;
}

// ======================================================
// FUNGSI MANUAL STEPPER
// ======================================================
void mulaiManualMaju20() {
  if (autoMode || kalibrasiMode || stepperKiriJalan) {
    Serial.println("ERROR: Sedang mode aktif!");
    errorBuzzer();
    return;
  }
  Serial.println("");
  Serial.println(">>> MULAI MAJU 20 PUTARAN <<<");
  Serial.print("Target: ");
  Serial.print(TARGET_STEPS_20);
  Serial.println(" steps");

  manualState = MANUAL_MAJU_20;
  putaranSaatIni = 0;

  digitalWrite(EN_PIN, LOW);
  stepper.setMaxSpeed(MANUAL_MAX_SPEED);
  stepper.setAcceleration(MANUAL_ACCEL);
  stepper.setCurrentPosition(0);
  stepper.moveTo(TARGET_STEPS_20);

  beep(100);
}

void mulaiManualMundur20() {
  if (autoMode || kalibrasiMode || stepperKiriJalan) {
    Serial.println("ERROR: Sedang mode aktif!");
    errorBuzzer();
    return;
  }
  Serial.println("");
  Serial.println(">>> MULAI MUNDUR 20 PUTARAN <<<");

  manualState = MANUAL_MUNDUR_20;
  putaranSaatIni = 0;

  digitalWrite(EN_PIN, LOW);
  stepper.setMaxSpeed(MANUAL_MAX_SPEED);
  stepper.setAcceleration(MANUAL_ACCEL);
  stepper.setCurrentPosition(0);
  stepper.moveTo(-TARGET_STEPS_20);

  beep(100);
}

void stopManual20() {
  Serial.println(">>> STOP MANUAL 20 <<<");
  stepper.stop();
  unsigned long timeout = millis() + 3000;
  while (stepper.distanceToGo() != 0 && millis() < timeout) {
    stepper.run();
  }
  long currentPos = stepper.currentPosition();
  stepper.setCurrentPosition(currentPos);
  stepper.moveTo(currentPos);
  stepper.setSpeed(0);
  digitalWrite(EN_PIN, HIGH);

  manualState = MANUAL_IDLE;
  putaranSaatIni = 0;

  beep(100);
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("MANUAL STOPPED");
  delay(1000);
  tampilLCD();
}

// ======================================================
// STEPPER KE KIRI (TOMBOL A)
// ======================================================
void toggleStepperKiri() {
  if (autoMode || kalibrasiMode) {
    Serial.println("ERROR: Sedang mode auto/kalibrasi!");
    errorBuzzer();
    return;
  }
  if (manualState != MANUAL_IDLE && manualState != MANUAL_SELESAI) {
    Serial.println("ERROR: Sedang mode manual!");
    errorBuzzer();
    return;
  }

  if (!stepperKiriJalan) {
    Serial.println("");
    Serial.println(">>> MULAI KE KIRI <<<");
    Serial.print("Dari: ");
    Serial.print(stepper.currentPosition());
    Serial.println(" -> 0");

    stepperKiriJalan = true;
    digitalWrite(EN_PIN, LOW);
    stepper.setMaxSpeed(AUTO_MAX_SPEED);
    stepper.setAcceleration(AUTO_ACCEL);
    stepper.moveTo(0);
    arahKanan = false;

    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("KE KIRI...      ");
    beep(100);
  } else {
    Serial.println(">>> STOP KE KIRI <<<");
    stepperKiriJalan = false;
    stopStepperNow();

    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("STOP KIRI");
    delay(1000);
    tampilLCD();
  }
}

// ======================================================
// KEYPAD
// ======================================================
void bacaKeypad() {
  char key = keypad.getKey();
  if (millis() - lastKey < 150) return;
  if (!key) return;

  lastKey = millis();
  beep(15);

  // ====================================================
  // TOMBOL C = KALIBRASI (TANPA MENGGANGGU SISTEM LAIN)
  // ====================================================
  if (key == 'C') {
    // Cek apakah bisa kalibrasi
    if (autoMode || manualState != MANUAL_IDLE || stepperKiriJalan) {
      Serial.println("ERROR: Sedang mode aktif!");
      errorBuzzer();
      return;
    }
    toggleKalibrasi();
    return;
  }

  // Tombol A = TOGGLE KE KIRI
  if (key == 'A') {
    // Kalibrasi mode tidak bisa akses ke kiri
    if (kalibrasiMode) {
      Serial.println("ERROR: Sedang kalibrasi!");
      errorBuzzer();
      return;
    }
    toggleStepperKiri();
    return;
  }

  // Tombol * = STOP
  if (key == '*') {
    if (stepperKiriJalan) {
      toggleStepperKiri();
    } else if (manualState != MANUAL_IDLE && manualState != MANUAL_SELESAI) {
      stopManual20();
    } else if (kalibrasiMode) {
      // Jika kalibrasi, stop motor tapi tetap di mode kalibrasi
      motorSpeed = 0;
      ledcWrite(RPWM, 0);
      ledcWrite(LPWM, 0);
      Serial.println("Motor kalibrasi di-stop");
    } else {
      stopProses();
    }
    return;
  }

  // Tombol B = Manual Maju 20 + Mundur 20
  if (key == 'B') {
    if (autoMode || kalibrasiMode || stepperKiriJalan) {
      Serial.println("ERROR: Sedang mode aktif!");
      errorBuzzer();
      return;
    }
    if (manualState != MANUAL_IDLE && manualState != MANUAL_SELESAI) {
      stopManual20();
      return;
    }
    mulaiManualMaju20();
    return;
  }

  // Jika sedang kalibrasi, tombol lain tidak berfungsi (kecuali C dan *)
  if (kalibrasiMode) return;

  // Jika mode manual aktif, tombol angka tidak berfungsi
  if (manualState != MANUAL_IDLE && manualState != MANUAL_SELESAI) return;

  // Tombol D = DELETE / HAPUS
  if (key == 'D') {
    if (inputIndex > 0) {
      inputIndex--;
      inputBuffer[inputIndex] = '\0';
      Serial.print("Hapus: inputBuffer = ");
      Serial.println(inputBuffer);
      tampilLCD();
    }
    return;
  }

  // Tombol angka = Input meter
  if (key >= '0' && key <= '9') {
    if (prosesSelesai) {
      prosesSelesai = false;
      inputBuffer[0] = '\0';
      inputIndex = 0;
    }
    if (inputIndex < 5) {
      inputBuffer[inputIndex] = key;
      inputIndex++;
      inputBuffer[inputIndex] = '\0';
      Serial.print("Input: ");
      Serial.println(inputBuffer);
      tampilLCD();
    }
  }
  else if (key == '#') {
    if (inputIndex > 0) {
      startProses();
    }
  }
}

// ======================================================
// START PROSES AUTO - 65 PUTARAN LOOP
// ======================================================
void startProses() {
  if (manualState != MANUAL_IDLE && manualState != MANUAL_SELESAI) {
    Serial.println("ERROR: Sedang mode manual!");
    errorBuzzer();
    return;
  }
  if (stepperKiriJalan) {
    Serial.println("ERROR: Stepper sedang ke kiri!");
    errorBuzzer();
    return;
  }
  if (kalibrasiMode) {
    Serial.println("ERROR: Sedang kalibrasi!");
    errorBuzzer();
    return;
  }

  targetMeter = atof(inputBuffer);
  if (targetMeter <= 0.0 || targetMeter > 9999.0) {
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("ERROR: Invalid");
    lcd.setCursor(0,1);
    lcd.print("Input!");
    errorBuzzer();
    delay(2000);
    inputBuffer[0] = '\0';
    inputIndex = 0;
    return;
  }

  // Cek apakah kalibrasi sudah dilakukan
  if (meterPerPulse <= 0.0 || meterPerPulse >= 1.0) {
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("ERROR: Belum");
    lcd.setCursor(0,1);
    lcd.print("Kalibrasi!");
    errorBuzzer();
    delay(2000);
    return;
  }

  prosesSelesai = false;
  autoMode = true;
  arahKanan = true;
  motorSpeed = 0;
  lastPulseDetected = millis();
  encoderReset = false; // Reset flag untuk proses baru

  // RESET ENCODER KE 0 DAN SET OFFSET 80CM
  noInterrupts();
  pulseCount = 0;
  lastPulseCount = 0;
  interrupts();
  
  // Set total meter dengan offset 80cm
  totalMeter = ENCODER_OFFSET_METER;
  sisaMeter = targetMeter - ENCODER_OFFSET_METER;
  if (sisaMeter < 0) sisaMeter = 0;
  
  Serial.println("=== ENCODER RESET ===");
  Serial.print("Offset 80cm ditambahkan, totalMeter = ");
  Serial.println(totalMeter, 3);
  Serial.print("Sisa meter = ");
  Serial.println(sisaMeter, 3);

  digitalWrite(EN_PIN, LOW);

  stepper.setMaxSpeed(AUTO_MAX_SPEED);
  stepper.setAcceleration(AUTO_ACCEL);
  stepper.setMinPulseWidth(5);
  stepper.setCurrentPosition(0);
  stepper.moveTo(STEPS_65_PUTARAN);

  Serial.println("");
  Serial.println("=== PROSES DIMULAI ===");
  Serial.print("Target: ");
  Serial.print(targetMeter);
    Serial.println(" meter");
  Serial.print("meterPerPulse: ");
  Serial.println(meterPerPulse, 6);
  Serial.println("Stepper loop: 65 putaran kanan <-> kiri");
  Serial.print("Encoder offset: ");
  Serial.print(ENCODER_OFFSET_METER);
  Serial.println(" meter (80cm)");

  beep(100);
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("MULAI");
  lcd.setCursor(0,1);
  lcd.print("Target:");
  lcd.print(targetMeter, 1);
  lcd.print("m");
}

// ======================================================
// STOP PROSES - BERHENTI TOTAL
// ======================================================
void stopProses() {
  autoMode = false;
  kalibrasiMode = false;

  unsigned long stopTimeout = millis() + 2000;
  while (motorSpeed > 0 && millis() < stopTimeout) {
    if (millis() - lastMotorRamp > 30) {
      lastMotorRamp = millis();
      motorSpeed -= 8;
      if (motorSpeed < 0) motorSpeed = 0;
      ledcWrite(RPWM, motorSpeed);
    }
    delay(5);
  }

  ledcWrite(RPWM, 0);
  ledcWrite(LPWM, 0);
  stopStepperNow();

  inputBuffer[0] = '\0';
  inputIndex = 0;
  prosesSelesai = true;

  // Hitung total aktual (sudah termasuk offset 80cm)
  float totalAktual = totalMeter;
  if (totalAktual < 0) totalAktual = 0;

  Serial.println("");
  Serial.println("=== PROSES SELESAI ===");
  Serial.print("Total meter (termasuk 80cm offset): ");
  Serial.print(totalAktual, 3);
  Serial.println("m");
  Serial.print("Panjang kabel yang digulung: ");
  Serial.print(totalAktual - ENCODER_OFFSET_METER, 3);
  Serial.println("m (dari encoder)");
  Serial.print("Target: ");
  Serial.print(targetMeter, 3);
  Serial.println("m");

  selesaiBuzzer();
  
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("SELESAI");
  lcd.setCursor(0,1);
  lcd.print("Total:");
  lcd.print(totalAktual, 2);
  lcd.print("m");
}

// ======================================================
// ENCODER + STEPPER 65 PUTARAN LOOP
// ======================================================
void bacaEncoder() {
  if (!autoMode && !kalibrasiMode) return;

  unsigned long currentPulse = pulseCount;

  if (currentPulse > lastPulseCount) {
    unsigned long newPulses = currentPulse - lastPulseCount;
    lastPulseCount = currentPulse;
    lastPulseDetected = millis();

    // Tambahkan pulse ke total meter (mulai dari offset 80cm)
    totalMeter += (newPulses * meterPerPulse);
    sisaMeter -= (newPulses * meterPerPulse);
    if (sisaMeter < 0) sisaMeter = 0;

    if (autoMode) {
      if (stepper.distanceToGo() == 0) {
        if (arahKanan) {
          stepper.moveTo(0);
          arahKanan = false;
          Serial.println("Kanan 65x selesai -> Langsung kiri 65x");
        } else {
          stepper.moveTo(STEPS_65_PUTARAN);
          arahKanan = true;
          Serial.println("Kiri 65x selesai -> Langsung kanan 65x");
        }
      }

      if (sisaMeter <= 0) {
        Serial.println("TARGET TERCAPAI! STOP SEMUA.");
        stopProses();
      }
    }
  }
}