#include "bme280.h"
#include "hardware/i2c.h"
#include "pico/binary_info.h"
#include "pico/stdlib.h"
#include <stdint.h>
#include <stdio.h>
/*
    Lib for Bosh BME280 Env Sensor. To be used in weather station context.
    Sources:
        - pico example for bmp280 a simpler sensor then bme280 that does not
        include humidity
   https://github.com/raspberrypi/pico-examples/blob/master/i2c/bmp280_i2c/bmp280_i2c.c
        - bme280 datasheet
   https://cdn.shopify.com/s/files/1/0174/1800/files/bst-bme280-ds002.pdf?v=1662743150
        - bmp280 datasheet
   https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bmp280-ds001.pdf
        - pimoroni eshop
   https://shop.pimoroni.com/products/bme280-breakout?variant=29420960677971
        - pimoroni pico sdk
   https://github.com/boschsensortec/BME280_SensorAPI/tree/c47f06eb44fc96970f0abfcc941ec16425b2a9e6

   Suggested Settings:
    - forced mode at 1 sample per min
    - oversampling * 1 on temp, humidity, and pressure
    - IIR Filter off
*/

// Define the custom pins for I2C
#define I2C_SDA_PIN 14 // Replace with your custom SDA pin
#define I2C_SCL_PIN 15 // Replace with your custom SCL pin

// Define the I2C instance to use (i2c0 or i2c1)
#define I2C_PORT i2c1

// device has default bus address of 0x76
#define ADDR _u(0x76)

// hardware registers
#define REG_CONFIG _u(0xF5)
#define REG_CTRL_MEAS _u(0xF4)
// #define REG_RESET _u(0xE0)

// #define REG_TEMP_XLSB _u(0xFC)
// #define REG_TEMP_LSB _u(0xFB)
// #define REG_TEMP_MSB _u(0xFA)

// #define REG_PRESSURE_XLSB _u(0xF9)
// #define REG_PRESSURE_LSB _u(0xF8)
// #define REG_PRESSURE_MSB _u(0xF7)

static void bmp280_get_compensation_params(i2c_inst_t *i2c,
                                           bme280_compensation_params *params) {
  // raw temp and pressure values need to be calibrated according to
  // parameters generated during the manufacturing of the sensor
  // there are 3 temperature params, and 9 pressure params, each with a LSB
  // and MSB register, so we read from 24 registers

  // read the first 24 compensation params
  uint8_t buf[25] = {0};
  uint8_t reg = 0x88;
  i2c_write_blocking(i2c, ADDR, &reg, 1, true); // true to keep master control of bus
  i2c_read_blocking(i2c, ADDR, buf, 25, false);

  params->dig_t1 = (uint16_t)(buf[1] << 8) | buf[0];
  params->dig_t2 = (int16_t)(buf[3] << 8) | buf[2];
  params->dig_t3 = (int16_t)(buf[5] << 8) | buf[4];

  params->dig_p1 = (uint16_t)(buf[7] << 8) | buf[6];
  params->dig_p2 = (int16_t)(buf[9] << 8) | buf[8];
  params->dig_p3 = (int16_t)(buf[11] << 8) | buf[10];
  params->dig_p4 = (int16_t)(buf[13] << 8) | buf[12];
  params->dig_p5 = (int16_t)(buf[15] << 8) | buf[14];
  params->dig_p6 = (int16_t)(buf[17] << 8) | buf[16];
  params->dig_p7 = (int16_t)(buf[19] << 8) | buf[18];
  params->dig_p8 = (int16_t)(buf[21] << 8) | buf[20];
  params->dig_p9 = (int16_t)(buf[23] << 8) | buf[22];
  params->dig_h1 = (int16_t)buf[24];

  // read second set of compensation params
  reg = 0xE1;
  i2c_write_blocking(i2c, ADDR, &reg, 1, true); // true to keep master control of bus
  i2c_read_blocking(i2c, ADDR, buf, 7, false); // false, we're done reading
  params->dig_h2 = (int16_t)(int16_t)(buf[1] << 8) | buf[0];
  params->dig_h3 = buf[2];
  int16_t dig_h4_msb = (int16_t)(int8_t)buf[3] * 16;
  int16_t dig_h4_lsb = (int16_t)(buf[4] & 0x0F);
  params->dig_h4 = dig_h4_msb | dig_h4_lsb;
  int16_t dig_h5_msb = (int16_t)(int8_t)buf[5] * 16;
  int16_t dig_h5_lsb = (int16_t)(buf[4] >> 4);
  params->dig_h5 = dig_h5_msb | dig_h5_lsb;
  params->dig_h6 = (int8_t)buf[6];
}

