/*
  Pneumatische Zisternen-Füllstandsmessung.
  Start der Messung und Übertragung der Ergebnise über WLAN.
  
  Arduino-Bord: "NodeMCU 1.0(ESP-12E Module)"
  Autor Wolfgang Neußer
  Stand: 24.10.2021

  Hardware:
  DOIT ESP12E Motor Shield mit L293D Motortreiber
  Amica NODE MCU ESP8266 12E
  SparkFun Qwiic MicroPressure Sensor
  Druckpumpe und Entlüftungsventil aus Oberarm-Blutdruckmesser
 
  Messablauf:
  1. Abluftventil schließen, Druckpumpe einschalten
  2. Druck kontinuierlich messen
     Wenn Druckanstieg beendet -> Pumpe ausschalten
  3. Beruhigungszeit
  4. Aktueller Druck - atmosphärischen Druck = Messdruck
     Beispiel: 29810 Pa = 3040 mmH2O = 100% Füllstand
  5. Abluftventil öffnen
*/

#include <Wire.h>
#include <SparkFun_MicroPressure.h>
#include <LiquidCrystal_I2C.h>

SparkFun_MicroPressure mpr;
LiquidCrystal_I2C lcdDisplay = LiquidCrystal_I2C(0x27, 16, 2);

// Zuordnung der Ein- Ausgänge
#define VENTIL          5                 // GPIO5 (PWM MotorA)
#define DA              0                 // GPIO0 (Richtung MotorA)
#define PUMPE           4                 // GPIO4 (PWM MotorB)
#define DB              2                 // GPIO2 (Richtung MotorB)
#define SDA             12                // GPIO12 I2C
#define SCL             13                // GPIO13 I2C
#define AUF             LOW               // Ventil öffnen
#define AUS             LOW               // Pumpe ausschalten
#define ZU              HIGH              // Ventil schliessen
#define EIN             HIGH              // Pumpe einschalten

// An eigene Zisterne anpassen (zur Berechnung der Füllmenge)
const int A = 3140;                       // Grundfläche der Zisterne in cm^2 (d * d * 3,14 / 4)
const int maxFuellhoehe = 3040;           // Füllhöhe der Zisterne in mm

int atmDruck = 97400;

/*
* HELPERS FOR INPUT / OUTPUT
*/

void initDisplay() {
   lcdDisplay.display();
   lcdDisplay.noBlink();
   lcdDisplay.noCursor();
   lcdDisplay.clear();
   lcdDisplay.backlight();
}

void disableDisplay() {
   lcdDisplay.noDisplay();
}

void printMsgToLCD(const String& line_one, const String& line_two) {
   lcdDisplay.clear();
   lcdDisplay.setCursor(0, 0);
   lcdDisplay.print(line_one);
   lcdDisplay.setCursor(0, 1);
   lcdDisplay.print(line_two);
}

void printDebugMsg(const String& type, const String& msg) {
   if (Serial.available()) {
      Serial.println(type + msg);
   }
   printMsgToLCD(type, msg);
}

void printMeasurements(const int& pressure) {
   const int wassersaeule = convertPressureToHeight(currentPressure);

   hoehe = String(wassersaeule / 10) + "cm";
   volumen = String((wassersaeule / 10) * A / 100) + "L";
   // Umrechnung Wassersäule in 0 - 100%
   fuellstand = String(map(wassersaeule, 0, maxFuellhoehe, 0, 100)) + "%";

   if (Serial.available()) {
      Serial.println("Füllhöhe: "+ hoehe);
      Serial.println("Volumen: " + volumen);
      Serial.println("Füllstand: " + fuellstand);
   }

   printMsgToLCD("Vol: " + volumen, "Stand: " + fuellstand);
}

