//
// Sensors: Reads pressure and e-compass sensors over I2C and stores readings in queryable internal data structures
//

#include "pressure.h"
#include "stm32f0xx_hal.h"


#include "config.h"


// Static variables
static I2C_HandleTypeDef hi2c1;

static int32_t pressure_hPa = 0;
static int32_t temp_celsius = 0;

static uint8_t sensors_pressurebuf[8]; // Buffer for i2c rx pressure data
static uint8_t sensors_pressure_freshdata = 0; // If pressure sensor has fresh data


// Initialize pressure sensor and E-compass
void pressure_init(void)
{
	GPIO_InitTypeDef GPIO_InitStruct;

	// I2C Pins
    GPIO_InitStruct.Pin = PIN_SENSORS_SCL|PIN_SENSORS_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_SENSORS, &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);

    // Blocking initialization of sensors

	// Power on the pressure sensor and configure //////////////////////////////////////////////////
	uint8_t temp[2];
	temp[0] = PRESSURE_CTRL1_PWRUP | PRESSURE_CTRL1_7HZ;
	HAL_Delay(10);
	res = HAL_I2C_Mem_Write(&hi2c1, PRESSURE_ADDRESS, 0x20, 1, temp, 1, 500);
}


// Initiate interrupt-based read of engineering sensors
void pressure_read(void)
{
	// Start reading pressure sensor
	HAL_Delay(10);
	volatile  HAL_StatusTypeDef res = HAL_I2C_Mem_Read(&hi2c1, PRESSURE_ADDRESS, PRESSURE_PRESS_REGXL | PRESSURE_AUTOINC, 1, sensors_pressurebuf, 5, 500);

	// Consume fresh data flag
	sensors_pressure_freshdata = 0;

	// Convert received bytes to pressure in hPa
	pressure_hPa = sensors_pressurebuf[0]<<8 | sensors_pressurebuf[1]<<16 | sensors_pressurebuf[2]<<24;
	pressure_hPa >>= 8; // Sign extend
	pressure_hPa /= 4096; // convert to hPa

	// Convert received bytes to temperature in Celsius
	int16_t temp_out = (sensors_pressurebuf[3] | sensors_pressurebuf[4]<<8);
	if(!temp_out)
		temp_celsius = 42;
	else
		temp_celsius = 42.5 + temp_out / 480.0;

}


// Get current temperature in Celsius
int32_t pressure_gettemp(void)
{
	return temp_celsius;
}


// Get current pressure in hPa
int32_t pressure_getpressure(void)
{
	return pressure_hPa;
}



inline I2C_HandleTypeDef* pressure_get_i2c_handle(void)
{
	return &hi2c1;
}