/*
 * sensors.c
 *
 *  Created on: 20 Apr 2020
 *      Author: Chris
 */

#include "sensors.h"

static struct bme280_dev* dev;
static struct bme280_dev* dev2;

void i2c_setup()
{
	printf("Setting up I2C driver on port 1... ");
	i2c_config_t config;
	config.mode = I2C_MODE_MASTER;
	config.sda_io_num = 33;
	config.sda_pullup_en = GPIO_PULLUP_ENABLE;
	config.scl_io_num = 32;
	config.scl_pullup_en = GPIO_PULLUP_ENABLE;
	config.master.clk_speed = 100000;
	i2c_param_config(I2C_NUM_0, &config);
	printf("Set driver parameters... ");
	esp_err_t err = i2c_driver_install(I2C_NUM_0, I2C_MODE_MASTER, 0, 0, 0);
	if (err == ESP_OK)
		printf("Driver installed!\n");
	else if (err == ESP_ERR_INVALID_ARG)
		printf("Driver install failed, invalid arguments!\n");
	else
		printf("Driver install failed!\n");
}

int8_t i2c_read(uint8_t dev_id, uint8_t reg_addr, uint8_t *data, uint16_t len) {
	i2c_cmd_handle_t cmd = i2c_cmd_link_create();
	i2c_master_start(cmd);
	i2c_master_write_byte(cmd, dev_id << 1 | I2C_MASTER_WRITE, 1);
	i2c_master_write_byte(cmd, reg_addr, 1);
	i2c_master_start(cmd);
	i2c_master_write_byte(cmd, dev_id << 1 | I2C_MASTER_READ, 1);
	if (len > 1) {
		i2c_master_read(cmd, data, len - 1, I2C_MASTER_ACK);
	}
	i2c_master_read_byte(cmd, data + len - 1, I2C_MASTER_NACK);
	i2c_master_stop(cmd);
	i2c_master_cmd_begin(I2C_NUM_0, cmd, 500 / portTICK_RATE_MS);
	i2c_cmd_link_delete(cmd);
	return 0;
}

int8_t i2c_write(uint8_t dev_id, uint8_t reg_addr, uint8_t *data, uint16_t len) {
	//printf("Writing to bus: dev_id=%x, reg_addr=%x, data=%p, length=%u\n", dev_id, reg_addr, data, len);
	i2c_cmd_handle_t cmd = i2c_cmd_link_create();
	i2c_master_start(cmd);
	i2c_master_write_byte(cmd, (dev_id << 1) | I2C_MASTER_WRITE, 1);
	i2c_master_write_byte(cmd, reg_addr, 1);
	i2c_master_write(cmd, data, len, 1);
	i2c_master_stop(cmd);
	i2c_master_cmd_begin(I2C_NUM_0, cmd, 500 / portTICK_RATE_MS);
	i2c_cmd_link_delete(cmd);
	return 0;
}

void i2c_delay(uint32_t period) {
	vTaskDelay(period / portTICK_PERIOD_MS);
}

void i2c_shutdown()
{
	printf("Shutting down I�C bus... ");
	esp_err_t err = i2c_driver_delete(I2C_NUM_0);
	if (err == ESP_ERR_INVALID_ARG)
		printf("Failed, invalid arguments!\n");
	else
		printf("Success!\n");
}

void read_sensor(int32_t* temp, uint32_t* pressure, uint32_t* humidity) {

	uint8_t settings_sel;
	uint32_t req_delay;
	struct bme280_data comp_data;
	dev->settings.osr_h = BME280_OVERSAMPLING_16X;
	dev->settings.osr_p = BME280_OVERSAMPLING_16X;
	dev->settings.osr_t = BME280_OVERSAMPLING_16X;
	dev->settings.filter = BME280_FILTER_COEFF_16;

	settings_sel = BME280_OSR_PRESS_SEL | BME280_OSR_TEMP_SEL | BME280_OSR_HUM_SEL | BME280_FILTER_SEL;
	bme280_set_sensor_settings(settings_sel, dev);

	req_delay = 12*bme280_cal_meas_delay(&(dev->settings));

	bme280_set_sensor_mode(BME280_FORCED_MODE, dev);
	dev->delay_ms(req_delay / portTICK_PERIOD_MS);
	bme280_get_sensor_data(BME280_ALL, &comp_data, dev);
	*temp = comp_data.temperature;
	*pressure = comp_data.pressure;
	*humidity = comp_data.humidity;
}

void read_sensor2(int32_t* temp, uint32_t* pressure, uint32_t* humidity) {

	uint8_t settings_sel;
	uint32_t req_delay;
	struct bme280_data comp_data;

	dev2->settings.osr_h = BME280_OVERSAMPLING_16X;
	dev2->settings.osr_p = BME280_OVERSAMPLING_16X;
	dev2->settings.osr_t = BME280_OVERSAMPLING_16X;
	dev2->settings.filter = BME280_FILTER_COEFF_16;

	settings_sel = BME280_OSR_PRESS_SEL | BME280_OSR_TEMP_SEL | BME280_OSR_HUM_SEL | BME280_FILTER_SEL;
	bme280_set_sensor_settings(settings_sel, dev2);

	req_delay = 12*bme280_cal_meas_delay(&(dev2->settings));

	bme280_set_sensor_mode(BME280_FORCED_MODE, dev2);
	dev2->delay_ms(req_delay / portTICK_PERIOD_MS);
	bme280_get_sensor_data(BME280_ALL, &comp_data, dev2);
	*temp = comp_data.temperature;
	*pressure = comp_data.pressure;
	*humidity = comp_data.humidity;
}

void init_sensors()
{
	// INIT SENSOR
	i2c_setup();
	dev = malloc(sizeof(struct bme280_dev));
	dev->dev_id = 0x76;
	dev->intf = BME280_I2C_INTF;
	dev->read = i2c_read;
	dev->write = i2c_write;
	dev->delay_ms = i2c_delay;
	bme280_init(dev);

	// INIT SENSOR2
	dev2 = malloc(sizeof(struct bme280_dev));
	dev2->dev_id = 0x77;
	dev2->intf = BME280_I2C_INTF;
	dev2->read = i2c_read;
	dev2->write = i2c_write;
	dev2->delay_ms = i2c_delay;
	bme280_init(dev2);
}