// --- Include necessary libraries ---
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <LedControl.h>
#include <DHT.h>
#include <TM1637Display.h>
#include <Servo.h>
#include <MD_Parola.h>

// --- Define hardware pins and constants ---
#define DHTPIN 9
#define DHTTYPE DHT22
#define trigPin 18
#define echoPin 19
#define MAX_DISTANCE 200
#define buzzerPin 49
#define VRx A0
#define VRy A1
#define SW 2
#define CLK 47
#define DIO 45
#define CS 53
#define MAX_DEVICES 4
#define PIR_PIN 12
#define POT1_PIN A2
#define POT2_PIN A3
#define POT3_PIN A4
#define POT4_PIN A5
#define HARDWARE_TYPE MD_MAX72XX::FC16_HW
#define RED_PIN    3
#define GREEN_PIN  6
#define BLUE_PIN   46
#define TEMP_THRESHOLD 35.0          // Temperature warning threshold
#define HUMID_THRESHOLD 70.0         // Humidity warning threshold

// --- Initialize display and modules ---
MD_Parola myDisplay = MD_Parola(HARDWARE_TYPE, CS, MAX_DEVICES);
Servo door1, door2, door3, door4;
LiquidCrystal_I2C lcd(0x27, 20, 4);
LiquidCrystal_I2C lcd2(0x3F, 16, 2);
LedControl lc = LedControl(51, 52, 53, 4);
DHT dht(DHTPIN, DHTTYPE);
TM1637Display display(CLK, DIO);

// --- Global variables ---
float temperature;               // Stores the current temperature reading
float humidity;                  // Stores the current humidity reading
String currentGear = "Neutral";  // Tracks the current gear status (Neutral, Forward, Reverse)
unsigned long lastGearChangeTime = 0; // Timestamp of last gear change, used for debouncing or timing
String currentOption = "";       // Tracks currently selected menu option
bool frontObstacleDetected = false; // Flag to indicate if an obstacle is detected in front

void setup() {
  // --- Initialize input/output pins and hardware components ---
  pinMode(buzzerPin, OUTPUT);    // Set buzzer pin as output for sound alerts
  Serial.begin(9600);            // Start serial communication at 9600 baud rate
  pinMode(SW, INPUT_PULLUP);     // Set switch pin with internal pull-up resistor
  pinMode(PIR_PIN, INPUT);       // PIR sensor pin input (motion detection)
  pinMode(trigPin, OUTPUT);      // Ultrasonic sensor trigger pin
  pinMode(echoPin, INPUT);       // Ultrasonic sensor echo pin
  pinMode(RED_PIN, OUTPUT);      // RGB LED Red pin output
  pinMode(GREEN_PIN, OUTPUT);    // RGB LED Green pin output
  pinMode(BLUE_PIN, OUTPUT);     // RGB LED Blue pin output
  display.clear();               // Clear any existing graphics on the display

  // --- Initialize LED matrix and LCD screens ---
  myDisplay.begin();             // Initialize the LED display module
  myDisplay.setIntensity(10);    // Set LED display brightness level
  myDisplay.displayClear();      // Clear the LED display

  display.setBrightness(0x0f);  // Set brightness of another display module (if used)

  lcd.init();                   // Initialize first LCD
  lcd.backlight();              // Turn on backlight for readability

  lcd2.init();                  // Initialize second LCD (if used)
  lcd2.backlight();             // Turn on backlight for second LCD

  lc.shutdown(0, false);        // Turn on LED matrix driver chip
  lc.setIntensity(0, 8);        // Set LED matrix brightness
  lc.clearDisplay(0);           // Clear LED matrix display

  dht.begin();                  // Initialize DHT temperature & humidity sensor

  display.setBrightness(5);     // Set brightness for display (redundant? check)

  showStartupAnimationWithMelody();  // Play startup animation and melody on buzzer

  delay(100);                  // Short delay to stabilize hardware
  lcd.clear();                 // Clear the LCD screen

  // --- Define custom characters/icons for LCD ---
  byte degreeSymbol[8] = {B00110, B01001, B01001, B00110, B00000, B00000, B00000, B00000};  // Degree (°) symbol
  byte tempIcon[8]     = {B00100, B01010, B01010, B01110, B11111, B11111, B01110, B00000};  // Thermometer icon
  byte humidIcon[8]    = {B00100, B00100, B01010, B01010, B10001, B10001, B01110, B00000};  // Water drop icon for humidity

  lcd.createChar(0, degreeSymbol);  // Register degree symbol as custom char 0
  lcd.createChar(1, tempIcon);      // Register temperature icon as custom char 1
  lcd.createChar(2, humidIcon);     // Register humidity icon as custom char 2

  initializeDoors();    // Initialize servo or motor positions for doors to default states
  showMainMenu();       // Display the main menu on serial and/or LCD
}

void loop() {
  // --- Continuously read user input from serial and process commands ---
  String command = readSerialCommand();  // Non-blocking read of serial input (custom function)
  if (command != "") {                    // If a command was entered
    processCommand(command);              // Execute the corresponding menu action
  }
}

// --- Display main menu options over Serial ---
void showMainMenu() {
  Serial.println("=== Main Menu ===");
  Serial.println("1: Show Temperature and Humidity");
  Serial.println("2: Joystick Options");
  Serial.println("3: Door Control");
  Serial.println("4: Exit");
  Serial.println("===================");
}

// --- Handle user menu selection based on Serial input ---
void processCommand(String cmd) {
  if (cmd == "1") {
    Serial.println("You selected: Show Temperature and Humidity");
    display.clear();          // Clear any graphical displays
    lc.clearDisplay(1);       // Clear LED matrix 1
    lc.clearDisplay(2);       // Clear LED matrix 2
    lcd.clear();              // Clear LCD screen
    showTemperatureAndHumidity();  // Show temperature and humidity continuously
    showMainMenu();           // Return to main menu after exiting
  } else if (cmd == "2") {
    Serial.println("You selected: Joystick Options");
    display.clear();
    lc.clearDisplay(1);
    lc.clearDisplay(2);
    lcd.clear();

    // Instructions printed to Serial Monitor on how to use joystick
    Serial.println("Use the joystick as follows:");
    Serial.println("⬆️  Up     = Forward Gear");
    Serial.println("⬇️  Down   = Reverse Gear");
    Serial.println("➡️  Right  = Right Indicator (hold to keep ON)");
    Serial.println("⬅️  Left   = Left Indicator (hold to keep ON)");
    Serial.println("⏺️  Center = Reset");
    Serial.println("⏺️  Center = Hazard (hold to keep ON)");


    changeGear();  // Enter joystick gear control mode
  }
  else if (cmd == "3") {
    Serial.println("You selected: Door Control");
    lcd.clear();
    display.clear();
    lc.clearDisplay(1);
    lc.clearDisplay(2);
    controlDoors();  // Enter door control mode
    close();         // Close resources if any after door control (function definition needed)
  } else if (cmd == "4") {
    Serial.println("You selected: Exit");
    lcd.clear();
    noTone(buzzerPin);    // Stop any buzzer tone playing
    display.clear();
    lc.clearDisplay(1);
    lc.clearDisplay(2);
  } else {
    Serial.println("Invalid command. Please try again.");
    showMainMenu();   // Show main menu again for invalid input
  }
}

