#define left_trig A2
#define left_echo A3

#define right_trig 3
#define right_echo 4

#define front_trig 13
#define front_echo 12

// Constants
#define MAX_DISTANCE 200  // Maximum detection distance
#define SAFE_DISTANCE 20  // Safe distance from obstacles
#define FILTER_SIZE 10     // Number of readings to average
int distance_constant = 58;

// Global variables
int leftDistanceBuffer[FILTER_SIZE] = {0};
int rightDistanceBuffer[FILTER_SIZE] = {0};
int frontDistanceBuffer[FILTER_SIZE] = {0};

int smoothReading(int* buffer, int newReading) {
  // Shift buffer
  for (int i = FILTER_SIZE - 1; i > 0; i--) {
    buffer[i] = buffer[i-1];
  }
  buffer[0] = newReading;
  
  // Calculate median (more robust than average)
  int sortedBuffer[FILTER_SIZE];
  memcpy(sortedBuffer, buffer, FILTER_SIZE * sizeof(int));
  
  // Simple bubble sort
  for (int i = 0; i < FILTER_SIZE - 1; i++) {
    for (int j = 0; j < FILTER_SIZE - i - 1; j++) {
      if (sortedBuffer[j] > sortedBuffer[j+1]) {
        int temp = sortedBuffer[j];
        sortedBuffer[j] = sortedBuffer[j+1];
        sortedBuffer[j+1] = temp;
      }
    }
  }
  
  // Return median value
  return sortedBuffer[FILTER_SIZE/2];
}

// Safe sensor reading function
unsigned int getSafeDistance(int PIN_TRIG , int PIN_ECHO) {
  digitalWrite(PIN_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG, LOW);

  // Read the result:
  int duration = pulseIn(PIN_ECHO, HIGH);
  unsigned int distance = duration / distance_constant;
  // Return MAX_DISTANCE if reading is invalid
  return (distance <= 0 || distance > MAX_DISTANCE) ? MAX_DISTANCE : distance;
}
void setup() {
  Serial.begin(9600);
  pinMode(left_trig, OUTPUT);
  pinMode(left_echo, INPUT);
  pinMode(right_trig, OUTPUT);
  pinMode(right_echo, INPUT);
  pinMode(front_trig, OUTPUT);
  pinMode(front_echo, INPUT);    
}

void loop() {
  // Get sensor readings with safety checks
  unsigned int rawLeftDistance = getSafeDistance(left_trig,left_echo);
  unsigned int rawFrontDistance = getSafeDistance(front_trig,front_echo);
  unsigned int rawRightDistance = getSafeDistance(right_trig,right_echo);
  
  // Apply smoothing filter
  int leftDistance = smoothReading(leftDistanceBuffer, rawLeftDistance);
  int frontDistance = smoothReading(frontDistanceBuffer, rawFrontDistance);
  int rightDistance = smoothReading(rightDistanceBuffer, rawRightDistance);
  
  // Debugging output
  Serial.print("Left: ");
  Serial.print(leftDistance);
  Serial.print(" cm, Front: ");
  Serial.print(frontDistance);
  Serial.print(" cm, Right: ");
  Serial.print(rightDistance);
  Serial.println(" cm");
}