The Complete ESP32 & HC-SR04 Ultrasonic Sensor Guide: From Basics to Professional Implementation with Arduino IDE

// Professional accuracy assessment routine
void evaluateSensorAccuracy(int samples = 100) {
  Serial.println("\n=== HC-SR04 ACCURACY EVALUATION ===");
  Serial.println("Place sensor at known distances for calibration");
  
  float knownDistances[] = {10.0, 50.0, 100.0, 200.0, 300.0}; // cm
  float totalError = 0.0;
  int validMeasurements = 0;
  
  for(int distIndex = 0; distIndex < 5; distIndex++) {
    Serial.print("\nTesting at ");
    Serial.print(knownDistances[distIndex]);
    Serial.println(" cm:");
    Serial.println("Place object at exact distance and press any key...");
    
    while(!Serial.available()); // Wait for user
    Serial.read(); // Clear buffer
    
    float sum = 0.0;
    int successfulReads = 0;
    
    for(int i = 0; i < samples; i++) {
      float measurement = getFilteredDistance();
      
      if(measurement > 0 && measurement < 500) { // Valid range check
        sum += measurement;
        successfulReads++;
      }
      delay(50);
    }
    
    if(successfulReads > samples * 0.8) { // Require 80% success rate
      float average = sum / successfulReads;
      float error = abs(average - knownDistances[distIndex]);
      float errorPercent = (error / knownDistances[distIndex]) * 100;
      
      Serial.print("  Average: ");
      Serial.print(average, 1);
      Serial.print(" cm, Error: ");
      Serial.print(error, 1);
      Serial.print(" cm (");
      Serial.print(errorPercent, 1);
      Serial.println("%)");
      
      totalError += error;
      validMeasurements++;
    } else {
      Serial.println("  Too many failed readings - check sensor placement");
    }
  }
  
  if(validMeasurements > 0) {
    Serial.print("\nOverall Average Error: ");
    Serial.print(totalError / validMeasurements, 1);
    Serial.println(" cm");
  }
}

// Enhanced power management for HC-SR04
class UltrasonicPowerManager {
private:
  int enablePin; // Optional MOSFET/enable circuit
  bool powerCyclingEnabled;
  unsigned long lastMeasurement;
  const unsigned long powerTimeout = 60000; // 1 minute
  
public:
  UltrasonicPowerManager(int pin = -1) : enablePin(pin), powerCyclingEnabled(pin != -1) {
    if(powerCyclingEnabled) {
      pinMode(enablePin, OUTPUT);
      enableSensor(true);
    }
    lastMeasurement = millis();
  }
  
  void enableSensor(bool state) {
    if(powerCyclingEnabled) {
      digitalWrite(enablePin, state ? HIGH : LOW);
      if(state) {
        delay(50); // Allow sensor to stabilize
      }
    }
  }
  
  void checkPowerState() {
    // Power cycle if no measurements for extended period
    if(powerCyclingEnabled && (millis() - lastMeasurement > powerTimeout)) {
      Serial.println("Power cycling sensor due to inactivity");
      enableSensor(false);
      delay(100);
      enableSensor(true);
      lastMeasurement = millis();
    }
  }
  
  void updateLastMeasurement() {
    lastMeasurement = millis();
  }
  
  void diagnosePowerIssues(float currentReading, float previousReading) {
    // Detect power-related anomalies
    if(currentReading == 0.0 && previousReading > 0.0) {
      Serial.println("Warning: Sudden loss of reading - check power connections");
    }
    
    if(abs(currentReading - previousReading) > 100.0) { // Unrealistic jump
      Serial.println("Warning: Erratic reading - possible power instability");
      Serial.println("Recommend: Add 100µF capacitor close to sensor VCC/GND");
    }
  }
};

HC-SR04 Echo (5V) ---[1kΩ]---+--- ESP32 GPIO (e.g., 18)
                              |
                            [2kΩ]
                              |
                             GND

// Implementation with protection
const int echoPin = 18;
const int trigPin = 5;

void setupProtectedPins() {
  // Add internal pull-down for extra protection
  pinMode(echoPin, INPUT_PULLDOWN);
  pinMode(trigPin, OUTPUT);
  
  // Test circuit before full operation
  testVoltageDivision();
}

void testVoltageDivision() {
  Serial.println("Testing voltage divider safety...");
  
  // Send test pulse
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  // Check if echo pin stays within safe range
  long pulseTime = pulseIn(echoPin, HIGH, 30000); // 30ms timeout
  
  if(pulseTime > 25000) { // Unrealistically long pulse
    Serial.println("WARNING: Echo signal may be exceeding safe voltage!");
    Serial.println("Immediately disconnect and verify voltage divider.");
    while(1); // Halt for safety
  }
}

// Comprehensive noise filtering implementation
class UltrasonicFilter {
private:
  float readings[10]; // Circular buffer for moving average
  int readIndex = 0;
  float total = 0;
  