// --- Continuously read and display temperature & humidity until user types 'exit' ---
void showTemperatureAndHumidity() {
  Serial.println("Type 'exit' to return to main menu.");  // User instruction

  while (true) {  // Infinite loop until 'exit' command
    float temperature = dht.readTemperature();  // Read temperature from DHT sensor
    float humidity = dht.readHumidity();        // Read humidity from DHT sensor

    lcd.clear();  // Clear LCD before updating display

    if (isnan(temperature) || isnan(humidity)) {  // Sensor read error check
      lcd.setCursor(0, 0);
      lcd.print("Sensor Error...");  // Display error message
    } else {
      lcd.setCursor(0, 0);
      lcd.print((char)1);          // Print custom temperature icon
      lcd.print(" Temp: ");
      lcd.print(temperature, 1);  // Print temperature with 1 decimal place
      lcd.write(byte(0));          // Print degree symbol
      lcd.print("C");

      lcd.setCursor(0, 1);
      lcd.print((char)2);          // Print custom humidity icon
      lcd.print(" Hum: ");
      lcd.print(humidity, 0);      // Print humidity as integer percentage
      lcd.print("%");

      checkAndHandleWarnings(temperature, humidity);  // Check for alerts or warnings based on readings
    }

    String command = readSerialCommand();  // Read user input from Serial
    if (command == "exit") {                // Exit condition
      Serial.println("Returning to main menu...");
      break;
    }

    delay(1000);  // Delay 1 second before next reading
  }

  // Clear all displays after exiting this mode
  display.clear();
  lc.clearDisplay(1);
  lc.clearDisplay(2);
  lcd.clear();
}

// --- Continuously read joystick and update gear and indicators until 'exit' ---
void changeGear() {
  Serial.println("Type 'exit' to return to main menu.");

  while (true) {
    updateGearFromJoystick();    // Read joystick inputs and set gear accordingly
    updateGearStatusOnLCD();     // Display current gear status on LCD
    updateIndicatorMode();       // Update indicators (e.g., arrows on display or LEDs)

    if (currentGear == "Reverse") {
      showReverseLinesOnDotMatrix();  // Show reverse lines pattern on LED dot matrix
    } else if (currentGear == "Forward") {
      checkPIRWarning();               // Check for obstacles with PIR sensor and warn
    } else if (currentGear == "Neutral") {
      clearNeutralStateDisplays();    // Clear any sensor or display info related to gear
    }

    String command = readSerialCommand();  // Read Serial input
    if (command == "exit") {                // Exit condition
      Serial.println("Returning to main menu...");
      break;
    }

    delay(100);  // Small delay for responsiveness
  }

  // Reset indicator LEDs and clear displays after exiting gear mode
  digitalWrite(RED_PIN, LOW);
  digitalWrite(GREEN_PIN, LOW);
  digitalWrite(BLUE_PIN, LOW);
  frontObstacleDetected = false;
  display.clear();
  lc.clearDisplay(1);
  lc.clearDisplay(2);
  lcd.clear();
  lcd2.clear();
  showMainMenu();  // Show main menu again
}

// --- Check if the input command is a recognized door control command ---
bool isDoorCommand(String cmd) {
  cmd.trim();
  return (
           cmd == "1" || cmd == "2" || cmd == "3" || cmd == "4" ||
           cmd == "5" || cmd == "6" ||
           cmd == "open all" || cmd == "close all" ||
           cmd.startsWith("close ")  // e.g., "close 2"
         );
}

// --- Handle door control commands and run appropriate door functions ---
void controlDoors() {
  lcd.clear();
  displayCarDynamic();     // Show graphical car view on display
  showDoorControlMenu();   // Print door control options to Serial Monitor

  while (true) {
    if (Serial.available()) {
      String cmd = Serial.readStringUntil('\n');  // Read command line from Serial
      cmd.trim();

      if (cmd == "6") {        // Exit door control menu
        display.clear();
        lc.clearDisplay(1);
        lc.clearDisplay(2);
        lcd.clear();
        showMainMenu();        // Return to main menu
        break;
      }

      processDoorCommand(cmd);  // Process other door commands
    }
  }
}

// --- Print the list of door control commands/options ---
void showDoorControlMenu() {
  Serial.println("====================");
  Serial.println("=== Door Control ===");
  Serial.println("1: Open Door 1");
  Serial.println("2: Open Door 2");
  Serial.println("3: Open Door 3");
  Serial.println("4: Open Door 4");
  Serial.println("close [1-4]: Close specific door");
  Serial.println("open all: Open all doors");
  Serial.println("close all: Close all doors");
  Serial.println("5: Potentiometer Control Mode");
  Serial.println("6: Main Menu");
  Serial.println("====================");
}

// --- Interpret door commands and perform corresponding door operations ---
void processDoorCommand(String cmd) {
  cmd.trim();

  if (cmd == "1") {
    openDoor(1);
    openDoorCh(1);
  } else if (cmd == "2") {
    openDoor(2);
    openDoorCh(2);
  } else if (cmd == "3") {
    openDoor(3);
    openDoorCh(3);
  } else if (cmd == "4") {
    openDoor(4);
    openDoorCh(4);
  } else if (cmd == "open all") {
    openAllDoors();
    openAllDoorsCh();
  } else if (cmd == "close all") {
    closeAllDoors();
    closeAllDoorsCh();
  } else if (cmd.startsWith("close ")) {
    int doorNum = cmd.substring(6).toInt();
    if (doorNum >= 1 && doorNum <= 4) {
      closeDoor(doorNum);
      closeDoorCh(doorNum);
    } else {
      Serial.println("Invalid door number.");
    }
  } else if (cmd == "5") {
    Serial.println("Entering Potentiometer Control Mode...");
    Serial.println("Send 'exit' to return to Door Menu.");
    potentiometerControlMode();  // Switch to potentiometer-based door control
  } else if (cmd == "6") {
    showMainMenu();  // Return to main menu
  } else {
    Serial.println("Unknown command.");  // Invalid input
  }
}

