#include <Arduino.h>

// Define pin numbers for the ultrasonic sensors
const int trigPinF = 16; // Trigger pin of first sensor
const int echoPinF = 17; // Echo pin of first sensor
const int trigPinRU = 5; // Trigger pin of second sensor
const int echoPinRU = 18; // Echo pin of second sensor
const int trigPinRL = 19; // Trigger pin of third sensor
const int echoPinRL = 21; // Echo pin of third sensor
const int trigPinLU = 23; // Trigger pin of fourth sensor
const int echoPinLU = 22; // Echo pin of fourth sensor
const int trigPinLL = 12; // Trigger pin of fifth sensor
const int echoPinLL = 14; // Echo pin of fifth sensor

// Define LED pin
const int ledPin = 2; // Change this to the pin where your LED is connected

// Function to read distance from an ultrasonic sensor
long readUltrasonicDistance(int trigPin, int echoPin) {
  // Send a short pulse to trigger the ultrasonic sensor
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

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

  // Convert the duration to distance (in centimeters) using the speed of sound
  return duration * 0.034 / 2;
}

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

  // Set the trigger and echo pins as INPUT and OUTPUT
  pinMode(trigPinF, OUTPUT);
  pinMode(echoPinF, INPUT);
  pinMode(trigPinRU, OUTPUT);
  pinMode(echoPinRU, INPUT);
  pinMode(trigPinRL, OUTPUT);
  pinMode(echoPinRL, INPUT);
  pinMode(trigPinLU, OUTPUT);
  pinMode(echoPinLU, INPUT);
  pinMode(trigPinLL, OUTPUT);
  pinMode(echoPinLL, INPUT);

  // Set the LED pin as OUTPUT
  pinMode(ledPin, OUTPUT);
}

void loop() {
  // Read distances from all sensors
  long distance1 = readUltrasonicDistance(trigPinF, echoPinF);
  long distance2 = readUltrasonicDistance(trigPinRU, echoPinRU);
  long distance3 = readUltrasonicDistance(trigPinRL, echoPinRL);
  long distance4 = readUltrasonicDistance(trigPinLU, echoPinLU);
  long distance5 = readUltrasonicDistance(trigPinLL, echoPinLL);

  // Print the distances to the serial monitor
  Serial.print("Distance Sensor Front: ");
  Serial.print(distance1);
  Serial.println(" cm");

  Serial.print("Distance Sensor Right Upper: ");
  Serial.print(distance2);
  Serial.println(" cm");

  Serial.print("Distance Sensor Right Lower: ");
  Serial.print(distance3);
  Serial.println(" cm");

  Serial.print("Distance Sensor Left Upper: ");
  Serial.print(distance4);
  Serial.println(" cm");

  Serial.print("Distance Sensor Left Lower: ");
  Serial.print(distance5);
  Serial.println(" cm");

  // Check if any sensor detects an object farther than 30 cm
  if (distance1 > 30 || distance2 > 30 || distance3 > 30 || distance4 > 30 || distance5 > 30) {
    digitalWrite(ledPin, HIGH); // Turn on the LED
  } 
  
  else {
    digitalWrite(ledPin, LOW); // Turn off the LED
  }

  // Add a delay before the next reading
  delay(1000);
}