// =============================================
//  Project: Sistem Kontrol Lift 4 Lantai (LCD1602)
//  Judul  : MAJU TEKNIK - Elevator Controller
//  Versi  : v1.2.0
//  Tanggal: 5 Juli 2025
//  Author : Pak Eko
// =============================================

#include <Wire.h>                        // Library komunikasi I2C
#include <LiquidCrystal_I2C.h>          // Library untuk LCD I2C hapus karna tidak cocok esp32
#include <EEPROM.h>                     // Library untuk menyimpan data ke EEPROM
//#include <hd44780ioClass/hd44780_I2Cexp.h>  // aktifkan pada saat upload

// Inisialisasi LCD1602 I2C
LiquidCrystal_I2C lcd(0x27, 16, 2);     // Alamat I2C 0x27, ukuran 16x2 karakter
// hd44780_I2Cexp lcd(0x27, 16, 2); // Pastikan alamat 0x27 sesuai scanner ukuran 16x2 karakter

// Definisi alamat EEPROM
#define EEPROM_SIZE 4                   // Ukuran EEPROM yang digunakan
#define POSISI_ADDR 0                   // Alamat penyimpanan posisi lantai terakhir

// Pin Stepper
#define STEP_PIN 5                     // Pin untuk sinyal STEP motor stepper
#define DIR_PIN  18                     // Pin untuk arah putaran motor stepper

// Pin Tombol dan Limit Switch untuk 4 lantai
const int tombolLantai[4] = {12, 13, 14, 27}; // Tombol untuk memilih lantai
const int limitLantai[4]  = {32, 33, 25, 26}; // Sensor limit switch tiap lantai

// Pin Relay dan Buzzer
#define PIN_RELAY_REM   17              // Relay rem mekanik lift
#define PIN_RELAY_PINTU 16              // Relay pembuka pintu
#define PIN_BUZZER      34              // Buzzer sebagai alarm atau indikator suara

// Variabel Sistem
const int langkahPerLantai = 1000;      // Jumlah langkah stepper antar lantai
int posisiLangkah = 0;                 // Posisi saat ini dalam langkah stepper
int posisiLantai = 0;                  // Posisi saat ini dalam nomor lantai
bool pintuSedangTerbuka = false;       // Status pintu terbuka atau tertutup
int tujuanLantaiGlobal = 0;            // Variabel untuk menyimpan lantai tujuan terakhir

// Fungsi untuk menampilkan pesan pembuka saat sistem dinyalakan
void tampilanWelcome() {
  lcd.clear();
  lcd.setCursor(2, 0);
  lcd.print("MAJU TEKNIK");
  lcd.setCursor(0, 1);
  lcd.print("Elevator v1.2.0");
  delay(2500);

  lcd.clear();
  lcd.setCursor(2, 0);
  lcd.print("By Pak Eko");
  lcd.setCursor(0, 1);
  lcd.print("barokallah fikum");
  delay(2500);
}

