// LEFT SENSOR
#define TRIG_LEFT 5
#define ECHO_LEFT 18

// RIGHT SENSOR
#define TRIG_RIGHT 17
#define ECHO_RIGHT 16

// OUTPUTS
#define BUZZER_LEFT 25
#define BUZZER_RIGHT 26
#define LED_PIN 19

// BUTTON
#define BUTTON_PIN 4

float distanceLeft;
float distanceRight;

float filteredLeft = 0;
float filteredRight = 0;

float alpha = 0.7;   // smoothing factor

// ---------- Function to measure distance ----------
float getDistance(int trigPin, int echoPin) {

  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);

  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);

  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH);

  float distance = duration * 0.034 / 2;

  return distance;
}

// ---------- Setup ----------
void setup() {

  Serial.begin(115200);

  pinMode(TRIG_LEFT, OUTPUT);
  pinMode(ECHO_LEFT, INPUT);

  pinMode(TRIG_RIGHT, OUTPUT);
  pinMode(ECHO_RIGHT, INPUT);

  pinMode(BUZZER_LEFT, OUTPUT);
  pinMode(BUZZER_RIGHT, OUTPUT);

  pinMode(LED_PIN, OUTPUT);

  pinMode(BUTTON_PIN, INPUT_PULLUP);
}

// ---------- Loop ----------
void loop() {

  // Read sensors
  distanceLeft = getDistance(TRIG_LEFT, ECHO_LEFT);
  distanceRight = getDistance(TRIG_RIGHT, ECHO_RIGHT);

  // Exponential smoothing
  filteredLeft = alpha * filteredLeft + (1 - alpha) * distanceLeft;
  filteredRight = alpha * filteredRight + (1 - alpha) * distanceRight;

  Serial.print("Left: ");
  Serial.print(filteredLeft);
  Serial.print(" cm   ");

  Serial.print("Right: ");
  Serial.println(filteredRight);

  // ---------- Obstacle Detection ----------

  if(filteredLeft < 50) {
    digitalWrite(BUZZER_LEFT, HIGH);
  } else {
    digitalWrite(BUZZER_LEFT, LOW);
  }

  if(filteredRight < 50) {
    digitalWrite(BUZZER_RIGHT, HIGH);
  } else {
    digitalWrite(BUZZER_RIGHT, LOW);
  }

  // Both sensors detect obstacle (front)
  if(filteredLeft < 30 && filteredRight < 30) {
    digitalWrite(BUZZER_LEFT, HIGH);
    digitalWrite(BUZZER_RIGHT, HIGH);
    digitalWrite(LED_PIN, HIGH);
  }
  else{
    digitalWrite(LED_PIN, LOW);
  }

  // ---------- Emergency Button ----------
  if(digitalRead(BUTTON_PIN) == LOW){

    Serial.println("!!! EMERGENCY BUTTON PRESSED !!!");

    digitalWrite(BUZZER_LEFT, HIGH);
    digitalWrite(BUZZER_RIGHT, HIGH);
    digitalWrite(LED_PIN, HIGH);

    delay(2000);

    digitalWrite(BUZZER_LEFT, LOW);
    digitalWrite(BUZZER_RIGHT, LOW);
    digitalWrite(LED_PIN, LOW);
  }

  delay(100);
}