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

const int sensorClosedPin = 2;
const int sensorOpenPin = 3;
const int buttonClosePin = 4;
const int buttonOpenPin = 5;
const int buttonStopPin = 6;
const int blueLEDPin = 7;
const int yellowLEDPin = 8;
const int greenLEDPin = 9;
const int redLEDPin = 10;
const int buzzerPin = 11;
const int buttonControlPin = 12;
const int relayOpenPin = 13;
const int relayClosePin = 14;
const int relayStopPin = 15;
const int relayTogglePin = 16;

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

enum DoorState {
    DOOR_CLOSED,
    DOOR_OPEN,
    DOOR_MOVING,
    STOP_EMERGENCY
};

DoorState currentState = DOOR_CLOSED;
DoorState previousState = DOOR_CLOSED;
unsigned long lastChangeTime = 0;
unsigned long movingStartTime = 0;
bool yellowLEDBlinking = false;

struct Relay {
    int pin;
    bool active;
    unsigned long startTime;
};

Relay relays[] = {
    {relayOpenPin, false, 0},
    {relayClosePin, false, 0},
    {relayStopPin, false, 0},
    {relayTogglePin, false, 0}
};

void setup() {
    Serial.begin(9600);
    pinMode(sensorClosedPin, INPUT_PULLUP);
    pinMode(sensorOpenPin, INPUT_PULLUP);
    pinMode(buttonClosePin, INPUT_PULLUP);
    pinMode(buttonOpenPin, INPUT_PULLUP);
    pinMode(buttonStopPin, INPUT_PULLUP);
    pinMode(blueLEDPin, OUTPUT);
    pinMode(yellowLEDPin, OUTPUT);
    pinMode(greenLEDPin, OUTPUT);
    pinMode(redLEDPin, OUTPUT);
    pinMode(buzzerPin, OUTPUT);
    pinMode(buttonControlPin, INPUT_PULLUP);

    for (Relay& relay : relays) {
        pinMode(relay.pin, OUTPUT);
        digitalWrite(relay.pin, LOW);
    }

    if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
        Serial.println(F("SSD1306 allocation failed"));
        while (true);
    }

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

void loop() {
    handleInputs();
    updateState();
    manageLEDs();
    checkBuzzer();
    manageRelays();
    updateDisplay();
}

void displayInitialState() {
    Serial.print("Initial State: ");
    Serial.print("Sensor Closed: ");
    Serial.println(digitalRead(sensorClosedPin));
    Serial.print("Sensor Open: ");
    Serial.println(digitalRead(sensorOpenPin));
}

void handleInputs() {
    if (digitalRead(buttonClosePin) == LOW) {
        activateRelay(relayClosePin);
    }
    if (digitalRead(buttonOpenPin) == LOW) {
        activateRelay(relayOpenPin);
    }
    if (digitalRead(buttonStopPin) == LOW) {
        activateRelay(relayStopPin);
        currentState = STOP_EMERGENCY;
        lastChangeTime = millis();
    }
    if (digitalRead(buttonControlPin) == LOW) {
        activateRelay(relayTogglePin);
    }
}

void updateState() {
    int sensorClosed = digitalRead(sensorClosedPin);
    int sensorOpen = digitalRead(sensorOpenPin);

    if (currentState != STOP_EMERGENCY) {
        if (sensorClosed == LOW) {
            if (currentState != DOOR_CLOSED) {
                Serial.println("State Changed: DOOR CLOSED");
                movingStartTime = 0;  // Reset moving time
            }
            currentState = DOOR_CLOSED;
        } else if (sensorOpen == LOW) {
            if (currentState != DOOR_OPEN) {
                Serial.println("State Changed: DOOR OPENED");
                movingStartTime = 0;  // Reset moving time
            }
            currentState = DOOR_OPEN;
        } else {
            if (currentState == DOOR_CLOSED) {
                Serial.println("State Changed: DOOR MOVING");
                Serial.println("Direction: Opening");
                movingStartTime = millis();  // Start timing for movement
            } else if (currentState == DOOR_OPEN) {
                Serial.println("State Changed: DOOR MOVING");
                Serial.println("Direction: Closing");
                movingStartTime = millis();  // Start timing for movement
            }
            currentState = DOOR_MOVING;
        }
    }

    if (currentState == STOP_EMERGENCY && millis() - lastChangeTime >= 1000) {
        digitalWrite(relayStopPin, LOW);
        currentState = DOOR_CLOSED;
    }

    previousState = currentState; // Update previous state
}