// --- Potentiometer control mode for door servos, continuously updates door angles ---
void potentiometerControlMode() {
  while (true) {
    updateDoorsFromPots();    // Continuously read pots and update servo angles
    delay(100);

    if (Serial.available()) {
      String input = Serial.readStringUntil('\n');
      input.trim();
      if (input == "exit") {
        Serial.println("Exiting Potentiometer Mode...");
        closeAllDoors();      // Close all doors before exiting potentiometer mode
        showDoorControlMenu(); // Show door control menu again
        break;
      }
    }
  }
}

// =================== PART 1 : LOGO & MELODY ===================

// Plays a square wave tone on the buzzer at the specified frequency (Hz) and duration (ms)
void playNote(int frequency, int duration) {
  long period = 1000000L / frequency;               // Calculate the period of one wave cycle in microseconds
  long cycles = (long)frequency * duration / 1000; // Calculate how many cycles to generate based on duration

  for (long i = 0; i < cycles; i++) {
    digitalWrite(buzzerPin, HIGH);                  // Turn buzzer ON
    delayMicroseconds(period / 2);                   // Wait for half the period
    digitalWrite(buzzerPin, LOW);                   // Turn buzzer OFF
    delayMicroseconds(period / 2);                   // Wait for the other half of the period
  }
}

// Displays a word on the LCD, printing each letter sequentially with a delay between letters
void displayWordSequentially(String word, int col, int row, int delayTime) {
  lcd.setCursor(col, row);                           // Set cursor position on LCD
  for (int i = 0; i < word.length(); i++) {
    lcd.print(word[i]);                              // Print each character
    delay(delayTime);                                // Wait before printing next character
  }
}

// Plays a sequence of tones synchronized with text to create a boot-up animation melody
void playSyncedMelodyWithLCD() {
  // Play logo tones corresponding to characters
  int logoNotes[] = { 262, 330, 392 }; // C4, E4, G4 notes
  for (int i = 0; i < 3; i++) {
    playNote(logoNotes[i], 200);
    delay(100);
  }

  // Play melody matching each character in "RM Machine"
  int nameNotes[] = { 392, 440, 494, 523, 587, 659, 698, 784, 880, 988 };
  String name = "RM Machine";
  for (int i = 0; i < name.length(); i++) {
    playNote(nameNotes[i], 120);
    delay(30);
  }

  // Play ascending scale tones for "Booting system..."
  int bootNotes[] = {
    262, 294, 330, 349, 370, 392, 415, 440,
    466, 494, 523, 554, 587, 622, 659, 698, 740
  };
  for (int i = 0; i < 17; i++) {
    playNote(bootNotes[i], 60);
    delay(20);
  }

  // Play final celebratory melody sequence
  int finalNotes[] = { 784, 880, 988, 1047, 1175, 1319 };
  for (int i = 0; i < 6; i++) {
    playNote(finalNotes[i], 100);
    delay(40);
  }
}

// Shows an animated logo and boot-up text on the LCD with synchronized melody playing on the buzzer
void showStartupAnimationWithMelody() {
  // Define custom 5x8 pixel characters for LCD graphics (corners and star)
  byte topLeft[8] = {B11100, B10010, B10001, B10000, B10000, B10000, B10000, B00000};
  byte topRight[8] = {B00111, B01001, B10001, B00001, B00001, B00001, B00001, B00000};
  byte bottomLeft[8] = {B10000, B10000, B10000, B10000, B10001, B10010, B11100, B00000};
  byte bottomRight[8] = {B00001, B00001, B00001, B00001, B10001, B01001, B00111, B00000};
  byte star[8] = {B00100, B01110, B11111, B10101, B11111, B01110, B00100, B00000};

  // Load these custom characters into LCD CGRAM
  lcd.createChar(0, topLeft);
  lcd.createChar(1, topRight);
  lcd.createChar(2, bottomLeft);
  lcd.createChar(3, bottomRight);
  lcd.createChar(4, star);
  lcd.clear();

  // Step 1: Display the logo with corresponding tones
  int logoNotes[] = {262, 330, 392}; // C4, E4, G4 notes
  lcd.setCursor(8, 0);
  lcd.write(byte(0)); playNote(logoNotes[0], 200); delay(100);
  lcd.write(byte(4)); playNote(logoNotes[1], 200); delay(100);
  lcd.write(byte(1)); playNote(logoNotes[2], 200); delay(200);

  // Step 2: Display brand name "RM Machine" with melody notes for each character
  String brand = "RM Machine";
  int brandNotes[] = {392, 440, 494, 523, 587, 659, 698, 784, 880, 988};
  lcd.setCursor(5, 1);
  for (int i = 0; i < brand.length(); i++) {
    lcd.print(brand[i]);
    playNote(brandNotes[i], 120);
    delay(80);
  }

  // Step 3: Scroll the text "Booting system..." with rising musical notes
  String bootText = "Booting system...";
  int bootNotes[] = {
    262, 294, 330, 349, 392, 415, 440, 466,
    494, 523, 554, 587, 622, 659, 698, 740, 784
  };
  lcd.setCursor(2, 2);
  for (int i = 0; i < bootText.length(); i++) {
    lcd.print(bootText[i]);
    playNote(bootNotes[i % 17], 70);
    delay(50);
  }

  // End animation: pause, clear display, and show welcome box message
  delay(500);
  lcd.clear();
  showBoxWelcome(); // Display framed welcome message without sound
  delay(500);
}

// Define custom characters for box framing on the LCD display
byte topLeft[8] = {B00000, B00000, B01110, B01000, B01000, B01000, B01000, B01000};
byte topRight[8] = {B00000, B00000, B01110, B00010, B00010, B00010, B00010, B00010};
byte botLeft[8] = {B01000, B01000, B01000, B01000, B01000, B01000, B01110, B00000};
byte botRight[8] = {B00010, B00010, B00010, B00010, B00010, B00010, B01110, B00000};
byte hor[8] = {B00000, B00000, B01110, B00000, B00000, B00000, B01110, B00000};
byte vertLeft[8] = {B01000, B01000, B01000, B01000, B01000, B01000, B01000, B01000};
byte vertRight[8] = {B00010, B00010, B00010, B00010, B00010, B00010, B00010, B00010};

