// Define Pins
const int TRIGGPIN1 = 4;
const int TRIGGPIN2 = 19;
const int ECHOPIN1 = 5;
const int ECHOPIN2 = 21;
const int LED1 = 13;
const int LED2 = 12;

long duration1;
long duration2;
int distance1;
int distance2;
int lastDistance1 = -1; // Initialize with a value that's outside the normal range of distances
int lastDistance2 = -1; // Initialize with a value that's outside the normal range of distances

void setup() {
  Serial.begin(115200);
  pinMode(TRIGGPIN1, OUTPUT);
  pinMode(ECHOPIN1, INPUT);
  pinMode(TRIGGPIN2, OUTPUT);
  pinMode(ECHOPIN2, INPUT);
  pinMode(LED1, OUTPUT);
  pinMode(LED2, OUTPUT);
  Serial.println("Test Sequence Initiating");
  digitalWrite(LED1, HIGH);
  delay(500);
  digitalWrite(LED2, HIGH);
  delay(500);
  digitalWrite(LED1, LOW);
  delay(500);
  digitalWrite(LED2, LOW);
  delay(500);


}
//Function Prototype
void ULSensor1();
void ULSensor2();

void loop() {
  ULSensor1();
  ULSensor2();

  // Prints the distance on the Serial Monitor
  if (distance1 != lastDistance1 || distance2 != lastDistance2) {
    Serial.print("Distance1: ");
    Serial.print(distance1);
    Serial.println(" cm");
    Serial.print("Distance2: ");
    Serial.print(distance2);
    Serial.println(" cm\n");
    
    lastDistance1 = distance1;
    lastDistance2 = distance2;
  }
  delay(1000);


  if (distance1 < 100) {
    digitalWrite(LED1, HIGH);

  }
  else {
    digitalWrite(LED1, LOW);
  }
if (distance2 < 100) {
    digitalWrite(LED2, HIGH);

  }
  else {
    digitalWrite(LED2, LOW);
  }
}

void ULSensor1() {
  digitalWrite(TRIGGPIN1, LOW);
  delayMicroseconds(2);
  // Sets the tRIGGPIN on HIGH state for 10 micro seconds
  digitalWrite(TRIGGPIN1, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGGPIN1, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  duration1 = pulseIn(ECHOPIN1, HIGH);
  // Calculating the distance
  distance1 = duration1 * 0.034 / 2;
}

void ULSensor2() {
  digitalWrite(TRIGGPIN2, LOW);
  delayMicroseconds(2);
  // Sets the tRIGGPIN on HIGH state for 10 micro seconds
  digitalWrite(TRIGGPIN2, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGGPIN2, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  duration2 = pulseIn(ECHOPIN2, HIGH);
  // Calculating the distance
  distance2 = duration2 * 0.034 / 2;
}