#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 pinnen voor de LEDs
#define RED_LED_PIN 8
#define YELLOW_LED_PIN 9
#define GREEN_LED_PIN 10

// Definieer de pinnen voor de drukknop, het relais en de LCD power
#define BUTTON_PIN 13
#define RELAY_PIN A1
#define LCDPOWER_PIN A3

// 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
bool flash = LOW;
bool startupFlag = true;
bool buttonPushed = false;
bool pumpFlag = false;
byte teller = 0;

// -------------------------- SETUP ------  ------ SETUP -------------------------- //
void setup() {
  pinMode(BUTTON_PIN, INPUT); // Interne pull-down weerstand niet inschakelen
  pinMode(LCDPOWER_PIN, OUTPUT);
  pinMode(GREEN_LED_PIN, OUTPUT);
  pinMode(YELLOW_LED_PIN, OUTPUT);
  pinMode(RED_LED_PIN, OUTPUT);

  Serial.begin(9600);
  lcd.begin(16, 2);
}

// -------------------------- 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;
    analogWrite(LCDPOWER_PIN, 255); // LCD Backlight on
    lcd.setCursor(0, 0);
    displayMedianDistance();
    lcd.setCursor(0, 1);
    lcd.print("DRUKKNOP GEDRUKT!");
    Serial.print("\n**************** ! DRUKKNOP GEDRUKT ! *************** \n\n");
    Serial.println(" ");
    delay(800);
    lcd.setCursor(0, 1);
    lcd.print("                ");
    buttonPushed = true;
  }
  if ((millis() > sensorStabilizeTime) or (startupFlag = true)) {
    startupFlag = false;
    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 ((waterLevel >= MAX_DISTANCE) or (distance >= (MAX_DISTANCE - 7))) { // Calculatie tot 200cm .. Als meetwaarde tussen 200 en 210 komt 'buiten bereik' melding
    outOfRangeTxt();
  }
  if (distance <= 0) {
    return;
  }

  // Geef gemeten en gecorrigeerde afstand weer
  if ((buttonPushed == true) && (pumpFlag == false)) {
    analogWrite(LCDPOWER_PIN, 255); // LCD Backlight on
    lcd.setCursor(0, 0);
    lcdAfstand();
    lcd.setCursor(0, 1);
    lcd.print("POMP WERKT NIET!");
    delay(500);
  }

  if ((buttonPushed == true) && (pumpFlag == true)) {
    analogWrite(LCDPOWER_PIN, 255); // LCD Backlight on
    /* lcd.setCursor(0, 0);
      lcdAfstand();
      delay(500); */

    lcd.setCursor(0, 0);
    lcd.print("POMP IN WERKING!");
    lcd.setCursor(0, 1);
    lcd.print("Niveau: ");
    lcd.print(waterLevel);
    lcd.print(" cm    ");
    delay(500);
  } else if (pumpFlag == true) {
    analogWrite(LCDPOWER_PIN, 255); // LCD Backlight on
    lcd.setCursor(0, 0);
    lcdAfstand();
    lcd.setCursor(0, 1);
    lcd.print("POMP IN WERKING!");
    delay(500);
  } else if ((buttonPushed == true) && (pumpFlag == false) && (teller =!0)) {
    lcd.setCursor(0, 1);
    lcd.print("POMP WERKT NIET!");
    delay(500);
    teller =+ teller;
    if (teller >= 4){
    teller = 0;
    }
  }

  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

    if (waterLevel <= 0) {
      return;
    } else {
      Serial.println("\nWaterstand 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 % volume
    redLight();

  } else if (waterLevelPercent >= 30 && waterLevelPercent < 70) {     // Gele LED brand tussen 30 en 70 % volume
    yellowLight();

  } else {                                                            // Groene LED brand vanaf +70 % volume
    greenLight();
  }

  if (((waterLevelPercent >= 22) && (flag == true)) or (waterLevel <= 0)) {
    digitalWrite(RELAY_PIN, LOW);  // Stop het pompen
    flashingRedLed();
    Serial.println("Relais GE-DEACTIVEERD, Pomp werkt niet of niet meer! \n");
    pumpFlag = false;
    flag = false;
    teller = teller+1;
    // return;
  } else if ((waterLevelPercent >= 15) && (waterLevelPercent < 22) or (flag == true)) {
    Serial.println("Pomp nog steeds in werking... \n");
    pumpFlag = true;
    flashingRedLed();
  } else if (waterLevelPercent < 15) {  // Controleer steeds de waterhoogte en bedien zo nodig ook het relais (tussen 15 en 22%)
    pumpFlag = true;
    flag = true;
    digitalWrite(RELAY_PIN, HIGH);  // Start het pompen
    Serial.println("Relais GEACTIVEERD, Pomp is in werking \n");
    flashingRedLed();
  }
  if (pumpFlag == true) {
    analogWrite(LCDPOWER_PIN, 255); // LCD Backlight on
    lcd.setCursor(0, 0);
    lcd.print("Inhoud: ");
    lcd.print(waterLevelPercent);
    lcd.print(" ");
    lcd.print(String(char(37)));
    lcd.print("     ");
    
    lcd.setCursor(0, 1);
    lcd.print("POMP IN WERKING!");
    delay(500);
  }
  else {
    lcd.clear();
    analogWrite(LCDPOWER_PIN, 0); // LCD Backlight off
  }
}

void outOfRangeTxt() {
  analogWrite(LCDPOWER_PIN, 255); // LCD Backlight on
  lcd.setCursor(0, 0);
  lcd.print("!METINGSWAARDEN!");
  lcd.setCursor(0, 1);
  lcd.print("!BUITEN BEREIK! ");
  Serial.println("\n*** ATTENTIE: Metingswaarden buiten bereik! ***  *** ATTENTIE: Metingswaarden buiten bereik! ***");
  runningLedLight();
  // return;
}

void lcdAfstand() {
  lcd.print("Afstand: ");
  lcd.print(distance);
  lcd.print(" cm");
}

// Laat de rode LED snel knipperen
void flashingRedLed() {
  flash = ! flash;
  digitalWrite(YELLOW_LED_PIN, LOW);
  digitalWrite(GREEN_LED_PIN, LOW);
  digitalWrite(RED_LED_PIN, flash);
  // delay(50);
}

void runningLedLight() {
  redLight();
  delay(110);
  yellowLight();
  delay(110);
  greenLight();
  delay(110);
  redLight();
  delay(110);
  yellowLight();
  delay(110);
  digitalWrite(YELLOW_LED_PIN, LOW);
}

void redLight() {
  digitalWrite(RED_LED_PIN, HIGH);
  digitalWrite(YELLOW_LED_PIN, LOW);
  digitalWrite(GREEN_LED_PIN, LOW);
}

void yellowLight() {
  digitalWrite(RED_LED_PIN, LOW);
  digitalWrite(YELLOW_LED_PIN, HIGH);
  digitalWrite(GREEN_LED_PIN, LOW);
}

void greenLight() {
  digitalWrite(RED_LED_PIN, LOW);
  digitalWrite(YELLOW_LED_PIN, LOW);
  digitalWrite(GREEN_LED_PIN, HIGH);
}