#include <LiquidCrystal.h>
#include <NewPing.h>

#define MAX_DISTANCE 210 // 10cm extra marge op 200cm om afwijkingsberekening bij te stelle nen mogelijk te maken!
#define MEDIAN_SAMPLE 5

// Definieer de pinnen voor de LCD
#define RS 12
#define E 11
#define D4 5
#define D5 4
#define D6 3
#define D7 2

#define TRIGGER_PIN  7  // Gemeenschappelijke trigger en echo pin
#define ECHO_PIN     7  // Gemeenschappelijke trigger en echo pin

#define buttonPin 13

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
LiquidCrystal lcd(RS, E, D4, D5, D6, D7); 

bool buttonState = LOW;
bool previousButtonState = LOW;
unsigned long lastDebounceTime = 0;
unsigned long debounceDelay = 70;
unsigned long sensorStabilizeTime = 0;

void setup() {
  Serial.begin(9600);
  lcd.begin(16, 2);
  pinMode(buttonPin, INPUT); // Interne pull-down weerstand niet inschakelen
}

void loop() {
  // Lees de status van de drukknop
  buttonState = digitalRead(buttonPin);

  // Controleer of de knop is ingedrukt
  // if (buttonState == LOW && previousButtonState == HIGH) {
    if ((buttonState == LOW) && (previousButtonState == HIGH) && (millis() >= (lastDebounceTime + debounceDelay))) {
    lastDebounceTime = millis();
    sensorStabilizeTime = millis() + 30;
    lcd.setCursor(0, 0);
    displayMedianDistance();
    lcd.setCursor(0, 1);
    lcd.print("DRUKKNOP GEDRUKT!");
    delay(1000);
    lcd.setCursor(0, 1);
    lcd.print("                "); 
  }
  if (millis() > sensorStabilizeTime) {
    displayMedianDistance();
  }

  // Bewaar de huidige knopstatus voor de volgende iteratie
  previousButtonState = buttonState;
}

void displayMedianDistance() {
  unsigned int uS = sonar.ping_median(MEDIAN_SAMPLE);

  unsigned int distance = sonar.convert_cm(uS);

  //Correctie toepassing op metingen
  if (distance <= 100) {
    distance = distance * 0.988; // Correctie van 2% op de afstand onder de 100cm
  } else {
    distance = distance * 0.975; // Correctie van 3% op de afstand boven de 100cm
  }
  if (distance >= (MAX_DISTANCE - 7)) { // Calculatie tot 200cm .. Als waarde tussen 200 en 210 komt 'buiten bereik' melding
    lcd.setCursor(0, 0);
    lcd.print("!METINGSWAARDEN!");
    lcd.setCursor(0, 1);
    lcd.print("!BUITEN BEREIK! ");
    delay(1500);
    lcd.clear();
    return;
  }
  if (distance <= 0) {
    return;
  }
  lcd.clear();
  lcd.print("Afstand: ");
  lcd.print(distance);
  lcd.print(" cm");
  Serial.println(distance);
}