#ifndef SOFTI2C_H
#define SOFTI2C_H

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


/** new implementation */
template <int PIN_SDA, int PIN_SCL, bool fast> class SoftI2C {

	static constexpr const char* NAME = "softI2C";

	inline void sdaDirOut()	{MyGPIO::setOutput(PIN_SDA);}
	inline void sdaDirIn()	{MyGPIO::setInput(PIN_SDA);}

	inline void sclDirOut()	{MyGPIO::setOutput(PIN_SCL);}
	inline void sclDirIn()	{MyGPIO::setInput(PIN_SCL);}

	inline void sdaHi()		{MyGPIO::set(PIN_SDA);}
	inline void sdaLo()		{MyGPIO::clear(PIN_SDA);}
	inline bool sdaRead()	{return MyGPIO::get(PIN_SDA);}

	inline void sclHi()		{MyGPIO::set(PIN_SCL);}
	inline void sclLo()		{MyGPIO::clear(PIN_SCL);}

	inline void waitLong() {
		if (fast) {
			for (uint8_t i = 0; i < 128; ++i) {
				__asm__ __volatile__("nop");
			}
		} else {
			for (uint16_t i = 0; i < 2048; ++i) {
				__asm__ __volatile__("nop");
			}
		}
	}

	inline void waitShort() {
		if (fast) {
			for (uint8_t i = 0; i < 16; ++i) {
				__asm__ __volatile__("nop");
			}
		} else {
			for (uint8_t i = 0; i < 240; ++i) {
				__asm__ __volatile__("nop");
			}
		}
	}

	void init() {
		debugMod2(NAME, "init. SDA: %d, SCL: %d", PIN_SDA, PIN_SCL);
	}

public:

	/** ctor */
	SoftI2C() {
		init();
	}

	/** start communication */
	inline void start() {

		sdaDirOut();
		sclDirOut();

		sdaHi();
		sclHi();

		waitLong();
		sdaLo();

		waitLong();
		sclLo();

		waitLong();

	}

	/** stop communication */
	inline void stop() {

		sdaDirOut();
		sclDirOut();

		sdaLo();
		sclLo();

		waitLong();
		sclHi();

		waitLong();
		sdaHi();

		sdaDirIn();					// free the bus
		sclDirIn();					// TESTING. free the bus

		waitLong();

	}


	/** read the given number of bytes from the slave and generate ACK/NACK as needed */
	void readBytes(uint8_t* dst, uint8_t len) {
		while(len) {
			*dst = _readByte();
			--len;
			++dst;
			if (len) {_writeAck();} else {_writeNAck();}	// done? or want more?
		}
	}



	// write one byte to the bus and check slave's ACK/NACK
	bool IN_FLASH writeByteAndCheck(const uint8_t byte) {
		_writeByte(byte);
		return _readAck();
	}

	// write several bytes to the bus and check slave's ACK/NACK
	bool IN_FLASH writeBytesAndCheck(const uint8_t* src, uint8_t len) {
		while(len) {
			const bool ok = writeByteAndCheck(*src);
			if (!ok) {return false;}
			--len; ++src;
		}
		return true;
	}


	inline bool startWrite(const uint8_t addr7) {
		start();
		_writeByte( (addr7<<1) | 0 );
		const bool ok = _readAck();
		return ok;
	}

	inline bool startRead(const uint8_t addr7) {
		start();
		_writeByte( (addr7<<1) | 1 );
		const bool ok = _readAck();
		return ok;
	}

	inline bool query(const uint8_t addr7) {
		const bool ok = startWrite(addr7);
		stop();
		return ok;
	}
	
	inline void queryAll() {
		debugMod(NAME, "queryAll()")
		for (uint8_t i = 0; i < 128; ++i) {
			if (query(i)) {
				debugMod1(NAME, "found %d", i)
			}
			waitLong();
			if (i%16 == 0) {vTaskDelay(100 / portTICK_PERIOD_MS);}
		}
	}





public:

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

	bool readReg(const uint8_t addr, const uint8_t reg, const uint8_t len, uint8_t* dst) {

		bool ok;

		// select register(s) to read
		ok = startWrite(addr);
		if (!ok) {debugMod(NAME, "failed start read(1)"); return false;}
		ok = writeByteAndCheck(reg);
		if (!ok) {debugMod(NAME, "failed select register"); return false;}
		stop();

		// read register(s)
		ok = startRead(addr);
		if (!ok) {debugMod(NAME, "failed start read(2)"); return false;}
		readBytes(dst, len);
		stop();

		return true;

	}


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

		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);
	}






