#include <Servo.h>

// Define servo objects
Servo hipServo1;
Servo kneeServo1;
Servo hipServo2;
Servo kneeServo2;
Servo hipServo3;
Servo kneeServo3;
Servo hipServo4;
Servo kneeServo4;

// Define servo pins
const int hipPin1 = 2;
const int kneePin1 = 3;
const int hipPin2 = 4;
const int kneePin2 = 5;
const int hipPin3 = 6;
const int kneePin3 = 7;
const int hipPin4 = 8;
const int kneePin4 = 9;

// Define ultrasonic sensor pins
const int trigPin1 = 10;
const int echoPin1 = 12;
const int trigPin2 = 11;
const int echoPin2 = 13;

// Smooth movement function
void smoothMove(Servo &servo, int startAngle, int endAngle, int stepDelay) {
  if (startAngle < endAngle) {
    for (int pos = startAngle; pos <= endAngle; pos++) {
      servo.write(pos);
      delay(stepDelay);
    }
  } else {
    for (int pos = startAngle; pos >= endAngle; pos--) {
      servo.write(pos);
      delay(stepDelay);
    }
  }
}

// Function to get distance from ultrasonic sensor
long getDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long duration = pulseIn(echoPin, HIGH);
  long distance = duration * 0.034 / 2;
  return distance;
}

void setup() {
  // Attach servos to pins
  hipServo1.attach(hipPin1);
  kneeServo1.attach(kneePin1);
  hipServo2.attach(hipPin2);
  kneeServo2.attach(kneePin2);
  hipServo3.attach(hipPin3);
  kneeServo3.attach(kneePin3);
  hipServo4.attach(hipPin4);
  kneeServo4.attach(kneePin4);

  // Initialize servos to a starting position
  hipServo1.write(90);
  kneeServo1.write(90);
  hipServo2.write(90);
  kneeServo2.write(90);
  hipServo3.write(90);
  kneeServo3.write(90);
  hipServo4.write(90);
  kneeServo4.write(90);

  // Initialize ultrasonic sensor pins
  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
}

void loop() {
  // Get distance from ultrasonic sensors
  long distance1 = getDistance(trigPin1, echoPin1);
  long distance2 = getDistance(trigPin2, echoPin2);

  // Adaptive walking sequence based on obstacle detection
  if (distance1 > 20 && distance2 > 20) {
    // No obstacles, perform normal walking sequence
    // Move first pair of legs
    // Lift legs 1 and 3
    smoothMove(kneeServo1, 90, 45, 10);
    smoothMove(kneeServo3, 90, 45, 10);

    // Move legs 1 and 3 forward
    smoothMove(hipServo1, 90, 45, 10);
    smoothMove(hipServo3, 90, 45, 10);

    // Lower legs 1 and 3
    smoothMove(kneeServo1, 45, 90, 10);
    smoothMove(kneeServo3, 45, 90, 10);

    delay(100); // small delay for stability

    // Move second pair of legs
    // Lift legs 2 and 4
    smoothMove(kneeServo2, 90, 45, 10);
    smoothMove(kneeServo4, 90, 45, 10);

    // Move legs 2 and 4 forward
    smoothMove(hipServo2, 90, 45, 10);
    smoothMove(hipServo4, 90, 45, 10);

    // Lower legs 2 and 4
    smoothMove(kneeServo2, 45, 90, 10);
    smoothMove(kneeServo4, 45, 90, 10);

    delay(100); // small delay for stability

    // Move legs back to original position
    smoothMove(hipServo1, 45, 90, 10);
    smoothMove(hipServo2, 45, 90, 10);
    smoothMove(hipServo3, 45, 90, 10);
    smoothMove(hipServo4, 45, 90, 10);
    delay(200);
  } else {
    // Obstacle detected, take a step back or turn
    // Move legs 2 and 4 back
    smoothMove(hipServo2, 90, 135, 10);
    smoothMove(hipServo4, 90, 135, 10);

    // Move legs 1 and 3 back
    smoothMove(hipServo1, 90, 135, 10);
    smoothMove(hipServo3, 90, 135, 10);

    delay(500); // small delay for stability

    // Move legs back to original position
    smoothMove(hipServo1, 135, 90, 10);
    smoothMove(hipServo2, 135, 90, 10);
    smoothMove(hipServo3, 135, 90, 10);
    smoothMove(hipServo4, 135, 90, 10);
    delay(200);
  }
}