#include "wm_type_def.h"
#include "wm_io.h"
#include "wm_gpio_afsel.h"
#include "wm_i2c.h"
#include "i2c.h"

void i2c_init(enum tls_io_name SDAPin, enum tls_io_name SCLPin, uint32_t frequency)
{
    wm_i2c_sda_config(SDAPin);
    wm_i2c_scl_config(SCLPin);
    tls_i2c_init(frequency);
}

bool i2c_write(uint8_t address, uint8_t reg, const uint8_t *const data, size_t length)
{
    if(!data) return false;

    //First we send the address followed by the register from where we want to start writing:
    tls_i2c_write_byte(address << 1, true);
    if(tls_i2c_wait_ack() != WM_SUCCESS) return false;
    tls_i2c_write_byte(reg, false);
    if(tls_i2c_wait_ack() != WM_SUCCESS) return false;

    for(size_t i = 0; i < length; i++)
    {
        tls_i2c_write_byte(data[i], false);
        if(tls_i2c_wait_ack() != WM_SUCCESS)
        {
            tls_i2c_stop();
            return false;
        }
    }

    tls_i2c_stop();

    return true;
}

bool i2c_write_reg(uint8_t address, uint8_t reg, uint8_t data)
{
    //First we send the address followed by the register we want to write to:
    tls_i2c_write_byte(address << 1, true);
    if(tls_i2c_wait_ack() != WM_SUCCESS) return false;
    tls_i2c_write_byte(reg, false);
    if(tls_i2c_wait_ack() != WM_SUCCESS) return false;
    tls_i2c_write_byte(data, false);
    if(tls_i2c_wait_ack() != WM_SUCCESS) return false;
    tls_i2c_stop();

    return true;
}

bool i2c_read(uint8_t address, uint8_t reg, uint8_t * const data, size_t length)
{
    if(!data)return false;

    //First we send the address followed by the register from where we want to start reading:
    tls_i2c_write_byte(address << 1, true);
    if(tls_i2c_wait_ack() != WM_SUCCESS) return false;
    tls_i2c_write_byte(reg, false);
    if(tls_i2c_wait_ack() != WM_SUCCESS) return false;

    tls_i2c_write_byte((address << 1) | 1, true);
    if(tls_i2c_wait_ack() != WM_SUCCESS) return false;

    for(size_t i = 0; i < length - 1; i++ )
    {
        data[i] = tls_i2c_read_byte(true, false);
    }

    data[length - 1] = tls_i2c_read_byte(false, true);

    return true;
}

bool i2c_read_reg(uint8_t address, uint8_t reg, uint8_t * const data)
{
    //First we send the address followed by the register we want to read from:
    tls_i2c_write_byte(address << 1, true);
    if(tls_i2c_wait_ack() != WM_SUCCESS) return false;
    tls_i2c_write_byte(reg, false);
    if(tls_i2c_wait_ack() != WM_SUCCESS) return false;

    tls_i2c_write_byte((address << 1) | 1, true);
    if(tls_i2c_wait_ack() != WM_SUCCESS) return false;

    uint8_t val = tls_i2c_read_byte(false, true);
    if(data) *data = val;
    
    return true;
}