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

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

const byte FLOORS = 6;
const byte ELEVATORS = 2;
const byte floorPins[FLOORS] = {2, 3, 4, 5, 6, 7};
const byte EMERGENCY_PIN = 8;
const byte LOAD_PIN = 9;
const byte FAULT_PIN = 10;
const byte NIGHT_PIN = 11;
const byte VIP_PIN = 12;

const byte BUZZER_PIN = A0;
const byte RUN_LED = 13;
const byte DOOR_LED = A1;
const byte ALARM_LED = A2;

enum ElevatorState {
  IDLE,
  MOVING,
  DOOR_OPEN,
  FAULTED
};

struct Button {
  byte pin;
  bool stableState;
  bool lastReading;
  unsigned long changedAt;
};

struct Elevator {
  byte floor;
  byte target;
  int direction;
  ElevatorState state;
  bool queue[FLOORS];
  bool overloaded;
  bool fault;
  unsigned long lastStepAt;
  unsigned long doorUntil;
  unsigned int servedCount;
};

Button floorButtons[FLOORS];
Button emergencyButton;
Button loadButton;
Button faultButton;
Button nightButton;
Button vipButton;

Elevator cars[ELEVATORS];

bool pendingCalls[FLOORS];
bool emergencyMode = false;
bool nightMode = false;
bool powerSave = false;
bool oledDimmed = false;
String lastEvent = "System ready";

unsigned long lastInputAt = 0;
unsigned long lastDisplayAt = 0;
unsigned long lastBlinkAt = 0;
unsigned long lastDoorBeepAt = 0;
bool alarmBlink = false;

void initButton(Button &button, byte pin) {
  button.pin = pin;
  pinMode(pin, INPUT_PULLUP);
  button.stableState = digitalRead(pin);
  button.lastReading = button.stableState;
  button.changedAt = millis();
}

bool pressed(Button &button) {
  bool reading = digitalRead(button.pin);
  unsigned long now = millis();

  if (reading != button.lastReading) {
    button.changedAt = now;
    button.lastReading = reading;
  }

  if ((now - button.changedAt) > 35 && reading != button.stableState) {
    button.stableState = reading;
    if (button.stableState == LOW) {
      lastInputAt = now;
      powerSave = false;
      return true;
    }
  }

  return false;
}

void clearQueue(Elevator &car) {
  for (byte floor = 0; floor < FLOORS; floor++) {
    car.queue[floor] = false;
  }
}

void openDoor(Elevator &car, unsigned long now) {
  car.state = DOOR_OPEN;
  car.direction = 0;
  car.doorUntil = now + (nightMode ? 2200 : 1600);
  car.queue[car.floor] = false;
  car.servedCount++;
}

bool hasQueuedStop(const Elevator &car) {
  for (byte floor = 0; floor < FLOORS; floor++) {
    if (car.queue[floor]) {
      return true;
    }
  }
  return false;
}

int selectNearestStop(const Elevator &car) {
  int bestFloor = -1;
  int bestDistance = 99;

  for (byte floor = 0; floor < FLOORS; floor++) {
    if (!car.queue[floor]) {
      continue;
    }

    int distance = abs((int)floor - (int)car.floor);
    if (distance < bestDistance) {
      bestDistance = distance;
      bestFloor = floor;
    }
  }

  return bestFloor;
}

int dispatchScore(const Elevator &car, byte floor, bool priority) {
  if (car.fault || car.overloaded || car.state == FAULTED) {
    return 999;
  }

  int score = abs((int)car.floor - (int)floor) * 10;

  if (car.state == IDLE) {
    score -= 8;
  }
  if (car.direction != 0) {
    bool sameDirection = ((int)floor - (int)car.floor) * car.direction >= 0;
    score += sameDirection ? 0 : 15;
  }
  if (priority) {
    score -= 5;
  }

  return score;
}

int chooseElevator(byte floor, bool priority) {
  int bestCar = -1;
  int bestScore = 999;

  for (byte i = 0; i < ELEVATORS; i++) {
    int score = dispatchScore(cars[i], floor, priority);
    if (score < bestScore) {
      bestScore = score;
      bestCar = i;
    }
  }

  return bestCar;
}

void addCall(byte floor, bool priority) {
  int carIndex = chooseElevator(floor, priority);

  if (carIndex >= 0 && carIndex < ELEVATORS) {
    cars[carIndex].queue[floor] = true;
    lastEvent = priority ? "VIP to F6" : "Call F" + String(floor + 1);
  } else {
    pendingCalls[floor] = true;
    lastEvent = "Call queued";
  }
}

