#include <LiquidCrystal_I2C.h>
#include "EspMQTTClient.h"
#include <Wire.h>
#include "SSD1306Ascii.h"
#include "SSD1306AsciiWire.h"

#define echoPin 4 // Pin D4 Arduino an Echo HC-SR04
#define trigPin 2 // Pin D2 Arduino an Trig HC-SR04
#define warnPin 6 // Pin D6 fuer zusätzliches Warnsignal
#define ventPin 8 // Pin D8 fuer Einlassventil
#define OLED_I2C_ADRESSE 0x3C // OLED-Display-Adresse auf I2C-Bus festlegen

SSD1306AsciiWire oled;
LiquidCrystal_I2C lcd(0x27,20,4);  // LCD Adresse auf 0x27 setzen, 20 Zeichen, 4 Zeilen

EspMQTTClient client(
  "WifiSSID",
  "WifiPassword",
  "192.168.1.100",  // MQTT Broker IP
  "MQTTUsername",   // MQTT Benutzername, falls noetig
  "MQTTPassword",   // MQTT Passwort, falls noetig
  "ZisterneFuellstand",      // Client name that uniquely identify your device
  1883		    // MQTT Port
);

// Variablen definieren
long dauer; // Variable fuer Reisezeit des Schalls
long distanz; // Variable fuer Distanzmessung
int fuellstand; // Variable fuer Fuellstand
int fuellprozent; // Variale Fuellstand in Prozent
int warnprozentniedrig; // Variable für Stand in Prozent, unter dem gewarnt werden soll
int warnprozenthoch; // Variable für Stand in Prozent, ueber dem gewarnt werden soll
int normprozent; // Variable fuer zu erreichenden Mindestpegel bei automatischer Auffuellung
int zistsenshoehe; // Variable Sensormontagehoehe in mm
int zistmaxfuell; // Variable maximale Wasserstandshoehe
int zistbreite; // Variable Zisternenbreite in mm
int zistlaenge; // Variable Zisternenlaenge in mm
int zistflaeche; // Variable Zisternengrundfläche in mm2
int messfehler; // Messfehler
uint8_t spalte[2]; // Spaltenanzahl OLED
uint8_t zeilenh;   // Hoehe pro Zeile OLED

void setup() {
  warnprozentniedrig = 20; // untere Warnschwelle in Prozent, Unterschreitung gibt Warnung aus
  warnprozenthoch = 100; // obere Warnschwelle in Prozent, Ueberschreitung gibt Warnung aus
  normprozent = warnprozentniedrig + 10; // Wunschpegel bei automatischerauffuellung, hier unterer Warnpegel plus 10%
  zistsenshoehe = 4000; // Hoehe von Boden Zisterne bis Sensor in mm, hier 4000mm
  zistmaxfuell = 3000; // maximale Wasserstandshoehe in mm,
  zistbreite = 5000; // Zisternenbreite in mm, hier 5m
  zistlaenge = 10000; // Zisternenlaenge in mm, hier 10m
  zistflaeche = 0; // Zisternengrundflaeche in mm2, falls nicht rechteckig. Auf 0 lassen bei Rechteck!
  messfehler = 0; // Messfehler zum Start zurücksetzen
  pinMode(trigPin, OUTPUT); // trigPin als Ausgang
  pinMode(echoPin, INPUT); // echoPin als Eingang
  pinMode(warnPin, OUTPUT); // warnPin als Ausgang
  pinMode(ventPin, OUTPUT); // ventPin als Ausgang
  digitalWrite(ventPin, LOW);
  if(zistflaeche==0) zistflaeche = zistbreite * zistlaenge; //falls keine Flaeche eingegeben wurde, Flaeche aus Breite und Laenge errechnen
  Serial.begin(115200); //Einstellung Schnittstelle zu MQTT und WiFi
  client.enableLastWillMessage("ZisterneFuellstand/lastwill", "offline"); //MQTT-Message bei Verbindungsverlust
  lcd.init();                      // LCD initialisieren
  lcd.backlight();
  lcd.setCursor(0,0);
  lcd.print("Fuellstand Zisterne"); // Ueberschrift auf LCD ausgeben
  Wire.begin();  // OLED initialisieren
  Wire.setClock(400000L);
  oled.begin(&Adafruit128x64, OLED_I2C_ADRESSE); // korrekten OLED-Display-Typ waehlen
  oled.setFont(System5x7); // Schriftart fuer OLED bestimmen
  spalte[0] = oled.fieldWidth(3); //Spalte 1 auf OLED bestimmen, um zwei Werte gezielt nebeneinander ausgeben zu koennen
  spalte[1] = oled.fieldWidth(16); //Spalte 2 auf OLED bestimmen
  zeilenh = oled.fontRows(); // OLED Zeichenhoehe der verwendeten Schriftart bestimmen, um Zeilenabstaende einzurichten
  oled.clear(); // OLED loeschen
  oled.clearField(7, 0, 0); // OLED "Cursor" positionieren (eigentlich Loeschfunktion, aber mit zu löschender Zeichenzahl = 0)
  oled.println("Fuellstand Zisterne"); // Uerberschrift auf OLED ausgeben
}

void onConnectionEstablished()  //wird ausgefuehrt bei Connect WiFi UND MQTT
{
  Serial.println("WLAN und MQTT verbunden");
  // Subscribe fuer "Zisterne/zustand" 
  // client.subscribe("Zisterne/zustand", [](const String & payload) {
  //Serial.println(payload);
  //});

  
  client.publish("Zisterne/zustand", "online"); // Publish Nachricht "online" an "Zisterne/zustand"
}