  // Statistical filtering
  const float maxRateOfChange = 50.0; // cm per reading
  const float outlierThreshold = 3.0; // Standard deviations
  
public:
  UltrasonicFilter() {
    for(int i = 0; i < 10; i++) {
      readings[i] = 0.0;
    }
  }
  
  float addReading(float rawDistance) {
    // Step 1: Basic validity check
    if(rawDistance <= 0 || rawDistance > 500) {
      return getFilteredValue(); // Return last good value
    }
    
    // Step 2: Rate of change limiting
    float previous = getFilteredValue();
    if(abs(rawDistance - previous) > maxRateOfChange && previous > 0) {
      Serial.print("Rate limit triggered: ");
      Serial.print(rawDistance);
      Serial.print(" vs ");
      Serial.println(previous);
      return previous; // Reject physically impossible jumps
    }
    
    // Step 3: Remove oldest reading from total
    total = total - readings[readIndex];
    
    // Step 4: Add new reading
    readings[readIndex] = rawDistance;
    total = total + rawDistance;
    
    // Step 5: Advance index
    readIndex = (readIndex + 1) % 10;
    
    // Step 6: Apply moving average
    float average = total / 10;
    
    // Step 7: Median filter for additional smoothing
    float median = computeMedian();
    
    // Weighted combination
    return (average * 0.7 + median * 0.3);
  }
  
  float getFilteredValue() {
    float validCount = 0;
    float sum = 0;
    
    for(int i = 0; i < 10; i++) {
      if(readings[i] > 0) {
        sum += readings[i];
        validCount++;
      }
    }
    
    return validCount > 0 ? sum / validCount : 0.0;
  }
  
private:
  float computeMedian() {
    float sorted[10];
    memcpy(sorted, readings, sizeof(readings));
    
    // Simple bubble sort for small array
    for(int i = 0; i < 9; i++) {
      for(int j = 0; j < 9 - i; j++) {
        if(sorted[j] > sorted[j+1]) {
          float temp = sorted[j];
          sorted[j] = sorted[j+1];
          sorted[j+1] = temp;
        }
      }
    }
    
    return (sorted[4] + sorted[5]) / 2.0; // Average of middle two
  }
};

// Professional HC-SR04 controller class
class HC_SR04_Advanced {
private:
  // Hardware pins
  uint8_t trigPin;
  uint8_t echoPin;
  
  // Timing configuration
  uint32_t timeoutMicroseconds;
  uint8_t triggerPulseWidth;
  
  // Environmental compensation
  float temperatureCelsius;
  float humidityPercent;
  const float soundSpeedBase = 0.0343; // cm/µs at 20°C
  
  // Performance tracking
  uint32_t successfulReadings;
  uint32_t failedReadings;
  uint32_t totalMeasurementTime;
  
  // Filter system
  UltrasonicFilter distanceFilter;
  UltrasonicPowerManager powerManager;
  
public:
  // Constructor with default parameters
  HC_SR04_Advanced(uint8_t trig, uint8_t echo, 
                   float temp = 20.0, float humidity = 50.0) 
    : trigPin(trig), echoPin(echo), temperatureCelsius(temp), 
      humidityPercent(humidity), successfulReadings(0), 
      failedReadings(0), totalMeasurementTime(0) {
    
    timeoutMicroseconds = 30000; // 30ms for up to 5m
    triggerPulseWidth = 10; // 10µs standard pulse
    
    // Initialize pins
    pinMode(trigPin, OUTPUT);
    digitalWrite(trigPin, LOW);
    
    // Echo pin with pulldown for protection
    pinMode(echoPin, INPUT_PULLDOWN);
    
    Serial.print("HC-SR04 Advanced initialized on pins Trig:");
    Serial.print(trigPin);
    Serial.print(", Echo:");
    Serial.println(echoPin);
  }
  
  // Measure distance with comprehensive error handling
  float measureDistance(bool applyFilter = true) {
    unsigned long measurementStart = micros();
    
    // Ensure clean trigger state
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    
    // Send trigger pulse
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(triggerPulseWidth);
    digitalWrite(trigPin, LOW);
    
    // Measure echo pulse width
    unsigned long pulseWidth = pulseIn(echoPin, HIGH, timeoutMicroseconds);
    
    unsigned long measurementEnd = micros();
    totalMeasurementTime += (measurementEnd - measurementStart);
    
    // Check for measurement failure
    if(pulseWidth == 0) {
      failedReadings++;
      
      // Diagnostic for common failure modes
      if(micros() - measurementStart > timeoutMicroseconds) {
        Serial.println("Error: Measurement timeout - no echo received");
        Serial.println("Possible causes:");
        Serial.println("  1. Object out of range (>500cm)");
        Serial.println("  2. Object absorbs ultrasound (soft materials)");
        Serial.println("  3. Sensor not properly connected");
        Serial.println("  4. Power supply issue");
      } else {
        Serial.println("Error: PulseIn returned 0 - check wiring");
      }
      
      return -1.0; // Error code
    }
    
    // Calculate raw distance
    float soundSpeed = calculateSoundSpeed(temperatureCelsius, humidityPercent);
    float rawDistance = (pulseWidth * soundSpeed) / 2.0;
    
    // Validate reading
    if(!isValidReading(rawDistance)) {
      failedReadings++;
      return -1.0;
    }
    
    successfulReadings++;
    
    // Apply filtering if requested
    if(applyFilter) {
      return distanceFilter.addReading(rawDistance);
    } else {
      return rawDistance;
    }
  }
  
