#include <Servo.h>

// Pin definitions
const int triggerPin = 7; // Trigger pin of ultrasonic sensor
const int echoPin = 6;    // Echo pin of ultrasonic sensor
const int servoPin = 9;   // Servo control pin
const int motorPin = 3;   // Motor control pin

// Constants
const int safeDistance = 10; // Distance threshold in inches

// Objects
Servo steeringServo;

// State variables
bool withinSafeDistance = false;
bool turnDirectionChosen = false; // True for left, false for right

void setup() {
  // Initialize serial communication
  Serial.begin(9600);

  // Initialize pins
  pinMode(triggerPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(motorPin, OUTPUT);

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

  // Center the servo
  steeringServo.write(90);
}

void loop() {
  // Measure distance
  int distance = getDistance();
  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" inches");

  if (distance < safeDistance) {
    if (!withinSafeDistance) {
      withinSafeDistance = true;
      turnDirectionChosen = random(2); // Randomly choose left or right
    }

    // Stop the motor briefly
    digitalWrite(motorPin, HIGH);
    delay(500);

    // Turn based on the chosen direction
    if (turnDirectionChosen) {
      turnLeft();
    } else {
      turnRight();
    }
  } else {
    if (withinSafeDistance) {
      withinSafeDistance = false;
    }

    // Move forward
    digitalWrite(motorPin, HIGH);
    steeringServo.write(90); // Center the servo
  }

  delay(50); // Short delay for stability
}

int getDistance() {
  // Send a 10-microsecond pulse to trigger pin
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);

  // Measure the duration of the echo pulse
  long duration = pulseIn(echoPin, HIGH);

  // Calculate distance in inches
  int distance = duration * 0.0133 / 2;

  return distance;
}

void turnLeft() {
  Serial.println("Turning left");
  steeringServo.write(45); // Turn servo to the left
  delay(1000);             // Wait for the turn to complete
  steeringServo.write(90); // Center the servo
}

void turnRight() {
  Serial.println("Turning right");
  steeringServo.write(135); // Turn servo to the right
  delay(1000);              // Wait for the turn to complete
  steeringServo.write(90);  // Center the servo
}