// Ultrasonic Distance Sensor Demo with 4 Sensors, Buzzer, LED, and Bluetooth
// Modified from keuwlsoft: www.keuwl.com 16th Sept 2015
// cc Attribution-ShareAlike

const int ledPin = 10; // LED pin
const int buzzerPin = 11; // Buzzer pin

// Pin definitions for the ultrasonic sensors
const int trigPins[4] = {2, 4, 6, 8}; // Trig pins for sensors (East, West, North, South)
const int echoPins[4] = {3, 5, 7, 9}; // Echo pins for sensors (East, West, North, South)

// Direction characters for Bluetooth communication
const char directions[4] = {'e', 'w', 'n', 's'}; // For distance
const char Directions[4] = {'E', 'W', 'N', 'S'}; // For color

long echotimes[4]; // in microseconds
float distances[4]; // in cm

void setup() {
  Serial.begin(9600);
  
  // Initialize ultrasonic sensors
  for (int i = 0; i < 4; i++) {
    pinMode(trigPins[i], OUTPUT);
    pinMode(echoPins[i], INPUT);
    digitalWrite(trigPins[i], LOW); // Start with trigger LOW
  }
  
  // Initialize buzzer and LED
  pinMode(buzzerPin, OUTPUT);
  pinMode(ledPin, OUTPUT);
  digitalWrite(buzzerPin, LOW); // Start with buzzer OFF
  digitalWrite(ledPin, LOW); // Start with LED OFF
  
  // Debug prints
  Serial.println("Setup completed. Starting measurements...");
}

void loop() {
  bool objectNear = false;
  
  for (int i = 0; i < 4; i++) {
    // Trigger a pulse-echo measurement for each sensor
    digitalWrite(trigPins[i], HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPins[i], LOW);
    
    // Get the result
    echotimes[i] = pulseIn(echoPins[i], HIGH);
    distances[i] = echotimes[i] * 0.034 / 2.0; // Convert echo time to distance in cm
    
    // Debug print for distance measurement
    Serial.print("Direction ");
    Serial.print(Directions[i]);
    //Serial.print(": Echo time = ");
    //Serial.print(echotimes[i]);
    Serial.print(" Distance = ");
    Serial.print(distances[i], 1);
    Serial.println(" cm");
    
    // Send distance over Bluetooth
    Serial.print("*" + String(directions[i]) + String(distances[i], 1) + "cm*");
    
    // Determine color and send over Bluetooth
    if (distances[i] < 100) {
      Serial.print("*" + String(Directions[i]) + "R255G0B0*"); // Red
      objectNear = true; // Object is near for blinking LED
    } else if (distances[i] >= 100 && distances[i] <= 200) {
      Serial.print("*" + String(Directions[i]) + "R255G200B0*"); // Orange
    } else {
      Serial.print("*" + String(Directions[i]) + "R0G255B0*"); // Green
    }
  }
  
  // Control buzzer and LED based on object proximity
  while (objectNear) {
    digitalWrite(buzzerPin, HIGH); // Turn on buzzer
     digitalWrite(ledPin, HIGH);
    delay(100);
    digitalWrite(ledPin, LOW);
    delay(100);
    Serial.println("Buzzer and LED ON: Object detected within 100 cm");
  } 
  
  /*
  else {
    digitalWrite(buzzerPin, LOW); // Turn off buzzer
     digitalWrite(ledPin, LOW);
    Serial.println("Buzzer and LED OFF: No object detected within 100 cm");
  }
  */
  /*
  // Blink LED if any object is near
  if (objectNear) {
    digitalWrite(ledPin, HIGH);
    delay(50);
    digitalWrite(ledPin, LOW);
    delay(50);
    Serial.println("LED Blinking: Object detected within 100 cm");
  } else {
    digitalWrite(ledPin, LOW);
  }
  */
  
  Serial.println("-----");
}