private:

	/** write one bit to the bus */
	inline void _writeBit(const bool out) {
		//sdaDirOut();						// switch to output mode
		if(out) {sdaHi();} else {sdaLo();}	// apply data
		waitShort();						// wait for data to settle. IMPORTANT!
		sclHi();							// clock pulse
		waitShort();
		sclLo();
		waitShort();
		//sdaDirIn();							// free the bus
	}

	/** read one bit from the bus */
	inline uint8_t _readBit() {
		//sdaDirIn();							// switch to input mode
		sclHi();							// clock pulse and read
		waitShort();
		const uint8_t val = sdaRead();
		sclLo();
		waitShort();
		return val;
	}





	// read 8 bits from the bus, WITHOUT sending ACK/NACK
	uint8_t _readByte() {
		sdaDirIn();							// switch SDA to input mode
		return
		    (_readBit() <<  7) |
		    (_readBit() <<  6) |
		    (_readBit() <<  5) |
		    (_readBit() <<  4) |
		    (_readBit() <<  3) |
		    (_readBit() <<  2) |
		    (_readBit() <<  1) |
		    (_readBit() <<  0);
	}

	inline bool _readAck() {
		sdaDirIn();							// switch to input mode
		const uint8_t res = _readBit();
		//printf("%d\n", res);
		return res == 0;					// ACK when input pulled low
	}


	// write one byte to the bus
	void _writeByte(const uint8_t byte) {
		sdaDirOut();						// switch SDA to output mode
		_writeBit(byte & (1<<7));
		_writeBit(byte & (1<<6));
		_writeBit(byte & (1<<5));
		_writeBit(byte & (1<<4));
		_writeBit(byte & (1<<3));
		_writeBit(byte & (1<<2));
		_writeBit(byte & (1<<1));
		_writeBit(byte & (1<<0));
	}

	inline void _writeAck() {
		sdaDirOut();						// switch SDA to output mode
		_writeBit(0);
	}

	inline void _writeNAck() {
		sdaDirOut();						// switch SDA to output mode
		_writeBit(1);
	}




};




