#ifndef HARDI2C_H
#define HARDI2C_H

#include "../Platforms.h"
#include "../Debug.h"

#include "driver/i2c.h"

/**
 * NOTE!
 * when called from core1, HardI2C seems to be very very slow!
 */
template <int PIN_SDA, int PIN_SCL> class HardI2C {

	static constexpr const char* NAME = "hardI2C";

	#define I2C_MASTER_NUM 0			// PORT 0
	#define I2C_MASTER_FREQ_HZ 400000	// 400 kHz
	#define I2C_MASTER_TX_BUF_DISABLE 0                           /*!< I2C master doesn't need buffer */
	#define I2C_MASTER_RX_BUF_DISABLE 0                           /*!< I2C master doesn't need buffer */

public:

	/** ctor */
	HardI2C() {
		initMaster();
		//ESP_ERROR_CHECK(i2c_set_start_timing(I2C_MASTER_NUM, 2, 2));
		//ESP_ERROR_CHECK(i2c_set_stop_timing(I2C_MASTER_NUM, 2, 2));
		//ESP_ERROR_CHECK(i2c_set_timeout(I2C_MASTER_NUM, 100));
	}
	
private:

	/** init I2C in master mode */
	void initMaster(void) {
		
		debugMod2(NAME, "init. SDA: %d, SCL: %d", PIN_SDA, PIN_SCL);
		


		int i2c_master_port = I2C_MASTER_NUM;
		i2c_config_t conf;
		conf.mode = I2C_MODE_MASTER;
		conf.sda_io_num = PIN_SDA;
		conf.sda_pullup_en = GPIO_PULLUP_DISABLE;	//GPIO_PULLUP_ENABLE;
		conf.scl_io_num = PIN_SCL;
		conf.scl_pullup_en = GPIO_PULLUP_DISABLE;	//GPIO_PULLUP_ENABLE;
		conf.master.clk_speed = I2C_MASTER_FREQ_HZ;
		ESP_ERROR_CHECK(i2c_param_config(i2c_master_port, &conf));
		ESP_ERROR_CHECK(i2c_driver_install(i2c_master_port, conf.mode, I2C_MASTER_RX_BUF_DISABLE, I2C_MASTER_TX_BUF_DISABLE, 0));
	
	}


public:


	#define WRITE_BIT I2C_MASTER_WRITE              /*!< I2C master write */
	#define READ_BIT I2C_MASTER_READ				/*!< I2C master read */
	static constexpr uint8_t ACK_CHECK_EN = 0x1;                        /*!< I2C master will check ack from slave*/
	static constexpr uint8_t ACK_CHECK_DIS = 0x0;                       /*!< I2C master will not check ack from slave */
	static constexpr i2c_ack_type_t ACK_VAL = (i2c_ack_type_t)0x0;                             /*!< I2C ack value */
	static constexpr i2c_ack_type_t NACK_VAL = (i2c_ack_type_t)0x1;                            /*!< I2C nack value */

	inline esp_err_t writeSlave(const uint8_t addr, const uint8_t* data, const uint8_t len) {
		
		i2c_cmd_handle_t handle = i2c_cmd_link_create();
			i2c_master_start(handle);
			i2c_master_write_byte(handle, (addr<<1) | WRITE_BIT, ACK_CHECK_EN);
			i2c_master_write(handle, (uint8_t*)data, len, ACK_CHECK_EN);
			i2c_master_stop(handle);
			
			esp_err_t ret = i2c_master_cmd_begin(I2C_MASTER_NUM, handle, 10 / portTICK_RATE_MS);	// 100 ms Timeout
		i2c_cmd_link_delete(handle);
		return ret;
		
	}
	
	inline esp_err_t readSlave(const uint8_t addr, uint8_t* data, const uint8_t len) {
		
		i2c_cmd_handle_t handle = i2c_cmd_link_create();
			i2c_master_start(handle);
			i2c_master_write_byte(handle, (addr<<1) | READ_BIT, ACK_CHECK_EN);
			if (len > 1) {
				i2c_master_read(handle, data, len - 1, ACK_VAL);
			}
			i2c_master_read_byte(handle, data + len - 1, NACK_VAL);
			i2c_master_stop(handle);
			
			esp_err_t ret = i2c_master_cmd_begin(I2C_MASTER_NUM, handle, 10 / portTICK_RATE_MS);	// 100 ms Timeout
		i2c_cmd_link_delete(handle);
		return ret;
		
	}


	bool readReg(const uint8_t addr, const uint8_t reg, const uint8_t len, uint8_t* dst) {
		if (writeSlave(addr, &reg, 1) != 0) {return false;}
		if (readSlave(addr, dst, len) != 0) {return false;}
		return true;		
	}


	bool writeReg(const uint8_t addr, const uint8_t reg, const uint8_t len, const uint8_t* src) {

		debugMod(NAME, "writeReg: TODO!");
		return true;
		
		/*
		bool ok;

		// address the slave in write mode and select the first register to read
		ok = startWrite(addr);
		if (!ok) {debugMod(NAME, "failed start write"); return false;}
		ok = writeByteAndCheck(reg);
		if (!ok) {debugMod1(NAME, "failed to select register %d", addr); return false;}
		ok = writeBytesAndCheck(src, len);
		if (!ok) {debugMod(NAME, "failed to write register contents"); return false;}

		// done
		stop();
		return true;
		*/

	}

	bool writeReg8(const uint8_t addr, const uint8_t reg, const uint8_t val) {
		return writeReg(addr, reg, 1, &val);
	}

	uint8_t readReg8(const uint8_t addr, const uint8_t reg) {
		uint8_t dst = 0xFF;
		readReg(addr, reg, 1, &dst);
		return dst;
	}

};



#endif // HARDI2C_H