#ifndef LCD_ILI9486p
#define LCD_ILI9486p

#include "../../io/GPIO.h"
#include "../../Debug.h"
#include <initializer_list>

// https://www.displayfuture.com/Display/datasheet/controller/ILI9486L.pdf

// http://www.lcdwiki.com/3.5inch_Arduino_Display-UNO

// https://github.com/ZinggJM/ILI9486_SPI/blob/master/src/ILI9486_SPI.cpp



// PARALLEL VERSION WITH 8 BIT 8080 BUS

#pragma GCC push_options
#pragma GCC optimize ("Os")


template <int PIN_RESET, int PIN_CS, int PIN_RS, int PIN_WR, int PIN_RD> class ILI9486p {

private:
	
	/*
	static constexpr const uint8_t PIN_D0 = 15;
	static constexpr const uint8_t PIN_D1 = 14;
	static constexpr const uint8_t PIN_D2 = 21;
	static constexpr const uint8_t PIN_D3 = 20;
	static constexpr const uint8_t PIN_D4 = 19;
	static constexpr const uint8_t PIN_D5 = 18;
	static constexpr const uint8_t PIN_D6 = 17;
	static constexpr const uint8_t PIN_D7 = 16;

	static constexpr const uint8_t PIN_B0 = CORE_PIN15_BIT;
	static constexpr const uint8_t PIN_B1 = CORE_PIN14_BIT;
	static constexpr const uint8_t PIN_B2 = CORE_PIN21_BIT;
	static constexpr const uint8_t PIN_B3 = CORE_PIN20_BIT;
	static constexpr const uint8_t PIN_B4 = CORE_PIN19_BIT;
	static constexpr const uint8_t PIN_B5 = CORE_PIN18_BIT;
	static constexpr const uint8_t PIN_B6 = CORE_PIN17_BIT;
	static constexpr const uint8_t PIN_B7 = CORE_PIN16_BIT;
	*/
	static constexpr const uint8_t PIN_D0 = 19;
	static constexpr const uint8_t PIN_D1 = 18;
	static constexpr const uint8_t PIN_D2 = 14;
	static constexpr const uint8_t PIN_D3 = 15;
	static constexpr const uint8_t PIN_D4 = 40;
	static constexpr const uint8_t PIN_D5 = 41;
	static constexpr const uint8_t PIN_D6 = 17;
	static constexpr const uint8_t PIN_D7 = 16;
	static constexpr const uint8_t PIN_B0 = CORE_PIN19_BIT;
	static constexpr const uint8_t PIN_B1 = CORE_PIN18_BIT;
	static constexpr const uint8_t PIN_B2 = CORE_PIN14_BIT;
	static constexpr const uint8_t PIN_B3 = CORE_PIN15_BIT;
	static constexpr const uint8_t PIN_B4 = CORE_PIN40_BIT;
	static constexpr const uint8_t PIN_B5 = CORE_PIN41_BIT;
	static constexpr const uint8_t PIN_B6 = CORE_PIN17_BIT;
	static constexpr const uint8_t PIN_B7 = CORE_PIN16_BIT;
	
	int w = 320;
	int h = 240;

	static constexpr const char* MOD = "ILI9486p";

	static constexpr const uint8_t REG_SWRESET = 0x01;	// Software Reset
	static constexpr const uint8_t REG_RDDID = 0x04;	// Read display identification information
	static constexpr const uint8_t REG_RDDST = 0x09;	// Read Display Status
	static constexpr const uint8_t REG_CASET = 0x2A;	// Column Address Set
	static constexpr const uint8_t REG_PASET = 0x2B;	// Page Address Set
	static constexpr const uint8_t REG_RAMWR = 0x2C;	// Memory Write
	static constexpr const uint8_t REG_RAMRD = 0x2E;	// Memory Read
	
	char pbuf[64];
	
public:

	ILI9486p() {
		
		if (PIN_RESET > 0)	{MyGPIO::setOutput(PIN_RESET);}
		if (PIN_CS > 0)		{MyGPIO::setOutput(PIN_CS);}
		MyGPIO::setOutput(PIN_RS);
		MyGPIO::setOutput(PIN_WR);
		MyGPIO::setOutput(PIN_RD);
		
		if (PIN_CS > 0)		{MyGPIO::set(PIN_CS);}
		MyGPIO::set(PIN_RD);
		MyGPIO::set(PIN_WR);
		
		outMode();
		chipSelect();
		
		/*
		// see manual page 666
		uint32_t pus = 0b10;
		uint32_t pue = 0b1;
		uint32_t pke = 0;
		uint32_t ode = 1;
		uint32_t speed = 0b00;
		uint32_t dse = 0b111;
		uint32_t sre = 0b0;
		uint32_t s = (pus<<14)|(pue<<13)|(pke<<12)|(ode<<11)|(sre<<0)|(dse<<3)|(speed<<6);
		
		
		CORE_PIN14_PADCONFIG = s;
		CORE_PIN15_PADCONFIG = s;
		CORE_PIN16_PADCONFIG = s;
		CORE_PIN17_PADCONFIG = s;
		CORE_PIN18_PADCONFIG = s;
		CORE_PIN19_PADCONFIG = s;
		CORE_PIN40_PADCONFIG = s;
		CORE_PIN41_PADCONFIG = s;
		
		CORE_PIN20_PADCONFIG = s;
		CORE_PIN21_PADCONFIG = s;
		CORE_PIN39_PADCONFIG = s;
		CORE_PIN38_PADCONFIG = s;
		*/
		
		IOMUXC_GPR_GPR26 = 0xffffffff;
				
	}
	
	/** perform display software reset and initialization */
	void init() {

		hardwareReset();
		
		// send reset command
		wait();
		sendCommand(REG_SWRESET);
		wait();
		
		//sendCommandAndData(0xb0, {0x00});	// Interface Mode Control
		sendCommandAndData(0x11, {});		// disable sleep
		
		wait();
		
		sendCommandAndData(0x3A, {0x55});	// Interface Pixel Format, 16 bits / pixel
		sendCommandAndData(0x36, {0x28});	// Memory Access Control
		sendCommandAndData(0xC2, {0x44});	// Power Control 3 (For Normal Mode)
		sendCommandAndData(0xC5, {0x00, 0x00, 0x00, 0x00});	// VCOM Control
		sendCommandAndData(0xE0, {0x0F, 0x1F, 0x1C, 0x0C, 0x0F, 0x08, 0x48, 0x98, 0x37, 0x0A, 0x13, 0x04, 0x11, 0x0D, 0x00});	// PGAMCTRL(Positive Gamma Control)
		sendCommandAndData(0xE1, {0x0F, 0x32, 0x2E, 0x0B, 0x0D, 0x05, 0x47, 0x75, 0x37, 0x06, 0x10, 0x03, 0x24, 0x20, 0x00});	// NGAMCTRL (Negative Gamma Correction)
		sendCommandAndData(0xE2, {0x0F, 0x32, 0x2E, 0x0B, 0x0D, 0x05, 0x47, 0x75, 0x37, 0x06, 0x10, 0x03, 0x24, 0x20, 0x00});	// Digital Gamma Control 1
		sendCommandAndData(0x36, {0x28});	// Memory Access Control, BGR
		sendCommandAndData(0x11, {}),		// # Sleep OUT
		sendCommandAndData(0x29, {}),		// Display ON
		
		wait();
		
	}
	
	void sendCommandAndData(uint8_t cmd, std::initializer_list<uint8_t> data) {
		sendCommand(cmd);
		for (uint8_t b : data) {sendData(b);}
	}

/*
	uint32_t getID() {
		outMode();
		sendCommand(REG_RDDID);
		inMode();
		uint32_t res = 0xFFFFFFFF;
		readBytes(reinterpret_cast<uint8_t*>(&res), 4);	// dummy-byte + 3 data bytes
		return res;
	}

	void getStatus() {
		uint8_t buf[5];
		//chipSelect();
		outMode();
		sendCommand(REG_RDDST);
		uint32_t res = 0xFFFFFFFF;
		inMode();
		readBytes(buf, 5);	// dummy-byte + 4 data bytes
		sprintf(pbuf, "Status: %02x %02x %02x %02x \n", buf[1],buf[2],buf[3],buf[4]);
		Serial.print(pbuf);
		//chipDeselect();
	}
*/

	void fill(const uint16_t color) {
		setAddrWindow(0,0,480,320);
		modeDATA();
		for (uint32_t i = 0; i < 480*320; ++i) {
			writeByte(color>>8); writeByte(color>>0);
		}
	}

	void fill(uint16_t x, uint16_t y, uint16_t w, uint16_t h, uint16_t color) {
		setAddrWindow(x,y,w,h);
		modeDATA();
		const uint32_t entries = uint32_t(w) * uint32_t(h);
		for (uint32_t i = 0; i < entries; ++i) {
			writeByte(color>>8); writeByte(color>>0);
		}
	}

	/** draw 5-6-5 encoded input data */
	void draw(uint16_t x, uint16_t y, uint16_t w, uint16_t h, const uint16_t* data) {
		setAddrWindow(x,y,w,h);
		modeDATA();
		const uint32_t entries = uint32_t(w) * uint32_t(h);
		for (uint32_t i = 0; i < entries; ++i) {
			writeByte(data[i]>>8); writeByte(data[i]>>0);
		}
	}
	
	/** draw 5-6-5 encoded input, stretch X by 2 */
	void draw565x2(uint16_t x, uint16_t y, uint16_t w, uint16_t h, const uint16_t* data) {
		setAddrWindow(x,y,w,h);
		modeDATA();
		for (uint32_t i = 0; i < w/2*h; ++i) {
			writeByte(data[i]>>8);	writeByte(data[i]>>0);
			writeByte(data[i]>>8);	writeByte(data[i]>>0);
		}
	}
	
	void beginDraw(uint16_t x, uint16_t y, uint16_t w, uint16_t h) {
		setAddrWindow(x,y,w,h);
		modeDATA();
	}
	void drawData(uint16_t* data, uint32_t len) {
		for (uint32_t i = 0; i < len; ++i) {
			writeByte(data[i]>>8);	writeByte(data[i]>>0);
		}
	}
		
	
	/** draw 5-6-5 encoded input, stretch X and Y by 2 */
	void draw565x2y2(uint16_t x, uint16_t y, uint16_t w, uint16_t h, const uint16_t* data) {
		setAddrWindow(x,y,w,h);
		modeDATA();
		
		uint32_t i = 0;
		for (uint16_t y = 0; y < h/2; ++y) {
			for (uint16_t x = 0; x < w/2; ++x) {
				writeByte(data[i+x]>>8);	writeByte(data[i+x]>>0);
				writeByte(data[i+x]>>8);	writeByte(data[i+x]>>0);
			}
			for (uint16_t x = 0; x < w/2; ++x) {
				writeByte(data[i+x]>>8);	writeByte(data[i+x]>>0);
				writeByte(data[i+x]>>8);	writeByte(data[i+x]>>0);
			}
			i += w/2;
		}
	}
	
	/** draw 3-3-2 encoded input data */
	void draw332(uint16_t x, uint16_t y, uint16_t w, uint16_t h, const uint8_t* data) {
		setAddrWindow(x,y,w,h);
		modeDATA();
		for (uint32_t i = 0; i < w*h; ++i) {
			const uint8_t t = data[i];
			const uint16_t c565 = (t&0b11100000)<<8 | (t&0b000111000)<<6 | (t&0b00000011)<<3;
			writeByte(c565 >> 8);
			writeByte(c565 >> 0);
		}
	}


	void fillRand() {
		setAddrWindow(0,0,w,h);
		modeDATA();
		const uint32_t entries = uint32_t(w) * uint32_t(h);
		for (uint32_t i = 0; i < entries; ++i) {
			const uint16_t rnd = rand();
			writeByte(rnd>>8);	writeByte(rnd>>0);
		}
	}
	

private:

	void wait() {
		//vTaskDelay(250 / portTICK_PERIOD_MS);
		delay(250);
	}

	void setAddrWindow(const uint16_t x1, const uint16_t y1, const uint16_t w, const uint16_t h) {
		
		// end (x,y)
		const uint16_t x2 = x1 + w - 1;
		const uint16_t y2 = y1 + h - 1;

		sendCommand(REG_CASET); 	// Column addr set
		sendData(x1 >> 8);	 		//Set the horizontal starting point to the high octet
		sendData(x1 & 0xff);	 	//Set the horizontal starting point to the low octet
		sendData(x2 >> 8);			//Set the horizontal end to the high octet
		sendData(x2 & 0xff);		//Set the horizontal end to the low octet

		sendCommand(REG_PASET); 	// Row addr set
		sendData(y1 >> 8);
		sendData(y1 & 0xff );
		sendData(y2 >> 8);
		sendData(y2 & 0xff);

		sendCommand(REG_RAMWR); 	// write to RAM
		
		// transmit data now

	}

	


	/** send the given command to the display */
	void sendCommand(const uint8_t cmd) {
		modeCMD();
		writeByte(cmd);
	}

	/** send the given data to the display */
	void sendData(const uint8_t data) {
		modeDATA();
		writeByte(data);
	}

	/** send the given data to the display */
	void writeBytes(const uint8_t* data, const uint32_t len) {
		for (uint32_t i = 0; i < len; ++i) {
			writeByte(data[i]);
		}
	}

//	/** read the given data from the display */
//	void readBytes(uint8_t* data, const uint32_t len) {
//		for (uint32_t i = 0; i < len; ++i) {
//			data[i] = readByte();
//		}
//	}
	
	
	
	/*
	void inMode() {
		MyGPIO::setInput(PIN_D0);
		MyGPIO::setInput(PIN_D1);
		MyGPIO::setInput(PIN_D2);
		MyGPIO::setInput(PIN_D3);
		MyGPIO::setInput(PIN_D4);
		MyGPIO::setInput(PIN_D5);
		MyGPIO::setInput(PIN_D6);
		MyGPIO::setInput(PIN_D7);	
	}
	
	
	uint8_t readByte() {
		
		waitShort();
		modeDATA();
		
		chipSelect();
		
		MyGPIO::clear(PIN_RD);	// commit
		waitShort();
		MyGPIO::set(PIN_RD);
		waitShort();
		
		uint8_t tmp = 
			(MyGPIO::get(PIN_D0) << 0) |
			(MyGPIO::get(PIN_D1) << 1) |
			(MyGPIO::get(PIN_D2) << 2) |
			(MyGPIO::get(PIN_D3) << 3) |
			(MyGPIO::get(PIN_D4) << 4) |
			(MyGPIO::get(PIN_D5) << 5) |
			(MyGPIO::get(PIN_D6) << 6) |
			(MyGPIO::get(PIN_D7) << 7);
		
		chipDeselect();
		
		return tmp;
		
	}
	*/
	
	void outMode() {
		MyGPIO::setOutput(PIN_D0);
		MyGPIO::setOutput(PIN_D1);
		MyGPIO::setOutput(PIN_D2);
		MyGPIO::setOutput(PIN_D3);
		MyGPIO::setOutput(PIN_D4);
		MyGPIO::setOutput(PIN_D5);
		MyGPIO::setOutput(PIN_D6);
		MyGPIO::setOutput(PIN_D7);
	}
	
	//static constexpr uint32_t mask = 0b11111111 << 16;
	static constexpr uint32_t mask = 0b11111111 << 16;

	uint8_t lastSent = 0;
	
	inline void writeByte(uint8_t b) {
		
		// fastest option and does not require reading from the bus??
		GPIO6_DR_TOGGLE = (lastSent ^ b) << 16;
		lastSent = b;
		
		//GPIO6_DR = (GPIO6_DR & ~mask) | (b << 16);
		
		// commit on rising edge
		MyGPIO::clear(PIN_WR);
		MyGPIO::set(PIN_WR);

	}
	

	
	
		/*
		static constexpr uint32_t mask = (1<<PIN_B0) | (1<<PIN_B1) | (1<<PIN_B2) | (1<<PIN_B3) | (1<<PIN_B4) | (1<<PIN_B5) | (1<<PIN_B6) | (1<<PIN_B7);
		const uint32_t toSet = 
			(b>>0 & 1)<<PIN_B0 |
			(b>>1 & 1)<<PIN_B1 |
			(b>>2 & 1)<<PIN_B2 |
			(b>>3 & 1)<<PIN_B3 |
			(b>>4 & 1)<<PIN_B4 |
			(b>>5 & 1)<<PIN_B5 |
			(b>>6 & 1)<<PIN_B6 |
			(b>>7 & 1)<<PIN_B7;
		GPIO6_DR = (GPIO6_DR & ~mask) | toSet;
		*/
		
		
		
		
		//cli();
		
		//GPIO6_DR = (GPIO6_DR & ~mask) | (b << 16);
		//GPIO6_DR = (b << 16);

		//uint32_t tmp = (b << 16);
		
		//GPIO6_DR_CLEAR = mask;
		//GPIO6_DR_SET = (b << 16) & mask;
		//asm("nop");
		//GPIO6_DR_CLEAR = mask & ~(b << 16);
		//GPIO6_DR_SET = (b << 16);
		
				//volatile uint32_t* reg = &GPIO6_DR;
		//*reg = (*reg & ~mask) | (b << 16);
		//static constexpr volatile uint8_t* reg = (uint8_t *)((uint32_t)&GPIO6_DR + 3);
		//*reg = b;
		
		
		/*
		MyGPIO::setOrClear(PIN_D0, b & (1<<0));
		MyGPIO::setOrClear(PIN_D1, b & (1<<1));
		MyGPIO::setOrClear(PIN_D2, b & (1<<2));
		MyGPIO::setOrClear(PIN_D3, b & (1<<3));
		MyGPIO::setOrClear(PIN_D4, b & (1<<4));
		MyGPIO::setOrClear(PIN_D5, b & (1<<5));
		MyGPIO::setOrClear(PIN_D6, b & (1<<6));
		MyGPIO::setOrClear(PIN_D7, b & (1<<7));
		*/
	
	inline void waitShort() {
		//asm("nop");
	}

	/** perform hardware reset (pull down the reset pin for some time) */
	inline void hardwareReset() {
		if (PIN_RESET > 0) {
			MyGPIO::clear(PIN_RESET);
			delay(250);
			MyGPIO::set(PIN_RESET);
			delay(250);
		 }
	 }

	/** select the display (CS=0) */
	inline void chipSelect() {
		if (PIN_CS > 0) {MyGPIO::clear(PIN_CS);}
	}

	/** unselect the display (CS=1) */
	inline void chipDeselect() {
		if (PIN_CS > 0) {MyGPIO::set(PIN_CS);}
	}

	/** switch to command-mode */
	inline void modeCMD() {
		MyGPIO::clear(PIN_RS);
	}

	/** switch to data-mode */
	inline void modeDATA() {
		MyGPIO::set(PIN_RS);
	}


};

#pragma GCC pop_options


#endif