#ifndef LCD_SSD1306
#define LCD_SSD1306

#include "../../io/SoftI2C.h"

// https://cdn-shop.adafruit.com/datasheets/SSD1306.pdf

//#define SSD1306_128_64 1
//#define SSD1306_64_48 1		// https://github.com/mcauser/Adafruit_SSD1306/blob/esp8266-64x48/Adafruit_SSD1306.cpp

#if defined SSD1306_128_64
	#define SSD1306_LCDWIDTH                  128
	#define SSD1306_LCDHEIGHT                 64
#elif defined SSD1306_128_32
	#define SSD1306_LCDWIDTH                  128
	#define SSD1306_LCDHEIGHT                 32
#elif defined SSD1306_96_16
	#define SSD1306_LCDWIDTH                  96
	#define SSD1306_LCDHEIGHT                 16
#elif defined SSD1306_64_48
	#define SSD1306_LCDWIDTH                  64
	#define SSD1306_LCDHEIGHT                 48
#else
	#error "SSD1306 display size not defined"
#endif

#define SSD1306_SETCONTRAST 0x81
#define SSD1306_DISPLAYALLON_RESUME 0xA4
#define SSD1306_DISPLAYALLON 0xA5
#define SSD1306_NORMALDISPLAY 0xA6
#define SSD1306_INVERTDISPLAY 0xA7
#define SSD1306_DISPLAYOFF 0xAE
#define SSD1306_DISPLAYON 0xAF

#define SSD1306_SETDISPLAYOFFSET 0xD3
#define SSD1306_SETCOMPINS 0xDA

#define SSD1306_SETVCOMDETECT 0xDB

#define SSD1306_SETDISPLAYCLOCKDIV 0xD5
#define SSD1306_SETPRECHARGE 0xD9

#define SSD1306_SETMULTIPLEX 0xA8

#define SSD1306_SETLOWCOLUMN 0x00
#define SSD1306_SETHIGHCOLUMN 0x10

#define SSD1306_SETSTARTLINE 0x40

#define SSD1306_MEMORYMODE 0x20
#define SSD1306_COLUMNADDR 0x21
#define SSD1306_PAGEADDR   0x22

#define SSD1306_COMSCANINC 0xC0
#define SSD1306_COMSCANDEC 0xC8

#define SSD1306_SEGREMAP 0xA0

#define SSD1306_CHARGEPUMP 0x8D

#define SSD1306_EXTERNALVCC 0x1
#define SSD1306_SWITCHCAPVCC 0x2

// Scrolling #defines
#define SSD1306_ACTIVATE_SCROLL 0x2F
#define SSD1306_DEACTIVATE_SCROLL 0x2E
#define SSD1306_SET_VERTICAL_SCROLL_AREA 0xA3
#define SSD1306_RIGHT_HORIZONTAL_SCROLL 0x26
#define SSD1306_LEFT_HORIZONTAL_SCROLL 0x27
#define SSD1306_VERTICAL_AND_RIGHT_HORIZONTAL_SCROLL 0x29
#define SSD1306_VERTICAL_AND_LEFT_HORIZONTAL_SCROLL 0x2A



