#ifndef MAX30102_H
#define MAX30102_H

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

// https://www.roboter-bausatz.de/media/pdf/bf/52/37/RBS12853_MAX30102-DS-917698.pdf
// https://github.com/sparkfun/SparkFun_MAX3010x_Sensor_Library/blob/master/src/MAX30105.cpp
// https://github.com/vrano714/max30102-tutorial-raspberrypi/blob/master/max30102.py

/** SPI-like touch controller */
template <typename I2C> class Max30102 {

public:

	class Listener {
	public:
		virtual void onIR(const uint32_t ir) = 0;
		virtual void onRed(const uint32_t red) = 0;
	};

private:

	I2C& i2c;

	static constexpr const char* NAME		= "Max30102";
	static constexpr uint8_t ADDR			= 0b1010111;

	// Status Registers
	static const uint8_t MAX30105_INTSTAT1 =		0x00;
	static const uint8_t MAX30105_INTSTAT2 =		0x01;
	static const uint8_t MAX30105_INTENABLE1 =		0x02;
	static const uint8_t MAX30105_INTENABLE2 =		0x03;

	// FIFO Registers
	static const uint8_t MAX30105_FIFOWRITEPTR = 	0x04;
	static const uint8_t MAX30105_FIFOOVERFLOW = 	0x05;
	static const uint8_t MAX30105_FIFOREADPTR = 	0x06;
	static const uint8_t MAX30105_FIFODATA =		0x07;

	// Configuration Registers
	static const uint8_t MAX30105_FIFOCONFIG = 		0x08;
	static const uint8_t MAX30105_MODECONFIG = 		0x09;
	static const uint8_t MAX30105_PARTICLECONFIG = 	0x0A;    // Note, sometimes listed as "SPO2" config in datasheet (pg. 11)
	static const uint8_t MAX30105_LED1_PULSEAMP = 	0x0C;
	static const uint8_t MAX30105_LED2_PULSEAMP = 	0x0D;
	//static const uint8_t MAX30105_LED3_PULSEAMP = 	0x0E;	// not available for MAX30102
	static const uint8_t MAX30105_LED_PROX_AMP = 	0x10;
	static const uint8_t MAX30105_MULTILEDCONFIG1 = 0x11;
	static const uint8_t MAX30105_MULTILEDCONFIG2 = 0x12;

	// Die Temperature Registers
	static const uint8_t MAX30105_DIETEMPINT = 		0x1F;
	static const uint8_t MAX30105_DIETEMPFRAC = 	0x20;
	static const uint8_t MAX30105_DIETEMPCONFIG = 	0x21;

	// Proximity Function Registers
	static const uint8_t MAX30105_PROXINTTHRESH = 	0x30;

	// Part ID Registers
	static const uint8_t MAX30105_REVISIONID = 		0xFE;
	static const uint8_t MAX30105_PARTID = 			0xFF;    // Should always be 0x15. Identical to MAX30102.

	// MAX30105 Commands
	// Interrupt configuration (pg 13, 14)
	static const uint8_t MAX30105_INT_A_FULL_MASK =		(uint8_t)~0b10000000;
	static const uint8_t MAX30105_INT_A_FULL_ENABLE = 	0x80;
	static const uint8_t MAX30105_INT_A_FULL_DISABLE = 	0x00;

	static const uint8_t MAX30105_INT_DATA_RDY_MASK = (uint8_t)~0b01000000;
	static const uint8_t MAX30105_INT_DATA_RDY_ENABLE =	0x40;
	static const uint8_t MAX30105_INT_DATA_RDY_DISABLE = 0x00;

	static const uint8_t MAX30105_INT_ALC_OVF_MASK = (uint8_t)~0b00100000;
	static const uint8_t MAX30105_INT_ALC_OVF_ENABLE = 	0x20;
	static const uint8_t MAX30105_INT_ALC_OVF_DISABLE = 0x00;

	static const uint8_t MAX30105_INT_PROX_INT_MASK = (uint8_t)~0b00010000;
	static const uint8_t MAX30105_INT_PROX_INT_ENABLE = 0x10;
	static const uint8_t MAX30105_INT_PROX_INT_DISABLE = 0x00;

	static const uint8_t MAX30105_INT_DIE_TEMP_RDY_MASK = (uint8_t)~0b00000010;
	static const uint8_t MAX30105_INT_DIE_TEMP_RDY_ENABLE = 0x02;
	static const uint8_t MAX30105_INT_DIE_TEMP_RDY_DISABLE = 0x00;

	static const uint8_t MAX30105_SAMPLEAVG_MASK =	(uint8_t)~0b11100000;
	static const uint8_t MAX30105_SAMPLEAVG_1 = 	0x00;
	static const uint8_t MAX30105_SAMPLEAVG_2 = 	0x20;
	static const uint8_t MAX30105_SAMPLEAVG_4 = 	0x40;
	static const uint8_t MAX30105_SAMPLEAVG_8 = 	0x60;
	static const uint8_t MAX30105_SAMPLEAVG_16 = 	0x80;
	static const uint8_t MAX30105_SAMPLEAVG_32 = 	0xA0;

	static const uint8_t MAX30105_ROLLOVER_MASK = 	0xEF;
	static const uint8_t MAX30105_ROLLOVER_ENABLE = 0x10;
	static const uint8_t MAX30105_ROLLOVER_DISABLE = 0x00;

	static const uint8_t MAX30105_A_FULL_MASK = 	0xF0;

	// Mode configuration commands (page 19)
	static const uint8_t MAX30105_SHUTDOWN_MASK = 	0x7F;
	static const uint8_t MAX30105_SHUTDOWN = 		0x80;
	static const uint8_t MAX30105_WAKEUP = 			0x00;

	static const uint8_t MAX30105_RESET_MASK = 		0xBF;
	static const uint8_t MAX30105_RESET = 			0x40;

	static const uint8_t MAX30105_MODE_MASK = 		0xF8;
	static const uint8_t MAX30105_MODE_REDONLY = 	0x02;
	static const uint8_t MAX30105_MODE_REDIRONLY = 	0x03;
	static const uint8_t MAX30105_MODE_MULTILED = 	0x07;

	// Particle sensing configuration commands (pgs 19-20)
	static const uint8_t MAX30105_ADCRANGE_MASK = 	0x9F;
	static const uint8_t MAX30105_ADCRANGE_2048 = 	0x00;
	static const uint8_t MAX30105_ADCRANGE_4096 = 	0x20;
	static const uint8_t MAX30105_ADCRANGE_8192 = 	0x40;
	static const uint8_t MAX30105_ADCRANGE_16384 = 	0x60;

	static const uint8_t MAX30105_SAMPLERATE_MASK = 0xE3;
	static const uint8_t MAX30105_SAMPLERATE_50 = 	0x00;
	static const uint8_t MAX30105_SAMPLERATE_100 = 	0x04;
	static const uint8_t MAX30105_SAMPLERATE_200 = 	0x08;
	static const uint8_t MAX30105_SAMPLERATE_400 = 	0x0C;
	static const uint8_t MAX30105_SAMPLERATE_800 = 	0x10;
	static const uint8_t MAX30105_SAMPLERATE_1000 = 0x14;
	static const uint8_t MAX30105_SAMPLERATE_1600 = 0x18;
	static const uint8_t MAX30105_SAMPLERATE_3200 = 0x1C;

	static const uint8_t MAX30105_PULSEWIDTH_MASK = 0xFC;
	static const uint8_t MAX30105_PULSEWIDTH_69 = 	0x00;
	static const uint8_t MAX30105_PULSEWIDTH_118 = 	0x01;
	static const uint8_t MAX30105_PULSEWIDTH_215 = 	0x02;
	static const uint8_t MAX30105_PULSEWIDTH_411 = 	0x03;

	//Multi-LED Mode configuration (pg 22)
	static const uint8_t MAX30105_SLOT1_MASK = 		0xF8;
	static const uint8_t MAX30105_SLOT2_MASK = 		0x8F;
	static const uint8_t MAX30105_SLOT3_MASK = 		0xF8;
	static const uint8_t MAX30105_SLOT4_MASK = 		0x8F;

	static const uint8_t SLOT_NONE = 				0x00;
	static const uint8_t SLOT_RED_LED = 			0x01;
	static const uint8_t SLOT_IR_LED = 				0x02;
	static const uint8_t SLOT_GREEN_LED = 			0x03;
	static const uint8_t SLOT_NONE_PILOT = 			0x04;
	static const uint8_t SLOT_RED_PILOT =			0x05;
	static const uint8_t SLOT_IR_PILOT = 			0x06;
	static const uint8_t SLOT_GREEN_PILOT = 		0x07;

	static const uint8_t MAX_30105_EXPECTEDPARTID = 0x15;

	Listener* listener = nullptr;


public:

	Max30102(I2C& i2c) : i2c(i2c) {
		;
	}

	void setListener(Listener* l) {
		this->listener = l;
	}

	bool isPresent() {
		return i2c.query(ADDR);
	}

	uint8_t getRevisionID() {
		return i2c.readReg8(ADDR, MAX30105_REVISIONID);
	}

	uint8_t getPartID() {
		return i2c.readReg8(ADDR, MAX30105_PARTID);
	}


	void softReset(void) {

		bitMask(MAX30105_MODECONFIG, MAX30105_RESET_MASK, MAX30105_RESET);

		// Poll for bit to clear, reset is then complete
		for (int i = 0; i < 100; ++i) {
			const uint8_t response = i2c.readReg8(ADDR, MAX30105_MODECONFIG);
			if ((response & MAX30105_RESET) == 0) break; //We're done!
			vTaskDelay(1 / portTICK_PERIOD_MS);
		}

		ESP_LOGI(NAME, "softReset done");

	}

//	void MAX30105::shutDown(void) {
//	  // Put IC into low power mode (datasheet pg. 19)
//	  // During shutdown the IC will continue to respond to I2C commands but will
//	  // not update with or take new readings (such as temperature)
//	  bitMask(MAX30105_MODECONFIG, MAX30105_SHUTDOWN_MASK, MAX30105_SHUTDOWN);
//	}



	void setModePulse() {

		//wakeUp();
		softReset();
		//wakeUp();

		writeReg8(MAX30105_INTENABLE1, 0x00);// | MAX30105_INT_DATA_RDY_ENABLE);//64);//16|64);//0xC0);
		writeReg8(MAX30105_INTENABLE2, 0x00);

		//writeReg8(MAX30105_PROXINTTHRESH, 0);
		//writeReg8(MAX30105_PifROXINTTHRESH, 64);


		//setLEDMode(MAX30105_MODE_REDONLY);
		setLEDMode(MAX30105_MODE_REDIRONLY);
		//setLEDMode(MAX30105_MODE_MULTILED);

		writeReg8(MAX30105_FIFOCONFIG, MAX30105_SAMPLEAVG_8 | MAX30105_ROLLOVER_ENABLE);

		//setFIFOAverage(MAX30105_SAMPLEAVG_8);
		setADCRange(MAX30105_ADCRANGE_16384);
		setSampleRate(MAX30105_SAMPLERATE_200);
		setPulseWidth(MAX30105_PULSEWIDTH_215);

		//Default is 0x1F which gets us 6.4mA
		//powerLevel = 0x02, 0.4mA - Presence detection of ~4 inch
		//powerLevel = 0x1F, 6.4mA - Presence detection of ~8 inch
		//powerLevel = 0x7F, 25.4mA - Presence detection of ~8 inch
		//powerLevel = 0xFF, 50.0mA - Presence detection of ~12 inch

		const int powerLevel = 0x30;
		setPulseAmplitudeRed(powerLevel);
		setPulseAmplitudeIR(powerLevel);
		setPulseAmplitudeProximity(powerLevel);

		// THIS ONE ONLY WORKS FOR "MAX30105_MODE_MULTILED" ?!
//		writeReg8(MAX30105_MULTILEDCONFIG1, (SLOT_RED_PILOT<<4) | (SLOT_RED_LED<<0) );
//		writeReg8(MAX30105_MULTILEDCONFIG2,  (SLOT_IR_PILOT<<4) |  (SLOT_IR_LED<<0) );
//		------
//		writeReg8(MAX30105_MULTILEDCONFIG1, (SLOT_IR_LED<<4) | (SLOT_RED_LED<<0) );
//		writeReg8(MAX30105_MULTILEDCONFIG2, 0 );

		clearFIFO();
	}

	void writeReg8(const uint8_t reg, const uint8_t val) {
		ESP_LOGI(NAME, "write reg: 0x%02x <- val: %d", reg, val);
		i2c.writeReg8(ADDR, reg, val);
	}

	uint8_t readReg8(const uint8_t reg) {
		return i2c.readReg8(ADDR, reg);
	}

//	//Check for new data but give up after a certain amount of time
//	//Returns true if new data was found
//	//Returns false if new data was not found
//	bool safeCheck(uint8_t maxTimeToCheck) {

//		for (int i = 0; i < maxTimeToCheck; ++i) {
//			if(check() == true) {
//				return(true);
//			}
//			vTaskDelay(1 / portTICK_PERIOD_MS);
//		}
//		return false;

//	}


	void check(void) {

//		const uint8_t intr1 = readReg8(MAX30105_INTSTAT1);
//		const uint8_t intr2 = readReg8(MAX30105_INTSTAT2);
//		// new data available?
//		//if (intr1 & 64) { .. }


		uint8_t readPointer = getReadPointer();
		const uint8_t writePointer = getWritePointer();

		//uint8_t readPointer = getReadPointer();
		//const uint8_t writePointer = getWritePointer();
		//ESP_LOGI(NAME, "intr. %d %d", readPointer, writePointer);

		while (readPointer != writePointer) {

			//printf(";%d;%d\n", readPointer, writePointer);

			uint8_t tmp[6];
			i2c.readReg(ADDR, MAX30105_FIFODATA, 6, tmp);

			const uint32_t val1 = ((tmp[0] << 16) | (tmp[1] << 8) | (tmp[2] << 0)) & 0x3FFFF; //Zero out all but 18 bits
			const uint32_t val2 = ((tmp[3] << 16) | (tmp[4] << 8) | (tmp[5] << 0)) & 0x3FFFF; //Zero out all but 18 bits

			//printf(";%d;%d;%d;%d\n", readPointer, writePointer, val1, val2);//, val3, val4);

			// inc
			readPointer = (readPointer + 1) % 32;

			// callback
			if (listener) {
				listener->onRed(val1);
				listener->onIR(val2);
			}

		}

	}




	int8_t getTemp() {

		// trigger temp acquire
		writeReg8(MAX30105_DIETEMPCONFIG, 1);

		 int8_t full = readReg8(MAX30105_DIETEMPINT);
		//uint8_f frac = i2c.readReg8(MAX30105_DIETEMPFRAC);
		return full;
	}

private:

	void wakeUp(void) {
	  // Pull IC out of low power mode (datasheet pg. 19)
	  bitMask(MAX30105_MODECONFIG, MAX30105_SHUTDOWN_MASK, MAX30105_WAKEUP);
	}

	void setLEDMode(uint8_t mode) {
	  // Set which LEDs are used for sampling -- Red only, RED+IR only, or custom.
	  // See datasheet, page 18
	  bitMask(MAX30105_MODECONFIG, MAX30105_MODE_MASK, mode);
	}



	void setPulseAmplitudeRed(uint8_t amplitude) {
		writeReg8( MAX30105_LED1_PULSEAMP, amplitude);
	}

	void setPulseAmplitudeIR(uint8_t amplitude) {
		writeReg8(MAX30105_LED2_PULSEAMP, amplitude);
	}

//	void setPulseAmplitudeGreen(uint8_t amplitude) {
//		writeReg8(MAX30105_LED3_PULSEAMP, amplitude);
//	}

	void setPulseAmplitudeProximity(uint8_t amplitude) {
		writeReg8(MAX30105_LED_PROX_AMP, amplitude);
	}

	void setFIFOAverage(uint8_t numberOfSamples) {
		bitMask(MAX30105_FIFOCONFIG, MAX30105_SAMPLEAVG_MASK, numberOfSamples);
	}

	void enableFIFORollover(void) {
		bitMask(MAX30105_FIFOCONFIG, MAX30105_ROLLOVER_MASK, MAX30105_ROLLOVER_ENABLE);
	}

	void setADCRange(uint8_t adcRange) {
		// adcRange: one of MAX30105_ADCRANGE_2048, _4096, _8192, _16384
		bitMask(MAX30105_PARTICLECONFIG, MAX30105_ADCRANGE_MASK, adcRange);
	}

	void setSampleRate(uint8_t sampleRate) {
		// sampleRate: one of MAX30105_SAMPLERATE_50, _100, _200, _400, _800, _1000, _1600, _3200
		bitMask(MAX30105_PARTICLECONFIG, MAX30105_SAMPLERATE_MASK, sampleRate);
	}

	void setPulseWidth(uint8_t pulseWidth) {
		// pulseWidth: one of MAX30105_PULSEWIDTH_69, _188, _215, _411
		bitMask(MAX30105_PARTICLECONFIG, MAX30105_PULSEWIDTH_MASK, pulseWidth);
	}


	void clearFIFO(void) {
		writeReg8(MAX30105_FIFOWRITEPTR, 0);
		writeReg8(MAX30105_FIFOOVERFLOW, 0);
		writeReg8(MAX30105_FIFOREADPTR, 0);
	}

	//Read the FIFO Write Pointer
	uint8_t getWritePointer(void) {
		return readReg8(MAX30105_FIFOWRITEPTR);
	}

	//Read the FIFO Read Pointer
	uint8_t getReadPointer(void) {
		return readReg8(MAX30105_FIFOREADPTR);
	}

	//Given a register, read it, mask it, and then set the thing
	void bitMask(const uint8_t reg, const uint8_t mask, const uint8_t thing) {

		const uint8_t orig = i2c.readReg8(ADDR, reg);
		const uint8_t masked = orig & mask;
		const uint8_t out = masked | thing;

		ESP_LOGI(NAME, "reg: %d - orig: %d new: %d", reg, orig, out);

		writeReg8(reg, out);

	}



};

#endif // MAX30102_H