#pragma once

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

// https://www.displayfuture.com/Display/datasheet/controller/SH1106.pdf
// Note: the SH1106 is almost the same as SSD1306,
// except for some addressing modes, when writing the whole display at once
// the device address is also the same

template <typename I2C, uint8_t w, uint8_t h> class SH1106 {

private:

	static constexpr uint8_t ADDR7 = 0b0111100;
	
	static constexpr uint8_t CMD_COL_ADDR_LO	= 0x00;
	static constexpr uint8_t CMD_COL_ADDR_HI	= 0x10;
	static constexpr uint8_t CMD_PUMP_VOLTAGE	= 0x30;
	static constexpr uint8_t CMD_START_LINE		= 0x40;
	static constexpr uint8_t CMD_DISPLAY_OFF	= 0xAE | 0;
	static constexpr uint8_t CMD_DISPLAY_ON		= 0xAE | 1;
	static constexpr uint8_t CMD_PAGE_ADDR		= 0xB0;

	
private:
	
	bool inited = false;
	I2C& i2c;

public:

	SH1106(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) {
		
		for (uint8_t y = 0; y < h/8; ++y) {
			
			sendCommand(CMD_COL_ADDR_LO | 0);
			sendCommand(CMD_COL_ADDR_HI | 0);
			sendCommand(CMD_PAGE_ADDR | y);
			
			startDataTransfer();
			for (uint8_t x = 0; x < w; ++x) {
				writeData(*data); ++data;
			}
			stopDataTransfer();
			
		}
	
	}


private:

	inline void startDataTransfer() {
		i2c.startWrite(ADDR7);
		i2c.writeByteAndCheck(0x40);
	}
	
	inline void writeData(uint8_t val) {
		i2c.writeByteAndCheck(val);
	}
	
	inline void stopDataTransfer() {
		i2c.stop();
	}
	
	void init() {

		sendCommand(CMD_DISPLAY_OFF);
		
		// Init
		
		sendCommand(CMD_DISPLAY_ON);
		
	}

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

};