#include <Servo.h>

// Define pins
const int led = 13;
const int trigPin = 9;
const int echoPin = 8;

// Variables
long duration;
int distance;

// Servo setup
Servo servo1, servo2;

// Servo position tracking
int currentPos1 = 90; // Initial position for Servo 1
int currentPos2 = 90; // Initial position for Servo 2

// Timing for updates
unsigned long lastUpdate = 0;
const int updateInterval = 15; // Update every 15ms

void setup() {
  // Initialize pins
  pinMode(led, OUTPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // Start serial communication
  Serial.begin(9600);

  // Attach and initialize servos
  servo1.attach(7);  // Servo 1 on pin 7
  servo2.attach(6);  // Servo 2 on pin 6

  servo1.write(currentPos1); // Initialize Servo 1 at 90 degrees
  servo2.write(currentPos2); // Initialize Servo 2 at 90 degrees

  delay(1000); // Allow servos to stabilize
}

// Function to calculate distance
int getDistance() {
  // Clear trigPin
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);

  // Trigger ultrasonic pulse
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Measure echo duration
  duration = pulseIn(echoPin, HIGH);

  // Calculate and return distance in cm
  if (duration == 0) {
    return 999; // No object detected
  }
  return duration * 0.034 / 2;
}

// Function to filter sensor readings for stability
int smoothDistance(int newReading) {
  static int previous = 0;
  return previous = (previous * 0.7 + newReading * 0.3); // Weighted average
}

// Function for smooth servo movement
void easedMove(int targetPos1, int targetPos2, int duration) {
  int steps = 50; // Number of steps for smooth movement
  int interval = duration / steps; // Time per step in ms

  for (int i = 0; i <= steps; i++) {
    float progress = (float)i / steps; // Progress between 0 and 1
    int newPos1 = currentPos1 + (targetPos1 - currentPos1) * progress;
    int newPos2 = currentPos2 + (targetPos2 - currentPos2) * progress;

    // Write positions to servos
    servo1.write(constrain(newPos1, 0, 180)); // Constrain to safe range
    servo2.write(constrain(newPos2, 0, 180));

    delay(interval); // Wait for the next step
  }

  // Update current positions
  currentPos1 = targetPos1;
  currentPos2 = targetPos2;
}

void loop() {
  // Update distance at regular intervals
  if (millis() - lastUpdate >= updateInterval) {
    lastUpdate = millis();

    // Measure and smooth distance
    distance = smoothDistance(getDistance());

    // Debugging: Print raw duration and distance
    Serial.print("Duration: ");
    Serial.print(duration);
    Serial.print(" | Smoothed Distance: ");
    Serial.println(distance);

    // Handle out-of-range or invalid readings
    if (distance > 400 || distance < 2) { // Out of sensor range
      Serial.println("Out of range or invalid reading.");
      digitalWrite(led, LOW); // Turn off LED for invalid readings
      return;
    }

    // LED logic
    if (distance <= 30) { // Object detected
      digitalWrite(led, HIGH);
    } else {
      digitalWrite(led, LOW);
    }

    // Servo logic with eased movement
    if (distance <= 30 && (currentPos1 != 0 || currentPos2 != 180)) {
      Serial.println("Object detected: Moving servos to target positions.");
      easedMove(0, 180, 1000); // Smoothly move to target positions over 1 second
      delay(2000); // Pause for 2 seconds
    } else if (distance > 30 && (currentPos1 != 90 || currentPos2 != 90)) {
      Serial.println("No object detected: Moving servos to rest positions.");
      easedMove(90, 90, 1000); // Smoothly move to rest positions over 1 second
    }
  }
}