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

// Pin definitions for floor LEDs
const int floor4Pin = 19;
const int floor43MiddlePin = 18;
const int floor3Pin = 5;
const int floor32MiddlePin = 17;
const int floor2Pin = 16;
const int floor21MiddlePin = 2;
const int floor1Pin = 15;

// Pin definitions for relays and power sources
const int relayPin = 4;
const int gridPowerPin = 13;
const int batteryPowerPin = 12;

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

// Variables to track lift positions
int currentFloor = 2; // Assuming starting at floor 2
int nextFloor = 1;    // Next stop
bool moveToNearestFloorFlag = false;  // Flag to track one-time move to nearest floor when grid is low

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

  // Initialize OLED
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;);
  }
  display.clearDisplay();

  // Set pin modes for LEDs, relays, and power input
  pinMode(floor4Pin, OUTPUT);
  pinMode(floor43MiddlePin, OUTPUT);
  pinMode(floor3Pin, OUTPUT);
  pinMode(floor32MiddlePin, OUTPUT);
  pinMode(floor2Pin, OUTPUT);
  pinMode(floor21MiddlePin, OUTPUT);
  pinMode(floor1Pin, OUTPUT);
  pinMode(relayPin, OUTPUT);

  pinMode(gridPowerPin, INPUT);
  pinMode(batteryPowerPin, INPUT);

  // Initialize all LEDs to off
  turnOffAllFloorLeds();

  // Initial OLED display
  updateDisplay("Batt", currentFloor, nextFloor);
  Serial.println("ARD Elevator initialized");
}

void loop() {
  // Read power sources
  bool gridPower = digitalRead(gridPowerPin);
  bool batteryPower = digitalRead(batteryPowerPin);

  // Print power readings
  Serial.print("Grid Power: ");
  Serial.println(gridPower ? "HIGH" : "LOW");
  Serial.print("Battery Power: ");
  Serial.println(batteryPower ? "HIGH" : "LOW");

  // Check power status and control the relay
  if (!gridPower) {
    digitalWrite(relayPin, HIGH);  // Relay ON when grid is low
    Serial.println("Grid power is LOW, Relay ON.");

    // If battery is available and the lift has not yet moved to the nearest full floor, move it
    if (batteryPower && !moveToNearestFloorFlag) {
      moveToNearestFullFloor();
      moveToNearestFloorFlag = true;  // Ensure this happens only once while grid is LOW
    } else if (!batteryPower) {
      Serial.println("Both Grid and Battery are LOW. Lift stopped.");
      updateDisplay("OFF", currentFloor, nextFloor);
      return; // Stop the lift
    }
  } else {
    digitalWrite(relayPin, LOW); // Relay OFF when grid is available
    Serial.println("Grid power is HIGH, Relay OFF.");
    moveToNearestFloorFlag = false; // Reset flag when grid power is restored
    // Operate the lift normally
    moveLift();
  }

  // Update OLED display with current mode and floors
  updateDisplay(gridPower ? "Grid" : "Batt", currentFloor, nextFloor);

  delay(1000); // Delay for simulation
}

// Function to simulate lift movement
void moveLift() {
  Serial.print("Lift currently at floor: ");
  Serial.println(currentFloor);

  // Simulate the next floor with middle floors in between
  if (currentFloor < nextFloor) {
    simulateLiftMovementUp();
  } else if (currentFloor > nextFloor) {
    simulateLiftMovementDown();
  }

  Serial.print("Next stop: ");
  Serial.println(nextFloor);

  if (currentFloor == nextFloor) {
    // Change next stop logic: toggle between floor 1 and 4 for simplicity
    nextFloor = (nextFloor == 1) ? 4 : 1;
    Serial.print("Changing next stop to: ");
    Serial.println(nextFloor);
  }
}

