#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// Tombol Call
const int pbCallUpL1    = 23;
const int pbCallUpL2    = 18;
const int pbCallDownL2  = 5;
const int pbCallDownL3  = 19;

// LED Indikator
const int ledPins[4] = {25, 26, 27, 14};  // LED indeks 0–3
bool ledState[4] = {false, false, false, false};

// Limit Switch
const int lsL1 = 32;
const int lsL2 = 33;
const int lsL3 = 13;

// Arah dari kontrol utama (revisi)
const int arahNaikPin   = 16;
const int arahTurunPin  = 17;

// Debounce
unsigned long lastDebounce[4] = {0, 0, 0, 0};
const unsigned long debounceDelay = 50;

unsigned long lastBlink = 0;
bool blinkState = true;
int currentFloor = -1;

void setup() {
  // Tombol call
  pinMode(pbCallUpL1, INPUT_PULLUP);
  pinMode(pbCallUpL2, INPUT_PULLUP);
  pinMode(pbCallDownL2, INPUT_PULLUP);
  pinMode(pbCallDownL3, INPUT_PULLUP);

  // LED
  for (int i = 0; i < 4; i++) {
    pinMode(ledPins[i], OUTPUT);
    digitalWrite(ledPins[i], LOW);
  }

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

  // OLED init
  Wire.begin(21, 22);
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(20, 25);
  display.print("Starting...");
  display.display();
  delay(1000);
}

void loop() {
  // Update lantai
  if (!digitalRead(lsL1)) currentFloor = 1;
  else if (!digitalRead(lsL2)) currentFloor = 2;
  else if (!digitalRead(lsL3)) currentFloor = 3;

  // Tombol call (dengan debounce)
  checkButton(pbCallUpL1, 0);
  checkButton(pbCallUpL2, 1);
  checkButton(pbCallDownL2, 2);
  checkButton(pbCallDownL3, 3);

  // Reset LED saat sampai lantai
  if (currentFloor == 1) setLED(0, false);
  if (currentFloor == 2) {
    setLED(1, false);
    setLED(2, false);
  }
  if (currentFloor == 3) setLED(3, false);

  // Arah naik/turun
  bool arahNaik = !digitalRead(arahNaikPin);
  bool arahTurun = !digitalRead(arahTurunPin);

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

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

  if (blinkState) {
    if (arahNaik && !arahTurun) drawUpArrow(100, 15);
    else if (arahTurun && !arahNaik) drawDownArrow(100, 15);
  }

  display.display();
}

// Tombol dengan debounce
void checkButton(int pin, int index) {
  if (!digitalRead(pin)) {
    if (millis() - lastDebounce[index] > debounceDelay) {
      setLED(index, true);
      lastDebounce[index] = millis();
    }
  }
}

// LED
void setLED(int index, bool state) {
  if (ledState[index] != state) {
    ledState[index] = state;
    digitalWrite(ledPins[index], state ? HIGH : LOW);
  }
}

// Label lantai
const char* getFloorLabel(int floor) {
  if (floor == 1) return "GF";
  if (floor == 2) return "1F";
  if (floor == 3) return "2F";
  return "";
}

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

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