#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>

const int stepPin = D2;
const int dirPin = D3;

const int floor1Btn = D4;
const int floor2Btn = D5;
const int floor3Btn = D6;
const int floor4Btn = D7;

const int servoPin = D8;
const int buzzerPin = D9;

const int floor1Led = D10;
const int floor2Led = D11;
const int floor3Led = D12;
const int floor4Led = D13;

const int emergencyButton = A0;

const int stepsPerFloor = 200;

long currentPosition = 0;

bool isMoving = false;
bool emergencyTriggered = false;

int currentTarget = 0;

int requestQueue[10];
int queueSize = 0;

bool lastButtonState[4] = {
  HIGH, HIGH, HIGH, HIGH
};

LiquidCrystal_I2C lcd(0x27, 16, 2);
Servo doorServo;

int buttonPins[4] = {
  floor1Btn,
  floor2Btn,
  floor3Btn,
  floor4Btn
};

int getCurrentFloor() {
  if (currentPosition == 0) return 1;
  if (currentPosition == 200) return 2;
  if (currentPosition == 400) return 3;
  if (currentPosition == 600) return 4;

  return 0;
}

void updateFloorLeds() {
  digitalWrite(floor1Led, LOW);
  digitalWrite(floor2Led, LOW);
  digitalWrite(floor3Led, LOW);
  digitalWrite(floor4Led, LOW);

  int floor = getCurrentFloor();

  if (floor == 1) digitalWrite(floor1Led, HIGH);
  if (floor == 2) digitalWrite(floor2Led, HIGH);
  if (floor == 3) digitalWrite(floor3Led, HIGH);
  if (floor == 4) digitalWrite(floor4Led, HIGH);
}

bool floorAlreadyQueued(int floor) {
  if (floor == currentTarget) {
    return true;
  }

  for (int i = 0; i < queueSize; i++) {
    if (requestQueue[i] == floor) {
      return true;
    }
  }

  return false;
}

void addFloorRequest(int floor) {
  if (!isMoving && getCurrentFloor() == floor) {
    return;
  }

  if (floorAlreadyQueued(floor)) {
    return;
  }

  if (queueSize < 10) {
    requestQueue[queueSize] = floor;
    queueSize++;

    Serial.print("Floor ");
    Serial.print(floor);
    Serial.println(" added to queue");
  }
}

void scanFloorButtons() {
  for (int i = 0; i < 4; i++) {
    bool state = digitalRead(buttonPins[i]);

    if (state == LOW && lastButtonState[i] == HIGH) {
      addFloorRequest(i + 1);
    }

    lastButtonState[i] = state;
  }
}

void clearQueue() {
  queueSize = 0;
  currentTarget = 0;
}

int getNextFloor() {
  if (queueSize == 0) {
    return 0;
  }

  int floor = requestQueue[0];

  for (int i = 0; i < queueSize - 1; i++) {
    requestQueue[i] = requestQueue[i + 1];
  }

  queueSize--;

  return floor;
}

void checkEmergency() {
  if (digitalRead(emergencyButton) == LOW) {
    emergencyTriggered = true;
  }
}

void smartDelay(unsigned long duration) {
  unsigned long start = millis();

  while (millis() - start < duration) {
    scanFloorButtons();
    checkEmergency();

    if (emergencyTriggered) {
      return;
    }

    delay(5);
  }
}

void showReady() {
  lcd.clear();

  int floor = getCurrentFloor();

  if (floor != 0) {
    lcd.setCursor(0, 0);
    lcd.print("Ready");

    lcd.setCursor(0, 1);
    lcd.print("Floor ");
    lcd.print(floor);
  } else {
    lcd.setCursor(0, 0);
    lcd.print("Stopped");

    lcd.setCursor(0, 1);
    lcd.print("Between Floors");
  }
}

void normalDoorOpen() {
  tone(buzzerPin, 1200, 250);
  delay(300);

  doorServo.write(90);

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Door Open");

  lcd.setCursor(0, 1);
  lcd.print("Floor ");
  lcd.print(getCurrentFloor());

  smartDelay(3000);

  doorServo.write(0);

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Door Closed");

  lcd.setCursor(0, 1);
  lcd.print("Floor ");
  lcd.print(getCurrentFloor());

  smartDelay(700);
}

