#include 
#include 
#include 
#include 

// --------------------- Pins ---------------------
// Ultrasonic
const int trigPin = 6;
const int echoPin = 5;

// Servo (scanner)
const int servoPin = 7;

// LEDs & Active Buzzer
const int redLed   = 3;
const int greenLed = 4;
const int buzzer   = 2;   // Active buzzer: HIGH=on, LOW=off

// LCD
LiquidCrystal_I2C lcd(0x27, 16, 2); // if no display, try 0x3F

// Stepper (28BYJ-48 + ULN2003)  IN1→8, IN2→9, IN3→10, IN4→11
const int IN1 = 8;
const int IN2 = 9;
const int IN3 = 10;
const int IN4 = 11;
const int stepsPerRevolution = 2048;     // typical for 28BYJ-48
const int stepperRPM = 16;
// Phase order (common): IN1, IN3, IN2, IN4; if not working, try IN1,IN2,IN3,IN4
Stepper stepperMotor(stepsPerRevolution, IN1, IN3, IN2, IN4);

// --------------------- Scan settings ---------------------
Servo radarServo;
const int alertDistance = 20;            // cm
const unsigned long updateInterval = 20; // ms per servo tick
int currentAngle = 0;
int increment    = 1;
unsigned long lastServoUpdate = 0;

// Detection threshold
const uint8_t ENTER_HITS = 3;            // Enter ALERT after 3 consecutive hits
int triggerCount = 0;

// --------------------- Beep/Blink (100ms) ---------------------
unsigned long lastBlinkTs = 0;
bool blinkOn = false;

// --------------------- Helpers (decl) ---------------------
float getDistance();
float getDistanceMedian3();
void setNormalState();
void lcdSetEmpty();
void lcdSetWarning();
void doSweepWithBeepBlink(); // Perform 0→180→0 sweep, with LED/Buzzer blinking

// --------------------- Setup ---------------------
void setup() {
  radarServo.attach(servoPin);

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  pinMode(redLed, OUTPUT);
  pinMode(greenLed, OUTPUT);
  pinMode(buzzer, OUTPUT);

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

  stepperMotor.setSpeed(stepperRPM);

  lcdSetEmpty();
  setNormalState();
}

// --------------------- Main Loop ---------------------
void loop() {
  unsigned long now = millis();

  // Servo scanning (only when not in ALERT mode)
  if (now - lastServoUpdate >= updateInterval) {
    lastServoUpdate = now;

    radarServo.write(currentAngle);

    // Distance measurement every 5 degrees
    if (currentAngle % 5 == 0) {
      float d = getDistanceMedian3();
      if (d < alertDistance) triggerCount++;
      else triggerCount = 0;

      // Enter ALERT after enough hits
      if (triggerCount >= ENTER_HITS) {
        triggerCount = 0;

        // Show warning on LCD, turn off green LED
        lcdSetWarning();
        digitalWrite(greenLed, LOW);

        // Loop: after each full sweep, check distance again
        while (true) {
          doSweepWithBeepBlink(); // Perform 0→180→0 sweep with LED/Buzzer

          // After sweep, check if object is still present
          float d2 = getDistanceMedian3();
          if (d2 >= alertDistance) {
            // No object → exit ALERT
            break;
          }
          // Still detected → repeat another sweep
        }

        // Stop buzzer and LED, restore normal state
        blinkOn = false;
        digitalWrite(redLed, LOW);
        digitalWrite(buzzer, LOW);
        setNormalState();
        lcdSetEmpty();
      }
    }

    // Update scanning angle
    currentAngle += increment;
    if (currentAngle >= 180) { currentAngle = 180; increment = -1; }
    else if (currentAngle <= 0) { currentAngle = 0; increment = 1; }
  }
}

// --------------------- Sweep (0→180→0) with Beep/Blink ---------------------
// Perform one complete sweep 0→180→0 smoothly,
// with LED and buzzer toggling every 100ms during the motion.
void doSweepWithBeepBlink() {
  const int halfTurnSteps = stepsPerRevolution / 2; // 180°

  // Reset blinking state
  lastBlinkTs = millis();
  blinkOn = false;
  digitalWrite(redLed, LOW);
  digitalWrite(buzzer, LOW);

  // Forward half-turn
  for (int i = 0; i < halfTurnSteps; i++) {
    stepperMotor.step(1);
    unsigned long now = millis();
    if (now - lastBlinkTs >= 100) {
      lastBlinkTs = now;
      blinkOn = !blinkOn;
      digitalWrite(redLed, blinkOn ? HIGH : LOW);
      digitalWrite(buzzer, blinkOn ? HIGH : LOW);
    }
  }

  // Backward half-turn
  for (int i = 0; i < halfTurnSteps; i++) {
    stepperMotor.step(-1);
    unsigned long now = millis();
    if (now - lastBlinkTs >= 100) {
      lastBlinkTs = now;
      blinkOn = !blinkOn;
      digitalWrite(redLed, blinkOn ? HIGH : LOW);
      digitalWrite(buzzer, blinkOn ? HIGH : LOW);
    }
  }

  // At the end, ensure LED and buzzer are off
  digitalWrite(redLed, LOW);
  digitalWrite(buzzer, LOW);
}

// --------------------- Distance (median filter) ---------------------
float getDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  unsigned long duration = pulseIn(echoPin, HIGH, 30000UL);
  if (duration == 0) return 9999.0; // Timeout → very far
  return duration * 0.034f / 2.0f;  // Convert to cm
}

float getDistanceMedian3() {
  float a = getDistance();
  float b = getDistance();
  float c = getDistance();
  // Return the median of three readings
  if (a > b) { float t = a; a = b; b = t; }
  if (b > c) { float t = b; b = c; c = t; }
  if (a > b) { float t = a; a = b; b = t; }
  return b;
}

// --------------------- UI helpers ---------------------
void setNormalState() {
  digitalWrite(greenLed, HIGH);
  digitalWrite(redLed, LOW);
  digitalWrite(buzzer, LOW);
}

void lcdSetEmpty() {
  lcd.clear();
  lcd.setCursor(1, 0);  // Centered "Area is Empty"
  lcd.print("Area is Empty");
  lcd.setCursor(0, 1);
  lcd.print("                ");
}

void lcdSetWarning() {
  lcd.clear();
  lcd.setCursor(4, 0); // Centered "WARNING!!"
  lcd.print("WARNING!!");
  lcd.setCursor(2, 1); // Centered "Foreign Body"
  lcd.print("Foreign Body");
}