#include <ESP32Servo.h>

// --- PIN DEFINITIONS ---
const int trigPin = 32;
const int echoPin = 35;
const int servoPin = 33;

// --- VARIABLES ---
Servo neckServo;
int pos = 0;      // Variable to store the servo's current angle
long duration;    // Variable for sound wave travel time
int distance;     // Variable for the calculated distance

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

  // Setup Ultrasonic Pins
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // Setup Servo
  ESP32PWM::allocateTimer(0);
  neckServo.setPeriodHertz(50);
  neckServo.attach(servoPin, 500, 2400);

  Serial.println("System Online. Beginning Sweep and Scan...");
  neckServo.write(90); // Start looking straight ahead
  delay(1000);
}

void loop() {
  // 1. SCAN RIGHT TO LEFT (0 degrees to 180 degrees)
  for (pos = 0; pos <= 180; pos += 2) { // Move 2 degrees at a time
    neckServo.write(pos);               // Tell servo to go to 'pos'
    delay(15);                          // Wait 15 milliseconds for it to physically move
    
    int currentDistance = readDistance(); // Trigger the eyes!
    
    // Print what the bot is seeing to the screen
    Serial.print("Angle: ");
    Serial.print(pos);
    Serial.print("° | Distance: ");
    Serial.print(currentDistance);
    Serial.println(" cm");
  }

  // 2. SCAN LEFT TO RIGHT (180 degrees back to 0 degrees)
  for (pos = 180; pos >= 0; pos -= 2) { 
    neckServo.write(pos);              
    delay(15);                         
    
    int currentDistance = readDistance();
    
    Serial.print("Angle: ");
    Serial.print(pos);
    Serial.print("° | Distance: ");
    Serial.print(currentDistance);
    Serial.println(" cm");
  }
}

// --- CUSTOM FUNCTION: Read Distance ---
// We put the math down here to keep the main loop clean!
int readDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  duration = pulseIn(echoPin, HIGH);
  int d = duration * 0.034 / 2;
  
  // If the sensor reads 0, it means it didn't hear an echo (out of range)
  if (d == 0) { d = 400; } // Default to max range
  return d;
}