#include "veye_i2c.h"

#include <arpa/inet.h>
#include <errno.h>
#include <fcntl.h>
#include <iostream>
#include <linux/i2c-dev.h>
#include <linux/i2c.h>
#include <string.h>
#include <sys/ioctl.h>
#include <unistd.h>

#include "veyeimx287m_types.h"

bool veye::imx287m::i2cRead(int fd, uint8_t i2c_addr, uint16_t reg, uint32_t &value)
{
    int err;
    uint8_t bufin[2] = {reg >> 8, reg & 0xff};
    uint8_t bufout[4] = {0};
    struct i2c_msg msgs[2] = {
        {
            //
            .addr = i2c_addr,
            .flags = 0,
            .len = 2,
            .buf = bufin
            //
        },
        {
            .addr = i2c_addr,
            .flags = I2C_M_RD,
            .len = 4,
            .buf = bufout,
        },
    };

    struct i2c_rdwr_ioctl_data msgset;
    msgset.msgs = msgs;
    msgset.nmsgs = 2;

    err = ioctl(fd, I2C_RDWR, &msgset);
    //printf("Read i2c addr %02X\n", i2c_addr);
    if (err != msgset.nmsgs) {
        std::cerr << "read i2c err " << err << std::endl;
        return false;
    }

    value = ntohl(*(uint32_t *) bufout);
    // fprintf(stderr, "addr %04x : value %08x \n", reg + i, value);

    return true;
}

bool veye::imx287m::test(uint32_t value)
{
    // const std::string i2cDevName{"/dev/i2c-10"};
    const std::string i2cDevName{"/dev/i2c-2"};
    constexpr uint32_t i2cDevAddr{0x3b};
    int fd = open(i2cDevName.c_str(), O_RDWR);

    if (!fd) {
        std::cerr << "cannot open i2c device " << i2cDevName << ", error: " << strerror(errno)
                  << std::endl;
        return false;
    }

    if (ioctl(fd, I2C_SLAVE_FORCE, i2cDevAddr) < 0) {
        std::cerr << "cannot set i2c slave. dev: " << i2cDevName << ", addr: " << i2cDevAddr
                  << ", error: " << strerror(errno) << std::endl;

        return false;
    }

    uint32_t expTime{0};
    // if (!i2cRead(fd, i2cDevAddr, (uint16_t) veye::imx287m::Register::ME_Time, expTime)) {
    //     return false;
    // }

    // std::cout << "exp time is: " << expTime << std::endl;
    std::cout << "set exp time to " << value;

    if (!i2cWrite(fd, (uint16_t) veye::imx287m::Register::ME_Time, value)) {
        return false;
    }

    if (!i2cRead(fd, i2cDevAddr, (uint16_t) veye::imx287m::Register::ME_Time, expTime)) {
        return false;
    }
    std::cout << "\texp time is: " << expTime << std::endl;

    return true;
}

bool veye::imx287m::i2cWrite(int fd, uint16_t reg, const uint32_t value)
{
    uint8_t msg[] = {reg >> 8, reg & 0xff, value >> 24, value >> 16, value >> 8, value >> 0};
    int len = sizeof(msg) / sizeof(msg[0]);

    if (write(fd, msg, len) != len) {
        std::cerr << "cannot write value. reg: " << reg << ", error: " << strerror(errno)
                  << std::endl;
        return false;
    }

    // TODO: find a best way to get up-to-date values
    usleep(100);

    return true;
}

veye::imx287m::i2c::i2c(const std::string &name, const int address)
    : m_name{name}
    , m_address{address}
{}

veye::imx287m::i2c::~i2c()
{
    if (m_fd > 0) {
        close(m_fd);
    }
}

bool veye::imx287m::i2c::open()
{
    m_fd = ::open(m_name.c_str(), O_RDWR);

    if (!m_fd) {
        std::cerr << "cannot open i2c device " << m_name << ", error: " << strerror(errno)
                  << std::endl;
        return false;
    }

    if (ioctl(m_fd, I2C_SLAVE_FORCE, m_address) < 0) {
        std::cerr << "cannot set i2c slave. dev: " << m_name << ", addr: " << m_address
                  << ", error: " << strerror(errno) << std::endl;

        return false;
    }

    return true;
}