#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

// Definieer de pinnen voor de Ultrasoon sensor
#define TRIGGER_PIN  7  // Gemeenschappelijke trigger en echo pin
#define ECHO_PIN     7  // Gemeenschappelijke trigger en echo pin

// Definieer de pin voor de drukknop
#define BUTTON_PIN 13

// Definieer de pinnen voor de LEDs en het relais
#define RED_LED_PIN 8
#define YELLOW_LED_PIN 9
#define GREEN_LED_PIN 10
#define RELAY_PIN A1

// Initialiseer Ultrasoonsensor en LCD display
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
LiquidCrystal lcd(RS, E, D4, D5, D6, D7);

// Definieer variabelen
bool buttonState = LOW;
bool previousButtonState = LOW;
unsigned long lastDebounceTime = 0;
unsigned long debounceDelay = 70;

unsigned long lastTimerStand = 0;
unsigned long timerStand = 3000;  // 3 seconden timer
unsigned long newTimerStand;

unsigned int distance;

unsigned long sensorStabilizeTime = 0;
const byte sensorHeight = 197;

int waterLevel = 0;
int waterLevelPercent = 0;
bool flag = false;    // Variabel 'flag' wordt enkel bi controle relaisstatus gebruikt

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

// -------------------------- LOOP ------    ------ LOOP -------------------------- //
void loop() {
  // Lees de status van de drukknop
  buttonState = digitalRead(BUTTON_PIN);

  // 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;

}

// -------------------------------- SUB ROUTINES -------------------------------- //
void displayMedianDistance() {
  unsigned int uS = sonar.ping_median(MEDIAN_SAMPLE);
  distance = sonar.convert_cm(uS);

  //Correctie toepassing op gemeten Ultrasoonwaarde
  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 meetwaarde 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;
  }

  // Geef gemeten en gecorrigeerde afstand weer op het LCD display
  lcd.clear();
  lcd.print("Afstand: ");
  lcd.print(distance);
  lcd.print(" cm");
  Serial.println("Gemeten afstand van sensor tot wateroppervlak: " + String(distance) + " cm");

  // Controleer de waterhoogte en bedien de LEDs en eventueel het relais
  checkWaterLevel();
}

void checkWaterLevel() {
  lastTimerStand = millis();

  // Print waterstandhoogte en het volume percentage enkel na verloop van de opgegeven tijd (timerStand)
  if (lastTimerStand > newTimerStand) {
    newTimerStand = millis() + timerStand;

    waterLevel = (sensorHeight - distance);  // Bereken de waterhoogte in cm
    waterLevelPercent = map(waterLevel, 0, sensorHeight, 0, 100); // Bereken het waterniveau in procenten
    Serial.println("\n Waterstand hoogte: " + String(waterLevel) + " cm \t " + "  Inhoud: " + String(waterLevelPercent) + " procent \n");
  }

  // Controleer steeds de waterhoogte en bedien de bijhorende LEDs
  if (waterLevelPercent < 30) {                                       // Rode LED brand tot 30 %
    digitalWrite(RED_LED_PIN, HIGH);
    digitalWrite(YELLOW_LED_PIN, LOW);
    digitalWrite(GREEN_LED_PIN, LOW);
  } else if (waterLevelPercent >= 30 && waterLevelPercent < 70) {     // Gele LED brand tussen 30 en 70 %
    digitalWrite(RED_LED_PIN, LOW);
    digitalWrite(YELLOW_LED_PIN, HIGH);
    digitalWrite(GREEN_LED_PIN, LOW);
  } else {                                                            // Groene LED brand vanaf +70 %
    digitalWrite(RED_LED_PIN, LOW);
    digitalWrite(YELLOW_LED_PIN, LOW);
    digitalWrite(GREEN_LED_PIN, HIGH);
  }

  // Controleer steeds de waterhoogte en bedien zo nodig ook het relais (tussen 15 en 22%)
  if (waterLevelPercent < 15) {
    flag = true;
    digitalWrite(RELAY_PIN, HIGH);  // Start het pompen
    Serial.println("Relais geactiveerd, Pomp is in werking \n");
    flashinRedLed();

  } else if ((waterLevelPercent >= 22) && (flag == true)) {
    digitalWrite(RELAY_PIN, LOW);  // Stop het pompen
    flashinRedLed();
    Serial.println("Relais gedeactiveerd, Pomp werkt niet meer! \n");
    flag = false;
     return;

  } else if ((waterLevelPercent >= 15) && (flag == true)) {
    Serial.println("Pomp nog steeds in werking \n");
    flashinRedLed();
  }
}

// Laat de rode LED snel knipperen
void flashinRedLed() {
  digitalWrite(RED_LED_PIN, HIGH);
  delay(100);
  digitalWrite(RED_LED_PIN, LOW);
  delay(100);
}