#include <Arduino.h>

// Mock DYPlayer class definition
namespace DY {
  class Player {
    public:
      Player(HardwareSerial* serial) {
        // Constructor
      }
      
      bool begin() {
        Serial.println("Mock DYPlayer initialized.");
        return true; // Simulate successful initialization
      }
      
      void setVolume(uint8_t volume) {
        Serial.print("Mock DYPlayer volume set to ");
        Serial.println(volume);
      }
      
      void playSpecified(uint16_t trackNumber) {
        Serial.print("Mock DYPlayer playing track ");
        Serial.println(trackNumber);
      }
  };
}

// Pin definitions for ESP32-S3

// Inputs
const int FLOOR_BUTTONS[4] = {3, 2, 1, 42}; // GPIO3, GPIO2, GPIO1, GPIO42 (Floor B, 1, 2, 3)
const int CLOSE_BUTTON = 4;                 // GPIO4
const int OPEN_BUTTON = 5;                  // GPIO5

const int GHOST_BUTTONS[3] = {6, 7, 8};     // GPIO6 - GPIO8

const int DOOR_CLOSED_SWITCH = 9;           // GPIO9
const int EXIT_DOOR_CLOSED_SWITCH = 10;     // GPIO10

// Outputs
const int FLOOR_LIGHTS[4] = {14, 13, 12, 11};  // GPIO14, GPIO13, GPIO12, GPIO11 (Floor B, 1, 2, 3)
const int GHOST_LIGHTS[4] = {15, 36, 37, 18};  // GPIO15, GPIO36, GPIO37, GPIO18

const int SMALL_VIBRATION_MOTOR = 21;          // GPIO21
const int BIG_VIBRATION_MOTOR = 41;            // GPIO41
const int ELEVATOR_CEILING_LIGHT = 35;         // GPIO35

// Serial Communication (ensure these pins support UART)
const int RX_PIN = 16; // GPIO16
const int TX_PIN = 17; // GPIO17

// Timing constants
const unsigned long GHOST_LIGHT_ON_DURATION = 3000;        // 3 seconds for ghost light to be on
const unsigned long GHOST_BUTTON_TIMEOUT = 10000;          // 10 seconds timeout after light turns off
const unsigned long FINAL_GHOST_COOLDOWN = 1000;           // 1 second cooldown after last ghost light
const unsigned long ELEVATOR_RUNNING_INTERVAL = 3000;      // Time between floor transitions
const unsigned long BLINK_INTERVAL_RUNNING = 500;
const unsigned long ELEVATOR_CRASHING_DURATION = 15000;
const unsigned long EXIT_DOOR_DEBOUNCE_TIME = 200;         // 200 milliseconds debounce time

// Create mock DYPlayer object
DY::Player player(&Serial1);

// State variables
enum ElevatorState {
  IDLE,
  WAITING_FOR_FIRST_FLOOR,
  GHOST_SEQUENCE,
  ELEVATOR_CLOSING,
  ELEVATOR_RUNNING,
  ELEVATOR_CRASHING,
  POST_CRASH_BLINKING,
  WAITING_FOR_EXIT_CLOSE
};

ElevatorState currentState = IDLE;
int currentFloor = 3; // Start at floor 3
int selectedFloor = -1; // No floor selected initially
int ghostLightIndex = 0;
unsigned long stateStartTime = 0;
unsigned long lastBlinkTime = 0;
bool lightOn = false;

int finalFloor = 0;  // 0 represents 'B' for basement

// Variables for ghost sequence phases
enum GhostPhase {
  LIGHT_ON,
  COOLDOWN
};

GhostPhase ghostPhase;
unsigned long ghostPhaseStartTime = 0;

// Variables for random blinking
unsigned long floorLightLastToggleTime[4] = {0, 0, 0, 0};
unsigned long floorLightBlinkInterval[4];
bool floorLightState[4] = {false, false, false, false};

unsigned long ceilingLightLastToggleTime = 0;
unsigned long ceilingLightBlinkInterval;
bool ceilingLightState = false;