// Fungsi untuk menampilkan status di layar LCD
void tampilkanStatus(String status, int lantai) {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(status.length() > 16 ? status.substring(0, 16) : status);
  lcd.setCursor(0, 1);
  if (lantai >= 0) {
    lcd.print("Lantai: ");
    lcd.print(lantai + 1);
  } else {
    lcd.print("Maju Teknik Lift");
  }
  Serial.print("LCD: ");
  Serial.println(status);
}
void setup() {
  Serial.begin(115200);                        // Inisialisasi Serial Monitor
  lcd.init();                                  // Inisialisasi LCD I2C
  lcd.backlight();                             // Nyalakan backlight LCD
  tampilanWelcome();                           // Tampilkan pesan selamat datang
  tampilkanStatus("Inisialisasi", -1);         // Status awal inisialisasi

  EEPROM.begin(EEPROM_SIZE);                   // Inisialisasi EEPROM
  posisiLantai = EEPROM.read(POSISI_ADDR);     // Ambil posisi terakhir dari EEPROM
  posisiLangkah = posisiLantai * langkahPerLantai;

  pinMode(STEP_PIN, OUTPUT);
  pinMode(DIR_PIN, OUTPUT);

  for (int i = 0; i < 4; i++) {
    pinMode(tombolLantai[i], INPUT_PULLUP);    // Input tombol lantai
    pinMode(limitLantai[i], INPUT_PULLUP);     // Input limit switch
  }

  pinMode(PIN_RELAY_REM, OUTPUT);              // Output relay rem
  pinMode(PIN_RELAY_PINTU, OUTPUT);            // Output relay pintu
  pinMode(PIN_BUZZER, OUTPUT);                 // Output buzzer

  digitalWrite(PIN_RELAY_REM, LOW);            // Pastikan rem mati awal
  digitalWrite(PIN_RELAY_PINTU, LOW);          // Pintu tertutup
  digitalWrite(PIN_BUZZER, LOW);               // Buzzer off

  // Cek jika bukan di lantai 1, maka lakukan homing
  if (digitalRead(limitLantai[0]) == HIGH) {
    tampilkanStatus("Homing...", -1);
    homingKeLantai1();
  }

  posisiLangkah = 0;
  posisiLantai = 0;
  EEPROM.write(POSISI_ADDR, posisiLantai);
  EEPROM.commit();

  tampilkanStatus("DI", posisiLantai);         // Tampilkan posisi awal lift
}

void loop() {
  for (int i = 0; i < 4; i++) {
    if (digitalRead(tombolLantai[i]) == LOW) { // Jika tombol lantai ditekan

      if (i == posisiLantai) {                 // Jika sudah di lantai itu
        if (!pintuSedangTerbuka) {
          prosesPintu();                       // Buka pintu
        } else {
          delay(2000);
          digitalWrite(PIN_RELAY_PINTU, LOW);
          tampilkanStatus("Pintu Tertutup", posisiLantai);
          pintuSedangTerbuka = false;
          delay(2000);
        }
        continue;
      }

      if (pintuSedangTerbuka) {
        delay(2000);
        digitalWrite(PIN_RELAY_PINTU, LOW);
        tampilkanStatus("Pintu Tertutup", posisiLantai);
        pintuSedangTerbuka = false;
        delay(2000);
      }

      tujuanLantaiGlobal = i;                  // Simpan tujuan untuk animasi
      animasiArah(i > posisiLantai);           // Tampilkan arah naik/turun
      keLantai(i);                             // Gerakkan lift
      posisiLantai = i;
      posisiLangkah = posisiLantai * langkahPerLantai;
      EEPROM.write(POSISI_ADDR, posisiLantai); // Simpan posisi ke EEPROM
      EEPROM.commit();
    }
  }
}

// Fungsi untuk menjalankan prosedur homing ke lantai 1
void homingKeLantai1() {
  digitalWrite(PIN_RELAY_PINTU, LOW);           // Pastikan pintu tertutup saat homing
  digitalWrite(DIR_PIN, LOW);
  while (digitalRead(limitLantai[0]) == HIGH) satuLangkah(1000);
  for (int i = 0; i < 100; i++) satuLangkah(1000);
  digitalWrite(DIR_PIN, HIGH);
  while (digitalRead(limitLantai[0]) == LOW) satuLangkah(1500);
  digitalWrite(DIR_PIN, LOW);
  while (digitalRead(limitLantai[0]) == HIGH) satuLangkah(1500);
  digitalWrite(PIN_RELAY_REM, HIGH);
  tampilkanStatus("Homing selesai", 0);
  delay(500);
}

