#include <ESP32Servo.h>       // Include the ESP32Servo library for ESP32
#include <Wire.h>              // Include the Wire library for I2C communication
#include <LiquidCrystal_I2C.h> // Include LiquidCrystal_I2C library for I2C LCD

const int trigPin = 12;       // GPIO for trigger pin of the ultrasonic sensor
const int echoPin = 13;       // GPIO for echo pin of the ultrasonic sensor
const int buzzerPin = 27;     // GPIO for the buzzer
const int bridgeServoPins[] = {14, 15}; // GPIOs for bridge servos
const int gateServoPins[] = {16, 17};   // GPIOs for gate servos
const int pirSensorPin = 34;  // GPIO for PIR sensor

const int criticalWaterLevel = 20; // Define the critical water level (in cm)
const int initialWaterLevel = 20;  // Define the initial water level threshold (in cm)

const int bridgeUpPosition = 0;     // Servo position to raise the bridge
const int bridgeDownPosition = 90;  // Servo position to lower the bridge
const int gateOpenPosition = 90;    // Gate position when open
const int gateClosedPosition = 0;   // Gate position when closed
const int buzzerFrequency = 1500;    // Frequency of the buzzer sound in Hz

const int redPin = 33;              // GPIO for RGB LED red
const int greenPin = 32;            // GPIO for RGB LED green
const int bluePin = 26;             // GPIO for RGB LED blue

Servo bridgeServos[2];       // Array for bridge servos
Servo gateServos[2];         // Array for gate servos
bool bridgeRaised = false;   // Flag to keep track of the bridge state

// Define SDA and SCL pins for I2C communication
const int SDA_PIN = 21;  // Default SDA pin
const int SCL_PIN = 22;  // Default SCL pin

// Initialize the 20x4 I2C LCD at address 0x27 (this may change based on the I2C scanner result)
LiquidCrystal_I2C lcd(0x27, 20, 4);

void setup() {
  Wire.begin(SDA_PIN, SCL_PIN);
  Serial.begin(115200);

  bool addressFound = false;
  for (uint8_t address = 1; address < 127; ++address) {
    Wire.beginTransmission(address);
    if (Wire.endTransmission() == 0) {
      lcd = LiquidCrystal_I2C(address, 20, 4); // Set the detected address to the LCD
      addressFound = true;
      break; // Stop after finding the first device
    }
    delay(50);
  }

  if (!addressFound) {
    Serial.println("No I2C device found. Check connections.");
    while (true); // Stop if no device is found
  }

  // Initialize LCD with the detected address
  lcd.begin(20, 4);
  lcd.backlight();

  // Initialize sensors, servos, and RGB LED pins
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(buzzerPin, OUTPUT);
  pinMode(pirSensorPin, INPUT);
  pinMode(redPin, OUTPUT);
  pinMode(greenPin, OUTPUT);
  pinMode(bluePin, OUTPUT);

  // Attach bridge servos to their respective pins
  for (int i = 0; i < 2; i++) {
    bridgeServos[i].setPeriodHertz(50);
    bridgeServos[i].attach(bridgeServoPins[i]);
    bridgeServos[i].write(bridgeDownPosition); // Initialize to down position
  }

  // Attach gate servos to their respective pins and set initial position to closed (0 degrees)
  for (int i = 0; i < 2; i++) {
    gateServos[i].setPeriodHertz(50);
    gateServos[i].attach(gateServoPins[i]);
    gateServos[i].write(gateClosedPosition); // Initialize to closed position (0 degrees)
  }

  // Display initial message
  showInitialMessage();
}