/*
#if ESP8266
	#include "fastGPIO.h"
#elif ESP32
	#include "GPIO.h"
#endif

// https://www.best-microcontroller-projects.com/i2c-tutorial.html

namespace i2c {

	static constexpr const char* NAME = "SoftI2C";

	#if ESP8266

	//	static inline void init() {
	//		//debugMod(NAME, "init()");
	//		GPIO5_OUTPUT_SET;			// clock is always output
	//		//GPIO5_INPUT_PULLUP_SET;		// pullup for i2c
	//		//GPIO4_INPUT_PULLUP_SET;		// pullup for i2c
	//	}

	//	static inline void sclLo() {GPIO5_L;}
	//	static inline void sclHi() {GPIO5_H;}
	//	static inline void sclDirOut() {GPIO5_OUTPUT_SET;}

	//	static inline void sdaDirIn() {GPIO4_INPUT_SET;}// GPIO4_INPUT_PULLUP_SET;}
	//	static inline void sdaDirOut() {GPIO4_OUTPUT_SET;}
	//	static inline uint8_t sdaRead() {return GPIO4_IN;}
	//	static inline void sdaLo() {GPIO4_L;}
	//	static inline void sdaHi() {GPIO4_H;}

		static constexpr const uint8_t PIN_SCL = 5;
		static constexpr const uint8_t PIN_SDA = 4;

	#elif ESP32
		//# error "Not yet supported"


	#endif


	#if (SOFTI2C_SPEED == 1)

		// safe
		static inline void waitShort()	{os_delay_us(1);}
		static inline void waitLong()	{os_delay_us(10);}

	#elif (SOFTI2C_SPEED == 2)

		// fast
		static inline void waitShort()	{asm("nop");asm("nop");}
		static inline void waitLong()	{os_delay_us(1);}

	#elif (SOFTI2C_SPEED == 3)

		// ultra fast
		static inline void waitShort()	{}
		static inline void waitLong()	{asm("nop");asm("nop");asm("nop");asm("nop");}

	#else

		#error "SOFTI2C Speed not set"

	#endif




	static inline void init() {
		GPIO::setOutput(PIN_SCL);		// always output
	}

	static inline void sclLo()		{GPIO::clear(PIN_SCL);}
	static inline void sclHi()		{GPIO::set(PIN_SCL);}
	static inline void sclDirOut()	{GPIO::setOutput(PIN_SCL);}

	static inline void sdaDirIn()	{GPIO::setInput(PIN_SDA);}// GPIO4_INPUT_PULLUP_SET;}
	static inline void sdaDirOut()	{GPIO::setOutput(PIN_SDA);}
	static inline uint8_t sdaRead()	{return GPIO::get(PIN_SDA);}
	static inline void sdaLo()		{GPIO::clear(PIN_SDA);}
	static inline void sdaHi()		{GPIO::set(PIN_SDA);}


	static inline void start() {

		init();
		sdaDirOut();
		sclDirOut();

		sdaHi();
		sclHi();

		waitLong();
		sdaLo();

		waitLong();
		sclLo();

		waitLong();

	}

	static inline void stop() {

		sdaDirOut();
		sclDirOut();

		sdaLo();
		sclLo();

		waitLong();
		sclHi();

		waitLong();
		sdaHi();

		sdaDirIn();							// free the bus

		waitLong();

	}


	// write one bit to the bus
	static inline void writeBit(const bool out) {
		sdaDirOut();						// switch to output mode
		if(out) {sdaHi();} else {sdaLo();}	// apply data
		sclHi();							// clock pulse
		waitShort();
		sclLo();
		waitShort();
		sdaDirIn();							// free the bus
	}

	// read one bit from the bus
	static inline uint8_t readBit() {
		sdaDirIn();							// switch to input mode
		sclHi();							// clock pulse and read
		waitShort();
		const uint8_t val = sdaRead();
		sclLo();
		waitShort();
		return val;
	}

	static inline bool readAck() {
		const uint8_t res = readBit();
		//os_printf("readAck() - Bus: %d\n", res);
		return res == 0;					// ACK when input pulled low
	}

	static inline void writeAck() {
		writeBit(0);
	}

	static inline void writeNAck() {
		writeBit(1);
	}

	// read 8 bits from the bus, WITHOUT sending ACK/NACK
	static uint8_t IN_FLASH readByte() {
		return
			(readBit() <<  7) |
			(readBit() <<  6) |
			(readBit() <<  5) |
			(readBit() <<  4) |
			(readBit() <<  3) |
			(readBit() <<  2) |
			(readBit() <<  1) |
			(readBit() <<  0);
	}

	// read the given number of bytes from the slave and generate ACK/NACK as needed
	static void readBytes(uint8_t* dst, uint8_t len) {
		while(len) {
			*dst = readByte();
			--len;
			++dst;
			if (len) {writeAck();} else {writeNAck();}	// done? or want more?
		}
	}

	// write one byte to the bus
	static void IN_FLASH writeByte(const uint8_t byte) {
			writeBit(byte & BIT( 7));
			writeBit(byte & BIT( 6));
			writeBit(byte & BIT( 5));
			writeBit(byte & BIT( 4));
			writeBit(byte & BIT( 3));
			writeBit(byte & BIT( 2));
			writeBit(byte & BIT( 1));
			writeBit(byte & BIT( 0));
	}

	// write one byte to the bus and check slave's ACK/NACK
	static bool IN_FLASH writeByteAndCheck(const uint8_t byte) {
		writeByte(byte);
		return readAck();
	}

	// write several bytes to the bus and check slave's ACK/NACK
	static bool IN_FLASH writeBytesAndCheck(const uint8_t* src, uint8_t len) {
		while(len) {
			const bool ok = writeByteAndCheck(*src);
			if (!ok) {return false;}
			--len; ++src;
		}
		return true;
	}


	static inline bool startWrite(const uint8_t addr7) {
		start();
		writeByte( (addr7<<1) | 0 );
		const bool ok = readAck();
		return ok;
	}

	static inline bool startRead(const uint8_t addr7) {
		start();
		writeByte( (addr7<<1) | 1 );
		const bool ok = readAck();
		return ok;
	}

	static inline bool query(const uint8_t addr7) {
		bool ok = startWrite(addr7);
		stop();
		return ok;
	}

}
	*/

#endif // SOFTSPI_H