#ifndef XPT2046_H
#define XPT2046_H

#include "../../Platforms.h"
#include "../../io/HardSPI.h"
#include "../../io/SoftSPI.h"

// https://www.buydisplay.com/download/ic/XPT2046.pdf

/** SPI-like touch controller */
//template <int PIN_CS, int PIN_MISO, int PIN_MOSI, int PIN_CLK> class XPT2046 {
template <int PIN_CS, typename SPI> class XPT2046 {

	//SoftSPI<PIN_MISO, PIN_MOSI, PIN_CLK, true> spi;
	//HardSPI<PIN_MISO, PIN_MOSI, PIN_CLK> spi;


	static constexpr int PD0 = 0;
	static constexpr int PD1 = 1;
	static constexpr int SER_DFR = 2;
	static constexpr int MODE = 3;
	static constexpr int ADDR = 4;
	static constexpr int START = 7;

	SPI& spi;

public:

	XPT2046(SPI& spi) : spi(spi) {
		//spi.init();
		GPIO::setOutput(PIN_CS);
	}

	struct Result {
		bool valid;
		uint16_t x;
		uint16_t y;
	};

	Result read() {

		GPIO::clear(PIN_CS);
		//vTaskDelay(1 / portTICK_PERIOD_MS);

		Result res;

		if (!isTouched()) {
			res.valid = false;
		} else {
			const uint16_t y = readAvg(1);
			const uint16_t x = readAvg(5);
			res.y = y;
			res.x = x;
			res.valid = (x != 0xFFFF) && (y != 0xFFFF);
		}

		GPIO::set(PIN_CS);

		return res;

	}

private:

	/** read 12-bit result from the given input-address using either differential mode or not */
	uint16_t read(const int inp, const bool differential) {

		//uint8_t buf[2];
		const uint8_t diff = differential ? 0 : 1;
		const uint8_t cmd = (1 << START) | (inp << ADDR) | (0 << MODE) | (diff << SER_DFR) | (1 << PD1) | (1 << PD0);

		//printf("touch cmd: %d\n", cmd);
//		spi.writeByte(cmd);
//		uint8_t v1 = spi.readByte();
//		uint8_t v2 = spi.readByte();
//		spi.read(buf, 2);

		// mainly needed for hardware SPI, where send8+read8+read8 does not work as expected
		// output: cccccccc 00000000 00000000 00000000
		// input:  -------- -rrrrrrr rrrrr--- --------
		const uint32_t tmp = spi.readWriteQuad(cmd << 24);
		const uint8_t v1 = (tmp >> 16) & 0xFF;
		const uint8_t v2 = (tmp >> 8) & 0xFF;
		const uint16_t res = (v1 << 8 | v2) >> 3;

		//printf("res: %d\n", res);

		return res;

	}

	bool isTouched() {

		// read Y-axis in non-differential mode. if result is close to 4095, screen is not touched
		for (int i = 0; i < 4; ++i) {
			if (read(1,false) > 3900) {return false;}
		}

		// read X-axis in non-differential mode. if result is close to 4095, screen is not touched
		for (int i = 0; i < 4; ++i) {
			if (read(5,false) > 3900) {return false;}
		}

		return true;

	}

	uint16_t readAvg(const int inp) {

		// dummy-reads to stabilize the result
		for (int i = 0; i < 4; ++i) {read(inp, true);}

		// perform several reads and estimate std-dev
		static constexpr uint8_t cnt = 12;
		uint32_t sum = 0;
		uint32_t sum2 = 0;
		for (int i = 0; i < cnt; ++i) {
			const uint16_t cur = read(inp, true);
			sum += cur;
			sum2 += cur*cur;
		}
		const uint32_t stdDev = (sum2/cnt) - (sum/cnt)*(sum/cnt);
		//printf("%d\n", stdDev);

		return (stdDev < 4096) ? ((sum/cnt)&0xFFFF) : (0xFFFF);

	}


};

#endif // XPT2046_H