#include <esp_err.h>
#include <driver/i2c.h>
#include "i2c_utils.h"

esp_err_t i2c_reg_read_byte(i2c_cmd_handle_t chain, uint8_t dev_addr, uint8_t reg_addr, uint8_t *dest)
{
    esp_err_t suc;
#define TRY(x) suc=x;if(suc!=ESP_OK)return suc;
    TRY(i2c_master_start(chain));
    TRY(i2c_master_write_byte(chain, (dev_addr << 1) | I2C_MASTER_WRITE, /*check ack*/true));
    TRY(i2c_master_write_byte(chain, reg_addr, /*check ack*/true));

    TRY(i2c_master_start(chain));
    TRY(i2c_master_write_byte(chain, (dev_addr << 1) | I2C_MASTER_READ, /*check ack*/true));
    TRY(i2c_master_read_byte(chain, dest, I2C_MASTER_NACK));
    TRY(i2c_master_stop(chain));
    return ESP_OK;
#undef TRY
}

esp_err_t i2c_reg_read(i2c_cmd_handle_t chain, uint8_t dev_addr, uint8_t reg_addr, uint8_t *dest, size_t count)
{
    esp_err_t suc;
#define TRY(x) suc=x;if(suc!=ESP_OK)return suc;
    TRY(i2c_master_start(chain));
    TRY(i2c_master_write_byte(chain, (dev_addr << 1) | I2C_MASTER_WRITE, /*check ack*/true));
    TRY(i2c_master_write_byte(chain, reg_addr, /*check ack*/true));

    TRY(i2c_master_start(chain));
    TRY(i2c_master_write_byte(chain, (dev_addr << 1) | I2C_MASTER_READ, /*check ack*/true));
    TRY(i2c_master_read(chain, dest, count, I2C_MASTER_LAST_NACK));
    TRY(i2c_master_stop(chain));
    return ESP_OK;
#undef TRY
}

esp_err_t i2c_reg_write_byte(i2c_cmd_handle_t chain, uint8_t dev_addr, uint8_t reg_addr, uint8_t value)
{
    esp_err_t suc;
#define TRY(x) suc=x;if(suc!=ESP_OK)return suc;
    TRY(i2c_master_start(chain));
    TRY(i2c_master_write_byte(chain, (dev_addr << 1) | I2C_MASTER_WRITE, true));
    TRY(i2c_master_write_byte(chain, reg_addr, true));
    TRY(i2c_master_write_byte(chain, value, true));
    TRY(i2c_master_stop(chain));
    return ESP_OK;
#undef TRY
}

esp_err_t i2c_reg_write(i2c_cmd_handle_t chain, uint8_t dev_addr, uint8_t reg_addr, const uint8_t *values, size_t count)
{
    esp_err_t suc;
#define TRY(x) suc=x;if(suc!=ESP_OK)return suc;
    TRY(i2c_master_start(chain));
    TRY(i2c_master_write_byte(chain, (dev_addr << 1) | I2C_MASTER_WRITE, true));
    TRY(i2c_master_write_byte(chain, reg_addr, true));
    TRY(i2c_master_write(chain, (uint8_t*)values, count, true));
    TRY(i2c_master_stop(chain));
    return ESP_OK;
#undef TRY
}