// Animates a growing box frame around the text "ENGINE READY" on the LCD screen
void showBoxWelcome() {
  // Load custom box drawing characters into LCD CGRAM
  lcd.createChar(0, topLeft);
  lcd.createChar(1, topRight);
  lcd.createChar(2, botLeft);
  lcd.createChar(3, botRight);
  lcd.createChar(4, hor);
  lcd.createChar(5, vertLeft);
  lcd.createChar(6, vertRight);

  String word = "ENGINE READY";       // Text to display inside the box
  int startCol = 3;                   // Starting column on LCD
  int rowTop = 0;                    // Top row for box border
  int rowMiddle = 1;                 // Middle row for text
  int rowBottom = 2;                 // Bottom row for box border

  // Animate the box expanding character by character
  for (int i = 0; i < word.length(); i++) {
    int width = i + 3;               // Current width of the box

    // Draw top border with left corner, horizontal lines, and right corner
    lcd.setCursor(startCol, rowTop);
    lcd.write(byte(0));
    for (int j = 0; j < width - 2; j++) lcd.write(byte(4));
    lcd.write(byte(1));

    // Draw middle row with vertical borders and text inside
    lcd.setCursor(startCol, rowMiddle);
    lcd.write(byte(5));
    for (int j = 0; j < width - 2; j++) {
      if (j <= i) lcd.print(word[j]);  // Print next character of the word
      else lcd.print(" ");              // Fill rest with spaces
    }
    lcd.setCursor(startCol + width - 1, rowMiddle);
    lcd.write(byte(6));

    // Draw bottom border with left corner, horizontal lines, and right corner
    lcd.setCursor(startCol, rowBottom);
    lcd.write(byte(2));
    for (int j = 0; j < width - 2; j++) lcd.write(byte(4));
    lcd.write(byte(3));

    delay(100);                       // Pause to create animation effect
  }
}

// =================== PART 2 : TEMPERATURE & HUMIDITY ===================

// Displays temperature (°C) and humidity (%) on the LCD
void displayTempAndHumidity(float temperature, float humidity) {
  lcd.setCursor(0, 0);
  lcd.print((char)1);  // Temperature icon (custom character)
  lcd.print(" Temp: ");
  lcd.print(temperature, 1);  // Show with 1 decimal place
  lcd.write(byte(0));         // Degree symbol
  lcd.print("C ");            // Extra space to erase leftover characters

  lcd.setCursor(0, 1);
  lcd.print((char)2);  // Humidity icon (custom character)
  lcd.print(" Hum: ");
  lcd.print(humidity, 0);  // Show as integer
  lcd.print("% ");         // Extra space to erase leftover characters
}

// Checks temp/humidity against thresholds and shows alert if exceeded
void checkAndHandleWarnings(float temperature, float humidity) {
  bool tempHigh = temperature > TEMP_THRESHOLD;    // Compare temperature to threshold
  bool humidHigh = humidity > HUMID_THRESHOLD;     // Compare humidity to threshold

  if (tempHigh || humidHigh) {
    // Flash warning messages with buzzer
    for (int i = 0; i < 3; i++) {  // Repeat flashing 3 times
      if (tempHigh) {
        lcd.setCursor(16, 0);  // Right side of top row
        lcd.print("HOT!");     // Display hot warning
        tone(buzzerPin, 1000); // Play high tone
      }
      if (humidHigh) {
        lcd.setCursor(12, 1);   // Right-middle of bottom row
        lcd.print("HUMID!");    // Display humidity warning
        tone(buzzerPin, 1200);  // Play higher tone
      }

      delay(400);  // Wait before clearing

      // Clear warning messages
      if (tempHigh) {
        lcd.setCursor(16, 0);
        lcd.print("     ");  // Overwrite with spaces
      }
      if (humidHigh) {
        lcd.setCursor(12, 1);
        lcd.print("       ");  // Overwrite with spaces
      }

      noTone(buzzerPin);  // Stop sound
      delay(400);         // Wait before next flash
    }
  } else {
    // Values are normal → clear any previous warnings
    lcd.setCursor(16, 0);
    lcd.print(" ");  // Ensure HOT! is gone
    lcd.setCursor(12, 1);
    lcd.print(" ");  // Ensure HUMID! is gone
    noTone(buzzerPin);  // Just in case
  }
}

// ======== PART 3: DISPLAYING REVERSE GEAR LINE & DETECTING FORWARD OBSTACLE ========

// Reads joystick input to determine gear direction: Forward, Reverse, or Neutral
void updateGearFromJoystick() {
  int vxValue = analogRead(VRx);             // Read horizontal axis (X) of joystick
  bool swPressed = digitalRead(SW) == LOW;   // Check if joystick button is pressed

  String newGear = currentGear;  // Start with current gear as default

  // Determine new gear based on joystick state
  if (swPressed) {
    newGear = "Neutral";          // Center press sets gear to Neutral
  } else if (vxValue > 800) {
    newGear = "Forward";          // Right movement sets Forward
  } else if (vxValue < 200) {
    newGear = "Reverse";          // Left movement sets Reverse
  }

  // Only update if gear has changed
  if (newGear != currentGear) {
    currentGear = newGear;                                // Update global gear state
    Serial.println("Gear changed to: " + currentGear);    // Print gear change to Serial
    updateGearStatusOnLCD();                              // Immediately reflect change on LCD
  }
}