// Main Config for the sensor. Defined here so it can be use to inital configure
// the sensor and to proc further reads 001 sets osrs_t temp over sampling to 1,
// 001 sets osrs_p pressure oversampling to 1, 01 sets sensor mode to forced
static const uint8_t main_config = 0b00100101;
void bme280_init(bme280_config *config, i2c_inst_t *i2c, uint8_t sda_pin,
                 uint8_t scl_pin) {
  i2c_init(i2c, 100 * 1000);
  gpio_set_function(sda_pin, GPIO_FUNC_I2C);
  gpio_set_function(scl_pin, GPIO_FUNC_I2C);
  gpio_pull_up(sda_pin);
  gpio_pull_up(scl_pin);

  uint8_t buf[2];

  // 000 for t_sb is normal mode sample rate n/a, 000 turns filter off, 0 turns
  // off spi
  buf[0] = 0xF5; // config
  buf[1] = 0x00; // all zeros
  i2c_write_blocking(i2c, ADDR, buf, 2, false);

  // send humidity oversample config
  // 001 at the end sets over sampling to 1
  const uint8_t humidity_config = 0b00000001;
  buf[0] = 0xF2; // ctrl_hum
  buf[1] = humidity_config;
  i2c_write_blocking(i2c, ADDR, buf, 2, false);

  // send temp oversample, pressure oversample, and mode configs
  buf[0] = 0xF4; // ctrl_meas
  buf[1] = main_config;
  i2c_write_blocking(i2c, ADDR, buf, 2, false);

  bme280_compensation_params params;
  bmp280_get_compensation_params(i2c, &params);

  config->params = params;
  config->i2c = i2c;
  return;
}

/**
 *Instructs the BME280 sensor to take a measurement in forced mode.
 */
static void bme280_proc_sensor_read(bme280_config *config) {
  uint8_t buf[2] = {0xF4, main_config};
  i2c_write_blocking(config->i2c, ADDR, buf, 2, false);
}

static void bme280_read_raw(bme280_config *config, int32_t *temp,
                            int32_t *pressure, int32_t *humidity) {
  // TODO: burst read 0xF7 0xFE
  // pressure 20 bit, temp 20 bit, humidity 16 bit
  uint8_t buf[8];
  uint8_t reg = 0xF7;
  i2c_write_blocking(config->i2c, ADDR, &reg, 1,
                     true); // true to keep master control of bus
  i2c_read_blocking(config->i2c, ADDR, buf, 8,
                    false); // false - finished with bus

  // store the 20 bit read in a 32 bit signed integer for conversion
  *pressure = (buf[0] << 12) | (buf[1] << 4) | (buf[2] >> 4);
  *temp = (buf[3] << 12) | (buf[4] << 4) | (buf[5] >> 4);
  *humidity = (buf[6] << 8) | buf[7];
}

static int32_t bme280_caculate_t_fine(bme280_config *config, int32_t temp) {
  bme280_compensation_params params = config->params;
  int32_t var1, var2;
  var1 = ((((temp >> 3) - ((int32_t)params.dig_t1 << 1))) *
          ((int32_t)params.dig_t2)) >>
         11;
  var2 = (((((temp >> 4) - ((int32_t)params.dig_t1)) *
            ((temp >> 4) - ((int32_t)params.dig_t1))) >>
           12) *
          ((int32_t)params.dig_t3)) >>
         14;
  return var1 + var2;
}