void redistributePendingCalls() {
  for (byte floor = 0; floor < FLOORS; floor++) {
    if (!pendingCalls[floor]) {
      continue;
    }

    int carIndex = chooseElevator(floor, false);
    if (carIndex >= 0 && carIndex < ELEVATORS) {
      cars[carIndex].queue[floor] = true;
      pendingCalls[floor] = false;
    }
  }
}

void handleInputs() {
  for (byte floor = 0; floor < FLOORS; floor++) {
    if (pressed(floorButtons[floor])) {
      addCall(floor, false);
    }
  }

  if (pressed(vipButton)) {
    addCall(FLOORS - 1, true);
  }

  if (pressed(emergencyButton)) {
    emergencyMode = !emergencyMode;
    for (byte i = 0; i < ELEVATORS; i++) {
      clearQueue(cars[i]);
      cars[i].queue[0] = emergencyMode;
      if (cars[i].state != FAULTED && !cars[i].fault) {
        cars[i].state = IDLE;
      }
    }
    lastEvent = emergencyMode ? "Emergency return" : "Emergency clear";
  }

  if (pressed(loadButton)) {
    cars[0].overloaded = !cars[0].overloaded;
    if (cars[0].overloaded) {
      cars[0].state = DOOR_OPEN;
      cars[0].doorUntil = millis() + 60000;
      clearQueue(cars[0]);
      lastEvent = "A overload";
    } else {
      cars[0].state = IDLE;
      lastEvent = "A load normal";
    }
  }

  if (pressed(faultButton)) {
    cars[1].fault = !cars[1].fault;
    if (cars[1].fault) {
      cars[1].state = FAULTED;
      clearQueue(cars[1]);
      lastEvent = "B fault";
    } else {
      cars[1].state = IDLE;
      lastEvent = "B repaired";
    }
  }

  if (pressed(nightButton)) {
    nightMode = !nightMode;
    lastEvent = nightMode ? "Night mode" : "Day mode";
  }
}

void updateElevator(Elevator &car, unsigned long now) {
  if (car.fault) {
    car.state = FAULTED;
    car.direction = 0;
    return;
  }

  if (car.overloaded) {
    car.state = DOOR_OPEN;
    car.direction = 0;
    car.doorUntil = now + 60000;
    return;
  }

  if (car.state == DOOR_OPEN) {
    if (now >= car.doorUntil) {
      car.state = IDLE;
      car.direction = 0;
    }
    return;
  }

  if (emergencyMode) {
    clearQueue(car);
    car.queue[0] = true;
  }

  if (car.state == IDLE) {
    int nextStop = selectNearestStop(car);
    if (nextStop < 0) {
      car.direction = 0;
      return;
    }

    car.target = nextStop;
    if (car.target == car.floor) {
      openDoor(car, now);
      return;
    }

    car.direction = car.target > car.floor ? 1 : -1;
    car.state = MOVING;
    car.lastStepAt = now;
  }

  unsigned long interval = emergencyMode ? 520 : (nightMode ? 1150 : 850);
  if (car.state == MOVING && now - car.lastStepAt >= interval) {
    car.lastStepAt = now;

    if (car.direction > 0 && car.floor < FLOORS - 1) {
      car.floor++;
    } else if (car.direction < 0 && car.floor > 0) {
      car.floor--;
    }

    if (car.queue[car.floor]) {
      openDoor(car, now);
      lastEvent = "Stop F" + String(car.floor + 1);
      return;
    }

    if (car.floor == car.target || !hasQueuedStop(car)) {
      car.state = IDLE;
      car.direction = 0;
    }
  }
}

void updatePowerSave(unsigned long now) {
  bool active = emergencyMode;

  for (byte i = 0; i < ELEVATORS; i++) {
    active = active || cars[i].state == MOVING || cars[i].state == DOOR_OPEN;
    active = active || hasQueuedStop(cars[i]);
  }

  if (!active && now - lastInputAt > 15000) {
    powerSave = true;
  }
}

void updateOutputs(unsigned long now) {
  bool moving = false;
  bool doorOpen = false;
  bool alarm = emergencyMode;

  for (byte i = 0; i < ELEVATORS; i++) {
    moving = moving || cars[i].state == MOVING;
    doorOpen = doorOpen || cars[i].state == DOOR_OPEN;
    alarm = alarm || cars[i].fault || cars[i].overloaded;
  }

  digitalWrite(RUN_LED, moving ? HIGH : LOW);
  digitalWrite(DOOR_LED, doorOpen ? HIGH : LOW);
  digitalWrite(ALARM_LED, alarm ? HIGH : LOW);

  if (alarm) {
    if (now - lastBlinkAt > 220) {
      lastBlinkAt = now;
      alarmBlink = !alarmBlink;
      if (alarmBlink) {
        tone(BUZZER_PIN, emergencyMode ? 1600 : 1200);
      } else {
        noTone(BUZZER_PIN);
      }
    }
  } else if (doorOpen && now - lastDoorBeepAt > 1000) {
    lastDoorBeepAt = now;
    tone(BUZZER_PIN, 880, 60);
  } else if (!doorOpen) {
    noTone(BUZZER_PIN);
  }
}