  // Continuous measurement with callback
  void startContinuousMeasurement(uint32_t intervalMs, void (*callback)(float)) {
    Serial.println("Starting continuous measurement mode");
    
    while(true) {
      float distance = measureDistance(true);
      
      if(distance > 0) {
        callback(distance);
      }
      
      // Dynamic interval based on distance
      uint32_t adaptiveInterval = calculateAdaptiveInterval(distance);
      delay(adaptiveInterval);
      
      // Update power management
      powerManager.updateLastMeasurement();
      powerManager.checkPowerState();
    }
  }
  
  // Get sensor health metrics
  void printDiagnostics() {
    Serial.println("\n=== HC-SR04 DIAGNOSTICS ===");
    
    float successRate = (successfulReadings + failedReadings) > 0 ? 
                       (float)successfulReadings / (successfulReadings + failedReadings) * 100 : 0;
    
    Serial.print("Success Rate: ");
    Serial.print(successRate, 1);
    Serial.println("%");
    
    Serial.print("Total Readings: ");
    Serial.println(successfulReadings + failedReadings);
    
    if(successfulReadings > 0) {
      Serial.print("Average Measurement Time: ");
      Serial.print(totalMeasurementTime / successfulReadings);
      Serial.println(" µs");
    }
    
    Serial.print("Current Temperature: ");
    Serial.print(temperatureCelsius, 1);
    Serial.println(" °C");
    
    Serial.print("Calculated Sound Speed: ");
    Serial.print(calculateSoundSpeed(temperatureCelsius, humidityPercent) * 10000, 1);
    Serial.println(" cm/µs");
  }
  
  // Update environmental parameters
  void updateEnvironment(float temperature, float humidity) {
    temperatureCelsius = temperature;
    humidityPercent = humidity;
    
    Serial.print("Environment updated: ");
    Serial.print(temperature, 1);
    Serial.print("°C, ");
    Serial.print(humidity, 0);
    Serial.println("% humidity");
  }
  
private:
  // Calculate sound speed based on temperature and humidity
  float calculateSoundSpeed(float temperatureC, float humidityPercent) {
    // More accurate formula considering humidity
    float soundSpeedDry = 331.3 + 0.606 * temperatureC; // m/s
    
    // Humidity correction (simplified)
    float humidityFactor = 1.0 + (humidityPercent / 100.0) * 0.001;
    float soundSpeedHumid = soundSpeedDry * humidityFactor;
    
    // Convert to cm/µs
    return soundSpeedHumid / 10000.0;
  }
  
  // Validate reading is within physical limits
  bool isValidReading(float distance) {
    if(distance < 2.0 || distance > 500.0) {
      return false;
    }
    
    // Check for physically impossible changes
    static float lastValidDistance = 0.0;
    if(lastValidDistance > 0.0 && abs(distance - lastValidDistance) > 100.0) {
      return false; // Can't change more than 1m between readings
    }
    
    lastValidDistance = distance;
    return true;
  }
  
  // Calculate adaptive measurement interval
  uint32_t calculateAdaptiveInterval(float distance) {
    if(distance < 50.0) {
      return 100; // 10Hz for close objects
    } else if(distance < 200.0) {
      return 250; // 4Hz for medium distance
    } else {
      return 500; // 2Hz for far objects
    }
  }
};

// Multi-sensor ultrasonic array controller
class UltrasonicArray {
private:
  struct SensorConfig {
    HC_SR04_Advanced* sensor;
    String name;
    float xOffset; // Position in cm relative to center
    float yOffset;
    float angleOffset; // Angular offset in degrees
    bool enabled;
  };
  
  SensorConfig sensors[8];
  uint8_t sensorCount;
  
  // Array-wide filtering
  const float maxConsistencyError = 5.0; // cm
  
public:
  UltrasonicArray() : sensorCount(0) {}
  
  // Add sensor to array
  void addSensor(HC_SR04_Advanced* sensor, const char* name, 
                 float x = 0.0, float y = 0.0, float angle = 0.0) {
    if(sensorCount >= 8) {
      Serial.println("Error: Maximum sensor count reached");
      return;
    }
    
    sensors[sensorCount].sensor = sensor;
    sensors[sensorCount].name = name;
    sensors[sensorCount].xOffset = x;
    sensors[sensorCount].yOffset = y;
    sensors[sensorCount].angleOffset = angle;
    sensors[sensorCount].enabled = true;
    
    sensorCount++;
    
    Serial.print("Added sensor: ");
    Serial.print(name);
    Serial.print(" at (");
    Serial.print(x, 1);
    Serial.print(", ");
    Serial.print(y, 1);
    Serial.print("), angle: ");
    Serial.print(angle, 1);
    Serial.println("°");
  }
  
