#include <Servo.h>

// -------- CONFIG --------
const int distanceThreshold = 20;   // cm: trigger distance for HC-SR04
const unsigned long detectDelay = 500;  // ms: delay after detection before activating

// Pins
const int trigPin = 2;
const int echoPin = 3;
const int buzzerPin = 4;
const int weaponServoPin = 5;
const int gateServoPin = 11;

// Timing (ms)
const unsigned long buzzDuration = 3000;
const unsigned long holdDuration = 5000;

// Servo angles
const int weaponRest = 0;
const int weaponTrigger = 90;
const int gateRest = 0;
const int gateTrigger = 80;

Servo weaponServo;
Servo gateServo;

bool triggered = false;
unsigned long triggerTime = 0;
unsigned long buzzEnd = 0;
unsigned long lastDetectTime = 0;
bool detectionConfirmed = false;

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

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(buzzerPin, OUTPUT);
  digitalWrite(buzzerPin, LOW);

  weaponServo.attach(weaponServoPin);
  gateServo.attach(gateServoPin);
  weaponServo.write(weaponRest);
  gateServo.write(gateRest);

  Serial.println("Security System Ready. Ultrasonic detection active...");
}

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

  // Show distance on serial
  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");

  // Detection logic with confirmation delay
  if (!triggered && distance > 0 && distance <= distanceThreshold) {
    if (!detectionConfirmed) {
      detectionConfirmed = true;
      lastDetectTime = now;  // first detection
    } else if (now - lastDetectTime >= detectDelay) {
      Serial.println("Object confirmed — triggering system!");
      triggerSystem(now);
      detectionConfirmed = false;
    }
  } else {
    detectionConfirmed = false;
  }

  // Stop buzzer after time
  if (triggered && now >= buzzEnd) {
    digitalWrite(buzzerPin, LOW);
  }

  // Reset servos after hold duration
  if (triggered && (now - triggerTime >= holdDuration)) {
    Serial.println("Resetting servos to rest.");
    weaponServo.write(weaponRest);
    gateServo.write(gateRest);
    triggered = false;
  }

  delay(100);
}

void triggerSystem(unsigned long now) {
  triggered = true;
  triggerTime = now;
  buzzEnd = now + buzzDuration;

  // Move servos once (no flicker)
  weaponServo.write(weaponTrigger);
  gateServo.write(gateTrigger);

  // Start buzzer
  digitalWrite(buzzerPin, HIGH);
}

long getDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH, 30000); // timeout 30 ms (~5m)
  if (duration == 0) return -1; // no echo detected

  long distance = duration * 0.034 / 2; // convert to cm
  return distance;
}