// Function to update turn signal and hazard light indicators based on joystick input and center button
void updateIndicatorMode() {
  static unsigned long lastFlashTime = 0;  // Time of last arrow blink toggle
  static bool showArrows = true;           // Determines whether to show or hide arrows (for blinking effect)

  int vy = analogRead(VRy);                // Read vertical joystick position
  bool swPressed = digitalRead(SW) == LOW; // Check if center joystick button is pressed
  unsigned long currentTime = millis();    // Get current system time

  // Track button states to avoid repeated Serial prints
  static bool rightPressed = false;
  static bool leftPressed = false;
  static bool centerPressed = false;

  // Run logic every 300ms to simulate blinking
  if (currentTime - lastFlashTime >= 300) {
    lastFlashTime = currentTime;
    showArrows = !showArrows;  // Toggle blinking state

    // ===== HAZARD LIGHTS MODE (CENTER PRESS) =====
    if (swPressed) {
      if (!centerPressed) {
        Serial.println("Hazard lights activated.");
        centerPressed = true;  // Mark center as pressed to avoid repeated logs
      }

      if (showArrows) {
        // Draw right arrow on LCD
        byte right4[] = { B11110, B11100, B11000, B10000, B00000, B00000, B00000, B00000 };
        byte right1[] = { B00000, B00000, B00000, B00001, B00001, B11111, B11111, B11111 };
        byte right2[] = { B00000, B00000, B00000, B00000, B10000, B11000, B11100, B11110 };
        byte right3[] = { B11111, B11111, B11111, B00001, B00001, B00000, B00000, B00000 };
        lcd2.createChar(0, right4); lcd2.setCursor(9, 1); lcd2.write(0);
        lcd2.createChar(1, right1); lcd2.setCursor(8, 0); lcd2.write(1);
        lcd2.createChar(2, right2); lcd2.setCursor(9, 0); lcd2.write(2);
        lcd2.createChar(3, right3); lcd2.setCursor(8, 1); lcd2.write(3);

        // Draw left arrow on LCD
        byte left4[] = { B11111, B11111, B11111, B10000, B10000, B00000, B00000, B00000 };
        byte left1[] = { B00000, B00000, B00000, B00000, B00001, B00011, B00111, B01111 };
        byte left2[] = { B00000, B00000, B00000, B10000, B10000, B11111, B11111, B11111 };
        byte left3[] = { B01111, B00111, B00011, B00001, B00000, B00000, B00000, B00000 };
        lcd2.createChar(4, left4); lcd2.setCursor(7, 1); lcd2.write(4);
        lcd2.createChar(5, left1); lcd2.setCursor(6, 0); lcd2.write(5);
        lcd2.createChar(6, left2); lcd2.setCursor(7, 0); lcd2.write(6);
        lcd2.createChar(7, left3); lcd2.setCursor(6, 1); lcd2.write(7);
      } else {
        // Clear arrow area when not showing
        for (int col = 6; col <= 9; col++) {
          lcd2.setCursor(col, 0); lcd2.print(" ");
          lcd2.setCursor(col, 1); lcd2.print(" ");
        }
      }
      return;  // Skip other checks during hazard mode
    } else {
      centerPressed = false;  // Reset center press flag when released
    }

    // ===== RIGHT TURN SIGNAL (JOYSTICK DOWN) =====
    if (vy < 200) {
      if (!rightPressed) {
        Serial.println("Right turn signal activated.");
        rightPressed = true;
      }

      if (showArrows) {
        // Draw right arrow
        byte right4[] = { B11110, B11100, B11000, B10000, B00000, B00000, B00000, B00000 };
        byte right1[] = { B00000, B00000, B00000, B00001, B00001, B11111, B11111, B11111 };
        byte right2[] = { B00000, B00000, B00000, B00000, B10000, B11000, B11100, B11110 };
        byte right3[] = { B11111, B11111, B11111, B00001, B00001, B00000, B00000, B00000 };
        lcd2.createChar(0, right4); lcd2.setCursor(9, 1); lcd2.write(0);
        lcd2.createChar(1, right1); lcd2.setCursor(8, 0); lcd2.write(1);
        lcd2.createChar(2, right2); lcd2.setCursor(9, 0); lcd2.write(2);
        lcd2.createChar(3, right3); lcd2.setCursor(8, 1); lcd2.write(3);
      } else {
        // Clear right arrow only
        for (int col = 8; col <= 9; col++) {
          lcd2.setCursor(col, 0); lcd2.print(" ");
          lcd2.setCursor(col, 1); lcd2.print(" ");
        }
      }
      return;
    } else {
      rightPressed = false;  // Reset flag if joystick not down
    }

    // ===== LEFT TURN SIGNAL (JOYSTICK UP) =====
    if (vy > 800) {
      if (!leftPressed) {
        Serial.println("Left turn signal activated.");
        leftPressed = true;
      }

      if (showArrows) {
        // Draw left arrow
        byte left4[] = { B11111, B11111, B11111, B10000, B10000, B00000, B00000, B00000 };
        byte left1[] = { B00000, B00000, B00000, B00000, B00001, B00011, B00111, B01111 };
        byte left2[] = { B00000, B00000, B00000, B10000, B10000, B11111, B11111, B11111 };
        byte left3[] = { B01111, B00111, B00011, B00001, B00000, B00000, B00000, B00000 };
        lcd2.createChar(0, left4); lcd2.setCursor(7, 1); lcd2.write(0);
        lcd2.createChar(1, left1); lcd2.setCursor(6, 0); lcd2.write(1);
        lcd2.createChar(2, left2); lcd2.setCursor(7, 0); lcd2.write(2);
        lcd2.createChar(3, left3); lcd2.setCursor(6, 1); lcd2.write(3);
      } else {
        // Clear left arrow only
        for (int col = 6; col <= 7; col++) {
          lcd2.setCursor(col, 0); lcd2.print(" ");
          lcd2.setCursor(col, 1); lcd2.print(" ");
        }
      }
      return;
    } else {
      leftPressed = false;  // Reset flag if joystick not up
    }

    // ===== NEUTRAL ZONE (JOYSTICK CENTERED) =====
    if (vy >= 200 && vy <= 800) {
      // Clear entire arrow area when no direction selected
      for (int col = 6; col <= 9; col++) {
        lcd2.setCursor(col, 0); lcd2.print(" ");
        lcd2.setCursor(col, 1); lcd2.print(" ");
      }
    }
  }
}

String lastDisplayedGear = "";  // Global variable to track last displayed gear

// Updates the LCD with the current gear status if it has changed
void updateGearStatusOnLCD() {
  static String lastDisplayedGear = "";  // Keeps track of the last gear shown on LCD

  // Update display only if the gear has changed
  if (currentGear != lastDisplayedGear) {
    lcd.setCursor(0, 0);                    // Move cursor to the start of the first line
    lcd.print("                    ");      // Clear the entire line
    lcd.setCursor(0, 0);                    // Move cursor again to the start
    lcd.print("Gear: " + currentGear);      // Print the new gear status
    lastDisplayedGear = currentGear;        // Update last displayed gear
  }
}

// Measures distance using ultrasonic sensor in centimeters
long getDistanceCM() {
  digitalWrite(trigPin, LOW);              // Ensure trigger pin is LOW
  delayMicroseconds(2);                    // Short delay for sensor stability
  digitalWrite(trigPin, HIGH);             // Send a 10us HIGH pulse
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);              // End the pulse

  long duration = pulseIn(echoPin, HIGH);  // Measure the duration of echo pulse
  long distance = duration * 0.034 / 2;    // Convert time to distance in cm
  return distance;                         // Return the measured distance
}

