#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define BTN_L1 32
#define BTN_L2 33
#define BTN_L3 25
#define BTN_DOOR_OPEN 26
#define BTN_DOOR_CLOSE 27
#define BTN_CABIN_LIGHT 14
#define BTN_EMERGENCY 15

#define LED_L1 4
#define LED_L2 5
#define LED_L3 18

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

HardwareSerial COPSerial(1); // UART1

String currentFloor = "L1";
String currentDir = "STOP"; // "UP", "DOWN", "STOP"

unsigned long lastPress[40] = {0};

void setup() {
  Serial.begin(115200);
  COPSerial.begin(9600, SERIAL_8N1, 21, 17); // RX=21, TX=17

  pinMode(BTN_L1, INPUT_PULLUP);
  pinMode(BTN_L2, INPUT_PULLUP);
  pinMode(BTN_L3, INPUT_PULLUP);
  pinMode(BTN_DOOR_OPEN, INPUT_PULLUP);
  pinMode(BTN_DOOR_CLOSE, INPUT_PULLUP);
  pinMode(BTN_CABIN_LIGHT, INPUT_PULLUP);
  pinMode(BTN_EMERGENCY, INPUT_PULLUP);

  pinMode(LED_L1, OUTPUT);
  pinMode(LED_L2, OUTPUT);
  pinMode(LED_L3, OUTPUT);

  Wire.begin(21, 22); // SDA, SCL
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println("Gagal inisialisasi OLED");
    while (true);
  }
  display.clearDisplay();
  updateOLED();
}

void loop() {
  checkButton(BTN_L1, "CALL:L1");
  checkButton(BTN_L2, "CALL:L2");
  checkButton(BTN_L3, "CALL:L3");

  checkButton(BTN_DOOR_OPEN, "CMD:DOOR_OPEN");
  checkButton(BTN_DOOR_CLOSE, "CMD:DOOR_CLOSE");
  checkButton(BTN_CABIN_LIGHT, "CMD:LIGHT");
  checkButton(BTN_EMERGENCY, "CMD:EMERGENCY");

  receiveCommand();
}

void checkButton(uint8_t pin, String message) {
  uint8_t idx = pin;
  if (digitalRead(pin) == LOW && millis() - lastPress[idx] > 300) {
    COPSerial.println(message);
    Serial.println("Kirim: " + message);
    lastPress[idx] = millis();
  }
}

void receiveCommand() {
  if (COPSerial.available()) {
    String cmd = COPSerial.readStringUntil('\n');
    cmd.trim();

    if (cmd == "LED:L1_ON") digitalWrite(LED_L1, HIGH);
    else if (cmd == "LED:L1_OFF") digitalWrite(LED_L1, LOW);
    else if (cmd == "LED:L2_ON") digitalWrite(LED_L2, HIGH);
    else if (cmd == "LED:L2_OFF") digitalWrite(LED_L2, LOW);
    else if (cmd == "LED:L3_ON") digitalWrite(LED_L3, HIGH);
    else if (cmd == "LED:L3_OFF") digitalWrite(LED_L3, LOW);
    else if (cmd.startsWith("FLOOR:")) {
      currentFloor = cmd.substring(6);
      updateOLED();
    }
    else if (cmd.startsWith("DIR:")) {
      currentDir = cmd.substring(4);
      updateOLED();
    }
  }
}

String formatFloor(String rawFloor) {
  if (rawFloor == "L1") return "GF";
  else if (rawFloor == "L2") return "1F";
  else if (rawFloor == "L3") return "2F";
  else return rawFloor;
}

void drawArrow(String dir) {
  int cx = 110;
  int cy = 10;

  if (dir == "UP") {
    display.fillTriangle(cx, cy, cx - 5, cy + 10, cx + 5, cy + 10, SSD1306_WHITE);
  } else if (dir == "DOWN") {
    display.fillTriangle(cx, cy + 10, cx - 5, cy, cx + 5, cy, SSD1306_WHITE);
  }
}

void updateOLED() {
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);

  display.setCursor(0, 0);
  display.print("Lantai: ");
  display.println(formatFloor(currentFloor));

  drawArrow(currentDir);
  display.display();
}