void manageLEDs() {
    switch (currentState) {
        case DOOR_CLOSED:
            digitalWrite(greenLEDPin, HIGH);
            digitalWrite(blueLEDPin, LOW);
            digitalWrite(redLEDPin, LOW);
            stopBlinkingYellowLED();
            break;

        case DOOR_OPEN:
            digitalWrite(greenLEDPin, LOW);
            digitalWrite(blueLEDPin, HIGH);
            digitalWrite(redLEDPin, LOW);
            stopBlinkingYellowLED();
            break;

        case DOOR_MOVING:
            digitalWrite(greenLEDPin, LOW);
            digitalWrite(blueLEDPin, LOW);
            digitalWrite(redLEDPin, LOW);
            manageYellowLED();
            break;

        case STOP_EMERGENCY:
            digitalWrite(greenLEDPin, LOW);
            digitalWrite(blueLEDPin, LOW);
            digitalWrite(yellowLEDPin, LOW);
            blinkRedLED();
            break;
    }
}

void manageYellowLED() {
    static unsigned long lastChangeTime = 0;
    if (!yellowLEDBlinking) {
        yellowLEDBlinking = true;
        lastChangeTime = millis();
    }

    if (millis() - lastChangeTime >= 200) {
        digitalWrite(yellowLEDPin, !digitalRead(yellowLEDPin));
        lastChangeTime = millis();
    }
}

void stopBlinkingYellowLED() {
    yellowLEDBlinking = false;
    digitalWrite(yellowLEDPin, LOW);
}

void blinkRedLED() {
    static unsigned long lastBlinkTime = 0;
    static bool redState = false;
    if (millis() - lastBlinkTime >= 500) {
        redState = !redState;
        digitalWrite(redLEDPin, redState);
        lastBlinkTime = millis();
    }
}

void activateRelay(int relayPin) {
    for (Relay& relay : relays) {
        if (relay.pin == relayPin) {
            relay.active = true;
            relay.startTime = millis();
            digitalWrite(relay.pin, HIGH);
            break;
        }
    }
}

void manageRelays() {
    unsigned long currentTime = millis();
    for (Relay& relay : relays) {
        if (relay.active && currentTime - relay.startTime >= 1000) {
            digitalWrite(relay.pin, LOW);
            relay.active = false;
        }
    }
}

void checkBuzzer() {
    // Implement buzzer functionality as needed
}

void updateDisplay() {
    display.clearDisplay();

    // Draw rectangle for closed or opened state
    if (currentState == DOOR_CLOSED) {
        drawClosedRectangle();
    } else if (currentState == DOOR_OPEN) {
        drawOpenRectangle();
    }

    // Display arrows for movement direction
    if (currentState == DOOR_MOVING) {
        bool isOpening = (previousState == DOOR_CLOSED);
        displayMovingDirection(isOpening);
    }

    // Update the main state text
    display.setTextSize(2);
    display.setCursor(8, 10);
    switch (currentState) {
        case DOOR_CLOSED:
            display.print("CLOSED");
            break;
        case DOOR_OPEN:
            display.print("OPENED");
            break;
        case DOOR_MOVING:
            display.print("MOVING");
            break;
        case STOP_EMERGENCY:
            display.print("STOPPED");
            break;
    }

    // Display elapsed time for moving or opened
    display.setTextSize(1);
    display.setCursor(0, 48);
    if (currentState == DOOR_MOVING || currentState == DOOR_OPEN) {
        unsigned long elapsed = (millis() - movingStartTime) / 1000;
        display.print("Elapsed: ");
        display.print(elapsed);
        display.print(" sec");
    }

    display.display();
}

void drawClosedRectangle() {
    display.fillRect(0, 0, 128, 8, SSD1306_WHITE);
}

void drawOpenRectangle() {
    display.drawRect(0, 0, 128, 8, SSD1306_WHITE);
}

void displayMovingDirection(bool opening) {
    display.setCursor(0, 0);  // Top-left corner
    display.setTextSize(2);
    if (opening) {
        display.print(">>>");  // Right arrows for opening
    } else {
        display.print("<<<");  // Left arrows for closing
    }
}