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

#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>
#include <EEPROM.h>

// ---------------------------------------------
// TFT Display
#define TFT_CS   5
#define TFT_DC   2
#define TFT_RST  4
Adafruit_ILI9341 display = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_RST);

// ---------------------------------------------
// EEPROM
#define EEPROM_SIZE 4
#define POSISI_ADDR 0

// ---------------------------------------------
// Stepper
#define STEP_PIN 21
#define DIR_PIN  19

// ---------------------------------------------
// Tombol & limit switch
const int tombolLantai[4] = {12, 13, 14, 27};
const int limitLantai[4]  = {32, 33, 25, 26};

// ---------------------------------------------
// Relay & Buzzer
#define PIN_RELAY_REM   17
#define PIN_RELAY_PINTU 22
#define PIN_BUZZER      34

// ---------------------------------------------
// Variabel sistem
const int langkahPerLantai = 1000;
int posisiLangkah = 0;
int posisiLantai = 0;
bool pintuSedangTerbuka = false;

// ---------------------------------------------
void setup() {
  Serial.begin(115200);

  display.begin();
  display.setRotation(0);
  display.fillScreen(ILI9341_BLACK);
  display.setTextSize(4);
  display.setTextColor(ILI9341_WHITE);
  tampilkanStatus("Inisialisasi...", -1);

  EEPROM.begin(EEPROM_SIZE);
  posisiLantai = EEPROM.read(POSISI_ADDR);
  posisiLangkah = posisiLantai * langkahPerLantai;

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

  for (int i = 0; i < 4; i++) {
    pinMode(tombolLantai[i], INPUT_PULLUP);
    pinMode(limitLantai[i], INPUT_PULLUP);
  }

  pinMode(PIN_RELAY_REM, OUTPUT);
  digitalWrite(PIN_RELAY_REM, LOW);
  pinMode(PIN_RELAY_PINTU, OUTPUT);
  digitalWrite(PIN_RELAY_PINTU, LOW);
  pinMode(PIN_BUZZER, OUTPUT);
  digitalWrite(PIN_BUZZER, LOW);

  if (digitalRead(limitLantai[0]) == HIGH) {
    tampilkanStatus("Recovery: Homing!", -1);
    homingKeLantai1();
  }

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

  tampilkanStatus("DI", posisiLantai);
  Serial.print("Posisi sekarang (step): ");
  Serial.println(posisiLangkah);
  delay(1000);
}

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

      if (i == posisiLantai) {
        if (!pintuSedangTerbuka) {
          Serial.println("Di lantai yang sama. Buka pintu dulu.");
          prosesPintu();
        } else {
          Serial.println("Pintu sudah terbuka. Tutup sebentar.");
          delay(2000);
          digitalWrite(PIN_RELAY_PINTU, LOW);
          tampilkanStatus("door lock", posisiLantai);
          pintuSedangTerbuka = false;
          delay(1000);
        }
        continue;
      }

      if (pintuSedangTerbuka) {
        Serial.println("Pintu masih terbuka. Tutup dulu.");
        delay(2000);
        digitalWrite(PIN_RELAY_PINTU, LOW);
        tampilkanStatus("door lock", posisiLantai);
        pintuSedangTerbuka = false;
        delay(1000);
      }

      if (!pintuSedangTerbuka) {
        Serial.println("Buka pintu untuk penumpang.");
        digitalWrite(PIN_RELAY_PINTU, HIGH);
        tampilkanStatus("Pintu Terbuka", posisiLantai);
        pintuSedangTerbuka = true;
        delay(15000);
        digitalWrite(PIN_RELAY_PINTU, LOW);
        tampilkanStatus("door lock", posisiLantai);
        pintuSedangTerbuka = false;
        delay(1000);
      }

      tampilkanStatus("Menuju L" + String(i + 1), i);
      animasiArah(i > posisiLantai);
      keLantai(i);
      posisiLantai = i;
      posisiLangkah = posisiLantai * langkahPerLantai;
      EEPROM.write(POSISI_ADDR, posisiLantai);
      EEPROM.commit();
      Serial.print("Step sekarang: ");
      Serial.println(posisiLangkah);
    }
  }
}

// ---------------------------------------------
void homingKeLantai1() {
  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);
}

// ---------------------------------------------
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: Stuck tengah!", -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);
}

// ---------------------------------------------
void prosesPintu() {
  digitalWrite(PIN_RELAY_PINTU, HIGH);         // Buka pintu
  digitalWrite(PIN_BUZZER, HIGH);              // Bunyi buka
  delay(100);
  digitalWrite(PIN_BUZZER, LOW);
  tampilkanStatus("Pintu Terbuka", posisiLantai);
  Serial.println("PINTU: BUKA");
  pintuSedangTerbuka = true;

  delay(15000);  // Biarkan pintu terbuka 15 detik

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

  delay(15000);  // Tambahan delay setelah menutup
}


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

// ---------------------------------------------
void satuLangkah(int delay_us) {
  digitalWrite(STEP_PIN, HIGH);
  delayMicroseconds(delay_us);
  digitalWrite(STEP_PIN, LOW);
  delayMicroseconds(delay_us);
}

// ---------------------------------------------
void tampilkanStatus(String status, int lantai) {
  display.fillScreen(ILI9341_BLACK);
  display.setRotation(0);
  display.setTextColor(ILI9341_WHITE);

  int centerX = display.width() / 2;
  int posY = 30;

  display.setTextSize(2);
  display.setCursor(centerX - 50, posY);
  display.println("Status :");
  display.setCursor(centerX - 50, posY + 25);
  display.println(status);

  if (lantai >= 0) {
    display.setTextSize(3);
    display.setCursor(centerX - 50, posY + 70);
    display.setTextColor(ILI9341_YELLOW);
    display.println("LANTAI");
    display.setTextSize(4);
    display.setCursor(centerX - 10, posY + 120);
    display.println(lantai + 1);
  }

  Serial.print("LCD: ");
  Serial.println(status);
}

// ---------------------------------------------
void animasiArah(bool naik) {
  display.setRotation(0);
  for (int y = 0; y < 5; y++) {
    display.fillRect(100, 220, 40, 40, ILI9341_BLACK);
    display.setTextColor(naik ? ILI9341_GREEN : ILI9341_RED);
    display.setTextSize(6);
    display.setCursor(100, 220);
    display.print(naik ? "^" : "v");
    delay(500);
    display.fillRect(100, 220, 40, 40, ILI9341_BLACK);
    delay(150);
  }
}

// ---------------------------------------------
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 STOP!", -1);
  while (1);
}