// Sets RGB LED color using analog PWM values (range: 0–255)
void setRGBColorAnalog(int redVal, int greenVal, int blueVal) {
  analogWrite(RED_PIN, redVal);            // Set red LED brightness
  analogWrite(GREEN_PIN, greenVal);        // Set green LED brightness
  analogWrite(BLUE_PIN, blueVal);          // Set blue LED brightness
}

bool messageShown = false;          // Flag for LCD message display

// Displays reverse gear distance and animations based on obstacle distance
void showReverseLinesOnDotMatrix() {
  long distance = getDistanceCM();                  // Measure distance using ultrasonic sensor
  static unsigned long lastToggleTime = 0;          // Stores the last time the display was toggled (for blinking)
  static bool state = false;                        // Toggle state for blinking effect
  const unsigned long blinkInterval = 100;          // Blinking interval for immediate danger in milliseconds
  unsigned long currentMillis = millis();           // Get current system time

  // Cap distance to prevent display overflow
  if (distance > 9999) distance = 9999;

  // Show distance on 7-segment display and LCD
  display.showNumberDec(distance, false);           // Show distance on TM1637 7-segment
  lcd.setCursor(0, 1);
  lcd.print("Distance:       ");                    // Clear previous distance line
  lcd.setCursor(10, 1);
  lcd.print(distance);                              // Print new distance
  lcd.print("cm  ");

  // Distance > 200cm → Safe
  if (distance > 200) {
    setRGBColorAnalog(0, 255, 0);                   // Set RGB to green
    lcd.setCursor(0, 2);
    lcd.print("Safe Distance   ");                  // Show safe message

    // Calm animation pattern (right arrow)
    lc.clearDisplay(1); lc.clearDisplay(2);
    lc.setRow(1, 0, B00011101); lc.setRow(2, 0, B11000000);
    lc.setRow(1, 1, B00010000); lc.setRow(2, 1, B01000000);
    lc.setRow(1, 2, B00010000); lc.setRow(2, 2, B01000000);
    lc.setRow(1, 3, B00011101); lc.setRow(2, 3, B11000000);
    lc.setRow(1, 4, B00010000); lc.setRow(2, 4, B01000000);
    lc.setRow(1, 5, B00010000); lc.setRow(2, 5, B01000000);
    lc.setRow(1, 6, B00011101); lc.setRow(2, 6, B11000000);
    lc.setRow(1, 7, B00010000); lc.setRow(2, 7, B01000000);

    tone(buzzerPin, 800, 80);                       // Soft beep
    delay(1200);                                    // Wait for animation
  }

  // Distance between 100 and 200cm → Warning
  else if (distance > 100) {
    setRGBColorAnalog(255, 255, 0);                 // Set RGB to yellow
    lcd.setCursor(0, 2);
    lcd.print("Warning!     ");                   // Show warning message

    // Medium alert animation
    lc.clearDisplay(1); lc.clearDisplay(2);
    lc.setRow(1, 0, B00011101); lc.setRow(2, 0, B11000000);
    lc.setRow(1, 1, B00010000); lc.setRow(2, 1, B01000000);
    lc.setRow(1, 2, B00010000); lc.setRow(2, 2, B01000000);
    lc.setRow(1, 3, B00011101); lc.setRow(2, 3, B11000000);
    lc.setRow(1, 4, B00010000); lc.setRow(2, 4, B01000000);

    tone(buzzerPin, 1000, 80);                      // Medium pitch beep
    delay(500);                                     // Shorter delay
  }

  // Distance between 40 and 100cm → Danger
  else if (distance > 40) {
    setRGBColorAnalog(255, 100, 0);                 // Set RGB to orange
    lcd.setCursor(0, 2);
    lcd.print("Danger!     ");                    // Show danger message

    // Aggressive alert animation
    lc.clearDisplay(1); lc.clearDisplay(2);
    lc.setRow(1, 0, B00011101); lc.setRow(2, 0, B11000000);
    lc.setRow(1, 1, B00010000); lc.setRow(2, 1, B01000000);

    tone(buzzerPin, 1200, 70);                      // Higher pitch beep
    delay(200);                                     // Short delay
  }

  // Distance ≤ 40cm → Immediate Danger
  else {
    setRGBColorAnalog(255, 0, 0);                   // Set RGB to red

    // Handle blinking state every `blinkInterval` ms
    if (currentMillis - lastToggleTime >= blinkInterval) {
      lastToggleTime = currentMillis;
      state = !state;                               // Toggle display state
    }

    if (state) {
      lcd.setCursor(0, 2);
      lcd.print("Obstacle: REAR!!  ");              // Immediate rear warning
      tone(buzzerPin, 1500);                        // Continuous high-pitched beep

      // Blinking vertical line pattern on dot matrix
      lc.setRow(1, 0, B00000001); lc.setRow(2, 0, B10000000);
      lc.setRow(1, 1, B00000001); lc.setRow(2, 1, B10000000);
      lc.setRow(1, 2, B00000001); lc.setRow(2, 2, B10000000);
      lc.setRow(1, 3, B00000001); lc.setRow(2, 3, B10000000);
      lc.setRow(1, 4, B00000001); lc.setRow(2, 4, B10000000);
      lc.setRow(1, 6, B00000001); lc.setRow(2, 6, B10000000);
      lc.setRow(1, 7, B00000001); lc.setRow(2, 7, B10000000);
    } else {
      // Clear display and stop buzzer on alternate blink
      lcd.setCursor(0, 2);
      lcd.print("                  ");
      noTone(buzzerPin);
      lc.clearDisplay(1);
      lc.clearDisplay(2);
    }
  }
}

// Clears all displays and RGB LED when in neutral or no-warning state
void clearNeutralStateDisplays() {
  // Clear both 8x8 dot matrix displays
  lc.clearDisplay(1);
  lc.clearDisplay(2);

  // Clear 7-segment display
  display.clear();

  // Clear LCD rows 1 and 2 (used for distance and warning messages)
  lcd.setCursor(0, 1);
  lcd.print("                    ");  // Clear row 1 (20 blank spaces)
  lcd.setCursor(0, 2);
  lcd.print("                    ");  // Clear row 2 (20 blank spaces)

  // Turn off the RGB LED
  setRGBColorAnalog(0, 0, 0);
}