void setup() {
  Serial.begin(115200);
  Serial1.begin(9600, SERIAL_8N1, RX_PIN, TX_PIN);

  // Initialize pin modes
  for (int i = 0; i < 4; i++) pinMode(FLOOR_BUTTONS[i], INPUT_PULLUP);
  pinMode(CLOSE_BUTTON, INPUT_PULLUP);
  pinMode(OPEN_BUTTON, INPUT_PULLUP);
  
  for (int i = 0; i < 4; i++) pinMode(FLOOR_LIGHTS[i], OUTPUT);
  for (int i = 0; i < 4; i++) pinMode(GHOST_LIGHTS[i], OUTPUT);
  for (int i = 0; i < 3; i++) pinMode(GHOST_BUTTONS[i], INPUT_PULLUP);
  
  pinMode(SMALL_VIBRATION_MOTOR, OUTPUT);
  pinMode(BIG_VIBRATION_MOTOR, OUTPUT);
  pinMode(DOOR_CLOSED_SWITCH, INPUT_PULLUP);
  pinMode(EXIT_DOOR_CLOSED_SWITCH, INPUT_PULLUP);
  pinMode(ELEVATOR_CEILING_LIGHT, OUTPUT);

  // Initial states
  digitalWrite(ELEVATOR_CEILING_LIGHT, HIGH); // Turn on elevator ceiling light
  ceilingLightState = true;
  digitalWrite(FLOOR_LIGHTS[currentFloor], HIGH); // Turn on the 3rd floor light
  floorLightState[currentFloor] = true;

  // Initialize mock DYPlayer
  if (!player.begin()) {
    Serial.println("Failed to initialize Mock DYPlayer.");
    while (1); // Halt execution
  }
  player.setVolume(20);

  // Initialize random number generator
  randomSeed(analogRead(0));

  Serial.println("System initialized.");
}

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

  switch (currentState) {
    case IDLE:
      // Check for 1st floor button press (index 1)
      if (digitalRead(FLOOR_BUTTONS[1]) == LOW) { // 1st floor button pressed
        selectedFloor = 1; // Index for 1st floor
        digitalWrite(FLOOR_LIGHTS[selectedFloor], HIGH); // Turn on 1st floor light
        floorLightState[selectedFloor] = true;
        Serial.println("1st floor button pressed.");
        currentState = WAITING_FOR_FIRST_FLOOR;
        Serial.println("Waiting for close button");
      }
      break;

    case WAITING_FOR_FIRST_FLOOR:
      if (digitalRead(CLOSE_BUTTON) == LOW) { // Close button pressed
        currentState = GHOST_SEQUENCE;
        ghostPhase = LIGHT_ON;
        stateStartTime = currentTime;
        ghostPhaseStartTime = currentTime;
        ghostLightIndex = 0;
        // Turn off the current floor light (3rd floor)
        digitalWrite(FLOOR_LIGHTS[currentFloor], LOW);
        floorLightState[currentFloor] = false;
        // Keep the selected floor light on (1st floor)
        // Turn off the ceiling light
        digitalWrite(ELEVATOR_CEILING_LIGHT, LOW);
        ceilingLightState = false;
        // Turn on the first ghost light
        digitalWrite(GHOST_LIGHTS[ghostLightIndex], HIGH);
        player.playSpecified(2); // Play breaker sound
        Serial.println("Starting ghost sequence");
      }
      break;

    case GHOST_SEQUENCE:
      if (ghostLightIndex >= 0 && ghostLightIndex < 4) {
        switch (ghostPhase) {
          case LIGHT_ON:
            if (currentTime - ghostPhaseStartTime >= GHOST_LIGHT_ON_DURATION) {
              // Turn off the ghost light after 3 seconds
              digitalWrite(GHOST_LIGHTS[ghostLightIndex], LOW);
              ghostPhase = COOLDOWN;
              ghostPhaseStartTime = currentTime;
              Serial.print("Ghost light ");
              Serial.print(ghostLightIndex + 1);
              Serial.println(" turned off, entering cooldown phase");
            }
            break;

          case COOLDOWN:
            int expectedButtonIndex = ghostLightIndex; // Since ghost buttons start from index 0

            // Determine cooldown duration for the current ghost light
            unsigned long cooldownDuration = (ghostLightIndex == 3) ? FINAL_GHOST_COOLDOWN : GHOST_BUTTON_TIMEOUT;

            // For ghostLightIndex == 3 (4th ghost light), there's no corresponding ghost button
            if (expectedButtonIndex < 3) {
              // Read the state of the expected ghost button
              int buttonState = digitalRead(GHOST_BUTTONS[expectedButtonIndex]);

              if (buttonState == LOW) {
                // Proceed to next ghost light immediately
                ghostLightIndex++;
                if (ghostLightIndex < 4) {
                  // Start next ghost light
                  digitalWrite(GHOST_LIGHTS[ghostLightIndex], HIGH);
                  ghostPhase = LIGHT_ON;
                  ghostPhaseStartTime = currentTime;
                  player.playSpecified(2); // Play breaker sound
                  Serial.print("Ghost button ");
                  Serial.print(expectedButtonIndex + 1);
                  Serial.println(" pressed, advancing to next ghost light");
                } else {
                  // Ghost sequence complete
                  digitalWrite(ELEVATOR_CEILING_LIGHT, HIGH); // Turn ceiling light back on
                  ceilingLightState = true;
                  currentState = ELEVATOR_CLOSING;
                  player.playSpecified(6); // Play elevator closing sound
                  Serial.println("Ghost sequence complete. Ceiling light on. Elevator closing.");
                }
              } else if (currentTime - ghostPhaseStartTime >= cooldownDuration) {
                // Timeout, proceed to next ghost light
                ghostLightIndex++;
                if (ghostLightIndex < 4) {
                  // Start next ghost light
                  digitalWrite(GHOST_LIGHTS[ghostLightIndex], HIGH);
                  ghostPhase = LIGHT_ON;
                  ghostPhaseStartTime = currentTime;
                  player.playSpecified(2); // Play breaker sound
                  Serial.println("Cooldown timeout, advancing to next ghost light");
                } else {
                  // Ghost sequence complete
                  digitalWrite(ELEVATOR_CEILING_LIGHT, HIGH); // Turn ceiling light back on
                  ceilingLightState = true;
                  currentState = ELEVATOR_CLOSING;
                  player.playSpecified(6); // Play elevator closing sound
                  Serial.println("Ghost sequence complete. Ceiling light on. Elevator closing.");
                }
              }
              // Else, continue waiting
            } else {
              // For ghostLightIndex == 3 (4th ghost light), wait for cooldownDuration before proceeding
              if (currentTime - ghostPhaseStartTime >= cooldownDuration) {
                // Ghost sequence complete
                currentState = ELEVATOR_CLOSING;
                digitalWrite(ELEVATOR_CEILING_LIGHT, HIGH); // Turn ceiling light back on
                ceilingLightState = true;
                player.playSpecified(6); // Play elevator closing sound
                Serial.println("Final cooldown complete. Elevator closing.");
              }
              // Else, continue waiting
            }
            break;
        }
      }
      break;

    case ELEVATOR_CLOSING:
      if (digitalRead(DOOR_CLOSED_SWITCH) == LOW) { // Door is closed (pin LOW)
        currentState = ELEVATOR_RUNNING;
        stateStartTime = currentTime;
        lastBlinkTime = currentTime;
        digitalWrite(SMALL_VIBRATION_MOTOR, HIGH);
        player.playSpecified(3); // Play elevator running sound
        Serial.println("Elevator running");
      }
      break;

    case ELEVATOR_RUNNING:
      // Keep the selected floor light on during travel
      digitalWrite(FLOOR_LIGHTS[selectedFloor], HIGH);
      floorLightState[selectedFloor] = true;

      // Blink the current floor's light
      if (currentTime - lastBlinkTime > BLINK_INTERVAL_RUNNING) {
        floorLightState[currentFloor] = !floorLightState[currentFloor];
        digitalWrite(FLOOR_LIGHTS[currentFloor], floorLightState[currentFloor] ? HIGH : LOW);
        lastBlinkTime = currentTime;
      }

      if (currentTime - stateStartTime > ELEVATOR_RUNNING_INTERVAL) {
        // Turn off the previous floor's light
        digitalWrite(FLOOR_LIGHTS[currentFloor], LOW);
        floorLightState[currentFloor] = false;

        if (currentFloor > selectedFloor) {
          currentFloor--; // Move to the next lower floor
          Serial.print("Elevator now at floor ");
          Serial.println(currentFloor);
        } else {
          // We've reached the selected floor (floor 1)
          currentState = ELEVATOR_CRASHING;
          stateStartTime = currentTime;
          lastBlinkTime = currentTime;
          digitalWrite(SMALL_VIBRATION_MOTOR, LOW);
          player.playSpecified(4); // Play crashing sound
          Serial.println("Elevator crashing");

          // Initialize variables for ELEVATOR_CRASHING
          // Turn off all floor lights
          for (int i = 0; i < 4; i++) {
            digitalWrite(FLOOR_LIGHTS[i], LOW);
            floorLightState[i] = false;
          }
          // Ceiling light steady ON
          digitalWrite(ELEVATOR_CEILING_LIGHT, HIGH);
          ceilingLightState = true;
        }
        stateStartTime = currentTime;
      }
      break;

    case ELEVATOR_CRASHING:
      {
        unsigned long crashElapsedTime = currentTime - stateStartTime;
        digitalWrite(SMALL_VIBRATION_MOTOR, HIGH);

        // Big motor activation
        bool bigMotorOn = false;
        if ((crashElapsedTime > 8000 && crashElapsedTime < 9000) ||
            (crashElapsedTime > 12000 && crashElapsedTime < 13500) ||
            (crashElapsedTime > 13000 && crashElapsedTime < ELEVATOR_CRASHING_DURATION)) {
          bigMotorOn = true;
        }
        digitalWrite(BIG_VIBRATION_MOTOR, bigMotorOn);

        if (bigMotorOn) {
          // Blink all lights super fast together with random intervals
          if (currentTime - lastBlinkTime >= random(50, 150)) { // Random interval between 50ms and 150ms
            lightOn = !lightOn;
            // Control all floor lights
            for (int i = 0; i < 4; i++) {
              digitalWrite(FLOOR_LIGHTS[i], lightOn ? HIGH : LOW);
            }
            // Control ceiling light
            digitalWrite(ELEVATOR_CEILING_LIGHT, lightOn ? HIGH : LOW);
            lastBlinkTime = currentTime;
          }
        } else {
          // When big motor is OFF
          // Ceiling light is steady ON
          digitalWrite(ELEVATOR_CEILING_LIGHT, HIGH);
          ceilingLightState = true;

          // Ensure other floor lights are OFF
          for (int i = 0; i < 4; i++) {
            if (i != currentFloor) {
              digitalWrite(FLOOR_LIGHTS[i], LOW);
              floorLightState[i] = false;
            }
          }

          // Blink current floor's light
          if (currentTime - lastBlinkTime >= BLINK_INTERVAL_RUNNING) {
            floorLightState[currentFloor] = !floorLightState[currentFloor];
            digitalWrite(FLOOR_LIGHTS[currentFloor], floorLightState[currentFloor] ? HIGH : LOW);
            lastBlinkTime = currentTime;
          }
        }

        // After 5 seconds, move to floor B
        if (crashElapsedTime >= 5000 && currentFloor > 0) {
          currentFloor = 0; // Floor B
          Serial.println("Elevator reached floor B during crash");
        }

        if (crashElapsedTime > ELEVATOR_CRASHING_DURATION) {
          currentState = POST_CRASH_BLINKING;
          digitalWrite(SMALL_VIBRATION_MOTOR, LOW);
          digitalWrite(BIG_VIBRATION_MOTOR, LOW);
          Serial.println("Elevator crashed to basement. Starting random blinking.");

          // Initialize random blinking intervals
          for (int i = 0; i < 4; i++) {
            floorLightBlinkInterval[i] = random(20, 200); // Random interval between 200ms and 1000ms
            floorLightLastToggleTime[i] = currentTime;
            floorLightState[i] = false;
            digitalWrite(FLOOR_LIGHTS[i], LOW);
          }
          ceilingLightBlinkInterval = random(20, 200);
          ceilingLightLastToggleTime = currentTime;
          ceilingLightState = false;
          digitalWrite(ELEVATOR_CEILING_LIGHT, LOW);
        }
        break;
      }

    case POST_CRASH_BLINKING:
      // Continue random blinking for floor lights
      for (int i = 0; i < 4; i++) {
        if (currentTime - floorLightLastToggleTime[i] >= floorLightBlinkInterval[i]) {
          floorLightState[i] = !floorLightState[i];
          digitalWrite(FLOOR_LIGHTS[i], floorLightState[i] ? HIGH : LOW);
          floorLightBlinkInterval[i] = random(20, 200);
          floorLightLastToggleTime[i] = currentTime;
        }
      }
      // Continue random blinking for ceiling light
      if (currentTime - ceilingLightLastToggleTime >= ceilingLightBlinkInterval) {
        ceilingLightState = !ceilingLightState;
        digitalWrite(ELEVATOR_CEILING_LIGHT, ceilingLightState ? HIGH : LOW);
        ceilingLightBlinkInterval = random(20, 200);
        ceilingLightLastToggleTime = currentTime;
      }

      // Check if exit door is opened
      if (digitalRead(EXIT_DOOR_CLOSED_SWITCH) == HIGH) { // Exit door opened (pin HIGH)
        currentState = WAITING_FOR_EXIT_CLOSE;
        stateStartTime = currentTime; // Initialize debounce timer
        Serial.println("Exit door opened. Waiting for it to close.");
      }
      break;

    case WAITING_FOR_EXIT_CLOSE:
      // Continue random blinking
      // Handle random blinking for floor lights
      for (int i = 0; i < 4; i++) {
        if (currentTime - floorLightLastToggleTime[i] >= floorLightBlinkInterval[i]) {
          floorLightState[i] = !floorLightState[i];
          digitalWrite(FLOOR_LIGHTS[i], floorLightState[i] ? HIGH : LOW);
          floorLightBlinkInterval[i] = random(20, 200);
          floorLightLastToggleTime[i] = currentTime;
        }
      }
      // Handle random blinking for ceiling light
      if (currentTime - ceilingLightLastToggleTime >= ceilingLightBlinkInterval) {
        ceilingLightState = !ceilingLightState;
        digitalWrite(ELEVATOR_CEILING_LIGHT, ceilingLightState ? HIGH : LOW);
        ceilingLightBlinkInterval = random(200, 1000);
        ceilingLightLastToggleTime = currentTime;
      }

      // Debounce logic
      if (digitalRead(EXIT_DOOR_CLOSED_SWITCH) == LOW) { // Exit door closed (pin LOW)
        // Check if door has been closed for debounce duration
        if (currentTime - stateStartTime >= EXIT_DOOR_DEBOUNCE_TIME) {
          currentState = IDLE;
          currentFloor = 3; // Reset to floor 3
          selectedFloor = -1; // No floor selected
          for (int i = 0; i < 4; i++) {
            digitalWrite(FLOOR_LIGHTS[i], LOW);
            floorLightState[i] = false;
          }
          digitalWrite(FLOOR_LIGHTS[currentFloor], HIGH); // 3rd floor light on
          floorLightState[currentFloor] = true;
          digitalWrite(ELEVATOR_CEILING_LIGHT, HIGH);
          ceilingLightState = true;
          Serial.println("Exit door closed. Reset to idle state.");
        }
      } else {
        // Reset debounce timer if door is open
        stateStartTime = currentTime;
      }
      break;
  }

  // Ensure the loop runs efficiently without delay
}