#include <EEPROM.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <avr/wdt.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// Tombol Call
const int pbCallUpL1 = 5;
const int pbCallUpL2 = 6;
const int pbCallDownL2 = 7;
const int pbCallDownL3 = 8;

// LED Indikator
const int ledUpL1 = 9;
const int ledUpL2 = 10;
const int ledDownL2 = 11;
const int ledDownL3 = 12;

// Limit Switch
const int lsL1 = A0;
const int lsL2 = A1;
const int lsL3 = A2;

// Arah dari kontrol utama
const int arahNaikPin = 2;
const int arahTurunPin = 3;

bool ledState[4] = {false, false, false, false}; // 0-UpL1, 1-UpL2, 2-DownL2, 3-DownL3
unsigned long lastBlink = 0;
bool blinkState = true;
int currentFloor = -1;

void setup() {
  // Tombol dan LED
  for (int i = 5; i <= 8; i++) pinMode(i, INPUT_PULLUP);
  for (int i = 9; i <= 12; i++) pinMode(i, OUTPUT);

  // Limit dan Arah
  pinMode(lsL1, INPUT_PULLUP);
  pinMode(lsL2, INPUT_PULLUP);
  pinMode(lsL3, INPUT_PULLUP);
  pinMode(arahNaikPin, INPUT_PULLUP);
  pinMode(arahTurunPin, INPUT_PULLUP);

  // Load status LED dari EEPROM
  for (int i = 0; i < 4; i++) {
    ledState[i] = EEPROM.read(i) == 1;
    digitalWrite(9 + i, ledState[i] ? HIGH : LOW);
  }

  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();

  wdt_enable(WDTO_2S); // Watchdog 2 detik
}

void loop() {
  wdt_reset();

  // Baca posisi lantai
  if (!digitalRead(lsL1)) currentFloor = 1;
  else if (!digitalRead(lsL2)) currentFloor = 2;
  else if (!digitalRead(lsL3)) currentFloor = 3;

  // Tombol call -> Self-hold
  if (!digitalRead(pbCallUpL1)) setLED(0, true);
  if (!digitalRead(pbCallUpL2)) setLED(1, true);
  if (!digitalRead(pbCallDownL2)) setLED(2, true);
  if (!digitalRead(pbCallDownL3)) setLED(3, true);

  // Matikan LED jika lift sampai
  if (currentFloor == 1) setLED(0, false);
  if (currentFloor == 2) {
    setLED(1, false);
    setLED(2, false);
  }
  if (currentFloor == 3) setLED(3, false);

  // Arah dari input
  bool arahNaik = !digitalRead(arahNaikPin);
  bool arahTurun = !digitalRead(arahTurunPin);

  // Blinking panah
  if (millis() - lastBlink > 100) {
    blinkState = !blinkState;
    lastBlink = millis();
  }

  // OLED Display
  display.clearDisplay();
  display.setTextSize(5);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(10, 15);  // Tengah layar
  display.print(getFloorLabel(currentFloor));

  // Panah arah berkedip
  if (blinkState) {
    if (arahNaik && !arahTurun) drawUpArrow(100, 15);
    if (arahTurun && !arahNaik) drawDownArrow(100, 15);
  }

  display.display();
}

String getFloorLabel(int floor) {
  if (floor == 1) return "GF";
  if (floor == 2) return "1F";
  if (floor == 3) return "2F";
  return "";
}

void drawUpArrow(int x, int y) {
  display.fillTriangle(x, y + 20, x + 10, y, x + 20, y + 20, SSD1306_WHITE);
}

void drawDownArrow(int x, int y) {
  display.fillTriangle(x, y, x + 10, y + 20, x + 20, y, SSD1306_WHITE);
}

void setLED(int index, bool state) {
  if (ledState[index] != state) {
    ledState[index] = state;
    digitalWrite(9 + index, state ? HIGH : LOW);
    EEPROM.write(index, state ? 1 : 0);
  }
}