// Updates the RGB LED color based on gear state and forward obstacle detection
void updateRGBColorBasedOnGearAndObstacle() {
  if (currentGear == "Forward") {
    if (frontObstacleDetected) {
      // Red if an obstacle is detected while in Forward gear
      setRGBColorAnalog(255, 0, 0);
    } else {
      // Green when driving forward with no obstacle
      setRGBColorAnalog(0, 255, 0);
    }
  } else {
    // In gears other than Forward, turn off RGB LED
    setRGBColorAnalog(0, 0, 0);
  }
}

// Continuously monitors the PIR sensor to detect obstacles in front and shows warnings
void checkPIRWarning() {
  static bool state = false;                    // Used to toggle blinking state
  static unsigned long lastToggleTime = 0;      // Timestamp of last toggle
  const unsigned long blinkInterval = 200;      // Blink interval for obstacle alert
  unsigned long currentMillis = millis();       // Current time

  int pirState = digitalRead(PIR_PIN);          // Read PIR sensor state

  if (currentGear == "Forward" && pirState == HIGH) {
    // Obstacle detected in front while in Forward gear
    frontObstacleDetected = true;

    // Toggle blinking based on time interval
    if (currentMillis - lastToggleTime >= blinkInterval) {
      lastToggleTime = currentMillis;
      state = !state;
    }

    if (state) {
      // Display warning message on LCD and play buzzer
      lcd.setCursor(0, 1);
      lcd.print("Obstacle: FRONT!!  ");
      tone(buzzerPin, 1500);

      // Blink a vertical line on both dot matrices
      lc.setRow(1, 0, B00000001); lc.setRow(2, 0, B10000000);
      lc.setRow(1, 1, B00000001); lc.setRow(2, 1, B10000000);
      lc.setRow(1, 2, B00000001); lc.setRow(2, 2, B10000000);
      lc.setRow(1, 3, B00000001); lc.setRow(2, 3, B10000000);
      lc.setRow(1, 4, B00000001); lc.setRow(2, 4, B10000000);
      lc.setRow(1, 6, B00000001); lc.setRow(2, 6, B10000000);
      lc.setRow(1, 7, B00000001); lc.setRow(2, 7, B10000000);
    } else {
      // Clear displays and silence buzzer between blinks
      noTone(buzzerPin);
      lc.clearDisplay(1);
      lc.clearDisplay(2);
      lcd.setCursor(0, 1);
      lcd.print("                   ");
    }
  } else {
    // No obstacle detected or not in Forward gear
    frontObstacleDetected = false;
    lc.clearDisplay(1);
    lc.clearDisplay(2);
    noTone(buzzerPin);
    lcd.setCursor(0, 1);
    lcd.print("                   ");
    lcd.setCursor(0, 2);
    lcd.print("                   ");

    // Set appropriate LED color
    if (currentGear == "Forward") {
      setRGBColorAnalog(0, 255, 0); // Green for safe Forward gear
    } else {
      setRGBColorAnalog(0, 0, 0);   // Off otherwise
    }
  }

  // Ensure LED reflects current state
  updateRGBColorBasedOnGearAndObstacle();
}

// =================== PART 4 : DOOR CONTROL ===================

// Initialize servo pins for doors
void initializeDoors() {
  door1.attach(8);
  door2.attach(4);
  door3.attach(7);
  door4.attach(5);
}

// Read a command from the Serial Monitor
String readSerialCommand() {
  if (Serial.available()) {
    String cmd = Serial.readStringUntil('\n');
    cmd.trim();  // Remove extra spaces
    return cmd;
  }
  return "";
}

// Open a specific door by number (1 to 4)
void openDoor(int door) {
  getServoByNumber(door).write(0);
  Serial.print("Door ");
  Serial.print(door);
  Serial.println(" opened.");
}

// Close a specific door by number (1 to 4)
void closeDoor(int door) {
  getServoByNumber(door).write(90);
  Serial.print("Door ");
  Serial.print(door);
  Serial.println(" closed.");
}

// Open all doors
void openAllDoors() {
  for (int i = 1; i <= 4; i++) {
    getServoByNumber(i).write(0);
  }
  Serial.println("All doors opened.");
}

// Close all doors
void closeAllDoors() {
  for (int i = 1; i <= 4; i++) {
    getServoByNumber(i).write(90);
  }
  Serial.println("All doors closed.");
}

// Internal close method (no serial output)
void close() {
  for (int i = 1; i <= 4; i++) {
    getServoByNumber(i).write(90);
  }
}

// Return the corresponding Servo object for a door number
Servo& getServoByNumber(int door) {
  switch (door) {
    case 1: return door1;
    case 2: return door2;
    case 3: return door3;
    case 4: return door4;
    default: return door1;
  }
}

// Array to track open/close status of each door
bool doorStates[4] = { false, false, false, false };

// Read potentiometers and update door angles and states
void updateDoorsFromPots() {
  // Read analog values from 4 potentiometers
  int potValues[4] = {
    analogRead(POT1_PIN),
    analogRead(POT2_PIN),
    analogRead(POT3_PIN),
    analogRead(POT4_PIN)
  };

  int angles[4];  // Mapped servo angles for each door
  static bool wasOpen[4] = {false, false, false, false};  // Previous door open/closed states

  // Convert potentiometer values to servo angles (0–90 degrees)
  for (int i = 0; i < 4; i++) {
    angles[i] = map(potValues[i], 0, 1023, 0, 90);
  }

  // Move servos based on calculated angles
  door1.write(angles[0]);
  door2.write(angles[1]);
  door3.write(angles[2]);
  door4.write(angles[3]);

  bool updated = false;  // Flag to check if any door state changed

  // Check if each door is open or closed and print state if changed
  for (int i = 0; i < 4; i++) {
    bool isOpen = (angles[i] <= 80);  // Consider open if angle is ≤ 80°
    if (isOpen != wasOpen[i]) {
      if (isOpen) {
        Serial.print("Door ");
        Serial.print(i + 1);
        Serial.println(" opened (via potentiometer).");
      } else {
        Serial.print("Door ");
        Serial.print(i + 1);
        Serial.println(" closed (via potentiometer).");
      }
      wasOpen[i] = isOpen;          // Update stored state
      doorStates[i] = isOpen;       // Update global state for display
      updated = true;
    }
  }

  // If any door state changed, update the LCD display
  if (updated) {
    lcd.clear();
    displayCarDynamic();  // Draw updated car with door states

    // Show symbolic door state on LCD using brackets
    for (int i = 0; i < 4; i++) {
      if (wasOpen[i]) {
        switch (i) {
          case 0: lcd.setCursor(8, 1); lcd.print("["); break;
          case 1: lcd.setCursor(11, 1); lcd.print("]"); break;
          case 2: lcd.setCursor(8, 2); lcd.print("["); break;
          case 3: lcd.setCursor(11, 2); lcd.print("]"); break;
        }
      } else {
        switch (i) {
          case 0: lcd.setCursor(8, 1); lcd.print(" "); break;
          case 1: lcd.setCursor(11, 1); lcd.print(" "); break;
          case 2: lcd.setCursor(8, 2); lcd.print(" "); break;
          case 3: lcd.setCursor(11, 2); lcd.print(" "); break;
        }
      }
    }
  }
}

