#include "i2c.h"
#include "driver/i2c_types.h"
#include "esp_err.h"
#include <string.h>

i2c_master_bus_handle_t bus;

void i2c_init(int sda, int scl) {
  i2c_master_bus_config_t cfg = {
      .i2c_port = I2C_PORT,
      .sda_io_num = sda,
      .scl_io_num = scl,
      .clk_source = I2C_CLK_SRC_DEFAULT,
      .glitch_ignore_cnt = 7,
      .flags.enable_internal_pullup = true,
  };

  i2c_new_master_bus(&cfg, &bus);
}

esp_err_t i2c_write_reg(i2c_master_dev_handle_t dev, uint8_t reg, uint8_t val) {
  uint8_t data[2] = {reg, val};

  esp_err_t r = i2c_master_transmit(dev, data, 2, -1);
  return r;
}

esp_err_t i2c_write_reg_multi(i2c_master_dev_handle_t dev, uint8_t reg, const uint8_t *data,
                          size_t len) {

  esp_err_t err;
  // reg + payload
  uint8_t buf[len + 1];
  buf[0] = reg;
  memcpy(&buf[1], data, len);

  err = i2c_master_transmit(dev, buf, len + 1, -1);

  return err;
}

esp_err_t i2c_read_reg(i2c_master_dev_handle_t dev, uint8_t reg, uint8_t *val) {
  i2c_master_transmit_receive(dev, &reg, 1, val, 1, -1);

  return ESP_OK;
}

esp_err_t i2c_read_multi(i2c_master_dev_handle_t dev, uint8_t reg, uint8_t *buf, int len) {

  i2c_master_transmit_receive(dev, &reg, 1, buf, len, -1);

  return ESP_OK;
}

esp_err_t i2c_read_reg_multi(i2c_master_dev_handle_t dev, uint8_t reg, uint8_t *buf,
                             size_t len) {

  esp_err_t err;

  err = i2c_master_transmit_receive(dev, &reg, 1, buf, len, -1);

  return err;
}