#include "i2c_rw.h"
#include "tools.h"
#ifdef I2C_RAW_DATA
#   include <iostream>
#   include <iomanip>
#endif // I2C_RAW_DATA

void i2c_write8(uint8_t byte)
{
    uint8_t i2c_response;
    i2c_response = bcm2835_i2c_write(reinterpret_cast<char*>(&byte), sizeof(byte));
    if (i2c_response != BCM2835_I2C_REASON_OK)
        throw std::system_error(i2c_response, std::generic_category(), "fail to write byte");
    #ifdef I2C_RAW_DATA
    std::cout << "device <--   " << std::setw(2) << std::setfill('0') << std::hex << (int)byte << std::dec << std::endl;
    #endif // I2C_RAW_DATA
    rpi_delayMicroseconds(50);
}

uint8_t i2c_read8()
{
    uint8_t byte;
    uint8_t i2c_response;
    i2c_response = bcm2835_i2c_read(reinterpret_cast<char*>(&byte), sizeof(byte));
    if (i2c_response != BCM2835_I2C_REASON_OK)
        throw std::system_error(i2c_response, std::generic_category(), "fail to read byte");
    #ifdef I2C_RAW_DATA
    std::cout << "device   --> " << std::setw(2) << std::setfill('0') << std::hex << (int)byte << std::dec << std::endl;
    #endif // I2C_RAW_DATA
    rpi_delayMicroseconds(50);
    return byte;
}

uint16_t i2c_read16()
{
    uint16_t word;
    uint8_t byte1 = i2c_read8();
    uint8_t byte2 = i2c_read8();
    word = byte1 * 256 + byte2;
    return word;
}

int16_t i2c_read16i()
{
    int16_t word;
    int8_t byte1 = i2c_read8();
    int8_t byte2 = i2c_read8();
    word = byte1 * 256 + byte2;
    return word;
}

void i2c_reg_set8(uint8_t reg, uint8_t data)
{
    i2c_write8(reg);
    i2c_write8(data);
}

uint8_t i2c_reg_get8 (uint8_t reg)
{
    i2c_write8(reg);
    return i2c_read8();
}

uint16_t i2c_reg_get16 (uint8_t reg)
{
    i2c_write8(reg);
    return i2c_read16();
}

int16_t i2c_reg_get16i (uint8_t reg)
{
    i2c_write8(reg);
    return i2c_read16i();
}

size_t i2c_read_reg(uint8_t reg, char* buf, int len)
{
    if (len > 32)
        len = 32;
    uint8_t i2c_response;
    i2c_response = bcm2835_i2c_read_register_rs(reinterpret_cast<char*>(&reg), buf, len);
    if (i2c_response != BCM2835_I2C_REASON_OK)
        throw std::system_error(i2c_response, std::generic_category(), "fail to read reg");
    #ifdef I2C_RAW_DATA
    std::cout << "device [" << std::setw(2) << std::setfill('0') << std::hex << (int)reg << "] -> ";
    for(int i=0; i<len; i++)
        std::cout << " " << std::setw(2) << std::setfill('0') << (int)buf[i];
    std::cout << std::dec << std::endl;
    #endif // I2C_RAW_DATA
    return len;
}