// Custom characters for LCD to show doors and car components
byte door2_closed[]         = { B11111, B11101, B11101, B11101, B11101, B11101, B11101, B11111 };
byte left_kapot[]           = { B00111, B01111, B11011, B11111, B11111, B11111, B11111, B10000 };
byte right_kapot[]          = { B11100, B11110, B11011, B11111, B11111, B11111, B11111, B00001 };
byte door1_closed[]         = { B11111, B10111, B10111, B10111, B10111, B10111, B10111, B11111 };
byte door3_closed[]         = { B11111, B10111, B10111, B10111, B10111, B10111, B10111, B11111 };
byte door4_closed[]         = { B11111, B11101, B11101, B11101, B11101, B11101, B11101, B11111 };
byte right_bottom_kapot[]   = { B00001, B11111, B11111, B11111, B11111, B11011, B11110, B11100 };
byte left_bottom_kapot[]    = { B10000, B11111, B11111, B11111, B11111, B11011, B01111, B00111 };

byte door1_openned[]        = { B01111, B00111, B00111, B00111, B00111, B00111, B00111, B01111 };
byte door2_openned[]        = { B11110, B11100, B11100, B11100, B11100, B11100, B11100, B11110 };
byte door3_openned[]        = { B01111, B00111, B00111, B00111, B00111, B00111, B00111, B01111 };
byte door4_openned[]        = { B11110, B11100, B11100, B11100, B11100, B11100, B11100, B11110 };

// Show the car’s layout and current door states using custom characters
void displayCarDynamic() {
  // Show fixed parts of the car: top and bottom hood (kapots)
  lcd.createChar(1, left_kapot);
  lcd.setCursor(9, 0);
  lcd.write(1);

  lcd.createChar(2, right_kapot);
  lcd.setCursor(10, 0);
  lcd.write(2);

  lcd.createChar(6, right_bottom_kapot);
  lcd.setCursor(10, 3);
  lcd.write(6);

  lcd.createChar(7, left_bottom_kapot);
  lcd.setCursor(9, 3);
  lcd.write(7);

  // Load door characters based on whether each door is open or closed
  lcd.createChar(3, doorStates[0] ? door1_openned : door1_closed);
  lcd.setCursor(9, 1);
  lcd.write(3);

  lcd.createChar(0, doorStates[1] ? door2_openned : door2_closed);
  lcd.setCursor(10, 1);
  lcd.write(0);

  lcd.createChar(4, doorStates[2] ? door3_openned : door3_closed);
  lcd.setCursor(9, 2);
  lcd.write(4);

  lcd.createChar(5, doorStates[3] ? door4_openned : door4_closed);
  lcd.setCursor(10, 2);
  lcd.write(5);
}

// Mark a specific door as open and update the LCD display accordingly
void openDoorCh(int doorNum) {
  if (doorNum >= 1 && doorNum <= 4) {
    doorStates[doorNum - 1] = true;   // Set the door state to open (true)
    displayCarDynamic();               // Refresh the dynamic car display on LCD

    // Update only the specific door symbol on the LCD without clearing entire screen
    switch (doorNum) {
      case 1: lcd.setCursor(8, 1); lcd.print("["); break;  // Left bracket for door 1
      case 2: lcd.setCursor(11, 1); lcd.print("]"); break; // Right bracket for door 2
      case 3: lcd.setCursor(8, 2); lcd.print("["); break;  // Left bracket for door 3
      case 4: lcd.setCursor(11, 2); lcd.print("]"); break; // Right bracket for door 4
    }
  }
}

// Mark a specific door as closed and update the LCD display accordingly
void closeDoorCh(int doorNum) {
  if (doorNum >= 1 && doorNum <= 4) {
    doorStates[doorNum - 1] = false;  // Set the door state to closed (false)
    displayCarDynamic();               // Refresh the dynamic car display on LCD

    // Erase the door symbol on the LCD by printing a space at the door position
    switch (doorNum) {
      case 1: lcd.setCursor(8, 1); lcd.print(" "); break;  // Clear symbol for door 1
      case 2: lcd.setCursor(11, 1); lcd.print(" "); break; // Clear symbol for door 2
      case 3: lcd.setCursor(8, 2); lcd.print(" "); break;  // Clear symbol for door 3
      case 4: lcd.setCursor(11, 2); lcd.print(" "); break; // Clear symbol for door 4
    }
  }
}

// Mark all doors as open and update the LCD display accordingly
void openAllDoorsCh() {
  for (int i = 0; i < 4; i++) doorStates[i] = true;  // Set all door states to open (true)

  lcd.clear();               // Clear entire LCD screen
  displayCarDynamic();       // Refresh the dynamic car display on LCD

  // Print the side brackets representing open doors for all four doors
  lcd.setCursor(8, 1); lcd.print("[");
  lcd.setCursor(11, 1); lcd.print("]");
  lcd.setCursor(8, 2); lcd.print("[");
  lcd.setCursor(11, 2); lcd.print("]");
}

// Mark all doors as closed and update the LCD display accordingly
void closeAllDoorsCh() {
  for (int i = 0; i < 4; i++) doorStates[i] = false;  // Set all door states to closed (false)

  lcd.clear();               // Clear entire LCD screen
  displayCarDynamic();       // Refresh the dynamic car display on LCD

  // Remove all side brackets by printing spaces at all door positions
  lcd.setCursor(8, 1); lcd.print(" ");
  lcd.setCursor(11, 1); lcd.print(" ");
  lcd.setCursor(8, 2); lcd.print(" ");
  lcd.setCursor(11, 2); lcd.print(" ");
}