void loop() {
  float distance = getDistance();  // Get the distance from the ultrasonic sensor
  float waterlevel = 35.0 - distance;  // Subtract the distance from 35 cm to get the water level
  Serial.print("Measured Water Level: ");
  Serial.print(waterlevel);
  Serial.println(" cm");

  bool pirSensorDetected = digitalRead(pirSensorPin) == HIGH;

  if (waterlevel < initialWaterLevel) {
    if (bridgeRaised) {
      displayAlert("Bridge lowering...", 7);
      countdown(7);
      pirSensorDetected = digitalRead(pirSensorPin) == HIGH;
      if (!pirSensorDetected) {
        adjustBridgeHeight(bridgeDownPosition);
        bridgeRaised = false;
        showWaterLevelMessage();
      }
    }
  } else if (waterlevel >= criticalWaterLevel) {
    if (!bridgeRaised) {
      displayAlert("WARNING: Evacuate!", 7);
      countdown(7);
      pirSensorDetected = digitalRead(pirSensorPin) == HIGH;
      if (!pirSensorDetected) {
        adjustBridgeHeight(bridgeUpPosition);
        bridgeRaised = true;
        showCautionMessage();
      }
    }
  } else if (bridgeRaised) {
    showCautionMessage();
  } else {
    showWaterLevelMessage();
    setRGBColor(0, 255, 0); // Green when safe (no water or warning)
  }

  if (pirSensorDetected) {
    Serial.println("Motion Detected");
  } else {
    Serial.println("No Motion Detected");
  }

  delay(1000);
}

float getDistance() {
  long duration;

  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH);
  return (duration / 2.0) / 29.1;
}

void activateAlert() {
  tone(buzzerPin, buzzerFrequency);
  flashPoliceLight();
}

void stopAlert() {
  noTone(buzzerPin);
  setRGBColor(0, 255, 0); // Green when safe
}

void flashPoliceLight() {
  setRGBColor(255, 0, 0);
  delay(250);
  setRGBColor(0, 0, 255);
  delay(100);
}

void adjustBridgeHeight(int position) {
  int currentPosition = bridgeServos[0].read();
  int steps = abs(position - currentPosition);
  int delayTime = 3000 / steps;

  // Move gate servos to open position (90 degrees) before moving the bridge
  for (int i = 0; i < 2; i++) {
    gateServos[i].write(gateOpenPosition);
  }

  // Move the bridge servos to the desired position
  for (int i = currentPosition; i != position; i += (position > currentPosition ? 1 : -1)) {
    for (int j = 0; j < 2; j++) {
      bridgeServos[j].write(i);
    }
    delay(delayTime);
  }

  // Delay for 3 seconds, then return gate servos to closed position (0 degrees)
  delay(1000);
  for (int i = 0; i < 2; i++) {
    gateServos[i].write(gateClosedPosition);
  }
}

void displayAlert(const char* message, int countdownStart) {
  lcd.clear();
  lcd.setCursor((20 - strlen(message)) / 2, 0);
  lcd.print(message);
  lcd.setCursor((20 - strlen("Elevating in ")) / 2, 1);
  lcd.print("Elevating in ");
  lcd.setCursor(16, 1);
  lcd.print(countdownStart);
  lcd.print(" s");
}

void countdown(int start) {
  for (int i = start; i >= 0; i--) {
    lcd.setCursor(16, 1);
    lcd.print(i);
    activateAlert();
    delay(1000);
    stopAlert();
  }
}

void clearDisplay() {
  lcd.clear();
}

void showWaterLevelMessage() {
  clearDisplay();
  lcd.setCursor((20 - strlen("Water Level: Low")) / 2, 0);
  lcd.print("Water Level: Low");
  lcd.setCursor((20 - strlen("Bridge Safe to Cross")) / 2, 1);
  lcd.print("Bridge Safe to Cross");
}

void showCautionMessage() {
  clearDisplay();
  lcd.setCursor((20 - strlen("Bridge Elevated")) / 2, 0);
  lcd.print("Bridge Elevated");
  lcd.setCursor((20 - strlen("Proceed with Caution")) / 2, 1);
  lcd.print("Proceed with Caution");
}

void setRGBColor(int red, int green, int blue) {
  analogWrite(redPin, red);
  analogWrite(greenPin, green);
  analogWrite(bluePin, blue);
}

void showInitialMessage() {
  clearDisplay();
  lcd.setCursor((20 - strlen("Keep Safe Everyone")) / 2, 0); 
  lcd.print("Keep Safe Everyone");
  lcd.setCursor((20 - strlen("Stay Alert and Aware")) / 2, 1); 
  lcd.print("Stay Alert and Aware");
}