/*
 * bmp280.cpp
 *
 */
#include "devices/bmp280.hpp"
#include "interfaces/spi.hpp"
#include "interfaces/i2c.hpp"
#include "interfaces/rtc.hpp"
#include "interfaces/log.hpp"

#include "utilities/debug_cfg.hpp"

bool cBMP280::initialized = false;
long cBMP280::pressure_dPa;
long cBMP280::temperature;

void cBMP280::initialize()
{
    cSPI::initialize();
    initialized = true;
    write(0xE0,0xB6); //E0 - reset
    //cUtils::delayMs(10);
    cRTC::delay_ms(10);
    write(0xF4, 0x30); //pOverSampling-100b x8, tOveSamplingr-001b x1,  sleep mode
    				   // 0x30 - 001 100 00
    				   // 0x30 - oT  oP  mode
    				   // 0x33 - 001 100 11
}

void cBMP280::write(uint8_t addr, uint8_t  value)
{
	uint8_t addWriteMask = addr & 0x7F;
	uint8_t payload[2] = {addWriteMask, value};
    cSPI::cs(cSPI::BME280, cSPI::CS_ACTIVE);
    cSPI::sendData(payload, 2);
    cSPI::cs(cSPI::BME280, cSPI::CS_OFF);
}

void cBMP280::read(uint8_t addr, uint8_t* value){
	uint8_t dataSPI[2];
	cSPI::cs(cSPI::BME280, cSPI::CS_ACTIVE);
	cSPI::sendData(&addr, 1, dataSPI, 2);
	cSPI::cs(cSPI::BME280, cSPI::CS_OFF);
	*value = dataSPI[1];
}

void cBMP280::readMultiple(uint8_t addr, uint8_t* buffer, uint8_t len)
{
	cSPI::cs(cSPI::BME280, cSPI::CS_ACTIVE);
	cSPI::sendData(&addr, 1, buffer, len+1);
	cSPI::cs(cSPI::BME280, cSPI::CS_OFF);
}

void cBMP280::readCalibrationData(uint16_t* calData)
{
	uint8_t dataSPI[25];
	uint8_t addr = 0x88;
	cSPI::cs(cSPI::BME280, cSPI::CS_ACTIVE);
	cSPI::sendData(&addr, 1, dataSPI, 25);
	cSPI::cs(cSPI::BME280, cSPI::CS_OFF);
	for (int i = 0; i < 12; i++)
	{
		calData[i] = (dataSPI[i*2+2]<<8) | dataSPI[i*2+1];
	}
}

bool cBMP280::waitForData()
{
	uint8_t status = 1, count = 8;
	//cUtils::delayMs(18); // wait for mesurement (datasheet 19.5-22.5 ms)
	cRTC::delay_ms(18);
	while ( ( (status & 0x09) != 0 ) && ( count != 0 ) )
	{
		read(0xF3, &status); // check reg 0xF3 for results ready (0x00): bit3-measuring, bit0-NVM reading
		// cLog::info("bmp280 status r0xF3 Is: 0x%02X \n\r", status & 0x9);
		count--;
		//cUtils::delayMs(1);
		if ((status & 0x09) != 0) cRTC::delay_ms(1);
	}

	return status & 0x9;
}

void cBMP280::getPressure()
{
    /*
    * The rest of code expects data to be returned at a 1 byte offset, 
    * this a "little" bit uggly way to filter data in SPI Rx Register where first byte
    * allways is 0xFF, because it is read while sending Register address and is not an actual response 
    * from sensor.
    */

	uint16_t cal[12]={0,0,0,0, 0,0,0,0, 0,0,0,0};
	uint8_t data[7];


	write(0xF4, 0x31); // start forced measure

	if (waitForData())
	{
		cBMP280::pressure_dPa=0;
		return;
	}

	readCalibrationData(cal);
	readMultiple(0xF7, data, 6);

//	write(0xF4, 0x33); 	//register F4: normal-11, pOver-100 x8, tOver-001 x1
						// 1:0 mode 00-sleep 01,10-forced 11-normal
						// 4:2 Controls oversampling of pressure data
						// 7:5 Controls oversampling of temperature data
						// Oversampling 000 = Skipped (output set to 0x80000 {524288})

	long adcBMP280t = ((long)data[4] << 12) | (data[5] << 4) | (data[6] >> 4);
	long adcBMP280p = ((long)data[1] << 12) | (data[2] << 4) | (data[3] >> 4);

	unsigned int calT1 = cal[0];
    signed short int calT2 = cal[1], calT3 = cal[2];

    unsigned int calP1 = cal[3];
    signed short int calP2 = cal[4], calP3 = cal[5], calP4 = cal[6],
        calP5 = cal[7], calP6 = cal[8], calP7 = cal[9],
        calP8 = cal[10], calP9 = cal[11];

    long v1 = ((( (adcBMP280t >> 3) - ((long)calT1 << 1))) * ((long)calT2)) >> 11;
    long v2 = (((((adcBMP280t >> 4) - ((long)calT1)) * ((adcBMP280t >> 4) - ((long)calT1))) >> 12) * ((long)calT3)) >> 14;
    long tBMP280 = (v1 + v2) / 2;

    double var1, var2, p;
    var1 = tBMP280 - 64000;
    var2 = var1 * var1 * ((double)calP6) / 32768.0d;
    var2 = var2 + var1 * ((double)calP5) * 2.0;
    var2 = (var2 / 4.0) + (((double)calP4) * 65536.0);
    var1 = (((double)calP3) * var1 * var1 / 524288.0 + ((double)calP2) * var1) / 524288.0;
    var1 = (1.0 + var1 / 32768.0) * ((double)calP1);

    p = 1048576 - adcBMP280p;
    p = (p - (var2 / 4096.0)) * 6250.0 / var1;
    var1 = ((double)calP9) * p * p / 2147483648.0;
    var2 = p * ((double)calP8) / 32768.0;

    cBMP280::pressure_dPa = (int)((p + (var1 + var2 + ((double)calP7)) / 16.0) / 10.0);
    if ( cBMP280::pressure_dPa > 60000 )
    	cBMP280::pressure_dPa = 0xFFFF;
    cBMP280::temperature = tBMP280*20/512;

    cLog::debug("bmp280 pres %d dPa", cBMP280::pressure_dPa);
    cLog::debug("bmp280 temp %d,%02d'C", cBMP280::temperature / 100, cBMP280::temperature % 100);
}