#include <Wire.h>
#include "MS5837.h"
#include <PubSubClient.h>
#include <ESP8266WiFi.h>
#include <WiFiClient.h>
#include <WiFiUdp.h>
#include "cactus_io_BME280_I2C.h"

WiFiClient wifiClient;                    // WiFi für MQTT
PubSubClient mqttclient(wifiClient);      // MQTT client

const char* ssid1 = "xxxxxxxx";
const char* password1 = "xxxxxxxx";
const char* ssid2 = "xxxxxxxx";
const char* password2 = "xxxxxxxx";
int messintervall = 50000;
static float pressure;

BME280_I2C bme(0x76);
static boolean bme_active = false;
    static float temp = bme.getTemperature_C();
    static float humidity = bme.getHumidity();

MS5837 tiefensensor;
static boolean tiefensensor_active = false;
    static float w_temp;
    static float w_depth;
    static float w_realdepth;
    static float w_altitude;
    static float w_pressure;

// ----------- MQTT SERVER -------------------------

    #define BASE "WasserTeststationFdb"               // <-----!!Entsprechend für jeden Sensor anpassen!!
    #define mqtt_client_id  BASE

    const char* mqtt_server = "xxxxxxxx";     // Friedberg
    const char* userName = "xxxxxxxx";
    const char* passWord = "xxxxxxxx";
    const char* thisDevice = BASE;

void setup()
{
  Serial.begin(115200);
  Serial.println("--------------------------------------------------------------------------------------------------------------------------------------");
  Serial.println("----------------------------------- Version 0.9 ----------------------------------");

  // ------------ MQTT - Server --------------
  mqttclient.setServer(mqtt_server, 1887);
  //mqttclient.setCallback(callback);
  mqttclient.setClient(wifiClient);
  setup_wifi();

    Wire.begin(4,5);    //(SDA = D2, SCL D1)
    delay(100);
    if (!tiefensensor.init()) {
      Serial.println("Es konnte kein Tiefensensor gefunden werden!");
      Serial.println("Bitte überprüfen Sie die Verkabelung!");
      mqttclient.publish(BASE "/Tiefensensoraktiv", "false");
    } else {
      tiefensensor_active = true;
      mqttclient.publish(BASE "/Tiefensensoraktiv", "true");
      tiefensensor.setModel(MS5837::MS5837_30BA);
      tiefensensor.setFluidDensity(997); // kg/m^3 (997 freshwater, 1029 for seawater)
    }

    delay(250);
    Serial.println(F("Sensor Bus 0"));

    Wire.begin(2,0);    //(SDA = D4, SCL D3)
    delay(100);
    //Setup für den BME280 Sensor
    if (!bme.begin()) {
      Serial.println("Es konnte kein BME280 Sensor gefunden werden!");
      Serial.println("Bitte überprüfen Sie die Verkabelung!");
      mqttclient.publish(BASE "/BMEaktiv", "false");
    } else {
      bme_active = true;
      mqttclient.publish(BASE "/BMEaktiv", "true");
      bme.setTempCal(-1);// Temp was reading high so subtract 1 degree
    }

    delay(250);
    Serial.println(F("Sensor Bus 1"));


  Serial.println("----------------------------------- L O O P ----------------------------------");

}

// ---------------------------------------------------------------------------------------
// -----------------------------------LOOP -----------------------------------------------
// ---------------------------------------------------------------------------------------

void loop() {
  if (!mqttclient.connected()) {
    reconnect();
  }

      // ------------- BME Sensor auslesen ------------------
  Wire.begin(2,0);    //(SDA = D4, SCL D3)
  delay(100);
  if (!bme_active){
    if (!bme.begin()) {
      Serial.println("Es konnte kein BME280 Sensor gefunden werden!");
      Serial.println("Bitte überprüfen Sie die Verkabelung!");
      mqttclient.publish(BASE "/BMEaktiv", "false");
    } else {
      bme_active = true;
      mqttclient.publish(BASE "/BMEaktiv", "true");
      bme.setTempCal(-1);// Temp was reading high so subtract 1 degree
    }
  } else {
    bme.readSensor();
    temp = bme.getTemperature_C();
    humidity = bme.getHumidity();
    pressure = bme.getPressure_MB();
    printBME2Serial(temp, humidity, pressure);
    //publishMQTTbme(humidity, temp, pressure);
    publishMQTTfloat(BASE "/Luftfeuchte", humidity);
    publishMQTTfloat(BASE "/Temperatur", temp);
    publishMQTTfloat(BASE "/Luftdruck", pressure);
    delay(250);
  }


      // ------------- Tiefensensor auslesen ----------------
  Wire.begin(4,5);    //(SDA = D2, SCL D1)
  delay(100);
  if (!tiefensensor_active){
    if (!tiefensensor.init()) {
      Serial.println("Es konnte kein Tiefensensor gefunden werden!");
      Serial.println("Bitte überprüfen Sie die Verkabelung!");
      mqttclient.publish(BASE "/Tiefensensoraktiv", "false");
    } else {
      tiefensensor_active = true;
      mqttclient.publish(BASE "/Tiefensensoraktiv", "true");
      tiefensensor.setModel(MS5837::MS5837_30BA);
      tiefensensor.setFluidDensity(997); // kg/m^3 (997 freshwater, 1029 for seawater)
    }
  } else  {
    tiefensensor.read();
    delay(500);
    w_temp = tiefensensor.temperature();
    w_depth = 100*tiefensensor.depth();
    w_realdepth = tiefensensor.pressure()-pressure;
    w_altitude = tiefensensor.altitude();
    w_pressure = tiefensensor.pressure();
    printMS5837Serial(w_temp, w_depth, w_realdepth);
    //publishMQTTdepth(w_temp, w_depth, w_realdepth, w_altitude, w_pressure);
    publishMQTTfloat(BASE "/Wassertemperatur", w_temp);
    publishMQTTfloat(BASE "/Wassertiefe", w_depth);
    publishMQTTfloat(BASE "/bereinigteTiefe", w_realdepth);
    publishMQTTfloat(BASE "/Wasserhoehe", w_altitude);
    publishMQTTfloat(BASE "/Wasserdruck", w_pressure);

    publishMQTTfloat(BASE "/Memory", ESP.getFreeHeap());
    delay(250);
  }


  mqttclient.loop();
}

