#include "BMP280.h"
#include "writepin.h"
#include <util/delay.h>

BMP280::BMP280(SpiMaster* spi, volatile uint8_t *port, const uint8_t pin): _port(port), _pin(pin), _spi(spi)
{
	writePin(_port, _pin, true);
}

void BMP280::write(const uint8_t address, const uint8_t data)
{
	writePin(_port, _pin, false);
	_spi->write(address & ~(1<<7));
	_spi->write(data);
	writePin(_port, _pin, true);
}

uint16_t BMP280::read16b(const uint8_t address)
{
	writePin(_port, _pin, false);
	_spi->write(address);
	uint16_t out = static_cast<uint16_t>(_spi->read()) << 8;
	out |= _spi->read();
	writePin(_port, _pin, false);
	return out;
}

uint8_t BMP280::read8b(const uint8_t address)
{
	writePin(_port, _pin, false);
	_spi->write(address);
	return _spi->read(); 
}

uint16_t BMP280::getPressure()
{
	write(CTRL_REG, 0b11100101);
	_delay_ms(10);
	uint16_t out = read16b(PRESS_REG_HIGH);
	_delay_ms(10);
	write(CTRL_REG, 0b11100100);
	return compensatePressure(out);
}

#ifndef PRECOMP_COMPENSATION
uint32_t BMP280::compensatePressure(const int32_t adc_P)
{
	int32_t var1, var2;
	uint32_t p;
	var1 = (((int32_t)t_fine)>>1) - (int32_t)64000;
	var2 = (((var1>>2) * (var1>>2)) >> 11 ) * ((int32_t)read16b(DIG_P6));
	var2 = var2 + ((var1*((int32_t)read16b(DIG_P5)))<<1);
	var2 = (var2>>2)+(((int32_t)read16b(DIG_P4))<<16);
	var1 = ((((read16b(DIG_P3) * (((var1>>2) * (var1>>2)) >> 13 )) >> 3) + ((((int32_t)read16b(DIG_P2)) * var1)>>1))>>18);
	var1 =((((32768+var1))*((int32_t)read16b(DIG_P1)))>>15);
	if (var1 == 0) return 0;
	p = (((uint32_t)(((int32_t)1048576)-adc_P)-(var2>>12)))*3125;
	if (p < 0x80000000)
	{
		p = (p << 1) / ((uint32_t)var1);
	}
	else
	{
		p = (p / (uint32_t)var1) * 2;
	}
	var1 = (((int32_t)read16b(DIG_P9)) * ((int32_t)(((p>>3) * (p>>3))>>13)))>>12;
	var2 = (((int32_t)(p>>2)) * ((int32_t)read16b(DIG_P8)))>>13;
	p = (uint32_t)((int32_t)p + ((var1 + var2 + (int16_t)read16b(DIG_P7)) >> 4));
	return p;
}

#else
uint32_t BMP280::compensatePressure(const int32_t adc_P)
{
	if constexpr(var1StepB == 0) return 0;
	uint32_t p = (((uint32_t)(((int32_t)1048576)-adc_P)-var2StepB))*3125;
	if (p < 0x80000000)
	{
		p = (p << 1) / var1StepB;
	}
	else
	{
		p = (p / var1StepB) * 2;
	}
	int16_t var1 = (((int32_t)digP9value) * ((int32_t)(((p>>3) * (p>>3))>>13)))>>12;
	int16_t var2 = (((int32_t)(p>>2)) * ((int32_t)digP8value))>>13;
	p = (uint32_t)((int32_t)p + ((var1 + var2 + (int16_t)digP7value) >> 4));
	return p;
}
#endif