#include <libopencm3/stm32/i2c.h>
#include <libopencm3/stm32/gpio.h>

#include "pinout.h"

#include "bmp280_comp.h"
#include "bmp280.h"

#define BMP280_7BIT_ADDR 0x76

// calibration value cache, shared with bmp280_comp.c

extern uint16_t dig_T1;
extern  int16_t dig_T2;
extern  int16_t dig_T3;

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

static fxp_t m_last_temperature = 0;
static fxp_t m_last_pressure = 0;

bool bmp280_init(void)
{
	// configure pins for I2C1
	gpio_set_af(BMP280_I2C_PORT, GPIO_AF4, BMP280_I2C_SCL | BMP280_I2C_SDA);
	gpio_mode_setup(BMP280_I2C_PORT, GPIO_MODE_AF, GPIO_PUPD_PULLUP, BMP280_I2C_SCL | BMP280_I2C_SDA);

	// Set up I²C
	i2c_reset(I2C1);

	i2c_set_speed(I2C1, i2c_speed_sm_100k, 48);

	i2c_peripheral_enable(I2C1);

	// Set up the Chip
	uint8_t wdata[2];
	uint8_t rdata[32];
	uint8_t wsize, rsize;

	// read the chip ID
	wsize = 0;
	wdata[wsize++] = 0xD0;
	rsize = 1;

	i2c_transfer7(I2C1, BMP280_7BIT_ADDR, wdata, wsize, rdata, rsize);

	if(rdata[0] != 0x58) {
		// unexpected chip id
		goto err_out;
	}

	// read calibration data
	wsize = 0;
	wdata[wsize++] = 0x88; // first calibration data register of first block
	rsize = 0xA1 + 1 - 0x88;

	i2c_transfer7(I2C1, BMP280_7BIT_ADDR, wdata, wsize, rdata, rsize);

	dig_T1 = (((uint16_t)rdata[ 1]) << 8) | rdata[ 0]; // 0x88, 0x89
	dig_T2 =  (((int16_t)(int8_t)rdata[ 3]) << 8) | rdata[ 2]; // 0x8A, 0x8B
	dig_T3 =  (((int16_t)(int8_t)rdata[ 5]) << 8) | rdata[ 4]; // 0x8C, 0x8D

	dig_P1 = (((uint16_t)rdata[ 7]) << 8) | rdata[ 6]; // 0x8E, 0x8F
	dig_P2 =  (((int16_t)(int8_t)rdata[ 9]) << 8) | rdata[ 8]; // 0x90, 0x91
	dig_P3 =  (((int16_t)(int8_t)rdata[11]) << 8) | rdata[10]; // 0x92, 0x93
	dig_P4 =  (((int16_t)(int8_t)rdata[13]) << 8) | rdata[12]; // 0x94, 0x95
	dig_P5 =  (((int16_t)(int8_t)rdata[15]) << 8) | rdata[14]; // 0x96, 0x97
	dig_P6 =  (((int16_t)(int8_t)rdata[17]) << 8) | rdata[16]; // 0x98, 0x99
	dig_P7 =  (((int16_t)(int8_t)rdata[19]) << 8) | rdata[18]; // 0x9A, 0x9B
	dig_P8 =  (((int16_t)(int8_t)rdata[21]) << 8) | rdata[20]; // 0x9C, 0x9D
	dig_P9 =  (((int16_t)(int8_t)rdata[23]) << 8) | rdata[22]; // 0x9E, 0x9F

	return true;

err_out:
	return false;
}


bool bmp280_start_measurement(void)
{
	uint8_t wdata[2];
	uint8_t wsize = 0;

	wdata[wsize++] = 0xF4; // measurement control register
	wdata[wsize++] = (0x01 << 5) | (0x01 << 2) | 0x01; // pressure x1, temp x1, forced mode

	i2c_transfer7(I2C1, BMP280_7BIT_ADDR, wdata, wsize, NULL, 0);

	return true;
}


bool bmp280_is_measurement_complete(void)
{
	uint8_t wdata[1];
	uint8_t rdata[1];
	uint8_t wsize = 0, rsize;

	wdata[wsize++] = 0xF3; // status register
	rsize = 1;

	i2c_transfer7(I2C1, BMP280_7BIT_ADDR, wdata, wsize, rdata, rsize);

	if((rdata[0] & 0x09) != 0) {
		// either "measuring" or "im_update" is active, so measurement is not complete yet.
		return false;
	} else {
		return true;
	}
}


bool bmp280_readout_and_compensate(void)
{
	uint8_t wdata[1];
	uint8_t rdata[8];
	uint8_t wsize = 0, rsize;

	wdata[wsize++] = 0xF7;
	rsize = 8;

	i2c_transfer7(I2C1, BMP280_7BIT_ADDR, wdata, wsize, rdata, rsize);

	int32_t press_raw =
		((int32_t)rdata[0] << 12)         // press_msb (0xF7)
		| ((int32_t)rdata[1] << 4)        // press_lsb (0xF8)
		| ((int32_t)rdata[2] >> 4);       // press_xlsb (0xF9)

	int32_t temp_raw =
		((int32_t)rdata[3] << 12)         // temp_msb (0xFA)
		| ((int32_t)rdata[4] << 4)        // temp_lsb (0xFB)
		| ((int32_t)rdata[5] >> 4);       // temp_xlsb (0xFC)

	m_last_temperature = bmp280_comp_temperature(temp_raw);
	m_last_pressure = bmp280_comp_pressure(press_raw);

	return true;
}


fxp_t bmp280_get_temperature(void)
{
	return m_last_temperature;
}


fxp_t bmp280_get_pressure(void)
{
	return m_last_pressure;
}