  // Simultaneous measurement (prevents acoustic interference)
  void measureAll(float results[]) {
    // Stagger measurements to prevent interference
    for(int i = 0; i < sensorCount; i++) {
      if(sensors[i].enabled) {
        results[i] = sensors[i].sensor->measureDistance();
        delay(25); // Minimum separation to avoid interference
      } else {
        results[i] = -1.0;
      }
    }
  }
  
  // Detect obstacles with sensor fusion
  ObstacleMap detectObstacles() {
    ObstacleMap map;
    float readings[8];
    
    measureAll(readings);
    
    for(int i = 0; i < sensorCount; i++) {
      if(readings[i] > 0) {
        // Convert sensor reading to global coordinates
        float globalAngle = sensors[i].angleOffset;
        float globalDistance = readings[i];
        
        // Calculate obstacle position
        float obstacleX = sensors[i].xOffset + 
                         globalDistance * cos(radians(globalAngle));
        float obstacleY = sensors[i].yOffset + 
                         globalDistance * sin(radians(globalAngle));
        
        // Add to map with confidence level
        float confidence = calculateConfidence(readings[i], i);
        map.addObstacle(obstacleX, obstacleY, confidence, sensors[i].name);
      }
    }
    
    return map;
  }
  
  // Create 360-degree sensing array (for robotics)
  void configure360Array(float radius) {
    float angles[] = {0.0, 45.0, 90.0, 135.0, 180.0, 225.0, 270.0, 315.0};
    
    for(int i = 0; i < min(8, sensorCount); i++) {
      float x = radius * cos(radians(angles[i]));
      float y = radius * sin(radians(angles[i]));
      
      sensors[i].xOffset = x;
      sensors[i].yOffset = y;
      sensors[i].angleOffset = angles[i];
      
      Serial.print("Sensor ");
      Serial.print(i);
      Serial.print(" positioned at ");
      Serial.print(angles[i], 0);
      Serial.println("°");
    }
  }
  
  // Calculate measurement confidence
  float calculateConfidence(float distance, int sensorIndex) {
    float baseConfidence = 0.9;
    
    // Reduce confidence for very short distances (noise-prone)
    if(distance < 5.0) {
      baseConfidence *= 0.7;
    }
    
    // Reduce confidence for maximum range
    if(distance > 350.0) {
      baseConfidence *= 0.6;
    }
    
    // Check consistency with neighboring sensors
    if(sensorIndex > 0 && sensorIndex < sensorCount - 1) {
      // Would compare with neighbors in actual implementation
    }
    
    return baseConfidence;
  }
  
  // Individual sensor control
  void enableSensor(int index, bool enable) {
    if(index >= 0 && index < sensorCount) {
      sensors[index].enabled = enable;
      Serial.print("Sensor ");
      Serial.print(index);
      Serial.print(" (");
      Serial.print(sensors[index].name);
      Serial.print(") ");
      Serial.println(enable ? "enabled" : "disabled");
    }
  }
};

// Supporting data structure
struct ObstacleMap {
  struct Obstacle {
    float x, y;
    float confidence;
    String detectedBy;
  };
  
  Obstacle obstacles[20];
  int obstacleCount = 0;
  
  void addObstacle(float x, float y, float confidence, const char* sensor) {
    if(obstacleCount < 20) {
      obstacles[obstacleCount].x = x;
      obstacles[obstacleCount].y = y;
      obstacles[obstacleCount].confidence = confidence;
      obstacles[obstacleCount].detectedBy = sensor;
      obstacleCount++;
    }
  }
  
  void printMap() {
    Serial.println("\n=== OBSTACLE MAP ===");
    for(int i = 0; i < obstacleCount; i++) {
      Serial.print("Obstacle ");
      Serial.print(i);
      Serial.print(": (");
      Serial.print(obstacles[i].x, 1);
      Serial.print(", ");
      Serial.print(obstacles[i].y, 1);
      Serial.print(") Confidence: ");
      Serial.print(obstacles[i].confidence, 2);
      Serial.print(" Detected by: ");
      Serial.println(obstacles[i].detectedBy);
    }
  }
};

// Object tracking with ultrasonic sensor
class ObjectTracker {
private:
  HC_SR04_Advanced* sensor;
  float trackingBuffer[50]; // Last 50 measurements
  int bufferIndex = 0;
  
  // Motion detection parameters
  const float motionThreshold = 10.0; // cm change for motion detection
  const float presenceThreshold = 5.0; // cm variance for presence detection
  
  // Tracking state
  enum TrackingState { NO_OBJECT, OBJECT_PRESENT, OBJECT_MOVING };
  TrackingState currentState = NO_OBJECT;
  
public:
  ObjectTracker(HC_SR04_Advanced* ultrasonicSensor) : sensor(ultrasonicSensor) {
    for(int i = 0; i < 50; i++) {
      trackingBuffer[i] = 0.0;
    }
  }
  