void drawShaft(byte carIndex, int x) {
  display.drawRect(x, 12, 22, 42, SSD1306_WHITE);

  for (byte floor = 0; floor < FLOORS; floor++) {
    int y = 47 - floor * 7;
    display.drawFastHLine(x, y, 22, SSD1306_WHITE);
    display.setCursor(x - 12, y - 5);
    display.print(floor + 1);
  }

  Elevator &car = cars[carIndex];
  int carY = 47 - car.floor * 7 - 5;
  display.fillRect(x + 4, carY, 14, 6, SSD1306_WHITE);

  display.setTextColor(SSD1306_BLACK, SSD1306_WHITE);
  display.setCursor(x + 7, carY);
  display.print(carIndex == 0 ? "A" : "B");
  display.setTextColor(SSD1306_WHITE);

  for (byte floor = 0; floor < FLOORS; floor++) {
    if (car.queue[floor]) {
      int y = 44 - floor * 7;
      display.fillCircle(x + 28, y, 2, SSD1306_WHITE);
    }
  }
}

String stateText(const Elevator &car) {
  if (car.fault) return "FLT";
  if (car.overloaded) return "OVL";
  if (car.state == MOVING) return car.direction > 0 ? "UP" : "DN";
  if (car.state == DOOR_OPEN) return "OPEN";
  return "IDLE";
}

void renderDisplay(unsigned long now) {
  if (now - lastDisplayAt < 120) {
    return;
  }
  lastDisplayAt = now;

  bool shouldDim = nightMode || powerSave;
  if (shouldDim != oledDimmed) {
    oledDimmed = shouldDim;
    display.dim(oledDimmed);
  }

  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);

  display.setCursor(0, 0);
  if (emergencyMode) {
    display.print("EMERGENCY");
  } else if (powerSave) {
    display.print("ENERGY SAVE");
  } else if (nightMode) {
    display.print("NIGHT MODE");
  } else {
    display.print("SMART ELEVATOR");
  }

  display.setCursor(101, 0);
  display.print("F");
  display.print(cars[0].floor + 1);
  display.print("/");
  display.print(cars[1].floor + 1);

  drawShaft(0, 26);
  drawShaft(1, 78);

  display.setCursor(0, 56);
  display.print("A:");
  display.print(stateText(cars[0]));
  display.print(" B:");
  display.print(stateText(cars[1]));

  display.setCursor(78, 56);
  display.print(lastEvent.substring(0, 8));

  display.display();
}

void setup() {
  for (byte floor = 0; floor < FLOORS; floor++) {
    initButton(floorButtons[floor], floorPins[floor]);
    pendingCalls[floor] = false;
  }

  initButton(emergencyButton, EMERGENCY_PIN);
  initButton(loadButton, LOAD_PIN);
  initButton(faultButton, FAULT_PIN);
  initButton(nightButton, NIGHT_PIN);
  initButton(vipButton, VIP_PIN);

  pinMode(BUZZER_PIN, OUTPUT);
  pinMode(RUN_LED, OUTPUT);
  pinMode(DOOR_LED, OUTPUT);
  pinMode(ALARM_LED, OUTPUT);

  for (byte i = 0; i < ELEVATORS; i++) {
    cars[i].floor = i == 0 ? 0 : 5;
    cars[i].target = cars[i].floor;
    cars[i].direction = 0;
    cars[i].state = IDLE;
    cars[i].overloaded = false;
    cars[i].fault = false;
    cars[i].lastStepAt = 0;
    cars[i].doorUntil = 0;
    cars[i].servedCount = 0;
    clearQueue(cars[i]);
  }

  lastInputAt = millis();

  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(13, 24);
  display.print("Smart Elevator");
  display.setCursor(24, 36);
  display.print("Wokwi Demo");
  display.display();
  delay(900);
}

void loop() {
  unsigned long now = millis();

  handleInputs();
  redistributePendingCalls();

  for (byte i = 0; i < ELEVATORS; i++) {
    updateElevator(cars[i], now);
  }

  updatePowerSave(now);
  updateOutputs(now);
  renderDisplay(now);
}