// Do not remove the include below
// #include <HardwareSerial.h>

#include "NetMeterBusMaster2.h"

#include "cmd.h"
#include "test.h"
#include "uptime.h"
// #include "meterBusServer.h"
#include "meterBusMaster.h"
#include "overCurrentProt.h"
#include <Ethernet.h>
#include "MqttClient.h"
#include <Streaming.h>
#include <Metro.h>
#include "config.h"


const uint8_t POWER_LED = 4;
const uint8_t ETHERNET_RESET = 14;


static uint8_t mac[] = { 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xEF };

static CmdServer cmdServer(2000);
static TestCmd testCmd;
static Uptime uptime;

static MeterBusMaster meterBusMaster;
static MqttClient mqttClient(&meterBusMaster);
// static MeterBusServer meterBusServer(2001, &meterBusMaster);
static OverCurrentProt overCurrentProt;

Metro wdogTick = Metro(1000);
Metro dhcpTick = Metro(60 * 1000);

extern "C" {
  void startup_early_hook( ) __attribute__ ((weak));
  void startup_early_hook() {
    // enable watchdog
    WDOG_UNLOCK = WDOG_UNLOCK_SEQ1;
    WDOG_UNLOCK = WDOG_UNLOCK_SEQ2;

    // one minute
    WDOG_TOVALL = 0xea60;
    WDOG_TOVALH = 0;
    WDOG_PRESC =  0;

    WDOG_STCTRLH = WDOG_STCTRLH_WDOGEN | WDOG_STCTRLH_ALLOWUPDATE | WDOG_STCTRLH_STOPEN | WDOG_STCTRLH_WAITEN;
  }
} // extern "C"

void setup() {
  pinMode(POWER_LED, OUTPUT);
  digitalWrite(POWER_LED, HIGH);

  Serial.begin(115200);
  delay(100);
  Serial.println("Starting up ...");

  digitalWrite(POWER_LED, LOW);

  configInit();

  pinMode(ETHERNET_RESET, OUTPUT);
  digitalWrite(ETHERNET_RESET, LOW);
  delay(100);
  digitalWrite(ETHERNET_RESET, HIGH);
  delay(100);
  Ethernet.begin(mac);
  Serial.println("Ethernet started");

  cmdServer.begin();
  testCmd.registerYourself(&cmdServer);
  uptime.begin(&cmdServer);

  overCurrentProt.begin(&cmdServer);
  meterBusMaster.begin(&cmdServer);
  // meterBusServer.begin();
  mqttClient.begin(&cmdServer);

  digitalWrite(POWER_LED, HIGH);

}

void loop() {
  static uint16_t maxTmroutl = 0;

  // watchdog refresh
  cli();
  WDOG_REFRESH = 0xa602;
  WDOG_REFRESH = 0xb480;
  sei();


  if (wdogTick.check() == 1) {
    uint16_t h = WDOG_TMROUTH;
    uint16_t l = WDOG_TMROUTL;
    maxTmroutl = (maxTmroutl > l) ? maxTmroutl : l;
    uint16_t c = WDOG_RSTCNT;

    Serial << "WDog, h: " << _HEX(h) << ", l: " << _HEX(l) << ", c: " << _HEX(c) << ", m: " << _HEX(maxTmroutl) << endl;
  }

  if (dhcpTick.check() == 1) {
    byte r = Ethernet.maintain();
    Serial << "Ethernet.maintain: " << r << endl;
  }

  //Serial << "*** 1" << endl;
  cmdServer.exec();
  //Serial << "*** 2" << endl;
  uptime.exec();
  //Serial << "*** 3" << endl;
  overCurrentProt.exec();
  //Serial << "*** 4" << endl;
  meterBusMaster.exec();
  //Serial << "*** 5" << endl;
  mqttClient.exec();
  //Serial << "*** 6" << endl;
}