  // Main tracking loop
  void update() {
    float distance = sensor->measureDistance();
    
    if(distance > 0) {
      // Add to circular buffer
      trackingBuffer[bufferIndex] = distance;
      bufferIndex = (bufferIndex + 1) % 50;
      
      // Analyze movement
      analyzeMovement(distance);
      
      // Detect presence
      detectPresence();
    }
  }
  
  // Get current tracking state
  TrackingState getState() {
    return currentState;
  }
  
  // Calculate object speed (if moving)
  float calculateSpeed() {
    if(currentState != OBJECT_MOVING) return 0.0;
    
    // Find two recent valid measurements with time difference
    float recentMeasurements[10];
    int measurementCount = 0;
    
    for(int i = 0; i < 10 && measurementCount < 2; i++) {
      int index = (bufferIndex - i - 1 + 50) % 50;
      if(trackingBuffer[index] > 0) {
        recentMeasurements[measurementCount++] = trackingBuffer[index];
      }
    }
    
    if(measurementCount == 2) {
      float distanceChange = abs(recentMeasurements[0] - recentMeasurements[1]);
      // Assuming 100ms between measurements (adjust based on your interval)
      float timeChange = 0.1; // seconds
      
      return distanceChange / timeChange; // cm/s
    }
    
    return 0.0;
  }
  
private:
  void analyzeMovement(float currentDistance) {
    // Calculate average of recent readings
    float sum = 0.0;
    int count = 0;
    
    for(int i = 0; i < 10; i++) {
      int index = (bufferIndex - i - 1 + 50) % 50;
      if(trackingBuffer[index] > 0) {
        sum += trackingBuffer[index];
        count++;
      }
    }
    
    if(count > 0) {
      float recentAverage = sum / count;
      float change = abs(currentDistance - recentAverage);
      
      if(change > motionThreshold) {
        currentState = OBJECT_MOVING;
        Serial.print("Object moving! Speed: ");
        Serial.print(calculateSpeed(), 1);
        Serial.println(" cm/s");
      } else if(currentState == OBJECT_MOVING && change < motionThreshold / 2) {
        currentState = OBJECT_PRESENT;
        Serial.println("Object stopped moving");
      }
    }
  }
  
  void detectPresence() {
    // Calculate variance in recent readings
    float sum = 0.0;
    float sumSquared = 0.0;
    int count = 0;
    
    for(int i = 0; i < 20; i++) {
      int index = (bufferIndex - i - 1 + 50) % 50;
      if(trackingBuffer[index] > 0) {
        sum += trackingBuffer[index];
        sumSquared += trackingBuffer[index] * trackingBuffer[index];
        count++;
      }
    }
    
    if(count > 5) {
      float mean = sum / count;
      float variance = (sumSquared / count) - (mean * mean);
      float stdDev = sqrt(variance);
      
      if(stdDev < presenceThreshold && mean < 300.0) {
        currentState = OBJECT_PRESENT;
      } else if(stdDev < 1.0 && mean > 400.0) {
        currentState = NO_OBJECT;
      }
    }
  }
};

// Complete parking sensor system
class ParkingSensorSystem {
private:
  HC_SR04_Advanced* sensors[4]; // Front, Rear, Left, Right
  UltrasonicArray sensorArray;
  
  // Parking zones (distance in cm)
  const float zoneRed = 30.0;
  const float zoneYellow = 60.0;
  const float zoneGreen = 120.0;
  
  // Alert system
  struct Alert {
    String sensor;
    String zone;
    float distance;
    unsigned long timestamp;
  };
  
  Alert activeAlerts[10];
  int alertCount = 0;
  
  // Buzzer/LED feedback
  int buzzerPin;
  int ledRedPin, ledYellowPin, ledGreenPin;
  
public:
  ParkingSensorSystem(int buzzer, int redLED, int yellowLED, int greenLED) 
    : buzzerPin(buzzer), ledRedPin(redLED), ledYellowPin(yellowLED), 
      ledGreenPin(greenLED) {
    
    // Setup output pins
    pinMode(buzzerPin, OUTPUT);
    pinMode(ledRedPin, OUTPUT);
    pinMode(ledYellowPin, OUTPUT);
    pinMode(ledGreenPin, OUTPUT);
    
    // Initialize with all clear
    updateDisplay(zoneGreen + 10, "none");
  }
  
  // Add sensor to system
  void addSensor(int position, HC_SR04_Advanced* sensor, const char* name) {
    if(position >= 0 && position < 4) {
      sensors[position] = sensor;
      sensorArray.addSensor(sensor, name);
    }
  }
  
  // Main monitoring loop
  void monitorParking() {
    float distances[4];
    
    // Measure all sensors
    for(int i = 0; i < 4; i++) {
      if(sensors[i] != nullptr) {
        distances[i] = sensors[i]->measureDistance();
        evaluateZone(i, distances[i]);
      }
    }
    
    // Find closest obstacle
    float closest = findClosestObstacle(distances, 4);
    
    // Update visual/audio feedback
    updateDisplay(closest, findCriticalSensor(distances, 4));
    
    // Log status periodically
    static unsigned long lastLog = 0;
    if(millis() - lastLog > 5000) {
      logParkingStatus(distances);
      lastLog = millis();
    }
  }
  