void handleSerialInput() {
   static String inputString;

   if (Serial.available()) {
      char inChar = (char)Serial.read();
      if ((inChar == '\r') || (inChar == '\n')) {
         if (inputString == "?") {
            Serial.println("Kommandos: ");
            Serial.println("p1 = Pumpe EIN");
            Serial.println("p0 = Pumpe AUS");
            Serial.println("v1 = Ventil ZU");
            Serial.println("v0 = Ventil AUF");
            Serial.println("start = Messung starten");
            Serial.println();
         }
         else if (inputString == "p1") {
            Serial.println("Pumpe EIN");
            digitalWrite(PUMPE, EIN);
         }
         else if (inputString == "p0") {
            Serial.println("Pumpe AUS");
            digitalWrite(PUMPE, AUS);
         }
         else if (inputString == "v1") {
            Serial.println("Ventil ZU");
            digitalWrite(VENTIL, ZU);
         }
         else if (inputString == "v0") {
            Serial.println("Ventil AUF");
            digitalWrite(VENTIL, AUF);
         }
         else if (inputString == "start") {
            Measurement();
         }
         inputString = "";
      } else inputString += inChar;
   } 
}

/*
* FUNCTIONS FOR MEASUREMENT
*/

int getPressureSensorValue() {
   static int measuredPressure = 0;
   static unsigned long messTakt = 0;
   // Messwert in Pascal auslesen und filtern - alle 10ms bei aufruf
   if (messTakt < millis()) {
      measuredPressure = ((measuredPressure * 50) + int(mpr.readPressure(PA))) / 51;
      messTakt = millis() + 10;
   }
   return measuredPressure;
}

int convertPressureToHeight(const int& pressure) {
   // Umrechnung Pa in mmH2O   
   const int wassersaeule = (pressure - atmDruck) * 10197 / 100000;
   if (wassersaeule < 0) return 0;
   else return wassersaeule;
}

int setAtmosphericPressure() {
   static unsigned long messTakt = 0;
   if (messTakt < millis()) {
      if (!digitalRead(VENTIL) && !digitalRead(PUMPE)) {
         atmDruck = getPressureSensorValue();      
      }
      messTakt = millis() + 1000;
   }
}

/*
* MAIN MEASUREMENT ORCHESTRATOR
*/

void Measurement() {
   initDisplay();
   printDebugMsg("Info: ", "Messung begonnen");

   int oldPressure = getPressureSensorValue();
   digitalWrite(VENTIL, ZU);
   digitalWrite(PUMPE, EIN);
   delay(500);

   while (true) {
      const int currentPressure = getPressureSensorValue();

      if (convertPressureToHeight(currentPressure) > (maxFuellhoehe + 200)) {
         digitalWrite(VENTIL, AUF);
         digitalWrite(PUMPE, AUS);
         printDebugMsg("Fehler: ", "Leitung verstopft");
         while(1);
      }
      
      if (currentPressure > oldPressure + 10) {
         oldPressure = currentPressure;
         delay(100);
      }
      else {
         break;
      }
   }

   const int finalPressure = getPressureSensorValue();

   digitalWrite(VENTIL, AUF);
   digitalWrite(PUMPE, AUS);

   printMeasurements(finalPressure);
   delay(5000);
   disableDisplay();
}

/*
* SETUP AND LOOP FOR ARDUINO RUNTIME
*/

void setup() {
   // Display initialisieren
   lcdDisplay.init();
   initDisplay();

   // Richtung Motor A
   pinMode(DA, OUTPUT);
   digitalWrite(DA, HIGH);
   // PWM Motor A
   pinMode(VENTIL, OUTPUT);
   digitalWrite(VENTIL, AUF);
   // Richtung Motor B
   pinMode(DB, OUTPUT);
   digitalWrite(DB, HIGH);
   // PWM Motor B
   pinMode(PUMPE, OUTPUT);
   digitalWrite(PUMPE, AUS); 

   // enable serial console
   Serial.begin(115200);                     
   delay(10);

   // I2C initialisieren mit 400 kHz
   Wire.begin(SDA, SCL, 400000);            

   // Drucksensor initialisieren
   // Die Default-Adresse des Sensors ist 0x18
   // Für andere Adresse oder I2C-Bus: mpr.begin(ADRESS, Wire1)
   if(!mpr.begin()) {
      printDebugMsg("Fehler: ", "Drucksensor");
      while(1);
   }

   printDebugMsg("Info: ", "System OK");
   delay(1000);
   disableDisplay();
}
                                          
void loop() {
   handleSerialInput();
   setAtmosphericPressure();
   
   // TODO: read button and start measurement

   // ensure valve is open and pump turned off
   digitalWrite(VENTIL, AUF);
   digitalWrite(PUMPE, AUS);
}