// =============================================================
// Deteksi jarak auto menggerakkan relay
// Dibuat oleh [Eko Andriyanto] - [09 April 2025 / V 1.1]
// =============================================================

#define RELAY_PIN 8
#define TRIG_PIN A1
#define ECHO_PIN A2

// Filter variables
const int numReadings = 5;
long readings[numReadings];
int readIndex = 0;

void setup() {
  Serial.begin(9600);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(RELAY_PIN, OUTPUT);
  digitalWrite(RELAY_PIN, LOW);
  
  // Inisialisasi filter
  for (int i = 0; i < numReadings; i++) {
    readings[i] = 0;
  }
}

void loop() {
  long distance = measureDistance();
  
  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");

  // Handle sensor error
  if(distance < 0) {
    Serial.println("Error: Sensor timeout!");
    delay(100);
    return;
  }

  // Hysteresis control
  const int thresholdOn = 18; // atur  jarak minimal
  const int thresholdOff = 22; // atur  jarak maksimal
  
  if (distance > thresholdOff) {
    digitalWrite(RELAY_PIN, LOW);
  } else if (distance <= thresholdOn) {
    digitalWrite(RELAY_PIN, HIGH);
  }

  delay(100);
}

long measureDistance() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  // Timeout 30ms (≈5 meter)
  long duration = pulseIn(ECHO_PIN, HIGH, 30000);
  
  if(duration == 0) return -1; // Sensor timeout
  
  long distance = duration * 0.034 / 2; // Fixed constant
  
  // Rolling average filter
  readings[readIndex] = distance;
  readIndex = (readIndex + 1) % numReadings;
  
  long filtered = 0;
  for (int i = 0; i < numReadings; i++) {
    filtered += readings[i];
  }
  return filtered / numReadings;
}