#include <Servo.h> // Include the Servo library

Servo servo; // Create a servo object

// Define the trigger and echo pins for the first HC-SR04 sensor
const int triggerPin1 = 2;
const int echoPin1 = 4;

// Define the trigger and echo pins for the second HC-SR04 sensor
const int triggerPin2 = 3; // Assuming you connected the trigger pin to pin 3
const int echoPin2 = 5;    // Assuming you connected the echo pin to pin 5

const int relayPin1 = 7; // Assuming you connected the first relay module control pin to pin 7
const int relayPin2 = 8; // Assuming you connected the second relay module control pin to pin 8

unsigned long relayStartTime1 = 0; // Variable to store the start time of relay 1 activation
unsigned long relayStartTime2 = 0; // Variable to store the start time of relay 2 activation
const unsigned long relayDuration = 30000; // Duration of relay activation in milliseconds (30 seconds)

const int servoPin = 9; // Pin connected to the servo

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

  // Set the trigger pins as outputs
  pinMode(triggerPin1, OUTPUT);
  pinMode(triggerPin2, OUTPUT);

  // Set the echo pins as inputs
  pinMode(echoPin1, INPUT);
  pinMode(echoPin2, INPUT);

  // Set the relay module control pins as outputs
  pinMode(relayPin1, OUTPUT);
  pinMode(relayPin2, OUTPUT);

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

void loop() {
  // Function to read distance from the first HC-SR04 sensor
  float distance1 = readDistance(triggerPin1, echoPin1);

  // Function to read distance from the second HC-SR04 sensor
  float distance2 = readDistance(triggerPin2, echoPin2);

  // Display the distances on the serial monitor
  Serial.print("Distance 1: ");
  Serial.print(distance1);
  Serial.println(" cm");

  Serial.print("Distance 2: ");
  Serial.print(distance2);
  Serial.println(" cm");

  // Control the relay module 1 based on distance from the first sensor
  controlRelay(relayPin1, distance1, relayStartTime1);

  // Control the relay module 2 based on distance from the second sensor
  controlRelay(relayPin2, distance2, relayStartTime2);

  // Control the servo based on the distance from the first sensor
  controlServo(distance1);

  delay(1000); // Wait for a second before taking another reading
}

// Function to read distance from the HC-SR04 sensor
float readDistance(int triggerPin, int echoPin) {
  // Ensure the trigger pin is low
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);

  // Generate a 10us pulse on the trigger pin
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);

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

  // Calculate the distance in centimeters
  float distance = (duration * 0.0343) / 2;

  return distance;
}

// Function to control the relay module based on distance and activation time
void controlRelay(int relayPin, float distance, unsigned long &relayStartTime) {
  if (distance <= 5) {
    // If distance is less than or equal to 5cm, and the relay is not already activated, turn on relay
    if (relayStartTime == 0) {
      digitalWrite(relayPin, HIGH);
      relayStartTime = millis(); // Record the start time of relay activation
    }
  } else {
    // If distance is greater than 5cm and the relay is activated, check if relay activation duration has elapsed
    if (relayStartTime != 0 && millis() - relayStartTime >= relayDuration) {
      // If duration has elapsed, turn off relay
      digitalWrite(relayPin, LOW);
      relayStartTime = 0; // Reset relay activation start time
    }
  }
}

// Function to control the servo based on distance
void controlServo(float distance) {
  // Control the servo similarly to relay 1
  if (distance <= 5) {
    // If distance is less than or equal to 5cm, set the servo to a specific angle (e.g., 90 degrees)
    servo.write(90); // Adjust as needed
  } else {
    // If distance is greater than 5cm, set the servo to another angle (e.g., 0 degrees)
    servo.write(0); // Adjust as needed
  }
}