#include "WiModbusGateway.h"

#include <SPI.h>
#include <WiFi.h>
#include <WiFiUDP.h>

#include "Metro.h"
// #include "Mudbus.h"
#include "LiquidCrystal.h"
#include "ModbusApp.h"
#include "PString.h"
#include "Streaming.h"
#include "PubSubClient.h"
#include <avr/wdt.h>





LiquidCrystal lcd = LiquidCrystal(A0, A1, A2, A3, A4, A5);

// Mudbus Mb;
// WiFiUDP udpSock;
WiFiClient wifiClient;
//byte server[] = { 192, 168, 87, 100 };
//byte server[] = { 172, 16, 2, 16 };
char server[] = "mqttbroker";
PubSubClient client(server, 1883, callback, wifiClient);
uint8_t disconnectState = 0;
uint32_t disconnectTime = 0;


// char ssid[] = "Kinderland";      // your network SSID (name)
// char pass[] = "test1234";   // your network password
char ssid[] = "MessWLAN";
char pass[] = "UNVmpwbr6heQnMQ7ykXT";

Metro minute = Metro(60000);
Metro second = Metro(1000);
uint32_t uptime;

static bool wifiEnabled = false;


void callback(char* topic, byte* payload, unsigned int length) {
  // handle message arrived
}


void printWifiStatus() {
  // print the SSID of the network you're attached to:
  Serial.print("SSID: ");
  Serial.println(WiFi.SSID());

  // print your WiFi shield's IP address:
  IPAddress ip = WiFi.localIP();
  Serial.print("IP Address: ");
  Serial.println(ip);
  lcd.setCursor(0, 3);
  lcd.print(ip); lcd.print("  ");

  // print the received signal strength:
  long rssi = WiFi.RSSI();
  Serial.print("signal strength (RSSI):");
  Serial.print(rssi);
  Serial.println(" dBm");
  lcd.print(rssi);
}

void setup() {
  Serial.begin(57600);
  while (!Serial) {
    ; // wait for serial port to connect. Needed for Leonardo only
  }

  lcd.begin(20, 4);

  Serial.println("Starting ... ");
  lcd.clear();
  lcd.home();
  lcd.print("Starting ...");


  if (wifiEnabled) {
    // check for the presence of the shield:
    if (WiFi.status() == WL_NO_SHIELD) {
      Serial.println("WiFi shield not present");
      lcd.setCursor(0, 1);
      lcd.print("WiFi shield not present");
      // don't continue:
      while(true);
    }

    int status = WL_IDLE_STATUS;
    while ( status != WL_CONNECTED) {
      Serial.print("Attempting to connect to SSID: ");
      Serial.println(ssid);
      lcd.setCursor(0, 1);
      lcd.print("Trying: ");
      lcd.print(ssid);
      // Connect to WPA/WPA2 network. Change this line if using open or WEP network:
      status = WiFi.begin(ssid, pass);

      // wait 10 seconds for connection:
      delay(10000);
    }
    Serial.println("Connected.");
    lcd.setCursor(0, 2);
    lcd.print("Connected.");

    printWifiStatus();

    //  if (! client.connect("WiFiPowerMeter")) {
    //    Serial.println("MQTT broker not found");
    //    lcd.home();
    //    lcd.clear();
    //    lcd.print("MQTT broker not found");
    //    while (true);
    //  } else {
    //    lcd.print(" *");
    //  }
  }

  delay(1000);
  lcd.home();
  lcd.clear();

  disconnectState = 3;
  disconnectTime = millis();


  // uptime = (uint32_t*) &(Mb.R[121]); // uptime
  uptime = 0;

  // Mb.R[123] = 0xdead; // magic
  // Mb.R[124] = 0xbeef;

  wdt_enable(WDTO_8S);


  // modbusAppBegin(&Mb);
  modbusAppBegin();
}


void updateDisplay() {
  static uint32_t lastUptime = 0;
  static uint8_t lastDisconnectState = 0;


  if ((disconnectState != lastDisconnectState) || (uptime != lastUptime)) {
    lastUptime = uptime;
    lastDisconnectState = disconnectState;
    lcd.home(); lcd.clear();

    uint32_t days = uptime / 86400;
    uint32_t rem = uptime % 86400;
    uint32_t hours = rem / 3600;
    rem = rem % 3600;
    uint32_t minutes = rem / 60;
    uint32_t seconds = rem % 60;
    lcd.setCursor(0,0);
    lcd.print(days); lcd.print("/"); lcd.print(hours); lcd.print(":");
    lcd.print(minutes); lcd.print(":"); lcd.print(seconds); lcd.print(" ");

    if (wifiEnabled) {
      long rssi = WiFi.RSSI();
      lcd.print(rssi); lcd.print(" ");
      uint8_t wifiStatus = WiFi.status();
      lcd.print(wifiStatus);
      lcd.print(disconnectState);
    }

    lcd.setCursor(0, 1);
    lcd.print(getVoltage()); lcd.print("V");
    lcd.setCursor(10, 1);
    lcd.print(getCurrent()); lcd.print("A");
    lcd.setCursor(0, 2);
    lcd.print(getCosPhi()); lcd.print("");
    lcd.setCursor(10, 2);
    lcd.print(getPower()); lcd.print("W");
    lcd.setCursor(0, 3);
    lcd.print(getEnergy()); lcd.print(" "); lcd.print(getNewEnergy()); lcd.print(" ");
    lcd.print("kWh");
  }
}

void loop() {
  wdt_reset();

  updateDisplay();

  modbusAppExec();
  // Mb.Run();

  if (wifiEnabled) {
    if ((disconnectState == 0) && (! client.loop())) {
      disconnectState = 1;
    }

    switch (disconnectState) {
    case 0:
      // Serial.println("discState 0");
      // everything fine
      break;
    case 1:
      Serial.println("discState 1");
      client.disconnect();
      disconnectTime = millis();
      disconnectState = 2;
      break;
    case 2:
      Serial.println("discState 3");
      if (disconnectTime + 2000 < millis()) {
        disconnectState = 3;
      }
      break;
    case 3:
      Serial.println("discState 3");
      if (client.connect("WiFiPowerMeter")) {
        disconnectTime = millis();
        disconnectState = 0;
      } else {
        disconnectState = 1;
      }
      break;
    default:
      disconnectState = 0;
      break;
    }

    if (minute.check() == 1) {
      char strbuf[256];
      memset(strbuf, sizeof(strbuf), 0);
      PString buf = PString(strbuf, sizeof(strbuf));
      buf << "{ \"metadata\": { \"device\": \"WiFiPowerMeter\" }, " <<
          "\"data\": {" <<
          "\"voltage\": " << getVoltage() << ", " <<
          "\"current\": " << getCurrent() << ", " <<
          "\"cosphi\": " << getCosPhi() << ", " <<
          "\"power\": " << getPower() << ", " <<
          "\"energy\": " << getEnergy() << ", " <<
          "\"newEnergy\": " << getNewEnergy() << ", " <<
          "\"uptime\": " << uptime <<
          "}" <<
          "}" << endl;
      if (disconnectState == 0) {
        client.publish("IoT/WiFiPowerMeter/Measurement", strbuf);
      }
    }
  }

  if (second.check() == 1) {
    uptime++;

    Serial.println("tick");

    if (wifiEnabled) {
      String msg = String("{ \"metadata\": { \"device\": \"WiFiPowerMeter\" }, \"data\": { \"uptime\": ") + uptime  + String("}}");
      if (disconnectState == 0) {
        client.publish("IoT/WiFiPowerMeter/Heartbeat", (char*)msg.c_str());
      }
    }
  }

}