template <typename I2C> class SSD1306 {

private:

	static constexpr uint8_t ADDR7 = 0b0111100;
	bool inited = false;

	I2C& i2c;

public:

	SSD1306(I2C& i2c) : i2c(i2c) {

	}

	bool isPresent() {
		return i2c.query(ADDR7);
	}
	
	/** checks if LCD is present and initializes it once / when it was gone */
	bool isPresentAndInit() {
		bool present = isPresent();
		if (!present) {inited = false;}
		if ( present && !inited) {initOnce();}
		return present;
	}

	void initOnce() {
		if (inited) {return;}
		init();
		inited = true;
	}


	void flush(const uint8_t* data) {


		sendCommand(0xC0);
		
		sendCommand2(0x20, 0b11111100);

		// special handling for 64x48 oled shield
        #if SSD1306_LCDWIDTH == 64 && SSD1306_LCDHEIGHT == 48
			sendCommand(SSD1306_COLUMNADDR);
		    sendCommand(32);
			sendCommand(32 + SSD1306_LCDWIDTH - 1);
        #else
			//sendCommand2(SSD1306_COLUMNADDR, 0, SSD1306_LCDWIDTH-1);
			sendCommand(SSD1306_COLUMNADDR);
		    sendCommand(0);							// Column start address (0 = reset)
			sendCommand(SSD1306_LCDWIDTH-1);		// Column end address (127 = reset)
			
        #endif


		//sendCommand2(SSD1306_PAGEADDR, 0, (SSD1306_LCDHEIGHT/8)-1);
		
		sendCommand(SSD1306_PAGEADDR);
		sendCommand(0);							// Page start address (0 = reset)
		sendCommand( (SSD1306_LCDHEIGHT/8)-1 );	// Page end address
		
		
		sendCommand(0x00 | 0x00);
		sendCommand(0x10 | 0x00);
		sendCommand(0xB0 | 0x02);

		
		
		
		
//		#if SSD1306_LCDHEIGHT == 64
//		    sendCommand(7); // Page end address
//		#endif
//        #if SSD1306_LCDHEIGHT == 48
//			sendCommand(3); // Page end address
//        #endif
//		#if SSD1306_LCDHEIGHT == 32
//			sendCommand(3); // Page end address
//		#endif
//		#if SSD1306_LCDHEIGHT == 16
//			sendCommand(1); // Page end address
//		#endif





//		for (uint16_t i=0; i<(SSD1306_LCDWIDTH*SSD1306_LCDHEIGHT/8); i++) {
//			// send a bunch of data in one xmission
//			i2c::startWrite(ADDR7);
//			bool ok = i2c::writeByteAndCheck(0x40);
//			if (!ok) {os_printf("failed line\n");}

//			for (uint8_t x = 0; x < 16; ++x) {
//				i2c::writeByteAndCheck(data[i]);
//				++i;
//			}
//			i--;
//			i2c::stop();
//		}

		i2c.startWrite(ADDR7);
		bool ok = i2c.writeByteAndCheck(0x40);
		if (!ok) {os_printf("failed write data\n");}

		//for (uint16_t i=0; i < (SSD1306_LCDWIDTH*SSD1306_LCDHEIGHT/8); i++) {
		for (uint16_t i=0; i < 16; i++) {
			i2c.writeByteAndCheck(data[i]);
			usleep(1000);
		}

		i2c.stop();

	}


private:

	void init() {

		uint8_t vccstate = 0;

		sendCommand(SSD1306_DISPLAYOFF);                    	// 0xAE
		
		sendCommand(SSD1306_SETDISPLAYCLOCKDIV, 0x80);      	// 0xD5, the suggested ratio 0x80

		sendCommand(SSD1306_SETMULTIPLEX, SSD1306_LCDHEIGHT-1); // 0xA8: multiplexer (31 == 32MUX, 63 == 64MUX)

		sendCommand(SSD1306_SETDISPLAYOFFSET, 0x00);			// 0xD3: vertical shift 0-63, no offset
		
		sendCommand(SSD1306_SETSTARTLINE | 0x0);				// line #0
		
		if (vccstate == SSD1306_EXTERNALVCC) {
			sendCommand(SSD1306_CHARGEPUMP, 0x10);				// 0x8D
		} else {
			sendCommand(SSD1306_CHARGEPUMP, 0x14);				// 0x8D
		}
		
		sendCommand(SSD1306_MEMORYMODE, 0x00);				// 0x20: memory mode(hor/ver/page) -> hor
		
		sendCommand(SSD1306_SEGREMAP | 0x0);

		//sendCommand(SSD1306_COMSCANINC);					// rotate 0
		sendCommand(SSD1306_COMSCANDEC);					// rotate 180?

		// OLD
//	   #if defined SSD1306_128_32
//		sendCommand(SSD1306_SETCOMPINS);                    // 0xDA
//		sendCommand(0x02);
//		sendCommand(SSD1306_SETCONTRAST);                   // 0x81
//		sendCommand(0x8F);

//	  #elif defined SSD1306_128_64
//		sendCommand(SSD1306_SETCOMPINS);                    // 0xDA
//		sendCommand(0x12);
//		sendCommand(SSD1306_SETCONTRAST);                   // 0x81
//		if (vccstate == SSD1306_EXTERNALVCC)
//		  { sendCommand(0x9F); }
//		else
//		  { sendCommand(0xCF); }

//	  #elif defined SSD1306_96_16
//		sendCommand(SSD1306_SETCOMPINS);                    // 0xDA
//		sendCommand(0x2);   //ada x12
//		sendCommand(SSD1306_SETCONTRAST);                   // 0x81
//		if (vccstate == SSD1306_EXTERNALVCC)
//		  { sendCommand(0x10); }
//		else
//		  { sendCommand(0xAF); }

//	  #endif

		// NEW
		sendCommand(SSD1306_SETCOMPINS, 0x12);				// 0xDA

		if (vccstate == SSD1306_EXTERNALVCC) {
			sendCommand(0x9F);
		} else {
			sendCommand(0xCF);
		}


		if (vccstate == SSD1306_EXTERNALVCC) {
			sendCommand(SSD1306_SETPRECHARGE, 0x22);		// 0xd9
		} else {
			sendCommand(SSD1306_SETPRECHARGE, 0xF1);		// 0xd9
		}

		sendCommand(SSD1306_SETVCOMDETECT, 0x40);			// 0xDB

		sendCommand(SSD1306_DISPLAYALLON_RESUME);           // 0xA4
		
		sendCommand(SSD1306_NORMALDISPLAY);                 // 0xA6

		sendCommand(SSD1306_DEACTIVATE_SCROLL);

		sendCommand(SSD1306_DISPLAYON);						//--turn on oled panel

	}

private:

	

	void sendCommand(uint8_t cmd) {
		bool ok;
		ok = i2c.startWrite(ADDR7);
		if (!ok) {os_printf("failed start write\n");}
		ok = i2c.writeByteAndCheck(0x00);	// command
		if (!ok) {os_printf("failed command mode\n");}
		ok = i2c.writeByteAndCheck(cmd);
		if (!ok) {os_printf("failed command\n");}
		i2c.stop();
	}
	
	void sendCommand(uint8_t cmd, uint8_t v1) {
		sendCommand(cmd);
		sendCommand(v1);
	}
	
	void sendCommand(uint8_t cmd, uint8_t v1, uint8_t v2) {
		sendCommand(cmd);
		sendCommand(v1);
		sendCommand(v2);
	}
	
	void sendCommand2(uint8_t cmd, uint8_t v1) {
		i2c.startWrite(ADDR7);
		i2c.writeByteAndCheck(0x00);
		i2c.writeByteAndCheck(cmd);
		//i2c.writeByteAndCheck(0x00);
		i2c.writeByteAndCheck(v1);
		i2c.stop();
	}
	
	void sendCommand2(uint8_t cmd, uint8_t v1, uint8_t v2) {
		i2c.startWrite(ADDR7);
		i2c.writeByteAndCheck(0x00);
		i2c.writeByteAndCheck(cmd);
		//i2c.writeByteAndCheck(0x00);
		i2c.writeByteAndCheck(v1);
		//i2c.writeByteAndCheck(0x00);
		i2c.writeByteAndCheck(v2);
		i2c.stop();
	}

};


#endif