void loop() {
  digitalWrite(trigPin, LOW); // Zustand trigPin zuruecksetzen
  delayMicroseconds(2);
  // trigPin fuer 10 Microsekunden auf HIGH (ACTIVE) setzen, um Puls zu senden
  digitalWrite(trigPin, HIGH); // trinPin auf high
  delayMicroseconds(10); // 10 Nanosekunden warten
  digitalWrite(trigPin, LOW); // trigPin wieder auf low
  dauer = pulseIn(echoPin, HIGH); // echoPin auslesen, gibt Reisezeit in Mikrosekunden zurück
  // Distanz ausrechnen
  distanz = dauer * 0.34 / 2; // Schallgeschwindigkeit geteilt durch 2 (hin und zurueck), Distanz in mm
  // Fuellstand ausrechnen
  fuellstand = (zistsenshoehe - distanz) * (zistflaeche /1000000) /10; // Zisterne ist 5m breit, 10m lang, 1000l pro m3
  fuellprozent = fuellstand * 100 / (zistflaeche /10000000 * zistmaxfuell); // Prozentsatz aus Fuellstand und maxmimalem Fuellstand errechnen
  // Plausibilitaetspruefung und Korrektur
    if (fuellstand < 0){ // Wenn Fuellstand unter 0 dann Fehler
    digitalWrite(warnPin, HIGH); //warnPin ausschalten
    fuellprozent = 0; // Fuellstand nullen
    fuellstand = 0;
    messfehler = 1; // messfehler-Variable auf 1 setzen
    lcd.setCursor(0,2); // Cursor auf dem LCD positionieren, Zeichen 0, Zeile 2   
    lcd.print("                    "); // Inhalte loeschen
    lcd.setCursor(0,3);
    lcd.print("MESSFEHLER!!!       "); // "Messfehler" ausgeben
    oled.clearField(27, zeilenh*7, 0); // OLED Cursor positionieren, unterste Zeile, etwas eingerueckt
    oled.print("MESSFEHLER!!!");  // "Messfehler" ausgeben
    client.publish("Zisterne/warn", "Messfehler"); // Messfehlerwarnung per mQTT senden
    }
  else{
    messfehler = 0;  // messfehler-Variable wieder auf 0 setzen
    lcd.setCursor(0,3);
    lcd.print("     ");
    oled.clearField(27, zeilenh*7, 13); // unterste Zeile auf OLED, "Messfehler" loeschen
  }
  // Ausgabe auf Display und MQTT
  lcd.setCursor(2,1);
  lcd.print("ca. ");
  lcd.print(fuellstand);
  lcd.print("l    ");
  lcd.setCursor(15,1);
  lcd.print(fuellprozent);
  lcd.print("% ");
 // oled.clearField(spalte[0], zeilenh*1, 10); // debug Ausgabe gemessene Distanz
 // oled.print(distanz); // debug Ausgabe gemessene Distanz
  oled.clearField(spalte[0], zeilenh*2, 10);
  oled.print("ca. ");
  oled.print(fuellstand);
  oled.print("l");
  oled.clearField(spalte[1], zeilenh*2, 4);
  oled.print(fuellprozent);
  oled.print("%");
  // client.publish("Zisterne/stand", fuellprozent);  // Publish Nachricht an "Zisterne/stand"
  // lcd.setCursor(19,1); // Messfehler debugging
  // lcd.print(messfehler); //Messfehler debugging
  if (messfehler != 1){ // nur ausfuehren, wenn kein Messfehler
    if (fuellprozent < warnprozentniedrig){ // nur ausfuehren, wenn Fuellstand unter 20%
      digitalWrite(warnPin, HIGH); // warnPin einschalten
      digitalWrite(ventPin, HIGH); // ventPin einschalten
      lcd.setCursor(0,2);
      lcd.print("WARNUNG! Fuellstand");
      lcd.setCursor(5,3);
      lcd.print("unter ");
      lcd.print(warnprozentniedrig);
      lcd.print("%!");
      oled.clearField(8, zeilenh*4, 0);
      oled.print("WARNUNG! Fuellstand");
      oled.clearField(35, zeilenh*5, 0);
      oled.print("unter ");
      oled.print(warnprozentniedrig);
      oled.print("%!");      
      client.publish("Zisterne/niedrig", "Fuellstand zu niedrig");  // zu niedriger Stand per MQTT senden
    }
    else{
      if(fuellprozent >= normprozent) digitalWrite(ventPin, LOW); // ventPin ausschalten, wenn Wasserstand wieder Normpegel erreicht hat     
      if(fuellprozent > warnprozenthoch){
        digitalWrite(warnPin, HIGH); // warnPin einschalten
        lcd.setCursor(0,2);
        lcd.print("WARNUNG! Fuellstand");
        lcd.setCursor(5,3);
        lcd.print("ueber ");
        lcd.print(warnprozenthoch);
        lcd.print("%!");
        oled.clearField(8, zeilenh*4, 0);
        oled.print("WARNUNG! Fuellstand");
        oled.clearField(35, zeilenh*5, 0);
        oled.print("ueber ");
        oled.print(warnprozenthoch);
        oled.print("%!");      
        client.publish("Zisterne/niedrig", "Fuellstand zu hoch");  // zu niedriger Stand per MQTT senden
    }
      else{
        digitalWrite(warnPin, LOW); // warnPin ausschalten
        // Warnanzeige loeschen
        lcd.setCursor(0,2); 
        lcd.print("                   ");
        lcd.setCursor(5,3);
        lcd.print("           ");
        oled.clearField(1, zeilenh*4, 20);
        oled.clearField(1, zeilenh*5, 20);
        client.publish("Zisterne/niedrig", "Fuellstand ok");  // Stand ok per MQTT senden
      }
    }
  }
}