#include <Servo.h>

Servo servoM, servoL, servoR; // Middle, left, and right servo

unsigned long previousMillis = 0;

int move1 = 0;
int move2 = 0;
int move3 = 0;
int move4 = 0;

// Define pins for sensors
const int lightSensorPin = A0; // Analog pin for light sensor
const int soundSensorPin = A1; // Analog pin for sound sensor
const int trigPin = 3; // Ultrasonic sensor trigger pin
const int echoPin = 2; // Ultrasonic sensor echo pin

void setup()
{
  Serial.begin(9600);

  servoM.attach(11);
  servoL.attach(12);
  servoR.attach(10);

  servoM.write(20);
  servoL.write(45);
  servoR.write(45);

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
}

void loop()
{
  unsigned long currentMillis = millis();

  // Read sensor values
  int lightValue = analogRead(lightSensorPin);
  int soundValue = analogRead(soundSensorPin);

  // Ultrasonic sensor
  long duration, distance;
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = duration * 0.034 / 2;

  // Obstacle avoidance logic
  if (distance < 20) // If obstacle is too close (adjust threshold as needed)
  {
    // Stop the servos
    servoM.write(90); // Stop the middle servo
    servoL.write(90); // Stop the left servo
    servoR.write(90); // Stop the right servo
      servoM.write(60); // Decrease middle servo angle
  servoL.write(120); // Increase left servo angle
  servoR.write(60); // Decrease right servo angle

  // Add delay or other timing logic for turning
  delay(1000); // Example: Turn for 1 second

  // To turn the Hexapod back to the original position
  servoM.write(90); // Reset middle servo angle
  servoL.write(90); // Reset left servo angle
  servoR.write(90); // Reset right servo angle
  }
  else
  {
    // Continue normal movement
    servoM.write(30); // Move the middle servo forward
    servoL.write(65); // Move the left servo forward
    servoR.write(65); // Move the right servo 
    servoM.write(50); // Move the middle servo forward
    servoL.write(85); // Move the left servo forward
    servoR.write(85); // Move the right servo forward
  }

  // Print sensor values
  Serial.print("Light Sensor Value: ");
  Serial.println(lightValue);
  Serial.print("Sound Sensor Value: ");
  Serial.println(soundValue);
  Serial.print("Ultrasonic Distance: ");
  Serial.print(distance);
  Serial.println(" cm");

  // Add delay or other timing logic as needed
  delay(1000);
}