#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)

// calibration registers
#define REG_DIG_T1_LSB _u(0x88)
// #define REG_DIG_T1_MSB _u(0x89)
// #define REG_DIG_T2_LSB _u(0x8A)
// #define REG_DIG_T2_MSB _u(0x8B)
// #define REG_DIG_T3_LSB _u(0x8C)
// #define REG_DIG_T3_MSB _u(0x8D)
// #define REG_DIG_P1_LSB _u(0x8E)
// #define REG_DIG_P1_MSB _u(0x8F)
// #define REG_DIG_P2_LSB _u(0x90)
// #define REG_DIG_P2_MSB _u(0x91)
// #define REG_DIG_P3_LSB _u(0x92)
// #define REG_DIG_P3_MSB _u(0x93)
// #define REG_DIG_P4_LSB _u(0x94)
// #define REG_DIG_P4_MSB _u(0x95)
// #define REG_DIG_P5_LSB _u(0x96)
// #define REG_DIG_P5_MSB _u(0x97)
// #define REG_DIG_P6_LSB _u(0x98)
// #define REG_DIG_P6_MSB _u(0x99)
// #define REG_DIG_P7_LSB _u(0x9A)
// #define REG_DIG_P7_MSB _u(0x9B)
// #define REG_DIG_P8_LSB _u(0x9C)
// #define REG_DIG_P8_MSB _u(0x9D)
// #define REG_DIG_P9_LSB _u(0x9E)
// #define REG_DIG_P9_MSB _u(0x9F)

// number of calibration registers to be read
// 17 * 2  = 34
#define NUM_COMPENSATION_PARAMS 34

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

  uint8_t buf[NUM_COMPENSATION_PARAMS] = {0};
  uint8_t reg = REG_DIG_T1_LSB;
  i2c_write_blocking(i2c, ADDR, &reg, 1,
                     true); // true to keep master control of bus
  // read in one go as register addresses auto-increment
  i2c_read_blocking(i2c, ADDR, buf, NUM_COMPENSATION_PARAMS,
                    false); // false, we're done reading

  // store these in a struct for later use
  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[25] << 8) | buf[24];
  params->dig_h2 = (int16_t)(buf[27] << 8) | buf[26];
  params->dig_h3 = (int16_t)(buf[29] << 8) | buf[28];
  params->dig_h4 = (int16_t)(buf[31] << 8) | buf[30];
  params->dig_h5 = (int16_t)(buf[33] << 8) | buf[32];
}

// 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[6];
  uint8_t reg = REG_PRESSURE_MSB;
  i2c_write_blocking(config->i2c, ADDR, &reg, 1,
                     true); // true to keep master control of bus
  i2c_read_blocking(config->i2c, ADDR, buf, 6,
                    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);
}

float bmp280_convert_temp(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;
  int32_t var3 = var1 + var2;
  // uses the BMP280 calibration parameters to compensate the temperature value
  // read from its registers
  int32_t var4 = (var3 * 5 + 128) >> 8;
  return (float)var4 / 100;
}

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;
  reading.temperature = bmp280_convert_temp(config, raw_temperature);
  reading.pressure = raw_pressure;
  reading.humidity = raw_humidity;
  return reading;
}