// BUTTONS input pins
#define button_open 48
#define button_close 46
#define button_emergency 44

// Door input pins
#define door1_open_input 39
#define door1_closed_input 41
#define door2_open_input 35
#define door2_closed_input 37
#define door3_open_input 31
#define door3_closed_input 33
#define door4_open_input 27
#define door4_closed_input 29

// LEDs Status output pins
#define led_open 24
#define led_closed 26 
#define led_faulted 22
#define led_emergency 28

// MOTOR control output pins
#define motor1 42
#define motor2 44 
#define motor3 46
#define motor4 48
#define motor1_pwr 43
#define motor2_pwr 45 
#define motor3_pwr 47
#define motor4_pwr 49

volatile bool emergency_triggered = false; // Flag for emergency
unsigned long switchHighTime = 0; // To store the time when the button is pressed
const unsigned long thresholdTime = 2000; // 2 seconds in milliseconds
int system_state = 2;
bool door1_open = LOW; 
bool door1_closed = LOW;
bool door2_open = LOW;
bool door2_closed = LOW;
bool door3_open = LOW;
bool door3_closed = LOW;
bool door4_open = LOW;
bool door4_closed = LOW;

void setup() {
  Serial.begin(9600);
  
  // Set the LED pins as OUTPUT
  pinMode(led_open, OUTPUT);
  pinMode(led_closed, OUTPUT);
  pinMode(led_faulted, OUTPUT);
  pinMode(led_emergency, OUTPUT);
  
  // Set the MOTOR Relay pins as OUTPUT
  pinMode(motor1, OUTPUT);
  pinMode(motor2, OUTPUT);
  pinMode(motor3, OUTPUT);
  pinMode(motor4, OUTPUT);
  pinMode(motor1_pwr, OUTPUT);
  pinMode(motor2_pwr, OUTPUT);
  pinMode(motor3_pwr, OUTPUT);
  pinMode(motor4_pwr, OUTPUT);
  
  // Set the door input pins as INPUT_PULLUP
  pinMode(door1_open_input, INPUT_PULLUP);
  pinMode(door1_closed_input, INPUT_PULLUP);
  pinMode(door2_open_input, INPUT_PULLUP);
  pinMode(door2_closed_input, INPUT_PULLUP);
  pinMode(door3_open_input, INPUT_PULLUP);
  pinMode(door3_closed_input, INPUT_PULLUP);
  pinMode(door4_open_input, INPUT_PULLUP);
  pinMode(door4_closed_input, INPUT_PULLUP);

  // Attach interrupt for the emergency button (falling edge)
  attachInterrupt(digitalPinToInterrupt(button_emergency), emergencyISR, FALLING);

  system_state = initial_state();
}

void loop() {
  Serial.println("Main Loop..."); // Serial message
  delay(1000); // Delay for readability
  if (emergency_triggered) {
    return; // Exit the loop early to prevent any further execution
  }
  digitalWrite(led_emergency, HIGH);
  digitalWrite(led_closed, HIGH);
  digitalWrite(led_open, HIGH);
  digitalWrite(led_faulted, HIGH);
  readButtons();
  updateDoorStates(); // Regularly update door states
  delay(1000); // Delay for readability
  digitalWrite(led_emergency, LOW);
  digitalWrite(led_closed, LOW);
  digitalWrite(led_open, LOW);
  digitalWrite(led_faulted, LOW);
}

// ISR for handling emergency
void emergencyISR() {
  emergency_triggered = true; // Set the flag
  Serial.println("Emergency Triggered!"); // Serial message
  digitalWrite(motor1_pwr, LOW);
  digitalWrite(motor2_pwr, LOW);
  digitalWrite(motor3_pwr, LOW);
  digitalWrite(motor4_pwr, LOW);
  digitalWrite(led_emergency, HIGH);
  digitalWrite(led_closed, LOW);
  digitalWrite(led_open, LOW);
}

// Function to initialize the state
int initial_state() {
  Serial.println("Checking initial state...");
  updateDoorStates(); // Read the initial state of the doors
  
  // Check if all doors are closed
  if (door1_closed == LOW && door2_closed == LOW && 
      door3_closed == LOW && door4_closed == LOW) {
    Serial.println("All doors are closed.");
    return 1; // All doors are closed
  }
  
  if (door1_open == LOW && door2_open == LOW && 
      door3_open == LOW && door4_open == LOW) {
    Serial.println("All doors are open.");
    return 0; // All doors are open
  }
  
  Serial.println("initial_state Opening doors...");
  return open_doors(); // Call the function to open the doors
}

// Function to OPEN doors
int open_doors() {
  Serial.println("Opening doors...");
  while (door1_open == HIGH && door2_open == HIGH && 
         door3_open == HIGH && door4_open == HIGH) {
    digitalWrite(led_open, HIGH); // Example action
    delay(5000); // Keep LED on for a second
    digitalWrite(led_open, LOW); // Turn off LED
    delay(5000); // Keep LED on for a second

    // Update door status here
    updateDoorStates();
  }
  Serial.println("Doors opened.");
  return 0; // Indicate completion
}

// Function to update door states
void updateDoorStates() {
  Serial.println("Updating door states...");
  
  door1_open = digitalRead(door1_open_input);
  Serial.print("door1_open: ");
  Serial.println(door1_open); // This will print the value of door1_open
  
  door1_closed = digitalRead(door1_closed_input);
  Serial.print("door1_closed: ");
  Serial.println(door1_closed);

  door2_open = digitalRead(door2_open_input);
  Serial.print("door2_open: ");
  Serial.println(door2_open);

  door2_closed = digitalRead(door2_closed_input);
  Serial.print("door2_closed: ");
  Serial.println(door2_closed);

  door3_open = digitalRead(door3_open_input);
  Serial.print("door3_open: ");
  Serial.println(door3_open);

  door3_closed = digitalRead(door3_closed_input);
  Serial.print("door3_closed: ");
  Serial.println(door3_closed);

  door4_open = digitalRead(door4_open_input);
  Serial.print("door4_open: ");
  Serial.println(door4_open);

  door4_closed = digitalRead(door4_closed_input);
  Serial.print("door4_closed: ");
  Serial.println(door4_closed);
}


// Function to read buttons
void readButtons() {
  bool button_open_value = digitalRead(button_open);
  bool button_close_value = digitalRead(button_close);

  // Check if the open button is pressed
  if (button_open_value == LOW) { // Button is pressed (active LOW)
    Serial.println("Open button pressed.");
    // Start the timer if not already started
    if (switchHighTime == 0) {
      switchHighTime = millis(); // Start the timer
    }
    // Check if 2 seconds have passed
    if (millis() - switchHighTime >= thresholdTime) {
      open_doors();
    }
  } else { // Button is not pressed
    switchHighTime = 0; // Reset the timer
  }
}