void i2c_init() {
    i2c_config_t conf;
    conf.mode = I2C_MODE_MASTER;
    conf.sda_io_num = I2C_SDA;
    conf.sda_pullup_en = GPIO_PULLUP_DISABLE;
    conf.scl_io_num = I2C_SCL;
    conf.scl_pullup_en = GPIO_PULLUP_DISABLE;
    conf.master.clk_speed = 100000;
    //conf.clk_flags = 0;

    i2c_param_config(I2C_NUM_1, &conf);
    i2c_driver_install(I2C_NUM_1, conf.mode, 0, 0, 0);
}

// write len bytes from *dat to dev
esp_err_t i2c_write(uint8_t dev, uint8_t *dat, size_t len) {
    i2c_cmd_handle_t i2c_h = i2c_cmd_link_create();

    i2c_master_start(i2c_h);
    i2c_master_write_byte(i2c_h, (dev << 1) | I2C_MASTER_WRITE, true);
    i2c_master_write(i2c_h, dat, len, true);
    i2c_master_stop(i2c_h);

    esp_err_t ret = i2c_master_cmd_begin(I2C_NUM_1, i2c_h, (1000 / portTICK_RATE_MS));
    i2c_cmd_link_delete(i2c_h);

    return ret;
}

// read len bytes from dev's reg into *dat
esp_err_t i2c_read(uint8_t dev, uint8_t reg, uint8_t *dat, size_t len) {
    i2c_write(dev, &reg, 1); //set register

    i2c_cmd_handle_t i2c_h = i2c_cmd_link_create();

    i2c_master_start(i2c_h);
    i2c_master_write_byte(i2c_h, (dev << 1) | I2C_MASTER_READ, true);
    i2c_master_read(i2c_h, dat, len, I2C_MASTER_ACK);
    i2c_master_stop(i2c_h);

    esp_err_t ret = i2c_master_cmd_begin(I2C_NUM_1, i2c_h, (1000 / portTICK_RATE_MS));
    i2c_cmd_link_delete(i2c_h);

    return ret;
}