  // Generate parking assist guidance
  String getParkingGuidance(float distances[]) {
    float front = distances[0];
    float rear = distances[1];
    float left = distances[2];
    float right = distances[3];
    
    // Simple parking guidance logic
    if(front < zoneRed && rear < zoneRed) {
      return "PERFECTLY PARKED!";
    } else if(front < zoneRed && rear > zoneYellow) {
      return "Move FORWARD";
    } else if(rear < zoneRed && front > zoneYellow) {
      return "Move BACKWARD";
    } else if(left < zoneRed && right > zoneYellow) {
      return "Adjust RIGHT";
    } else if(right < zoneRed && left > zoneYellow) {
      return "Adjust LEFT";
    } else if(front < zoneYellow && rear < zoneYellow) {
      return "Centered front/back";
    } else {
      return "Clear on all sides";
    }
  }
  
private:
  void evaluateZone(int sensorIndex, float distance) {
    String zone;
    
    if(distance <= zoneRed) {
      zone = "RED";
      addAlert(sensorIndex, zone, distance);
      triggerAudioAlert(1); // Continuous beep
    } else if(distance <= zoneYellow) {
      zone = "YELLOW";
      addAlert(sensorIndex, zone, distance);
      triggerAudioAlert(2); // Slow beep
    } else if(distance <= zoneGreen) {
      zone = "GREEN";
      triggerAudioAlert(3); // Single beep
    } else {
      zone = "CLEAR";
    }
    
    // Update sensor-specific display (if available)
    updateSensorDisplay(sensorIndex, zone, distance);
  }
  
  void addAlert(int sensorIndex, String zone, float distance) {
    if(alertCount < 10) {
      activeAlerts[alertCount].sensor = "Sensor " + String(sensorIndex);
      activeAlerts[alertCount].zone = zone;
      activeAlerts[alertCount].distance = distance;
      activeAlerts[alertCount].timestamp = millis();
      alertCount++;
    }
    
    // Keep only recent alerts
    cleanupOldAlerts();
  }
  
  void cleanupOldAlerts() {
    unsigned long currentTime = millis();
    const unsigned long alertLifetime = 10000; // 10 seconds
    
    for(int i = 0; i < alertCount; i++) {
      if(currentTime - activeAlerts[i].timestamp > alertLifetime) {
        // Remove old alert
        for(int j = i; j < alertCount - 1; j++) {
          activeAlerts[j] = activeAlerts[j + 1];
        }
        alertCount--;
        i--;
      }
    }
  }
  
  void triggerAudioAlert(int pattern) {
    switch(pattern) {
      case 1: // Continuous (red zone)
        tone(buzzerPin, 2000);
        break;
      case 2: // Slow beep (yellow zone)
        tone(buzzerPin, 1500, 200);
        delay(400);
        break;
      case 3: // Single beep (green zone)
        tone(buzzerPin, 1000, 100);
        delay(1000);
        break;
      default:
        noTone(buzzerPin);
    }
  }
  
  void updateDisplay(float closestDistance, String criticalSensor) {
    // Control LEDs based on closest obstacle
    digitalWrite(ledRedPin, closestDistance <= zoneRed ? HIGH : LOW);
    digitalWrite(ledYellowPin, 
                 closestDistance > zoneRed && closestDistance <= zoneYellow ? HIGH : LOW);
    digitalWrite(ledGreenPin, closestDistance > zoneYellow ? HIGH : LOW);
    
    // Optional: Display on OLED
    displayParkingInfo(closestDistance, criticalSensor);
  }
  
  void displayParkingInfo(float distance, String sensor) {
    // This would interface with OLED display
    Serial.print("Closest: ");
    Serial.print(distance, 1);
    Serial.print("cm (");
    Serial.print(sensor);
    Serial.println(")");
  }
  
  float findClosestObstacle(float distances[], int count) {
    float closest = 1000.0;
    
    for(int i = 0; i < count; i++) {
      if(distances[i] > 0 && distances[i] < closest) {
        closest = distances[i];
      }
    }
    
    return closest < 1000.0 ? closest : 999.0;
  }
  
  String findCriticalSensor(float distances[], int count) {
    float minDist = 1000.0;
    int minIndex = -1;
    
    for(int i = 0; i < count; i++) {
      if(distances[i] > 0 && distances[i] < minDist) {
        minDist = distances[i];
        minIndex = i;
      }
    }
    
    const char* sensorNames[] = {"Front", "Rear", "Left", "Right"};
    return minIndex >= 0 ? sensorNames[minIndex] : "None";
  }
  
  void updateSensorDisplay(int sensorIndex, String zone, float distance) {
    // Individual sensor feedback (could be different LEDs)
    // Implementation depends on available hardware
  }
  
