#include "bme280.h"
#include "bme280_defs.h"
#include "driver/i2c.h"
#include <u8g2.h>
#include "u8g2_esp32_hal.h"
#include <esp_log.h>

void i2c_setup()
{
	printf("Setting up I²C driver... ");
	//i2c_driver_install(0, I2C_MODE_MASTER, 0, 0, 0);
	i2c_config_t config;
	config.mode = I2C_MODE_MASTER;
	config.sda_io_num = 18;
	config.sda_pullup_en = GPIO_PULLUP_ENABLE;
	config.scl_io_num = 19;
	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");
}

void i2c_detect()
{
	printf("Scanning I²C bus:\n");
	uint8_t address;
	printf("     0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f\r\n");
	for (int i = 0; i < 128; i += 16) {
		printf("%02x: ", i);
		for (int j = 0; j < 16; j++) {
			fflush(stdout);
			address = i + j;
			i2c_cmd_handle_t cmd = i2c_cmd_link_create();
			i2c_master_start(cmd);
			i2c_master_write_byte(cmd, (address << 1) | I2C_MASTER_WRITE, 0x1);
			i2c_master_stop(cmd);
			esp_err_t ret = i2c_master_cmd_begin(I2C_NUM_0, cmd, 50 / portTICK_RATE_MS);
			i2c_cmd_link_delete(cmd);
			if (ret == ESP_OK) {
				printf("%02x ", address);
			} else if (ret == ESP_ERR_TIMEOUT) {
				printf("UU ");
			} else {
				printf("-- ");
			}
		}
		printf("\r\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(struct bme280_dev* dev, int32_t* temp, uint32_t* pressure, uint32_t* humidity) {

	uint8_t settings_sel;
	uint32_t req_delay;
	struct bme280_data comp_data;

	/* Recommended mode of operation: Indoor navigation */
	dev->settings.osr_h = BME280_OVERSAMPLING_1X;
	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);
	/*Calculate the minimum delay required between consecutive measurement based upon the sensor enabled
		 *  and the oversampling configuration. */
	req_delay = 12*bme280_cal_meas_delay(&(dev->settings));
	printf("req_delay=%i\r\n", req_delay);

	/* Continuously stream sensor data */
	bme280_set_sensor_mode(BME280_FORCED_MODE, dev);
	/* Wait for the measurement to complete and print data @25Hz */
	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 print_data(u8g2_t* u8g2, int32_t temp, uint32_t pressure, uint32_t humidity) {
	int32_t temp1 = temp / 100;
	int32_t temp2 = (abs(temp) % 100) / 10;
	uint32_t press1 = pressure / 100;
	uint32_t humid1 = humidity / 1024;
	uint32_t humid2 = (humidity - humid1*1024) * 10 / 1024;
	//temp1 = 12;
	//temp2 = 3;
	int32_t temp3 = -1*temp1;
	int32_t temp4 = temp2;
	//press1 = 9999;
	//humid1 = 13;
	//humid2 = 4;
	char temp1str[31];
	char temp1str2[32];
	sprintf(temp1str, "%d,%d", temp1, temp2);
	if (temp1 < 10) {
		sprintf(temp1str2, " %s", temp1str);
	} else {
		sprintf(temp1str2, "%s", temp1str);
	}
	char temp2str[31];
	char temp2str2[33];
	sprintf(temp2str, "%d,%d", temp3, temp4);
	uint8_t spaces = 0;
	if (temp3 > 0)
		spaces++;
	if (abs(temp3) < 10)
		spaces++;
	if (spaces == 2)
		sprintf(temp2str2, "  %s", temp2str);
	else if (spaces == 1)
		sprintf(temp2str2, " %s", temp2str);
	else {
		sprintf(temp2str2, "%s", temp2str);
	}
	char tempstr[70];
	sprintf(tempstr, "%s %s  °C", temp1str2, temp2str2);
	u8g2_ClearBuffer(u8g2);

	char pressstr[27];
	char humidstr[27];

	sprintf(pressstr, "%d  %d hPa", press1, press1);
	sprintf(humidstr, "%d,%d  %d,%d   %%", humid1, humid2, humid1, humid2);

	u8g2_SetFont(u8g2, u8g2_font_profont17_mf);
	int8_t fontheight = u8g2_GetAscent(u8g2);
	int8_t fontmargin = abs(u8g2_GetDescent(u8g2))+2;
	u8g2_DrawStr(u8g2, 0, fontheight, " IN    OUT   ");
	u8g2_DrawStr(u8g2, 0, 2*fontheight + fontmargin, tempstr);
	u8g2_DrawStr(u8g2, 0, 3*fontheight + 2*fontmargin, humidstr);
	u8g2_DrawStr(u8g2, 0, 4*fontheight + 3*fontmargin, pressstr);
	u8g2_SendBuffer(u8g2);
}

void app_main(void)
{
	int32_t temp = 0;
	uint32_t pressure = 0;
	uint32_t humidity = 0;

	// INIT SENSOR
	i2c_setup();
	struct bme280_dev 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 DISPLAY
	u8g2_esp32_hal_t u8g2_esp32_hal = U8G2_ESP32_HAL_DEFAULT;
	u8g2_esp32_hal.sda   = 18;
	u8g2_esp32_hal.scl  = 19;

	u8g2_esp32_hal_init(u8g2_esp32_hal);
	u8g2_t u8g2;
	u8g2_Setup_ssd1306_i2c_128x64_vcomh0_f(&u8g2, U8G2_R0, u8g2_esp32_i2c_byte_cb,
		u8g2_esp32_gpio_and_delay_cb);
	u8x8_SetI2CAddress(&u8g2.u8x8,0x3C << 1);
	u8g2_InitDisplay(&u8g2); // send init sequence to the display, display is in sleep mode after this,
	u8g2_SetPowerSave(&u8g2, 0); // wake up display

	while (1) {
		read_sensor(&dev, &temp, &pressure, &humidity);
		printf("%i °c, %i hPa, %i %%\r\n", temp, pressure, humidity);
		print_data(&u8g2, temp, pressure, humidity);
		vTaskDelay(250 / portTICK_PERIOD_MS);
	}
}