void handleEmergency() {
  isMoving = false;

  clearQueue();

  Serial.println("!!! EMERGENCY !!!");
  Serial.println("Motor stopped immediately");
  Serial.println("Queue cleared");
  Serial.println("Door opened");

  digitalWrite(floor1Led, LOW);
  digitalWrite(floor2Led, LOW);
  digitalWrite(floor3Led, LOW);
  digitalWrite(floor4Led, LOW);

  tone(buzzerPin, 500);

  doorServo.write(90);

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("EMERGENCY STOP");

  lcd.setCursor(0, 1);
  lcd.print("DOOR OPEN");

  while (digitalRead(emergencyButton) == LOW) {
    delay(10);
  }

  delay(3000);

  noTone(buzzerPin);

  doorServo.write(0);

  emergencyTriggered = false;

  updateFloorLeds();

  showReady();

  Serial.println("Emergency released");
  Serial.println("Select a new floor");
}

bool moveToFloor(int targetFloor) {
  long targetPosition =
      (targetFloor - 1) * stepsPerFloor;

  long difference =
      targetPosition - currentPosition;

  if (difference == 0) {
    return true;
  }

  bool direction =
      difference > 0 ? HIGH : LOW;

  long totalSteps = abs(difference);

  digitalWrite(dirPin, direction);

  Serial.print("Moving ");

  if (direction == HIGH) {
    Serial.print("UP ");
  } else {
    Serial.print("DOWN ");
  }

  Serial.print(totalSteps);
  Serial.println(" steps...");

  for (long i = 0; i < totalSteps; i++) {
    scanFloorButtons();
    checkEmergency();

    if (emergencyTriggered) {
      Serial.print("Emergency at step ");
      Serial.print(i);
      Serial.print(" / ");
      Serial.println(totalSteps);

      return false;
    }

    digitalWrite(stepPin, HIGH);
    delayMicroseconds(800);

    digitalWrite(stepPin, LOW);
    delayMicroseconds(800);

    if (direction == HIGH) {
      currentPosition++;
    } else {
      currentPosition--;
    }
  }

  Serial.println("Movement finished");

  return true;
}

void goToFloor(int targetFloor) {
  currentTarget = targetFloor;
  isMoving = true;

  int currentFloor = getCurrentFloor();

  lcd.clear();

  if (currentFloor != 0 &&
      targetFloor > currentFloor) {

    lcd.setCursor(0, 0);
    lcd.print("Going UP");

  } else if (currentFloor != 0 &&
             targetFloor < currentFloor) {

    lcd.setCursor(0, 0);
    lcd.print("Going DOWN");

  } else {

    long targetPosition =
        (targetFloor - 1) * stepsPerFloor;

    if (targetPosition > currentPosition) {
      lcd.setCursor(0, 0);
      lcd.print("Going UP");
    } else {
      lcd.setCursor(0, 0);
      lcd.print("Going DOWN");
    }
  }

  lcd.setCursor(0, 1);
  lcd.print("To Floor ");
  lcd.print(targetFloor);

  bool completed =
      moveToFloor(targetFloor);

  isMoving = false;
  currentTarget = 0;

  if (!completed) {
    handleEmergency();
    return;
  }

  updateFloorLeds();

  Serial.print("Arrived at Floor ");
  Serial.println(targetFloor);

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Arrived");

  lcd.setCursor(0, 1);
  lcd.print("Floor ");
  lcd.print(targetFloor);

  smartDelay(700);

  if (emergencyTriggered) {
    handleEmergency();
    return;
  }

  normalDoorOpen();

  if (emergencyTriggered) {
    handleEmergency();
    return;
  }

  showReady();
}

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

  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);

  pinMode(floor1Btn, INPUT_PULLUP);
  pinMode(floor2Btn, INPUT_PULLUP);
  pinMode(floor3Btn, INPUT_PULLUP);
  pinMode(floor4Btn, INPUT_PULLUP);

  pinMode(emergencyButton, INPUT_PULLUP);

  pinMode(buzzerPin, OUTPUT);

  pinMode(floor1Led, OUTPUT);
  pinMode(floor2Led, OUTPUT);
  pinMode(floor3Led, OUTPUT);
  pinMode(floor4Led, OUTPUT);

  doorServo.attach(servoPin);
  doorServo.write(0);

  lcd.init();
  lcd.backlight();

  updateFloorLeds();

  lcd.setCursor(0, 0);
  lcd.print("Elevator Ready");

  lcd.setCursor(0, 1);
  lcd.print("Floor 1");

  Serial.println("Elevator ready");
  Serial.println("Starting at Floor 1");
}

void loop() {
  scanFloorButtons();

  checkEmergency();

  if (emergencyTriggered) {
    handleEmergency();
    return;
  }

  if (!isMoving && queueSize > 0) {
    int nextFloor = getNextFloor();

    goToFloor(nextFloor);
  }

  delay(5);
}