#include 
#include 

// Create LCD object (I2C address 0x27)
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Pins
const int buzzerPin = 2;
const int greenPin  = 3;
const int yellowPin = 4;
const int redPin    = 5;
const int trigPin   = 11;
const int echoPin   = 10;

// Distance thresholds (cm)
#define SAFE_DIST     13      // Safe when distance >= 13 cm
#define WARNING_DIST   5      // Warning when distance >= 5 cm

// System states
enum State { SAFE, WARNING, DANGER };

// Current running state
State currentState = SAFE;
// Target state based on distance
State nextState = SAFE;
// Last state for LCD update
State lastState = SAFE;

// Timers for non-blocking control
unsigned long ledTimer  = 0;
unsigned long beepTimer = 0;
unsigned long lcdTimer  = 0;

// Yellow LED status
bool yellowOn = false;

// Read distance from ultrasonic sensor
float getDistance() {
  // Send trigger pulse
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Read echo time
  unsigned long duration = pulseIn(echoPin, HIGH, 30000);

  // If no echo, return a large value
  if (duration == 0) return 999;

  // Convert time to distance (cm)
  return duration / 58.0;
}

// Show distance on LCD
void updateDistanceLine(float dist) {
  lcd.setCursor(0, 0);
  lcd.print("Dist: ");

  if (dist > 99) {
    lcd.print("--.- cm ");
  } else {
    lcd.print(dist, 1);
    lcd.print(" cm   ");
  }
}

// Show system status on LCD
void updateStatusLine(State state) {
  lcd.setCursor(0, 1);
  lcd.print("Status:        ");
  lcd.setCursor(8, 1);

  if (state == SAFE)    lcd.print("SAFE");
  if (state == WARNING) lcd.print("WARNING");
  if (state == DANGER)  lcd.print("DANGER");
}

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

  // Set pin modes
  pinMode(buzzerPin, OUTPUT);
  pinMode(greenPin, OUTPUT);
  pinMode(yellowPin, OUTPUT);
  pinMode(redPin, OUTPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // Stop buzzer at start
  noTone(buzzerPin);

  // Initialize LCD
  lcd.init();
  lcd.backlight();
  lcd.clear();

  // Show initial LCD content
  updateDistanceLine(999);
  updateStatusLine(currentState);
  lastState = currentState;
}

void loop() {
  // Read distance
  float dist = getDistance();
  Serial.println(dist);

  // Decide next state based on distance
  if (dist >= SAFE_DIST)         nextState = SAFE;
  else if (dist >= WARNING_DIST) nextState = WARNING;
  else                           nextState = DANGER;

  unsigned long now = millis();

  // Run state machine
  switch (currentState) {

    case SAFE:
      digitalWrite(greenPin, HIGH);
      digitalWrite(yellowPin, LOW);
      digitalWrite(redPin, LOW);
      noTone(buzzerPin);
      currentState = nextState;
      break;

    case WARNING:
      digitalWrite(greenPin, LOW);
      digitalWrite(redPin, LOW);

      // Blink yellow LED every 500 ms
      if (now - ledTimer >= 500) {
        ledTimer = now;
        yellowOn = !yellowOn;
        digitalWrite(yellowPin, yellowOn);

        // Buzzer follows yellow LED
        if (yellowOn) tone(buzzerPin, 2000);
        else noTone(buzzerPin);

        // Change state only after one blink cycle
        if (currentState != nextState && !yellowOn) {
          currentState = nextState;
        }
      }
      break;

    case DANGER:
      digitalWrite(greenPin, LOW);
      digitalWrite(yellowPin, LOW);
      digitalWrite(redPin, HIGH);

      // Beep every 300 ms
      if (now - beepTimer >= 300) {
        beepTimer = now;
        tone(buzzerPin, 2000, 120);

        // Change state at the end of beep cycle
        if (currentState != nextState) {
          noTone(buzzerPin);
          currentState = nextState;
        }
      }
      break;
  }

  // Update distance on LCD every 250 ms
  if (now - lcdTimer >= 250) {
    lcdTimer = now;
    updateDistanceLine(dist);
  }

  // Update status on LCD only when state changes
  if (currentState != lastState) {
    updateStatusLine(currentState);
    lastState = currentState;
  }
}