#include "I2C_Driver.h"

namespace drivers
{

  I2C::I2C(): m_initialized(true)
  {
    Wire.begin(I2C_SDA_PIN, I2C_SCL_PIN);
  }

  I2C::~I2C()
  {
    Wire.end();
    m_initialized = false;
  }

  const bool I2C::read(const uint8_t deviceAddr, const uint8_t deviceReg, const uint8_t len, std::vector<uint8_t> &data)
  {
    std::lock_guard<std::mutex> lock(m_mutex);
    Wire.beginTransmission(deviceAddr);
    Wire.write(deviceReg);
    switch (Wire.endTransmission(true))
    {
    case 0:
      break; // no error, break switch
    case 1:
      LOG_ERROR("Data to long to fit in buffer: [%d]", len);
    case 2:
      LOG_ERROR("Received NAK on address transmit");
    case 3:
      LOG_ERROR("Received NAK on data transmit");
    case 4:
      LOG_ERROR("Unknown Error");
      return false;
    }
    const uint8_t nBytes = Wire.requestFrom(deviceAddr, len);
    if (nBytes < len)
    {
      LOG_ERROR("Received data is less than expected: len[%d], nBytes[%d]", len, nBytes);
    }
    data.clear();
    data.resize(nBytes); // resize out buffer to received data len, no check if data len is correct
    for (auto i = 0; i < nBytes; i++)
    {
      data[i] = static_cast<uint8_t>(Wire.read());
    }
    return true;
  }

  const bool I2C::write(const uint8_t deviceAddr, const uint8_t deviceReg, const std::vector<uint8_t> &data)
  {
    std::lock_guard<std::mutex> lock(m_mutex);
    Wire.beginTransmission(deviceAddr);
    Wire.write(deviceReg);
    for (auto d : data)
    {
      Wire.write(d);
    }

    switch (Wire.endTransmission(true))
    {
    case 0:
      break; // no error, break switch
    case 1:
      LOG_ERROR("Data to long to fit in buffer: [%d]", data.size());
    case 2:
      LOG_ERROR("Received NAK on address transmit");
    case 3:
      LOG_ERROR("Received NAK on data transmit");
    case 4:
      LOG_ERROR("Unknown Error");
      return false;
    }
    return true;
  }

} // namespace drivers