  void logParkingStatus(float distances[]) {
    Serial.println("\n=== PARKING STATUS UPDATE ===");
    Serial.println("Sensor distances:");
    
    const char* sensorNames[] = {"Front", "Rear", "Left", "Right"};
    for(int i = 0; i < 4; i++) {
      if(sensors[i] != nullptr) {
        Serial.print("  ");
        Serial.print(sensorNames[i]);
        Serial.print(": ");
        
        if(distances[i] > 0) {
          Serial.print(distances[i], 1);
          Serial.print("cm");
          
          if(distances[i] <= zoneRed) {
            Serial.print(" [STOP!]");
          } else if(distances[i] <= zoneYellow) {
            Serial.print(" [CAUTION]");
          } else if(distances[i] <= zoneGreen) {
            Serial.print(" [OK]");
          } else {
            Serial.print(" [CLEAR]");
          }
        } else {
          Serial.print("NO READING");
        }
        
        Serial.println();
      }
    }
    
    Serial.print("Active alerts: ");
    Serial.println(alertCount);
    
    if(alertCount > 0) {
      Serial.println("Current alerts:");
      for(int i = 0; i < alertCount; i++) {
        Serial.print("  ");
        Serial.print(activeAlerts[i].sensor);
        Serial.print(": ");
        Serial.print(activeAlerts[i].zone);
        Serial.print(" zone (");
        Serial.print(activeAlerts[i].distance, 1);
        Serial.println("cm)");
      }
    }
    
    Serial.println("========================");
  }
};

// Complete diagnostic and optimization system
class UltrasonicDiagnostic {
public:
  static void runFullDiagnostic(HC_SR04_Advanced& sensor) {
    Serial.println("\n=== HC-SR04 COMPREHENSIVE DIAGNOSTIC ===");
    
    // 1. Basic connectivity test
    Serial.println("1. BASIC CONNECTIVITY TEST:");
    testConnectivity(sensor);
    
    // 2. Performance benchmark
    Serial.println("\n2. PERFORMANCE BENCHMARK:");
    runPerformanceBenchmark(sensor);
    
    // 3. Environmental assessment
    Serial.println("\n3. ENVIRONMENTAL ASSESSMENT:");
    assessEnvironmentalFactors();
    
    // 4. Accuracy validation
    Serial.println("\n4. ACCURACY VALIDATION:");
    validateAccuracy(sensor);
    
    // 5. Recommendations
    Serial.println("\n5. RECOMMENDATIONS:");
    provideRecommendations();
    
    Serial.println("===============================");
  }
  
  static void testConnectivity(HC_SR04_Advanced& sensor) {
    Serial.println("Testing sensor connectivity...");
    
    int successCount = 0;
    const int testAttempts = 10;
    
    for(int i = 0; i < testAttempts; i++) {
      float distance = sensor.measureDistance(false); // No filtering
      
      if(distance > 0) {
        successCount++;
        Serial.print("  Attempt ");
        Serial.print(i + 1);
        Serial.print(": ");
        Serial.print(distance, 1);
        Serial.println(" cm");
      } else {
        Serial.print("  Attempt ");
        Serial.print(i + 1);
        Serial.println(": FAILED");
      }
      
      delay(100);
    }
    
    float successRate = (float)successCount / testAttempts * 100;
    Serial.print("Connectivity Success Rate: ");
    Serial.print(successRate, 1);
    Serial.println("%");
    
    if(successRate < 80.0) {
      Serial.println("  → WARNING: Poor connectivity detected");
      Serial.println("  → Check: Wiring, power supply, voltage levels");
    }
  }
  
  static void runPerformanceBenchmark(HC_SR04_Advanced& sensor) {
    Serial.println("Measuring performance characteristics...");
    
    // Test response time
    unsigned long startTime = micros();
    const int measurements = 50;
    
    for(int i = 0; i < measurements; i++) {
      sensor.measureDistance(false);
    }
    
    unsigned long endTime = micros();
    float avgTime = (endTime - startTime) / (float)measurements;
    
    Serial.print("  Average measurement time: ");
    Serial.print(avgTime / 1000.0, 1);
    Serial.println(" ms");
    
    // Test maximum sampling rate
    Serial.println("  Determining maximum sampling rate...");
    
    int maxRate = 0;
    for(int rate = 10; rate <= 100; rate += 10) {
      if(testSamplingRate(sensor, rate)) {
        maxRate = rate;
      } else {
        break;
      }
    }
    
    Serial.print("  Maximum reliable sampling rate: ");
    Serial.print(maxRate);
    Serial.println(" Hz");
    
    // Provide recommendations based on results
    if(avgTime > 30000) { // 30ms
      Serial.println("  → RECOMMENDATION: Measurement time is high");
      Serial.println("  → Consider: Reducing timeout, checking for obstacles");
    }
    
    if(maxRate < 20) {
      Serial.println("  → RECOMMENDATION: Low sampling rate");
      Serial.println("  → Consider: Shorter distances, better sensor placement");
    }
  }
  
