#ifndef SENS_BME280
#define SENS_BME280

#include "../../Platforms.h"
#include "../../Debug.h"

template <typename I2C> class BME280 {

	static constexpr const char* NAME		= "BME280";

	static constexpr uint8_t ADDR7			= 0b1110110;

	static constexpr uint8_t REG_CTRL1		= 0xF2;	// humidity
	static constexpr uint8_t REG_CTRL2		= 0xF4;	// temp, pressure, mode

	static constexpr uint8_t REG_STATUS		= 0xF3;
	static constexpr uint8_t REG_PRESSURE	= 0xF7;
	static constexpr uint8_t REG_TEMPERATURE= 0xFA;
	static constexpr uint8_t REG_HUMIDITY	= 0xFD;

	static constexpr uint8_t REG_DIG_T1		= 0x88;
	static constexpr uint8_t REG_DIG_T2		= 0x8A;
	static constexpr uint8_t REG_DIG_T3		= 0x8C;


public:

	bool started = false;

	/** internal sensor calibration values */
	struct Calibration {

		uint16_t dig_T1 = 0;
		int16_t dig_T2 = 0;
		int16_t dig_T3 = 0;

		uint16_t dig_P1 = 0;
		int16_t dig_P2 = 0;
		int16_t dig_P3 = 0;
		int16_t dig_P4 = 0;
		int16_t dig_P5 = 0;
		int16_t dig_P6 = 0;
		int16_t dig_P7 = 0;
		int16_t dig_P8 = 0;
		int16_t dig_P9 = 0;

		uint8_t dig_H1 = 0;
		int16_t dig_H2 = 0;
		uint8_t dig_H3 = 0;
		int16_t dig_H4 = 0;
		int16_t dig_H5 = 0;
		int8_t dig_H6 = 0;

	} cal;


	I2C& i2c;

	BME280(I2C& i2c) : i2c(i2c) {

	}


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

private:

	void readCalib() {

		debugMod(NAME, "readCalib()");

		// read all 24 calibration bytes for temperature and pressure
		uint8_t b1[24];
		readRegister(REG_DIG_T1, b1, 24);

		cal.dig_T1 = (b1[1] << 8) | b1[0];
		cal.dig_T2 = (b1[3] << 8) | b1[2];
		cal.dig_T3 = (b1[5] << 8) | b1[4];

		cal.dig_P1 = (b1[7] << 8) | b1[6];
		cal.dig_P2 = (b1[9] << 8) | b1[8];
		cal.dig_P3 = (b1[11] << 8) | b1[10];
		cal.dig_P4 = (b1[13] << 8) | b1[12];
		cal.dig_P5 = (b1[15] << 8) | b1[14];
		cal.dig_P6 = (b1[17] << 8) | b1[16];
		cal.dig_P7 = (b1[19] << 8) | b1[18];
		cal.dig_P8 = (b1[21] << 8) | b1[20];
		cal.dig_P9 = (b1[23] << 8) | b1[22];

		// humidity
		readRegister(0xA1, &cal.dig_H1, 1);
		readRegister(0xE1, b1, 7);
		cal.dig_H2 = (b1[1] << 8) | b1[0];
		cal.dig_H3 = b1[3];
		cal.dig_H4 = (b1[3] << 4) | (b1[4] & 0b000001111);
		cal.dig_H5 = (b1[5] << 4) | ((b1[4] & 0b111100000) >> 4);
		cal.dig_H6 = (b1[6]);

		//os_printf("calib temp: %d %d %d\n", cal.dig_T1, cal.dig_T2, cal.dig_T3);
		//os_printf("calib pres: %d %d %d %d %d %d %d %d %d\n", cal.dig_P1, cal.dig_P2, cal.dig_P3, cal.dig_P4, cal.dig_P5, cal.dig_P6, cal.dig_P7, cal.dig_P8, cal.dig_P9);
		//os_printf("calib humi: %d %d %d %d %d %d\n", cal.dig_H1, cal.dig_H2, cal.dig_H3, cal.dig_H4, cal.dig_H5, cal.dig_H6);
		debugMod3(NAME, "calTemp: %d %d %d",					cal.dig_T1, cal.dig_T2, cal.dig_T3);
		debugMod9(NAME, "calPres: %d %d %d %d %d %d %d %d %d",	cal.dig_P1, cal.dig_P2, cal.dig_P3, cal.dig_P4, cal.dig_P5, cal.dig_P6, cal.dig_P7, cal.dig_P8, cal.dig_P9);
		debugMod6(NAME, "calHumi: %d %d %d %d %d %d",			cal.dig_H1, cal.dig_H2, cal.dig_H3, cal.dig_H4, cal.dig_H5, cal.dig_H6);

	}

	void start() {
		debugMod(NAME, "start()");
		const uint8_t cfgHumi = 0b101;	// 16x oversampling
		const uint8_t cfgPres = 0b101;	// 16x oversampling
		const uint8_t cfgTemp = 0b101;	// 16x oversampling
		const uint8_t cfgMode = 0b11;
		const uint8_t cfg1 = (cfgHumi << 1);
		const uint8_t cfg2 = (cfgTemp << 5) | (cfgPres << 2) | (cfgMode << 0);
		writeRegister(REG_CTRL1, &cfg1, 1);
		writeRegister(REG_CTRL2, &cfg2, 1);
	}



public:

	void startOnce() {
		if (started) {return;}
		debugMod(NAME, "startOnce()");
		readCalib();
		start();
		started = true;
	}

	uint8_t getStatus() {
		uint8_t res[1];
		readRegister(REG_STATUS, res, 1);
		//os_printf("Status: %d \n", res[0]);
		return 0;
	}

	/** get current pressure in hPa */
	float getPressure() {
		uint8_t res[3];
		readRegister(REG_PRESSURE, res, 3);
		//os_printf("res: %d - %d - %d \n", res[0], res[1], res[2]);
		const uint32_t tmp = ((res[0] << 16) | (res[1] << 8) | (res[2] << 0)) >> 4;
		const uint32_t pres = BME280_compensate_P_int64(tmp);
		const float presF = pres / 256.0f / 100.0f;	// convert from Q24.8 to float and from Pa to hPa
		const uint32_t p0 = pres / 256;
		const uint32_t p1 = (uint32_t) presF;
		const uint32_t p2 = (presF - p1) * 100000;
		debugMod4(NAME, "[pres] ADC: %d -> %d Pa | %d.%d hPa", tmp, p0, p1,p2);
		return presF;
	}

	float getTemperature() {
		uint8_t res[3];
		readRegister(REG_TEMPERATURE, res, 3);
		//os_printf("res: %d - %d - %d \n", res[0], res[1], res[2]);
		const uint32_t tmp = ((res[0] << 16) | (res[1] << 8) | (res[2] << 0)) >> 4;
		const  int32_t temp = BME280_compensate_T_int32(tmp);
		const float tempF = temp / 100.0f;
		debugMod2(NAME, "[temp] ADC: %d -> %d", tmp, temp);
		return tempF;
	}

	float getHumidity() {
		uint8_t res[2];
		readRegister(REG_HUMIDITY, res, 2);
		//os_printf("res: %d - %d \n", res[0], res[1]);
		const uint32_t tmp = (res[0] << 8) | (res[1] << 0);
		const  int32_t humi = bme280_compensate_H_int32(tmp);
		const float humiF = humi / 1024.0f;
		const uint16_t h0 = humi / 1024;
		const uint16_t h1 = (uint16_t) humiF;
		const uint16_t h2 = (humiF - humi) * 10000;
		debugMod4(NAME, "[humi] ADC: %d -> %d -> %d.%d %%", tmp, h0, h1,h2);
		return humiF;
	}

	bool readRegister(const uint8_t addr, uint8_t* dst, const uint8_t len) {

		bool ok;

		// address the slave in write mode and select the first register to read
		ok = i2c.startWrite(ADDR7);
		if (!ok) {os_printf("failed start write\n"); return false;}
		ok = i2c.writeByteAndCheck(addr);
		if (!ok) {os_printf("failed to select register %d\n", addr); return false;}

		//i2c::stop();

		// address the slave in read mode and read [len] registers
		ok = i2c.startRead(ADDR7);
		if (!ok) {os_printf("failed start read\n"); return 0;}
		i2c.readBytes(dst, len);

		// done
		i2c.stop();
		return true;

	}

	bool writeRegister(const uint8_t addr, const uint8_t* src, const uint8_t len) {

		bool ok;

		// address the slave in write mode and select the first register to read
		ok = i2c.startWrite(ADDR7);
		if (!ok) {os_printf("failed start write\n"); return false;}
		ok = i2c.writeByteAndCheck(addr);
		if (!ok) {os_printf("failed to select register %d\n", addr); return false;}
		ok = i2c.writeBytesAndCheck(src, len);
		if (!ok) {os_printf("failed to write register contents \n"); return false;}

		// done
		i2c.stop();
		return true;

	}



private:

	/** conversions from ADC values to real-world values. from Bosch BMP280 manual! */

	using BME280_S32_t = int32_t;
	using BME280_U32_t = uint32_t;
	using BME280_S64_t = int64_t;
	BME280_S32_t t_fine = 0;

	// Returns temperature in DegC, resolution is 0.01 DegC. Output value of “5123” equals 51.23 DegC.
	// t_fine carries fine temperature as global value
	BME280_S32_t BME280_compensate_T_int32(BME280_S32_t adc_T) {
		BME280_S32_t var1, var2, T;
		var1 = ((((adc_T>>3) - ((BME280_S32_t)cal.dig_T1<<1))) * ((BME280_S32_t)cal.dig_T2)) >> 11;
		var2 = (((((adc_T>>4) - ((BME280_S32_t)cal.dig_T1)) * ((adc_T>>4) - ((BME280_S32_t)cal.dig_T1))) >> 12) * ((BME280_S32_t)cal.dig_T3)) >> 14;
		t_fine = var1 + var2;
		T = (t_fine * 5 + 128) >> 8;
		return T;
	}

	// Returns pressure in Pa as unsigned 32 bit integer in Q24.8 format (24 integer bits and 8 fractional bits).
	// Output value of “24674867” represents 24674867/256 = 96386.2 Pa = 963.862 hPa
	BME280_U32_t BME280_compensate_P_int64(BME280_S32_t adc_P) {
		BME280_S64_t var1, var2, p;
		var1 = ((BME280_S64_t)t_fine) - 128000;
		var2 = var1 * var1 * (BME280_S64_t)cal.dig_P6;
		var2 = var2 + ((var1*(BME280_S64_t)cal.dig_P5)<<17);
		var2 = var2 + (((BME280_S64_t)cal.dig_P4)<<35);
		var1 = ((var1 * var1 * (BME280_S64_t)cal.dig_P3)>>8) + ((var1 * (BME280_S64_t)cal.dig_P2)<<12);
		var1 = (((((BME280_S64_t)1)<<47)+var1))*((BME280_S64_t)cal.dig_P1)>>33;
		if (var1 == 0) {return 0;} // avoid exception caused by division by zero
		p = 1048576-adc_P;
		p = (((p<<31)-var2)*3125)/var1;
		var1 = (((BME280_S64_t)cal.dig_P9) * (p>>13) * (p>>13)) >> 25;
		var2 = (((BME280_S64_t)cal.dig_P8) * p) >> 19;
		p = ((p + var1 + var2) >> 8) + (((BME280_S64_t)cal.dig_P7)<<4);
		return (BME280_U32_t)p;
	}

	// Returns humidity in %RH as unsigned 32 bit integer in Q22.10 format (22 integer and 10 fractional bits).
	// Output value of “47445” represents 47445/1024 = 46.333 %RH
	BME280_U32_t bme280_compensate_H_int32(BME280_S32_t adc_H) {
		BME280_S32_t v_x1_u32r;
		v_x1_u32r = (t_fine - ((BME280_S32_t)76800));
		v_x1_u32r = (((((adc_H << 14) - (((BME280_S32_t)cal.dig_H4) << 20) - (((BME280_S32_t)cal.dig_H5) * v_x1_u32r)) +
		((BME280_S32_t)16384)) >> 15) * (((((((v_x1_u32r * ((BME280_S32_t)cal.dig_H6)) >> 10) * (((v_x1_u32r *
		((BME280_S32_t)cal.dig_H3)) >> 11) + ((BME280_S32_t)32768))) >> 10) + ((BME280_S32_t)2097152)) *
		((BME280_S32_t)cal.dig_H2) + 8192) >> 14));
		v_x1_u32r = (v_x1_u32r - (((((v_x1_u32r >> 15) * (v_x1_u32r >> 15)) >> 7) * ((BME280_S32_t)cal.dig_H1)) >>
		4));
		v_x1_u32r = (v_x1_u32r < 0 ? 0 : v_x1_u32r);
		v_x1_u32r = (v_x1_u32r > 419430400 ? 419430400 : v_x1_u32r);
		return (BME280_U32_t)(v_x1_u32r>>12);
	}


};

#endif