static float bme280_convert_temp(int32_t t_fine) {
  int32_t temp = (t_fine * 5 + 128) >> 8;
  return (float)temp / 100;
}

static float bme280_convert_pressure(bme280_config *config, int32_t t_fine,
                                     int32_t pressure) {
  bme280_compensation_params params = config->params;
  int32_t var1, var2;
  uint32_t converted = 0.0;
  var1 = (((int32_t)t_fine) >> 1) - (int32_t)64000;
  var2 = (((var1 >> 2) * (var1 >> 2)) >> 11) * ((int32_t)params.dig_p6);
  var2 += ((var1 * ((int32_t)params.dig_p5)) << 1);
  var2 = (var2 >> 2) + (((int32_t)params.dig_p4) << 16);
  var1 = (((params.dig_p3 * (((var1 >> 2) * (var1 >> 2)) >> 13)) >> 3) +
          ((((int32_t)params.dig_p2) * var1) >> 1)) >>
         18;
  var1 = ((((32768 + var1)) * ((int32_t)params.dig_p1)) >> 15);
  if (var1 == 0) {
    return 0; // avoid exception caused by division by zero
  }
  converted =
      (((uint32_t)(((int32_t)1048576) - pressure) - (var2 >> 12))) * 3125;
  if (converted < 0x80000000) {
    converted = (converted << 1) / ((uint32_t)var1);
  } else {
    converted = (converted / (uint32_t)var1) * 2;
  }
  var1 = (((int32_t)params.dig_p9) *
          ((int32_t)(((converted >> 3) * (converted >> 3)) >> 13))) >>
         12;
  var2 = (((int32_t)(converted >> 2)) * ((int32_t)params.dig_p8)) >> 13;
  converted =
      (uint32_t)((int32_t)converted + ((var1 + var2 + params.dig_p7) >> 4));
  return (float)converted / 100;
}

static float bmp280_convert_humidity(bme280_config *config, int32_t t_fine,
                                     int32_t raw_humidity) {
  double humidity_min = 0.0;
  double humidity_max = 100.0;
  double humidity, var1, var2, var3, var4, var5, var6;

  bme280_compensation_params params = config->params;
  var1 = ((double)t_fine) - 76800.0;
  var2 = (((double)params.dig_h4) * 64.0 +
          (((double)params.dig_h5) / 16384.0) * var1);
  var3 = raw_humidity - var2;
  var4 = ((double)params.dig_h2) / 65536.0;
  var5 = (1.0 + (((double)params.dig_h3) / 67108864.0) * var1);
  var6 = 1.0 + (((double)params.dig_h6) / 67108864.0) * var1 * var5;
  var6 = var3 * var4 * (var5 * var6);
  humidity = var6 * (1.0 - ((double)params.dig_h1) * var6 / 524288.0);

  if (humidity > humidity_max) {
    humidity = humidity_max;
  } else if (humidity < humidity_min) {
    humidity = humidity_min;
  }

  return (float)humidity;
}

bme280_reading bme280_read(bme280_config *config) {
  // instruct sensor to make a reading since we are in forced mode
  bme280_proc_sensor_read(config);

  int32_t raw_temperature = 0;
  int32_t raw_pressure = 0;
  int32_t raw_humidity = 0;
  bme280_read_raw(config, &raw_temperature, &raw_pressure, &raw_humidity);
  bme280_reading reading;
  int32_t t_fine = bme280_caculate_t_fine(config, raw_temperature);
  reading.temperature = bme280_convert_temp(t_fine);
  reading.pressure = bme280_convert_pressure(config, t_fine, raw_pressure);
  reading.humidity = bmp280_convert_humidity(config, t_fine, raw_humidity);
  return reading;
}