#define DEBUGLOG_DEFAULT_LOG_LEVEL_DEBUG

#include <DebugLog.h>
#include <DebugLogEnable.h>
#include <Arduino.h>
#include <PubSubClient.h>

#include <PCF85063_Driver.h>
#include <ETH_Driver.h>

#include <digitalIO.h>

#include "utils.h"

void callback(char *topic, uint8_t *payload, unsigned int length)
{
  std::string pl;
  pl.resize(length);
  std::snprintf(pl.data(), length, "%s", payload);
  LOG_INFO("Message: Topic [", topic, "], Payload [", pl.c_str(), "]");
}

void myTask(void *mqtt)
{
  while (true)
  {
    ((PubSubClient *)(mqtt))->loop();
    vTaskDelay(pdMS_TO_TICKS(100));
  }
};

/////////////// GLOBALS ///////////////
void setup()
{
  Serial.begin(9600);
  LOG_ATTACH_SERIAL(Serial);
}

void loop()
{
  const uint8_t tempBoardAddr(0xAA);
  const uint8_t relayBoardAddr(0x01);
  const uint8_t baseRegister(0x00);
  uint16_t k(0);
  uint8_t ethRetries(0);

  //////////////// DEVICES ////////////////
  // Declared here to keep devices local to the main loop otherwise the kernel crashes //
  auto i2c = drivers::I2C();
  auto bus = drivers::MODBUS(9600, SERIAL_8N1);
  auto rtc = drivers::PCF85063(i2c, PCF85063_ADDRESS);
  auto eth = drivers::Ethernet("waveshare-test");
  auto io = digitalIO(i2c, bus, {relayBoardAddr});

  Network.onEvent([&eth](arduino_event_id_t event, arduino_event_info_t info)
                  { eth.onEvent(event, info); });

  while (!eth.isConnected() && ethRetries++ < 5)
  {
    LOG_WARN("Waiting for Ethernet retry", ethRetries);
    delay(1000);
  }

  // Get RTC time at startup
  time_t ntpTime;
  drivers::PCF85063::datetime_t dt;

  // MQTT Test
  NetworkClient tcp;
  PubSubClient mqtt(tcp);

  mqtt.setServer("10.0.2.249", 1883);
  mqtt.setCallback(callback);
  mqtt.connect("esp32-client");
  mqtt.subscribe("test/esp32-in");
  xTaskCreatePinnedToCore(myTask, "mqttLoop", 4096, &mqtt, 2, NULL, 1);

  ////////////////////////////////////////
  ///////// MAIN LOOP INSIDE LOOP ////////
  ////////////////////////////////////////

  while (true)
  {
    LOG_INFO("[", k++, "] Loop");
    std::vector<uint16_t> results;
    std::vector<bool> values;

    eth.getNtpTime(ntpTime);
    dt = drivers::PCF85063::fromEpoch(ntpTime);
    LOG_INFO("Netwrok Datetime", rtc.datetime2str(dt).c_str());
    LOG_INFO("Current Datetime", rtc.getTimeStr().c_str());
    mqtt.publish("test/esp32-out", ("[" + std::to_string(k) + "] -> " + rtc.getTimeStr()).c_str());

    if (bus.readHoldingRegisters(tempBoardAddr, baseRegister, 1, results))
    {
      for (auto i(0); i < results.size(); i++)
      {
        LOG_INFO("[", i, "]Temperature: ", results.at(i) / 10.0f);
      }
      results.clear();
    }

    for (auto j(0); j < io.getOutNum(); j++)
    {
      //io.digitalIOWrite(j, true);
      LOG_INFO("Input", j, io.digitalIORead(j) ? "True" : "False");
      delay(500);
    }


    delay(5000);
  }

  ////////////////////////////////////////
  ///////// MAIN LOOP INSIDE LOOP ////////
  ////////////////////////////////////////
}