#include <ESP32Servo.h>

#define PERSON_TRIG_PIN 19
#define PERSON_ECHO_PIN 4
#define SERVO_PIN 23

Servo radarServo;

const int centerAngle = 90;
const int scanDelay = 150;
const int objectThreshold = 30;

bool scanningRight = true;
int currentAngle = centerAngle;

long readDistanceCM() {
  digitalWrite(PERSON_TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(PERSON_TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(PERSON_TRIG_PIN, LOW);

  long duration = pulseIn(PERSON_ECHO_PIN, HIGH, 30000);
  long distance = duration * 0.034 / 2;
  return distance;
}

void setup() {
  Serial.begin(115200);
  pinMode(PERSON_TRIG_PIN, OUTPUT);
  pinMode(PERSON_ECHO_PIN, INPUT);
  radarServo.attach(SERVO_PIN);
  radarServo.write(centerAngle);
  delay(1000);
}

void loop() {
  radarServo.write(currentAngle);
  delay(scanDelay);

  long distance = readDistanceCM();
  Serial.print("Angle: ");
  Serial.print(currentAngle);
  Serial.print("°, Distance: ");
  Serial.print(distance);
  Serial.println(" cm");

  if (distance > 0 && distance < objectThreshold) {
    Serial.println("🔍 Object detected within 30cm!");

    // Stop at detected angle
    delay(2000); // Hold position for 2 seconds

    // Return to center
    radarServo.write(centerAngle);
    Serial.println("↩️ Returning to center position");
    delay(1000);

    // Reset scanning direction
    scanningRight = true;
    currentAngle = centerAngle;
    return;
  }

  // Update scanning angle
  if (scanningRight) {
    currentAngle += 2;
    if (currentAngle >= 180) {
      scanningRight = false;
      currentAngle = 180;
    }
  } else {
    currentAngle -= 2;
    if (currentAngle <= 0) {
      scanningRight = true;
      currentAngle = 0;
    }
  }
}