#include "esp_err.h"
#include "driver/i2c.h"

#include "MMC56x3_hal.h"
#include "freertos/portmacro.h"
#include "hal/i2c_types.h"

#define MMC56x3_I2C_ADDRESS 0x30

esp_err_t i2c_read_reg(uint8_t address, uint8_t* data, uint16_t count){
  esp_err_t err=ESP_OK;
  i2c_write(&address, 1);
  i2c_read(data, count);
	return err;
}

esp_err_t i2c_write_reg(uint8_t address, uint8_t* data, uint16_t count){
  esp_err_t err=ESP_OK;
  uint8_t buff[count+1];
  buff[0]=address;
  for(int i=0; i<count; i++) buff[i+1]=data[i];
  i2c_write(&buff, count+1);
  return err;
}


esp_err_t i2c_read(uint8_t* data, uint16_t count){
  i2c_cmd_handle_t cmdLnk = i2c_cmd_link_create();
	esp_err_t err = ESP_OK;
	err = i2c_master_start(cmdLnk);
	err = i2c_master_write_byte(cmdLnk, MMC56x3_I2C_ADDRESS << 1 | I2C_MASTER_READ, true);
	if(count>1)
    err = i2c_master_read(cmdLnk, data, count-1, I2C_MASTER_ACK);
	err = i2c_master_read(cmdLnk, data + count - 1, 1, I2C_MASTER_NACK);
	err = i2c_master_stop(cmdLnk);
	i2c_master_cmd_begin(I2C_NUM_0,cmdLnk,1000 / portTICK_PERIOD_MS);
	i2c_cmd_link_delete(cmdLnk);
	return err;
}
esp_err_t i2c_write(const uint8_t* data, uint16_t count){
  i2c_cmd_handle_t cmdLnk = i2c_cmd_link_create();
	esp_err_t err = ESP_OK;
	err = i2c_master_start(cmdLnk);
	err = i2c_master_write_byte(cmdLnk, MMC56x3_I2C_ADDRESS << 1 | I2C_MASTER_WRITE, true);
	err = i2c_master_write(cmdLnk, data, count, true);
	err = i2c_master_stop(cmdLnk);
	i2c_master_cmd_begin(I2C_NUM_0,cmdLnk,1000 / portTICK_PERIOD_MS); /* TODO:non const port */
	i2c_cmd_link_delete(cmdLnk);
	return err;
}

void i2c_hal_wait_ms(uint16_t len){
  vTaskDelay(len / portTICK_PERIOD_MS);
}