#ifndef UNIT_TEST
  #include <Arduino.h>
#endif

#include "EspMQTTremake.h"
#include <ESP8266WiFi.h>
// PUB SUB CLIENT LIB AAN PASSEN #define MQTT_SOCKET_TIMEOUT 5

extern "C" {
  #include "user_interface.h"
}

unsigned int CRC16(unsigned int crc, unsigned char *buf, int len)
{
  for (int pos = 0; pos < len; pos++)
  {
    crc ^= (unsigned int)buf[pos];    // XOR byte into least sig. byte of crc

    for (int i = 8; i != 0; i--) {    // Loop over each bit
      if ((crc & 0x0001) != 0) {      // If the LSB is set
        crc >>= 1;                    // Shift right and XOR 0xA001
        crc ^= 0xA001;
      }
      else                            // Else LSB is not set
        crc >>= 1;                    // Just shift right
    }
  }

  return crc;
}


void onConnectionEstablished();


//===Change values from here===
const int sleepSeconds = 10;
const bool outputOnSerial = true;


EspMQTTClient client(
  "XXX",                     // Wifi ssid
  "XXX",                     // Wifi password
  "XXX.XXX.XXX.XXX",         // MQTT broker ip
  1883,                      // MQTT broker port
  "XXX",                     // MQTT username
  "XXX",                     // MQTT password
  "XXX",                     // Client name
  onConnectionEstablished,   // Connection established callback
  false,                     // Enable web updater
  true                       // Enable debug messages
);

//===Change values to here===


const int requestPin =  3;
bool flag_telgram = true;

// Vars to store meter readings
long mEVLT = 0; //Meter reading Electrics - consumption low tariff
long mEVHT = 0; //Meter reading Electrics - consumption high tariff
long mEOLT = 0; //Meter reading Electrics - return low tariff
long mEOHT = 0; //Meter reading Electrics - return high tariff
long mEAV = 0;  //Meter reading Electrics - Actual consumption
long mEAT = 0;  //Meter reading Electrics - Actual return
long mGAS = 0;    //Meter reading Gas
long prevGAS = 0;
long hGAS = 0;

#define MAXLINELENGTH 128 // longest normal line is 47 char (+3 for \r\n\0)
char telegram[MAXLINELENGTH];
unsigned int currentCRC=0;

void setup()
{
  pinMode(requestPin, OUTPUT);
  digitalWrite(requestPin, LOW);
  Serial.begin(115200);
  Serial.println("Booting");
  digitalWrite(requestPin, HIGH);
  Serial.println("request hoog"); 
}

void onConnectionEstablished()
{
//  client.subscribe("stat/p1_meter/mGAS", [] (const String &payload)
//  {
//    Serial.println(payload);
//    prevGAS = atol(payload.c_str());
//  });
}


void Updatemqtt()
{
  int counter=0;
  WiFi.mode(WIFI_STA);
  wifi_station_set_hostname("p1_test");
  client.loop();
  while ( WiFi.status() != WL_CONNECTED  && !client.isConnected()) {
    delay(500);
    counter = counter + 1;
    if (counter >= 10){
         delay(1000);
         //WiFi.disconnect();
         //Serial.print("WiFi disconnected, IP address: "); Serial.println(WiFi.localIP());
         //Serial.printf("Sleep for %d seconds\n\n", sleepSeconds);
         ESP.deepSleep(60e6); 
      }
    Serial.print(".");
  }
  Serial.println("Ready");
  Serial.print("IP address: ");
  Serial.println(WiFi.localIP());
  delay(500);
  client.loop();
  delay(500);
  client.loop();
  delay(500);
  client.loop();
  
  if (outputOnSerial) {
    Serial.println("********* BEGIN LOOP *********");
    Serial.print("Elektra - meterstand verbruik HOOG tarief (Wh): ");
    Serial.println(mEVHT);
    Serial.print("Elektra - meterstand verbruik LAAG tarief (Wh): ");
    Serial.println(mEVLT);
    Serial.print("Elektra - actueel verbruik (W): ");
    Serial.println(mEAV);
    Serial.print("Gas - meterstand (m3): ");
    Serial.println(mGAS);
  }
    hGAS = mGAS - prevGAS;
    client.publish("stat/p1_meter/mEVLT" ,String(mEVLT));
    delay(200);
    client.publish("stat/p1_meter/mEVHT" ,String(mEVHT));
    delay(200);
    client.publish("stat/p1_meter/mEAV"  ,String(mEAV));
    delay(200);
    client.publish("stat/p1_meter/mGAS"  ,String(mGAS),true);
    delay(200);
    if(hGAS != 0 and prevGAS != 0){
        client.publish("stat/p1_meter/mGAS_h"  ,String(hGAS));
        delay(200);    
    }

}

bool isNumber(char* res, int len)
{
  for (int i = 0; i < len; i++) {
    if (((res[i] < '0') || (res[i] > '9'))  && (res[i] != '.' && res[i] != 0)) {
      return false;
    }
  }
  return true;
}

int FindCharInArrayRev(char array[], char c, int len)
{
  for (int i = len - 1; i >= 0; i--) {
    if (array[i] == c) {
      return i;
    }
  }
  return -1;
}

