#include 
#include 
#include 

// Servo and ultrasonic sensor
Servo radarServo;
const int trigPin    = 11;
const int echoPin    = 10;
const int servoPin   = 12;

// LEDs and passive buzzer (controlled by tone()/noTone())
const int redLed     = 3;
const int greenLed   = 4;
const int buzzer     = 2;

// I2C LCD (address 0x27, 16×2 characters)
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Configuration parameters
const int alertDistance        = 30;     // distance threshold for alert (cm)
const unsigned long updateInterval   = 20;     // servo scan interval (ms)
const int triggerThreshold     = 2;      // number of consecutive detections needed
const unsigned long displayDuration     = 500;    // LCD display time (ms)
const unsigned long blinkInterval       = 200;    // LED blink interval (ms)

// Scanning state
int currentAngle     = 0;
int increment        = 1;
unsigned long lastServoUpdate = 0;

// Alert control
bool displayAlarm    = false;  // whether showing text on LCD
bool alarmActive     = false;  // whether in blink alert state
int triggerCount     = 0;
int detectedAngle    = 0;
unsigned long displayStartTime = 0;
unsigned long lastBlinkTime     = 0;
bool blinkState       = false;

void setup() {
  radarServo.attach(servoPin);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(redLed, OUTPUT);
  pinMode(greenLed, OUTPUT);
  // buzzer uses tone()/noTone(), no pinMode needed

  lcd.init();
  lcd.backlight();
  lcd.clear();

  // show idle state
  lcdSetEmpty();
  digitalWrite(greenLed, HIGH);
}

void loop() {
  unsigned long now = millis();

  // 1. Smooth scanning & distance measurement
  if (now - lastServoUpdate >= updateInterval) {
    lastServoUpdate = now;
    radarServo.write(currentAngle);

    if (currentAngle % 5 == 0) {
      float d = getDistance();

      // if not already showing or blinking an alert
      if (!displayAlarm && !alarmActive) {
        if (d < alertDistance) triggerCount++;
        else                   triggerCount = 0;

        if (triggerCount >= triggerThreshold) {
          triggerCount = 0;
          detectedAngle = currentAngle;
          startAlarmDisplay();
        }
      }
      // if currently blinking and object is out of range, exit alert
      else if (alarmActive && d >= alertDistance) {
        exitAlarm();
      }
    }

    // update servo angle back and forth between 0°–180°
    currentAngle += increment;
    if (currentAngle >= 180) {
      currentAngle = 180; increment = -1;
    } else if (currentAngle <= 0) {
      currentAngle = 0;   increment = 1;
    }
  }

  // 2. Manage LCD display duration & transition to blinking
  if (displayAlarm) {
    // if display time has passed, switch to blinking state
    if (now - displayStartTime >= displayDuration) {
      displayAlarm = false;
      alarmActive  = true;
      lastBlinkTime = now;
    }
  }
  else if (alarmActive) {
    alarmBlink();
  }
}

// start showing alert text (for displayDuration)
void startAlarmDisplay() {
  displayAlarm     = true;
  alarmActive      = false;
  displayStartTime = millis();

  digitalWrite(greenLed, LOW);
  digitalWrite(redLed, LOW);
  noTone(buzzer);

  lcd.clear();
  // first line: centered "{detectedAngle}°"
  char buf[6];
  sprintf(buf, "%d", detectedAngle);
  int len1 = strlen(buf) + 1;
  int start1 = (16 - len1) / 2;
  lcd.setCursor(start1, 0);
  lcd.print(buf);
  lcd.write(223);

  // second line: centered "Foreign Body"
  const char* msg = "Foreign Body";
  int len2 = strlen(msg);
  int start2 = (16 - len2) / 2;
  lcd.setCursor(start2, 1);
  lcd.print(msg);
}

// non-blocking blink of red LED and buzzer
void alarmBlink() {
  unsigned long now = millis();
  if (now - lastBlinkTime >= blinkInterval) {
    lastBlinkTime = now;
    blinkState = !blinkState;
    digitalWrite(redLed, blinkState);
    if (blinkState) tone(buzzer, 1000);
    else           noTone(buzzer);
  }
}

// exit alert and return to idle state
void exitAlarm() {
  displayAlarm = false;
  alarmActive  = false;
  triggerCount = 0;

  digitalWrite(redLed, LOW);
  noTone(buzzer);
  digitalWrite(greenLed, HIGH);
  lcdSetEmpty();
}

// show idle state on LCD
void lcdSetEmpty() {
  lcd.clear();
  lcd.setCursor(1, 0);
  lcd.print("Area is Empty");
}

// perform ultrasonic distance measurement (cm)
float getDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long duration = pulseIn(echoPin, HIGH, 30000);
  return duration * 0.034 / 2;
}