#include <Servo.h>

// Create servo objects
Servo servo1;  // Tilt
Servo servo2;  // Pan
Servo servo3;  // Roll
Servo servo4;  // Shutter

const int potPin1 = A0;  // Analog pins for potentiometers
const int potPin2 = A1;
const int potPin3 = A2;
const int potPin4 = A3;

#define ECHO_PIN 2
#define TRIG_PIN 3

void setup() {
  // Servo setup
  servo1.attach(9);  // Attach the servos to their respective pins
  servo2.attach(10);
  servo3.attach(11);
  servo4.attach(12);

  // Distance sensor setup
  Serial.begin(115200);
  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
}

float readDistanceCM() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  int duration = pulseIn(ECHO_PIN, HIGH);
  return duration * 0.034 / 2;
}

void loop() {
  // Servo control
  int potValue1 = analogRead(potPin1);  // Read the potentiometer values
  int potValue2 = analogRead(potPin2);
  int potValue3 = analogRead(potPin3);
  int potValue4 = analogRead(potPin4);

  int angle1 = map(potValue1, 0, 1023, 0, 180);  // Map the values to servo angles
  int angle2 = map(potValue2, 0, 1023, 0, 180);
  int angle3 = map(potValue3, 0, 1023, 0, 180);
  int angle4 = map(potValue4, 0, 1023, 0, 180);

  servo1.write(angle1);  // Set the servo positions
  servo2.write(angle2);
  servo3.write(angle3);
  servo4.write(angle4);

  delay(15);  // Small delay for stability.

  // Distance measurement
  float distance = readDistanceCM();

  bool isNearby = distance < 100;
  digitalWrite(LED_BUILTIN, isNearby);

  Serial.print("Measured distance: ");
  Serial.println(distance);

  delay(100);
}