#define DEBUGLOG_DEFAULT_LOG_LEVEL_DEBUG

#include <DebugLog.h>
#include <DebugLogEnable.h>
#include <Arduino.h>
#include <RS485_Driver.h>
#include <TCA9554PWR_Driver.h>

#include "utils.h"

/////////////// GLOBALS ///////////////
uint16_t k(0);
bool v(true);

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

  //////////////// DEVICES ////////////////
  auto i2c = drivers::I2C();
  auto relays = drivers::TCA9554PWR(i2c, TCA9554_ADDRESS);
  auto bus = drivers::MODBUS(9600, SERIAL_8N1);

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

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

    LOG_INFO("\n\n====>Write Status 1");
    bus.writeCoils(relayBoardAddr, 0x00, {true, false, true, false, true, false, true, false});
    bus.readCoils(relayBoardAddr, 0x00, 8, values);
    printBool("====>Status 1", values);

    delay(5000);
    LOG_INFO("\n\n====>Write Status 2");
    bus.writeCoils(relayBoardAddr, 0x00, {false, true, false, true, false, true, false, true});
    bus.readCoils(relayBoardAddr, 0x00, 8, values);
    printBool("====>Status 2", values);
    
    delay(5000);
    LOG_INFO("\n\n====>Read Inputs");
    bus.readInputs(relayBoardAddr, 0x00, 8, values);
    printBool("====>Inputs", values);

    delay(5000);
  }
}