#include <Servo.h>

Servo myServo;  // Create servo object
const int trigPin = 10;
const int echoPin = 11;
const int ledPin = 3;  // Example for one LED (you can expand for more)
long duration;
int distance;

// Angles for sweeping
int angles[] = {15, 65, 115, 165};

void setup() {
  myServo.attach(9);  // Attach the servo to pin 9
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(ledPin, OUTPUT);
  
  Serial.begin(9600);  // Begin serial communication for debugging
}

void loop() {
  for (int i = 0; i < 4; i++) {
    int angle = angles[i];
    myServo.write(angle);  // Move the servo to the desired angle
    delay(500);  // Wait for servo to reach position
    
    // Measure distance
    distance = measureDistance();
    Serial.print("Distance at angle ");
    Serial.print(angle);
    Serial.print(": ");
    Serial.println(distance);

    // Map the distance to brightness (1 inch = dim, 130 inches = bright)
    int brightness = map(distance, 1, 130, 0, 255);
    analogWrite(ledPin, brightness);
    
    delay(1000);  // Wait before next angle
  }
}

int measureDistance() {
  // Clears the trigPin
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  
  // Sets the trigPin on HIGH state for 10 microseconds
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  // Reads the echoPin, returns the sound wave travel time in microseconds
  duration = pulseIn(echoPin, HIGH);
  
  // Calculating the distance in inches
  distance = duration * 0.0133 / 2;  // Speed of sound in air (microseconds per inch)
  
  return distance;
}