#ifndef ULTRASONIC_SETUP_H
#define ULTRASONIC_SETUP_H

#include <config.h>


void setupUltrasonicPins() {
    pinMode(trigPin, OUTPUT);
    pinMode(echoPin, INPUT);
}

float readDistanceOnce() { // Reads distance once in cm
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(20);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH, 25000);
  if (duration == 0) return NAN;
  float distance = (duration * 0.0343f) * 0.5f; // cm
  return distance;
}

float readUltrasonicRobust() {
  float samples[NUM_SAMPLES]; // Array to hold samples
  int validCount = 0; // Count of valid samples

  for (int i = 0; i < NUM_SAMPLES; i++) {
    esp_task_wdt_reset(); // reset watchdog timer

    float dist = readDistanceOnce();
    
    if (dist > MIN_READING_CM && dist < MAX_DEPTH_CM) { //Add only valid readings
      samples[validCount++] = dist;
    }
    delay(SAMPLE_DELAY_MS); // Wait between samples
  }

  if (validCount < (NUM_SAMPLES / 2)) return NAN; // Not enough valid samples

  // Sort
  for (int i = 0; i < validCount - 1; i++) {
      for (int j = i + 1; j < validCount; j++) {
          if (samples[i] > samples[j]) {
              float temp = samples[i];
              samples[i] = samples[j];
              samples[j] = temp;
          }
      }
  }
  return samples[validCount / 2]; // Return median value of valid samples
}

#endif // ULTRASONIC_SETUP_H