const int triggerPin = 23;  // Trigger pin of the ultrasonic sensor
const int echoPin = 22;     // Echo pin of the ultrasonic sensor
const int relayPin = 4;     // Pin connected to the relay
const int LED_PIN = 5;      // Pin connected to the LED
const int servoPin = 18;    // Pin connected to the servo motor
const int maxDistance = 200; // Maximum distance in centimeters for the LED, relay, and servo to turn on

int servoAngle = 90; // Initial servo angle (90 degrees)
unsigned long servoPulse = 0; // Store the last servo pulse time

void setup() {
  pinMode(triggerPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(relayPin, OUTPUT);
  pinMode(LED_PIN, OUTPUT);
  pinMode(servoPin, OUTPUT);

  digitalWrite(relayPin, LOW); // Initially, turn off the relay
  digitalWrite(LED_PIN, LOW);  // Initially, turn off the LED

  // Move the servo to its initial position
  moveServo(servoAngle);

  Serial.begin(115200);
}

void loop() {
  // Trigger the ultrasonic sensor
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);

  // Measure the echo time and calculate the distance
  long duration = pulseIn(echoPin, HIGH);
  int distance = duration * 0.034 / 2;

  if (distance > maxDistance) {
    digitalWrite(relayPin, HIGH); // Turn on the relay
    digitalWrite(LED_PIN, HIGH);  // Turn on the LED
    servoAngle = 0;              // Set the servo angle to 0 degrees
  } else {
    digitalWrite(relayPin, LOW); // Turn off the relay
    digitalWrite(LED_PIN, LOW);  // Turn off the LED
    servoAngle = 90;             // Set the servo angle to 90 degrees
  }

  // Move the servo to the new angle
  moveServo(servoAngle);

  // Print the distance to the serial monitor for debugging
  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");

  delay(100); // Introduce a small delay to control the loop rate
}

void moveServo(int angle) {
  servoPulse = map(angle, 0, 180, 500, 2500);
  unsigned long now = micros();
  if (now - servoPulse >= 20000) {
    digitalWrite(servoPin, HIGH);
    delayMicroseconds(servoPulse);
    digitalWrite(servoPin, LOW);
  }
}