#include <ESP32Servo.h>  // Include the ESP32Servo library

// Define pin connections
const int servoPin = 23;    // GPIO pin connected to the servo control wire
const int trigPin = 22;     // GPIO pin connected to the Trig pin of the HC-SR04
const int echoPin = 21;     // GPIO pin connected to the Echo pin of the HC-SR04

Servo myServo;             // Create a Servo object

void setup() {
  Serial.begin(115200);    // Initialize serial communication for debugging

  myServo.attach(servoPin); // Attach the servo to the defined pin

  pinMode(trigPin, OUTPUT); // Set Trig pin as output
  pinMode(echoPin, INPUT);  // Set Echo pin as input
}

void loop() {
  // Sweep the servo from 0 to 90 degrees
  for (int pos = 0; pos <= 90; pos += 1) {
    myServo.write(pos);
    delay(15); // Wait for the servo to reach the position
    long duration = measureDistance(); // Measure distance
    Serial.print("Angle: ");
    Serial.print(pos);
    Serial.print(" degrees, Distance: ");
    Serial.print(duration / 29 / 2); // Convert duration to distance in cm
    Serial.println(" cm");
  }
  
  // Sweep the servo from 90 to 0 degrees
  for (int pos = 90; pos >= 0; pos -= 1) {
    myServo.write(pos);
    delay(15); // Wait for the servo to reach the position
    long duration = measureDistance(); // Measure distance
    Serial.print("Angle: ");
    Serial.print(pos);
    Serial.print(" degrees, Distance: ");
    Serial.print(duration / 29 / 2); // Convert duration to distance in cm
    Serial.println(" cm");
  }
}

long measureDistance() {
  // Clear the Trig pin
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  
  // Set the Trig pin high for 200 microseconds
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(200);
  digitalWrite(trigPin, LOW);
  
  // Read the Echo pin
  long duration = pulseIn(echoPin, HIGH);
  
  return duration;
}