#include "TCA9554PWR_Driver.h"

namespace drivers
{

  TCA9554PWR::TCA9554PWR(I2C &i2c, const uint8_t address) : m_i2c(i2c), m_address(address)
  {
    writeRegister(TCA9554_OUTPUT_REG, Low);              // set all pins to Low state
    writeRegister(TCA9554_CONFIG_REG, TCA9554_OUT_MODE); // set all pins as output (relay mode for this board)
  }

  TCA9554PWR::~TCA9554PWR() {
    writeRegister(TCA9554_OUTPUT_REG, Low);              // set all pins to Low state
    writeRegister(TCA9554_CONFIG_REG, TCA9554_OUT_MODE); // set all pins as output (relay mode for this board)
  }

  const bool TCA9554PWR::writeRegister(const uint8_t reg, const uint8_t val)
  {
    if (m_i2c.write(m_address, reg, {val}))
      return true;
    LOG_ERROR("Unable to write register: reg[%d], val[%d] ", reg, val);
    return false;
  }

  const bool TCA9554PWR::readRegister(const uint8_t reg, uint8_t &val)
  {
    std::vector<uint8_t> data;
    if (m_i2c.read(m_address, reg, 1, data))
    {
      val = data.back();
      return true;
    }
    LOG_ERROR("Unable to read register: reg[%d]");
    return false;
  }

  const bool TCA9554PWR::setOut(const uint8_t ch, const bool state)
  {
    uint8_t currState(0);
    uint8_t newState(0);

    if (ch < OUT_PIN1 || ch > OUT_PIN8)
    {
      LOG_ERROR("Invalid write to output channel: [%d]", ch);
      return false;
    }
    if (!readPort(currState))
      return false;
    if (state)
      newState = (High << ch) | currState;
    else
      newState = (~(High << ch)) & currState;
    return setPort(newState);
  }

  const bool TCA9554PWR::setPort(const uint8_t state)
  {
    if (writeRegister(TCA9554_OUTPUT_REG, state))
      return true;
    LOG_ERROR("Unable to write IO port: state[%02x]", state);
    return false;
  }

  const bool TCA9554PWR::readOut(const uint8_t ch)
  {
    uint8_t currState(0);
    if (ch < OUT_PIN1 || ch > OUT_PIN8)
    {
      LOG_ERROR("Invalid read to output channel: [%d]", ch);
      return false;
    }
    if (!readPort(currState))
      return false;
    return (currState && (High >> ch));
  }

  const bool TCA9554PWR::readPort(uint8_t &state)
  {
    if (readRegister(TCA9554_INPUT_REG, state))
      return true;
    LOG_ERROR("Unable to read IO port: state[%02x]", state);
    return false;
  }

}