  static void assessEnvironmentalFactors() {
    Serial.println("Assessing environmental factors...");
    
    // These would be actual sensor readings in a full implementation
    Serial.println("  Note: For accurate assessment, connect:");
    Serial.println("    - Temperature sensor (for sound speed compensation)");
    Serial.println("    - Humidity sensor (optional, minor effect)");
    
    Serial.println("  Common environmental issues:");
    Serial.println("    1. Temperature variations affect sound speed");
    Serial.println("    2. Air currents can deflect ultrasound");
    Serial.println("    3. Background noise at 40kHz can interfere");
    Serial.println("    4. Dust/moisture can attenuate signal");
  }
  
  static void validateAccuracy(HC_SR04_Advanced& sensor) {
    Serial.println("Place sensor at known distances for validation");
    Serial.println("Recommended test distances: 10cm, 50cm, 100cm, 200cm");
    Serial.println("Press any key when ready to begin...");
    
    while(!Serial.available());
    Serial.read();
    
    float testDistances[] = {10.0, 50.0, 100.0, 200.0};
    const int samplesPerDistance = 20;
    
    for(int d = 0; d < 4; d++) {
      Serial.print("\nTesting at ");
      Serial.print(testDistances[d]);
      Serial.println(" cm:");
      Serial.println("Place object and press any key...");
      
      while(!Serial.available());
      Serial.read();
      
      delay(1000); // Allow positioning
      
      float sum = 0.0;
      int validSamples = 0;
      float minReading = 1000.0;
      float maxReading = 0.0;
      
      for(int s = 0; s < samplesPerDistance; s++) {
        float reading = sensor.measureDistance(false);
        
        if(reading > 0) {
          sum += reading;
          validSamples++;
          
          if(reading < minReading) minReading = reading;
          if(reading > maxReading) maxReading = reading;
        }
        
        delay(50);
      }
      
      if(validSamples > 0) {
        float average = sum / validSamples;
        float error = average - testDistances[d];
        float errorPercent = (error / testDistances[d]) * 100;
        float range = maxReading - minReading;
        
        Serial.print("  Average: ");
        Serial.print(average, 1);
        Serial.print(" cm, Error: ");
        Serial.print(error, 1);
        Serial.print(" cm (");
        Serial.print(errorPercent, 1);
        Serial.println("%)");
        
        Serial.print("  Range: ");
        Serial.print(minReading, 1);
        Serial.print(" - ");
        Serial.print(maxReading, 1);
        Serial.print(" cm (variation: ");
        Serial.print(range, 1);
        Serial.println(" cm)");
        
        if(abs(errorPercent) > 5.0) {
          Serial.print("  → WARNING: High error rate (");
          Serial.print(errorPercent, 1);
          Serial.println("%)");
          Serial.println("  → Check: Sensor calibration, temperature compensation");
        }
        
        if(range > 10.0) {
          Serial.print("  → WARNING: High measurement variation (");
          Serial.print(range, 1);
          Serial.println(" cm)");
          Serial.println("  → Consider: Adding filtering, stabilizing power");
        }
      } else {
        Serial.println("  → ERROR: No valid readings obtained");
      }
    }
  }
  
  static void provideRecommendations() {
    Serial.println("Based on diagnostic results:");
    Serial.println("1. For accuracy < 1cm:");
    Serial.println("   - Implement temperature compensation");
    Serial.println("   - Use multiple measurements with filtering");
    Serial.println("   - Ensure stable 5V power supply");
    
    Serial.println("\n2. For long-range detection (> 3m):");
    Serial.println("   - Use high-quality sensors");
    Serial.println("   - Ensure perpendicular surface alignment");
    Serial.println("   - Consider environmental conditions");
    
    Serial.println("\n3. For fast sampling (> 20Hz):");
    Serial.println("   - Use interrupt-based timing");
    Serial.println("   - Implement measurement pipelining");
    Serial.println("   - Reduce measurement timeout");
    
    Serial.println("\n4. For noisy environments:");
    Serial.println("   - Add electrical filtering (capacitors)");
    Serial.println("   - Implement advanced software filters");
    Serial.println("   - Consider sensor shielding");
  }
  
private:
  static bool testSamplingRate(HC_SR04_Advanced& sensor, int rateHz) {
    unsigned long testDuration = 1000; // 1 second
    unsigned long interval = 1000 / rateHz;
    int requiredMeasurements = rateHz * (testDuration / 1000);
    int successfulMeasurements = 0;
    
    unsigned long startTime = millis();
    
    while(millis() - startTime < testDuration) {
      unsigned long measurementStart = millis();
      
      float distance = sensor.measureDistance(false);
      if(distance > 0) {
        successfulMeasurements++;
      }
      
      unsigned long elapsed = millis() - measurementStart;
      if(elapsed < interval) {
        delay(interval - elapsed);
      }
    }
    
    float successRate = (float)successfulMeasurements / requiredMeasurements * 100;
    return successRate > 90.0; // Require 90% success rate
  }
};