#include "bme280.h"
#include "stm32f0xx_hal.h"
#include "config.h"

static uint16_t dig_T1;
static int16_t dig_T2;
static int16_t dig_T3;

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

static uint8_t dig_H1;
static int16_t dig_H2;
static uint8_t dig_H3;
static int16_t dig_H4;
static int16_t dig_H5;
static uint8_t dig_H6;

static int16_t pressure;
static int16_t temperature;
static int16_t humidity;

static I2C_HandleTypeDef hi2c1;


// Initialize the BME280 PTU on I2C
bool bme280_init(void)
{
	GPIO_InitTypeDef GPIO_InitStruct;

	// I2C Pins
    GPIO_InitStruct.Pin = PIN_BME280_SCL|PIN_BME280_SDA;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;
    GPIO_InitStruct.Pull = GPIO_PULLUP;
    GPIO_InitStruct.Speed = GPIO_SPEED_HIGH;
    GPIO_InitStruct.Alternate = GPIO_AF1_I2C1;
    HAL_GPIO_Init(PORT_BME280, &GPIO_InitStruct);

    // Initialize I2C peripheral
    __I2C1_CLK_ENABLE();
    hi2c1.Instance = I2C1;
    hi2c1.Init.Timing = 0x00108EFB;
    hi2c1.Init.OwnAddress1 = 0x0;
    hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
    hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLED;
    hi2c1.Init.OwnAddress2 = 0;
    hi2c1.Init.OwnAddress2Masks = I2C_OA2_NOMASK;
    hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLED;
    hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLED;
    volatile HAL_StatusTypeDef res = HAL_I2C_Init(&hi2c1);

    // Configure Analog filter
    res =  HAL_I2CEx_AnalogFilter_Config(&hi2c1, I2C_ANALOGFILTER_ENABLED);

	// Enable I2C interrupt
    HAL_NVIC_SetPriority(I2C1_IRQn, 7, 4);
    HAL_NVIC_EnableIRQ(I2C1_IRQn);


	volatile uint8_t result = bme280_read_byte(BME280_CHIP_ID_REG);


	if (result != 0x60)
	{
		return false;
	}

	bme280_write(BME280_CTRL_MEAS_REG, 0x00);
	bme280_write(BME280_CONFIG_REG, 0b01100000); // 4hz sampling rate
	bme280_write(BME280_CTRL_HUMIDITY_REG, 0x01); //0b00000001, oversample*1
	bme280_write(BME280_CTRL_MEAS_REG, 0x27); //0b00100111


	//calibrate

	dig_T1 = ((uint16_t)((bme280_read_byte(BME280_DIG_T1_MSB_REG) << 8) + bme280_read_byte(BME280_DIG_T1_LSB_REG)));
	dig_T2 = ((int16_t)((bme280_read_byte(BME280_DIG_T2_MSB_REG) << 8) + bme280_read_byte(BME280_DIG_T2_LSB_REG)));
	dig_T3 = ((int16_t)((bme280_read_byte(BME280_DIG_T3_MSB_REG) << 8) + bme280_read_byte(BME280_DIG_T3_LSB_REG)));

	dig_P1 = ((uint16_t)((bme280_read_byte(BME280_DIG_P1_MSB_REG) << 8) + bme280_read_byte(BME280_DIG_P1_LSB_REG)));
	dig_P2 = ((int16_t)((bme280_read_byte(BME280_DIG_P2_MSB_REG) << 8) + bme280_read_byte(BME280_DIG_P2_LSB_REG)));
	dig_P3 = ((int16_t)((bme280_read_byte(BME280_DIG_P3_MSB_REG) << 8) + bme280_read_byte(BME280_DIG_P3_LSB_REG)));
	dig_P4 = ((int16_t)((bme280_read_byte(BME280_DIG_P4_MSB_REG) << 8) + bme280_read_byte(BME280_DIG_P4_LSB_REG)));
	dig_P5 = ((int16_t)((bme280_read_byte(BME280_DIG_P5_MSB_REG) << 8) + bme280_read_byte(BME280_DIG_P5_LSB_REG)));
	dig_P6 = ((int16_t)((bme280_read_byte(BME280_DIG_P6_MSB_REG) << 8) + bme280_read_byte(BME280_DIG_P6_LSB_REG)));
	dig_P7 = ((int16_t)((bme280_read_byte(BME280_DIG_P7_MSB_REG) << 8) + bme280_read_byte(BME280_DIG_P7_LSB_REG)));
	dig_P8 = ((int16_t)((bme280_read_byte(BME280_DIG_P8_MSB_REG) << 8) + bme280_read_byte(BME280_DIG_P8_LSB_REG)));
	dig_P9 = ((int16_t)((bme280_read_byte(BME280_DIG_P9_MSB_REG) << 8) + bme280_read_byte(BME280_DIG_P9_LSB_REG)));

	dig_H1 = ((uint8_t)(bme280_read_byte(BME280_DIG_H1_REG)));
	dig_H2 = ((int16_t)((bme280_read_byte(BME280_DIG_H2_MSB_REG) << 8) + bme280_read_byte(BME280_DIG_H2_LSB_REG)));
	dig_H3 = ((uint8_t)(bme280_read_byte(BME280_DIG_H3_REG)));
	dig_H4 = ((int16_t)((bme280_read_byte(BME280_DIG_H4_MSB_REG) << 4) + (bme280_read_byte(BME280_DIG_H4_LSB_REG) & 0x0F)));
	dig_H5 = ((int16_t)((bme280_read_byte(BME280_DIG_H5_MSB_REG) << 4) + ((bme280_read_byte(BME280_DIG_H4_LSB_REG) >> 4) & 0x0F)));
	dig_H6 = ((uint8_t)bme280_read_byte(BME280_DIG_H6_REG));

	return true;
}


