#include <Wire.h>
#include <LiquidCrystal_I2C.h>

// LCD I2C address (usually 0x27 or 0x3F)
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Ultrasonic sensor pins
#define TRIG_PIN 5
#define ECHO_PIN 18

// LED pins
#define GREEN_LED 13   // 20% < level ≤ 50%
#define YELLOW_LED 12  // 50% < level ≤ 65%
#define RED_LED 14     // >65% (steady until 85%, then blinking)

// Optional potentiometer pin (for simulation)
#define POT_PIN 34

// Thresholds (%)
#define LOW_THRESHOLD 20       // below this: all LEDs off
#define NORMAL_MAX 50          // up to this: green on
#define WARNING_MAX 65         // up to this: yellow on
#define CRITICAL_MAX 85        // up to this: red steady, above this: red blinking

// Manhole / tank dimensions (adjust to your setup)
#define TANK_HEIGHT_CM 300.0    // total depth of manhole from sensor to bottom
#define SENSOR_OFFSET_CM 50.0   // distance from sensor to bottom when empty

// Blinking interval for red LED (milliseconds)
#define BLINK_INTERVAL 200

float sewage_level_percent = 0;
bool usePotentiometer = true;  // set to true if using potentiometer instead of ultrasonic

// For averaging ultrasonic readings (reduces noise)
const int numReadings = 5;
float readings[numReadings];
int readIndex = 0;
float total = 0;
float average = 0;

// For blinking red LED
unsigned long previousMillis = 0;
bool redLedState = LOW;

void setup() {
  Serial.begin(115200);
  Serial.println("Sewage Level Monitor Starting...");

  // Initialize LCD
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("Sewage Level");
  lcd.setCursor(0, 1);
  lcd.print("Monitor");
  delay(2000);
  lcd.clear();

  // Set pin modes
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(GREEN_LED, OUTPUT);
  pinMode(YELLOW_LED, OUTPUT);
  pinMode(RED_LED, OUTPUT);

  // Ensure all LEDs start off
  digitalWrite(GREEN_LED, LOW);
  digitalWrite(YELLOW_LED, LOW);
  digitalWrite(RED_LED, LOW);

  if (usePotentiometer) {
    pinMode(POT_PIN, INPUT);
  } else {
    // Initialize averaging array
    for (int i = 0; i < numReadings; i++) {
      readings[i] = 0;
    }
  }
}

void loop() {
  // Read sewage level from selected source
  if (usePotentiometer) {
    int potValue = analogRead(POT_PIN);
    sewage_level_percent = map(potValue, 0, 4095, 0, 100);
  } else {
    sewage_level_percent = readStableUltrasonicLevel();
  }

  // Constrain to valid range
  sewage_level_percent = constrain(sewage_level_percent, 0, 100);

  // Update LEDs and LCD
  updateLEDs(sewage_level_percent);
  updateLCD(sewage_level_percent);

  // Optional: print level to Serial Monitor (can be removed if not needed)
  Serial.print("Sewage Level: ");
  Serial.print(sewage_level_percent);
  Serial.println("%");

  delay(200); // update every 200ms
}

// Returns averaged ultrasonic reading (caters for noise and interference from data intake from sensor)
float readStableUltrasonicLevel() {
  float newReading = readRawUltrasonic();
  total = total - readings[readIndex];
  readings[readIndex] = newReading;
  total = total + newReading;
  readIndex = (readIndex + 1) % numReadings;
  average = total / numReadings;
  return average;
}

// Single ultrasonic measurement
float readRawUltrasonic() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  long duration = pulseIn(ECHO_PIN, HIGH, 30000); // timeout 30ms
  if (duration == 0) return sewage_level_percent; // keep previous if timeout

  float distance = duration * 0.034 / 2;          // convert to cm
  float max_distance = SENSOR_OFFSET_CM + TANK_HEIGHT_CM;
  float sewage_height = max_distance - distance;   // height of sewage in manhole
  sewage_height = constrain(sewage_height, 0, TANK_HEIGHT_CM);
  return (sewage_height / TANK_HEIGHT_CM) * 100.0; // percentage
}

// Controls LEDs according to the new logic
void updateLEDs(float level) {
  // Turn off all LEDs first
  digitalWrite(GREEN_LED, LOW);
  digitalWrite(YELLOW_LED, LOW);
  digitalWrite(RED_LED, LOW);

  // Determine which LED to activate
  if (level > LOW_THRESHOLD && level <= NORMAL_MAX) {
    // Normal range: green steady
    digitalWrite(GREEN_LED, HIGH);
  }
  else if (level > NORMAL_MAX && level <= WARNING_MAX) {
    // Caution range: yellow steady
    digitalWrite(YELLOW_LED, HIGH);
  }
  else if (level > WARNING_MAX && level <= CRITICAL_MAX) {
    // Critical range: red steady
    digitalWrite(RED_LED, HIGH);
  }
  else if (level > CRITICAL_MAX) {
    // Overflow range: red blinking
    unsigned long currentMillis = millis();
    if (currentMillis - previousMillis >= BLINK_INTERVAL) {
      previousMillis = currentMillis;
      redLedState = !redLedState;
      digitalWrite(RED_LED, redLedState);
    }
  }
  // For level ≤ LOW_THRESHOLD, all LEDs remain off
}

// Updates LCD with level and appropriate message
void updateLCD(float level) {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Level: ");
  lcd.print(level, 1);
  lcd.print("%");

  lcd.setCursor(0, 1);
  if (level <= LOW_THRESHOLD) {
    lcd.print("Low Levels");
  }
  else if (level > LOW_THRESHOLD && level <= NORMAL_MAX) {
    lcd.print("Normal Levels");
  }
  else if (level > NORMAL_MAX && level <= WARNING_MAX) {
    lcd.print("Caution Rising!!");
  }
  else if (level > WARNING_MAX && level <= CRITICAL_MAX) {
    lcd.print("Critical Unblock Manhole!!");
  }
  else if (level > CRITICAL_MAX) {
    lcd.print("Sewage Overflowing!");
  }
}