// Fungsi untuk menggerakkan lift ke lantai tujuan
void keLantai(int tujuan) {
  if (tujuan == posisiLantai) return;

  digitalWrite(PIN_RELAY_REM, LOW);
  digitalWrite(PIN_RELAY_PINTU, LOW);

  bool naik = (tujuan > posisiLantai);
  digitalWrite(DIR_PIN, naik ? HIGH : LOW);

  unsigned long totalLangkah = abs(tujuan - posisiLantai) * langkahPerLantai;
  gerakDenganAkselerasi(totalLangkah, naik);

  unsigned long startMillis = millis();
  unsigned long timeout = 10000;

  while (digitalRead(limitLantai[tujuan]) == HIGH) {
    satuLangkah(1500);
    if (millis() - startMillis > timeout) {
      tampilkanStatus("ERROR", -1);
      emergencyStop();
      return;
    }
  }

  for (int i = 0; i < 10; i++) satuLangkah(1000);
  digitalWrite(DIR_PIN, !naik);
  while (digitalRead(limitLantai[tujuan]) == HIGH) satuLangkah(1500);

  digitalWrite(PIN_RELAY_REM, HIGH);
  tampilkanStatus("DI", tujuan);
  digitalWrite(PIN_BUZZER, HIGH);
  delay(300);
  digitalWrite(PIN_BUZZER, LOW);
  delay(2000);
  prosesPintu();
  tampilkanStatus("DI", tujuan);
  delay(1000);
}

// Fungsi untuk membuka dan menutup pintu lift
void prosesPintu() {
  digitalWrite(PIN_RELAY_PINTU, HIGH);
  digitalWrite(PIN_BUZZER, HIGH);
  delay(100);
  digitalWrite(PIN_BUZZER, LOW);
  tampilkanStatus("Pintu Terbuka", posisiLantai);
  Serial.println("PINTU: BUKA");
  pintuSedangTerbuka = true;

  delay(5000);

  digitalWrite(PIN_RELAY_PINTU, LOW);
  tampilkanStatus("Pintu Tertutup", posisiLantai);
  Serial.println("PINTU: TUTUP");
  pintuSedangTerbuka = false;

  delay(5000);
  animasiArah(false);
}

// Fungsi untuk menggerakkan stepper dengan akselerasi
void gerakDenganAkselerasi(unsigned long totalLangkah, bool arahNaik) {
  digitalWrite(DIR_PIN, arahNaik ? HIGH : LOW);
  unsigned long akselerasiStep = totalLangkah * 0.3;
  unsigned long deselerasiStep = akselerasiStep;
  unsigned long konstanStep = totalLangkah - akselerasiStep - deselerasiStep;

  for (unsigned long i = 0; i < akselerasiStep; i++) {
    int delayUs = map(i, 0, akselerasiStep, 1500, 300);
    satuLangkah(delayUs);
  }

  for (unsigned long i = 0; i < konstanStep; i++) satuLangkah(300);

  for (unsigned long i = 0; i < deselerasiStep; i++) {
    int delayUs = map(i, 0, deselerasiStep, 300, 1500);
    satuLangkah(delayUs);
  }
}

// Fungsi untuk satu langkah stepper
void satuLangkah(int delay_us) {
  digitalWrite(STEP_PIN, HIGH);
  delayMicroseconds(delay_us);
  digitalWrite(STEP_PIN, LOW);
  delayMicroseconds(delay_us);
}

// Fungsi untuk menampilkan arah gerakan ke layar LCD
void animasiArah(bool naik) {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Bergerak: ");
  lcd.print(naik ? "^^^" : "vvv");
  lcd.setCursor(0, 1);
  lcd.print("Menuju Lantai ");
  lcd.print(tujuanLantaiGlobal + 1);
  delay(1500);
}

// Fungsi jika terjadi keadaan darurat (emergency)
void emergencyStop() {
  digitalWrite(STEP_PIN, LOW);
  digitalWrite(DIR_PIN, LOW);
  digitalWrite(PIN_RELAY_REM, LOW);
  digitalWrite(PIN_RELAY_PINTU, LOW);
  digitalWrite(PIN_BUZZER, LOW);
  tampilkanStatus("EMERGENCY!", -1);
  while (1);
}