#include <Arduino.h>

// Pin definitions
constexpr int PIN_GREEN_LED = 12;
constexpr int PIN_ORANGE_LED = 14;
constexpr int PIN_RED_LED = 27;
constexpr int CLOSED_SENSOR = 19;
constexpr int OPEN_SENSOR = 23;
constexpr int PIN_BUTTON = 5;

// Door states
enum DoorState {
  CLOSING,
  CLOSED,
  LOCKED,
  OPENING,
  OPEN,
  ERROR
};

// Time constants
constexpr unsigned long SHORT_PRESS_MIN = 1800;
constexpr unsigned long SHORT_PRESS_MAX = 3500;
constexpr unsigned long LONG_PRESS_MIN = 4800;
constexpr unsigned long LONG_PRESS_MAX = 7500;
constexpr unsigned long DOOR_MOVEMENT_DURATION = 5000;
constexpr unsigned long BUTTON_FEEDBACK_DURATION = 500;


const char* STATE_NAMES[] = {
  "CLOSING",
  "CLOSED",
  "LOCKED",
  "OPENING",
  "OPEN",
  "ERROR"
};

// Global variables
DoorState currentState = CLOSED;
DoorState previousState = CLOSED;
bool buttonAlreadyPressed = false;
unsigned long pressTime = 0;
unsigned long releaseTime = 0;
int redBlinkCounter = 0;

// Functions
void displayDoorState() {
  if (currentState != previousState){
    Serial.println(STATE_NAMES[currentState]);
    previousState = currentState;
  }
}

void manageLockStateLed() {
  if (currentState == LOCKED) {
    if (redBlinkCounter == 10) {
      redBlinkCounter = 0;
      digitalWrite(PIN_RED_LED, HIGH);
      delay(300);
      digitalWrite(PIN_RED_LED, LOW);
    } else {
      redBlinkCounter++;
    }
  }
}

void toggleLock() {
  if (currentState == CLOSED) {
    currentState = LOCKED;
    digitalWrite(PIN_RED_LED, LOW);
  } else if (currentState == LOCKED) {
    currentState = CLOSED;
    digitalWrite(PIN_RED_LED, HIGH);
  }
}

void blinkOrangeLed() {
  digitalWrite(PIN_ORANGE_LED, HIGH);
  delay(BUTTON_FEEDBACK_DURATION);
  digitalWrite(PIN_ORANGE_LED, LOW);
}

bool checkError() {
 
  bool openSensorActive = (digitalRead(OPEN_SENSOR) == LOW);
  bool closedSensorActive = (digitalRead(CLOSED_SENSOR) == LOW);
  bool sensorConflict = (openSensorActive && closedSensorActive) || 
                        (!openSensorActive && !closedSensorActive);
  bool closedStateError = (currentState == CLOSED) && 
                          (!closedSensorActive || openSensorActive);
  bool openStateError = (currentState == OPEN) && 
                        (!openSensorActive || closedSensorActive); 
  return sensorConflict || closedStateError || openStateError;
}

void activateErrorMode() {
  currentState = ERROR;

  digitalWrite(PIN_RED_LED, LOW);
  digitalWrite(PIN_ORANGE_LED, LOW);
  digitalWrite(PIN_GREEN_LED, LOW);

  while (true)
  {
    digitalWrite(PIN_RED_LED, HIGH);
    delay(100);
    digitalWrite(PIN_RED_LED, LOW);
    digitalWrite(PIN_ORANGE_LED, HIGH);
    delay(100);
    digitalWrite(PIN_ORANGE_LED, LOW);
    digitalWrite(PIN_GREEN_LED, HIGH);
    delay(100);
    digitalWrite(PIN_GREEN_LED, LOW);
  }
}

void openDoor() {
  digitalWrite(PIN_RED_LED, LOW);
  digitalWrite(PIN_ORANGE_LED, HIGH);
  currentState = OPENING;
  Serial.println(STATE_NAMES[currentState]);
  delay(DOOR_MOVEMENT_DURATION);

  if (digitalRead(OPEN_SENSOR) == LOW && digitalRead(CLOSED_SENSOR) == HIGH) {
    currentState = OPEN;
    digitalWrite(PIN_GREEN_LED, HIGH);
    digitalWrite(PIN_ORANGE_LED, LOW);
  } else {
    activateErrorMode();
  }
}

void closeDoor() {
  digitalWrite(PIN_GREEN_LED, LOW);
  digitalWrite(PIN_ORANGE_LED, HIGH);
  currentState = CLOSING;
  Serial.println(STATE_NAMES[currentState]);
  delay(DOOR_MOVEMENT_DURATION);

  if (digitalRead(OPEN_SENSOR) == HIGH && digitalRead(CLOSED_SENSOR) == LOW) {
    currentState = CLOSED;
    digitalWrite(PIN_RED_LED, HIGH);
    digitalWrite(PIN_ORANGE_LED, LOW);
  } else {
    activateErrorMode();
  }
}

void executeAction() {
  if (currentState == CLOSED) {
    openDoor();
  } else if (currentState == OPEN) {
    closeDoor();
  }
}

void processButtonAction(unsigned long pressDuration) {
  if (pressDuration > SHORT_PRESS_MIN && pressDuration < SHORT_PRESS_MAX) {
    executeAction();
  }

  else if (pressDuration > LONG_PRESS_MIN && pressDuration < LONG_PRESS_MAX) {
    toggleLock();
  }
}

void handleButtonPress() {
  if (digitalRead(PIN_BUTTON) == LOW) {
    if (!buttonAlreadyPressed) {
      buttonAlreadyPressed = true;
      pressTime = millis();

      blinkOrangeLed();
    }
  } else if (digitalRead(PIN_BUTTON) == HIGH && buttonAlreadyPressed) {
    releaseTime = millis();
    unsigned long pressDuration = releaseTime - pressTime;

    Serial.print("Button pressed for: ");
    Serial.print(pressDuration);
    Serial.println();

    processButtonAction(pressDuration);

    buttonAlreadyPressed = false;
  }
}

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

  pinMode(PIN_RED_LED, OUTPUT);
  pinMode(PIN_ORANGE_LED, OUTPUT);
  pinMode(PIN_GREEN_LED, OUTPUT);
  pinMode(OPEN_SENSOR, INPUT_PULLUP);
  pinMode(CLOSED_SENSOR, INPUT_PULLUP);
  pinMode(PIN_BUTTON, INPUT_PULLUP);

  closeDoor(); // Initialize door state to closed
}

void loop() {
  if (checkError()) {
    activateErrorMode();
  }

  handleButtonPress();

  manageLockStateLed();

  displayDoorState();

  delay(20);
}