#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"





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

// Mudbus Mb;
WiFiUDP udpSock;


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

Metro tick = Metro(10000);
Metro second = Metro(1000);
uint32_t uptime;

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 ...");



  // 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();


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

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

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


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


void updateDisplay() {
  static uint32_t lastUptime = 0;


  if (uptime != lastUptime) {
    lastUptime = uptime;
    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("d "); lcd.print(hours); lcd.print(":");
    lcd.print(minutes); lcd.print(":"); lcd.print(seconds); lcd.print(" ");

    long rssi = WiFi.RSSI();
    lcd.print(rssi); lcd.print(" ");
    uint8_t wifiStatus = WiFi.status();
    lcd.print(wifiStatus);

    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(getFrequency()); lcd.print("Hz");
    lcd.setCursor(10, 2);
    lcd.print(getPower()); lcd.print("W");
    lcd.setCursor(0, 3);
    lcd.print(getEnergy()); lcd.print(" "); lcd.print(getNewEnergy());
    lcd.setCursor(17, 3);
    lcd.print("kWh");
  }
}

void loop() {
  modbusAppExec();
  // Mb.Run();

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

    char strbuf[256];
    memset(strbuf, sizeof(strbuf), 0);
    PString buf = PString(strbuf, sizeof(strbuf));

    udpSock.beginPacket("172.16.2.16", 9999);

    buf << "{ \"metadata\": { \"table\": \"WiFiPowerMeter\" }, \"data\": {";
    udpSock.write(strbuf, buf.length());

    buf.begin();
    buf << "\"voltage\": " << getVoltage() << ", ";
    udpSock.write(strbuf, buf.length());

    buf.begin();
    buf << "\"current\": " << getCurrent() << ", ";
    udpSock.write(strbuf, buf.length());

    buf.begin();
    buf << "\"frequency\": " << getFrequency() << ", ";
    udpSock.write(strbuf, buf.length());

    buf.begin();
    buf << "\"power\": " << getPower() << ", ";
    udpSock.write(strbuf, buf.length());

    buf.begin();
    buf << "\"energy\": " << getEnergy() << ", ";
    udpSock.write(strbuf, buf.length());

    buf.begin();
    buf << "\"uptime\": " << uptime;
    udpSock.write(strbuf, buf.length());

    buf.begin();
    buf << "}";
    udpSock.write(strbuf, buf.length());

    buf.begin();
    buf << "}" << endl;
    udpSock.write(strbuf, buf.length());

    udpSock.endPacket();


  }

  if (tick.check() == 1) {
    Serial.println("tick");
  }

  updateDisplay();
}