long getValidVal(long valNew, long valOld, long maxDiffer)
{
  //check if the incoming value is valid
      if(valOld > 0 && ((valNew - valOld > maxDiffer) && (valOld - valNew > maxDiffer)))
        return valOld;
      return valNew;
}

long getValue(char* buffer, int maxlen)
{
  int s = FindCharInArrayRev(buffer, '(', maxlen - 2);
  if (s < 8) return 0;
  if (s > 32) s = 32;
  int l = FindCharInArrayRev(buffer, '*', maxlen - 2) - s - 1;
  if (l < 4) return 0;
  if (l > 12) return 0;
  char res[16];
  memset(res, 0, sizeof(res));

  if (strncpy(res, buffer + s + 1, l)) {
    if (isNumber(res, l)) {
      return (1000 * atof(res));
    }
  }
  return 0;
}

bool decodeTelegram(int len)
{
  //need to check for start
  int startChar = FindCharInArrayRev(telegram, '/', len);
  int endChar = FindCharInArrayRev(telegram, '!', len);
  bool validCRCFound = false;
  if(startChar>=0)
  {
    //start found. Reset CRC calculation
    currentCRC=CRC16(0x0000,(unsigned char *) telegram+startChar, len-startChar);
    if(outputOnSerial)
    {
      for(int cnt=startChar; cnt<len-startChar;cnt++)
        Serial.print(telegram[cnt]);
    }

  }
  else if(endChar>=0)
  {
    //add to crc calc
    currentCRC=CRC16(currentCRC,(unsigned char*)telegram+endChar, 1);
    char messageCRC[5];
    strncpy(messageCRC, telegram + endChar + 1, 4);
    messageCRC[4]=0; //thanks to HarmOtten (issue 5)
    if(outputOnSerial)
    {
      for(int cnt=0; cnt<len;cnt++)
        Serial.print(telegram[cnt]);
    }
    validCRCFound = (strtol(messageCRC, NULL, 16) == currentCRC);
    if(validCRCFound)
      Serial.println("\nVALID CRC FOUND!");
    else
      Serial.println("\n===INVALID CRC FOUND!===");
    currentCRC = 0;
  }
  else
  {
    currentCRC=CRC16(currentCRC, (unsigned char*)telegram, len);
    if(outputOnSerial)
    {
      for(int cnt=0; cnt<len;cnt++)
        Serial.print(telegram[cnt]);
    }
  }

  long val =0;
  long val2=0;
  // 1-0:1.8.1(000992.992*kWh)
  // 1-0:1.8.1 = Elektra verbruik laag tarief (DSMR v4.0)
  if (strncmp(telegram, "1-0:1.8.1", strlen("1-0:1.8.1")) == 0)
    mEVLT =  getValue(telegram, len);

  // 1-0:1.8.2(000560.157*kWh)
  // 1-0:1.8.2 = Elektra verbruik hoog tarief (DSMR v4.0)
  if (strncmp(telegram, "1-0:1.8.2", strlen("1-0:1.8.2")) == 0)
    mEVHT = getValue(telegram, len);

  // 1-0:2.8.1(000348.890*kWh)
  // 1-0:2.8.1 = Elektra opbrengst laag tarief (DSMR v4.0)
  if (strncmp(telegram, "1-0:2.8.1", strlen("1-0:2.8.1")) == 0)   
    mEOLT = getValue(telegram, len);

  // 1-0:2.8.2(000859.885*kWh)
  // 1-0:2.8.2 = Elektra opbrengst hoog tarief (DSMR v4.0)
  if (strncmp(telegram, "1-0:2.8.2", strlen("1-0:2.8.2")) == 0)
    mEOHT = getValue(telegram, len);

  // 1-0:1.7.0(00.424*kW) Actueel verbruik
  // 1-0:2.7.0(00.000*kW) Actuele teruglevering
  // 1-0:1.7.x = Electricity consumption actual usage (DSMR v4.0)
  if (strncmp(telegram, "1-0:1.7.0", strlen("1-0:1.7.0")) == 0)
    mEAV = getValue(telegram, len);

  if (strncmp(telegram, "1-0:2.7.0", strlen("1-0:2.7.0")) == 0)
    mEAT = getValue(telegram, len);

  // 0-1:24.2.1(150531200000S)(00811.923*m3)
  // 0-1:24.2.1 = Gas (DSMR v4.0) on Kaifa MA105 meter
  if (strncmp(telegram, "0-1:24.2.1", strlen("0-1:24.2.1")) == 0)
    mGAS = getValue(telegram, len);

  return validCRCFound;
}

void readTelegram() {
 if (Serial.available()) {
    memset(telegram, 0, sizeof(telegram));
    while (Serial.available()) {
      int len = Serial.readBytesUntil('\n', telegram, MAXLINELENGTH);
      telegram[len] = '\n';
      telegram[len+1] = 0;
      yield();
      if(decodeTelegram(len+1))
      {
         Updatemqtt();
         digitalWrite(requestPin, LOW);
         Serial.println("request laag");
         delay(1000);
         client.loop();
         delay(1000);
         //WiFi.disconnect();
         //Serial.print("WiFi disconnected, IP address: "); Serial.println(WiFi.localIP());
         //Serial.printf("Sleep for %d seconds\n\n", sleepSeconds);
         ESP.deepSleep(30e6); 
      }
    }
  }
}

void loop() {
  //digitalWrite(requestPin, HIGH);
  readTelegram();
}