// Acquire new readings from sensor and update values in RAM
void bme280_update(void)
{
	uint8_t result[8];
	bme280_read(BME280_PRESSURE_MSB_REG, result, 8);

	pressure = bme280_convert_pressure(result[0] << 12 | result[1] << 4 | (result[2] & 0xf0) << 0) / 256 / 10; // convert to mBar/hPa
	temperature = bme280_convert_temperature(result[3] << 12 | result[4] << 4 | (result[5] & 0xf0) << 0); // 0.01C
	humidity = bme280_convert_humidity(result[6] << 8 | result[7] << 0) / 1024; // 0.01% RH
}


// returns pressure in mb with 0.1mb resolution
int32_t bme280_get_pressure(void)
{
	return pressure;
}

// returns temperature in DegC with 0.01 DegC resolution
int32_t bme280_get_temperature(void)
{

	return temperature;
}

// returns humidity in %RH with 0.01% resolution
int32_t bme280_get_humidity(void)
{

	return humidity;
}




void bme280_read(uint8_t register_address, uint8_t* data, uint8_t length)
{
	HAL_I2C_Mem_Read(&hi2c1, BME280_I2C_ADDR, register_address, 1, data, length, 500);
}

uint8_t bme280_read_byte(uint8_t register_address)
{
	uint8_t result;
	bme280_read(register_address, &result, 1);
	return result;
}

void bme280_write(uint8_t register_address, uint8_t data)
{
	//register_address &= 0x7F;
	uint8_t data_arr[1];
	data_arr[0] = data;
	HAL_I2C_Mem_Write(&hi2c1, BME280_I2C_ADDR, register_address, 1, data_arr, 1, 500);
}

// 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
int32_t t_fine;
int32_t bme280_convert_temperature(int32_t adc_T)
{
	int32_t var1, var2, T;
	var1 = ((((adc_T>>3) - ((int32_t)dig_T1<<1))) * ((int32_t)dig_T2)) >> 11;
	var2 = (((((adc_T>>4) - ((int32_t)dig_T1)) * ((adc_T>>4) - ((int32_t)dig_T1))) >> 12) *
			((int32_t)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
uint32_t bme280_convert_pressure(int32_t adc_P)
{
	int64_t var1, var2, p;
	var1 = ((int64_t)t_fine) - 128000;
	var2 = var1 * var1 * (int64_t)dig_P6;
	var2 = var2 + ((var1*(int64_t)dig_P5)<<17);
	var2 = var2 + (((int64_t)dig_P4)<<35);
	var1 = ((var1 * var1 * (int64_t)dig_P3)>>8) + ((var1 * (int64_t)dig_P2)<<12);
	var1 = (((((int64_t)1)<<47)+var1))*((int64_t)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 = (((int64_t)dig_P9) * (p>>13) * (p>>13)) >> 25;
	var2 = (((int64_t)dig_P8) * p) >> 19;
	p = ((p + var1 + var2) >> 8) + (((int64_t)dig_P7)<<4);
	return (uint32_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
uint32_t bme280_convert_humidity(int32_t adc_H)
{
	int32_t v_x1_u32r;
	v_x1_u32r = (t_fine - ((int32_t)76800));
	v_x1_u32r = (((((adc_H << 14) - (((int32_t)dig_H4) << 20) - (((int32_t)dig_H5) * v_x1_u32r)) +
			((int32_t)16384)) >> 15) * (((((((v_x1_u32r * ((int32_t)dig_H6)) >> 10) * (((v_x1_u32r *
					((int32_t)dig_H3)) >> 11) + ((int32_t)32768))) >> 10) + ((int32_t)2097152)) *
					((int32_t)dig_H2) + 8192) >> 14));
	v_x1_u32r = (v_x1_u32r - (((((v_x1_u32r >> 15) * (v_x1_u32r >> 15)) >> 7) * ((int32_t)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 (uint32_t)(v_x1_u32r>>12);
}

I2C_HandleTypeDef* bme280_get_i2c_handle(void)
{
	return &hi2c1;
}