// Function to simulate lift movement going up with middle floors
void simulateLiftMovementUp() {
  if (currentFloor == 1) {
    // Simulate moving from floor 1 to floor 2 via floor 21 middle
    digitalWrite(floor21MiddlePin, HIGH);
    delay(500);  // Delay to simulate the movement
    digitalWrite(floor21MiddlePin, LOW);
    currentFloor = 2;
    activateFloorLed(currentFloor);
  } else if (currentFloor == 2) {
    // Simulate moving from floor 2 to floor 3 via floor 32 middle
    digitalWrite(floor32MiddlePin, HIGH);
    delay(500);
    digitalWrite(floor32MiddlePin, LOW);
    currentFloor = 3;
    activateFloorLed(currentFloor);
  } else if (currentFloor == 3) {
    // Simulate moving from floor 3 to floor 4 via floor 43 middle
    digitalWrite(floor43MiddlePin, HIGH);
    delay(500);
    digitalWrite(floor43MiddlePin, LOW);
    currentFloor = 4;
    activateFloorLed(currentFloor);
  }
}

// Function to simulate lift movement going down with middle floors
void simulateLiftMovementDown() {
  if (currentFloor == 4) {
    // Simulate moving from floor 4 to floor 3 via floor 43 middle
    digitalWrite(floor43MiddlePin, HIGH);
    delay(500);
    digitalWrite(floor43MiddlePin, LOW);
    currentFloor = 3;
    activateFloorLed(currentFloor);
  } else if (currentFloor == 3) {
    // Simulate moving from floor 3 to floor 2 via floor 32 middle
    digitalWrite(floor32MiddlePin, HIGH);
    delay(500);
    digitalWrite(floor32MiddlePin, LOW);
    currentFloor = 2;
    activateFloorLed(currentFloor);
  } else if (currentFloor == 2) {
    // Simulate moving from floor 2 to floor 1 via floor 21 middle
    digitalWrite(floor21MiddlePin, HIGH);
    delay(500);
    digitalWrite(floor21MiddlePin, LOW);
    currentFloor = 1;
    activateFloorLed(currentFloor);
  }
}

// Function to move lift to the nearest full floor (not middle) when grid is LOW and battery is HIGH
void moveToNearestFullFloor() {
  Serial.println("Battery power available, moving to nearest full floor...");

  // If current floor is a middle floor, move to the nearest full floor
  if (currentFloor == 2 || currentFloor == 3) {
    if (abs(currentFloor - 1) < abs(currentFloor - 4)) {
      nextFloor = 1;
    } else {
      nextFloor = 4;
    }
  } else {
    Serial.println("Lift already at a full floor.");
  }

  // Move lift to nearest full floor
  moveLift();
}

// Function to turn off all floor LEDs
void turnOffAllFloorLeds() {
  digitalWrite(floor4Pin, LOW);
  digitalWrite(floor43MiddlePin, LOW);
  digitalWrite(floor3Pin, LOW);
  digitalWrite(floor32MiddlePin, LOW);
  digitalWrite(floor2Pin, LOW);
  digitalWrite(floor21MiddlePin, LOW);
  digitalWrite(floor1Pin, LOW);
}

// Function to activate the corresponding floor LED
void activateFloorLed(int floor) {
  turnOffAllFloorLeds(); // Turn off all LEDs before lighting the current floor

  switch (floor) {
    case 4:
      digitalWrite(floor4Pin, HIGH);
      Serial.println("Floor 4 LED ON");
      break;
    case 3:
      digitalWrite(floor3Pin, HIGH);  // Only floor 3 LED ON, not middle floor LED
      Serial.println("Floor 3 LED ON");
      break;
    case 2:
      digitalWrite(floor2Pin, HIGH);  // Only floor 2 LED ON, not middle floor LED
      Serial.println("Floor 2 LED ON");
      break;
    case 1:
      digitalWrite(floor1Pin, HIGH);
      Serial.println("Floor 1 LED ON");
      break;
  }
}

// Function to update OLED display with mode and floors
void updateDisplay(String mode, int currentFloor, int nextFloor) {
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);

  // Title
  display.setCursor(0, 0);
  display.println("ARD Elevator");

  // Mode display
  display.setCursor(0, 10);
  display.print("Mode: ");
  display.println(mode);

  // Current floor display
  display.setCursor(0, 20);
  display.print("Current Floor: ");
  display.println(currentFloor);

  // Next floor display
  display.setCursor(0, 30);
  display.print("Next Floor: ");
  display.println(nextFloor);

  display.display(); // Update OLED display
}