#include "fgc/sensors/I2cBus.h"

#include "fgc/Logger.h"

#include <fcntl.h>
#include <linux/i2c-dev.h>
#include <sys/ioctl.h>
#include <unistd.h>

namespace fgc {

I2cBus::I2cBus(std::string device, uint8_t addr) : device_(std::move(device)), addr_(addr) {}

I2cBus::~I2cBus() { close(); }

bool I2cBus::open() {
    close();
    fd_ = ::open(device_.c_str(), O_RDWR);
    if (fd_ < 0) {
        LOG_ERROR << "I2C: failed to open " << device_;
        return false;
    }
    if (::ioctl(fd_, I2C_SLAVE, addr_) < 0) {
        LOG_ERROR << "I2C: failed to select slave 0x" << std::hex << int(addr_) << std::dec
                  << " on " << device_;
        close();
        return false;
    }
    return true;
}

bool I2cBus::functionality(unsigned long& funcs) const {
    if (fd_ < 0) return false;
    return ::ioctl(fd_, I2C_FUNCS, &funcs) >= 0;
}

void I2cBus::close() {
    if (fd_ >= 0) {
        ::close(fd_);
        fd_ = -1;
    }
}

bool I2cBus::write(const std::vector<uint8_t>& tx) {
    if (fd_ < 0) return false;
    return ::write(fd_, tx.data(), tx.size()) == static_cast<ssize_t>(tx.size());
}

bool I2cBus::writeRead(const std::vector<uint8_t>& tx, std::vector<uint8_t>& rx) {
    if (fd_ < 0) return false;
    if (!tx.empty() && !write(tx)) return false;
    if (rx.empty()) return true;
    return ::read(fd_, rx.data(), rx.size()) == static_cast<ssize_t>(rx.size());
}

}  // namespace fgc