// ---------------------------------------------------------------------------------------
// ----------------------- Setup Routines ------------------------------------------------
// ---------------------------------------------------------------------------------------
void setup_wifi(){                 // ------- Connect to WiFi -----------
  Serial.println();
  Serial.println("Booting Sketch...");
  WiFi.mode(WIFI_STA);
  WiFi.begin(ssid1, password1);

  while(WiFi.waitForConnectResult() != WL_CONNECTED){
    mqttclient.disconnect();
    WiFi.begin(ssid1, password1);
    Serial.print("WiFi(1) "); Serial.print(ssid1); Serial.println(" failed, retrying.");
    delay(80);
    WiFi.begin(ssid2, password2);
    Serial.print("WiFi(2) "); Serial.print(ssid2); Serial.println(" failed, retrying.");
    delay(80);
  }
  Serial.println("");
  Serial.println("WiFi connected");
  Serial.println("IP address: ");
  Serial.println(WiFi.localIP());
  delay(200);
  if (!mqttclient.connected()) {
    reconnect();
  }
}
void reconnect(){
  // Loop until we're reconnected
  while (!mqttclient.connected())
  {
    Serial.print("Attempting MQTT connection...");
    delay(100);
    if (mqttclient.connect(thisDevice,userName,passWord))
    {
      Serial.println("connected");
      static String ipaddress = WiFi.localIP().toString();            // char String Array für IP Adresse
      char ipchar[ipaddress.length()+1];                              // IP Adresse in String umwandeln
      ipaddress.toCharArray(ipchar,ipaddress.length()+1);             // noch iwas damit machen
      mqttclient.publish(BASE "/IP", ipchar);                         // IP Adresse als MQTT senden
    } else {
      Serial.print("failed, rc=");
      Serial.print(mqttclient.state());
      Serial.println(" try again in 5 seconds");
      // Wait 6 seconds before retrying
      delay(500);
    }
  }
}

// --------------------------------------------------------------------------
// --------------- Print to MQTT --------------------------------------------
// --------------------------------------------------------------------------

int publishMQTTfloat(const char* topic, float value){
  char value_as_string[10];
  sprintf(value_as_string, "%f", value);
  //dtostrf( value, 7, 2, value_as_string );
  return mqttclient.publish(topic, value_as_string);
}


// --------------------------------------------------------------------------
// --------------- Print to Serial ------------------------------------------
// --------------------------------------------------------------------------
void printBME2Serial(float temp, float humidity, float pressure){
  Serial.print(" \t"); Serial.print("Temperature");
  Serial.print(" \t");Serial.print(" \t"); Serial.print("Luftfeuchtigkeit: ");
  Serial.print(" \t");Serial.print(" \t"); Serial.println("Luftdruck: ");
  Serial.print(" \t");
  Serial.print(temp);Serial.print(" C");Serial.print(" \t");Serial.print(" \t");Serial.print(" \t");
  Serial.print(humidity);Serial.print(" %");Serial.print(" \t");Serial.print(" \t");
  Serial.print(pressure);Serial.println(" mb");
}
void printMS5837Serial(float watertemp, float waterdepth, float waterrealdepth){
  Serial.print(" \t"); Serial.print("Temperature");
  Serial.print(" \t");Serial.print(" \t"); Serial.print("Tiefe: ");
  Serial.print(" \t");Serial.print(" \t"); Serial.println("Wasserdruck: ");
  Serial.print(" \t");
  Serial.print(watertemp); Serial.print(" deg C"); Serial.print(" \t");Serial.print(" \t");
  Serial.print(waterdepth); Serial.print(" cm"); Serial.print(" \t");Serial.print(" \t");
  Serial.print(waterrealdepth); Serial.println(" m");
}