#include <bme280.h>
#include <pico/stdlib.h>
#include <stdio.h>
#include <stdlib.h>

/**
 * Funktionen für Die umrechnung mithilfe der Kallibrierungstaten wurden mit
 * ChatGPT erstellt
 *
 * Die gesamte bme280.c musste ich selber schreiben, da für meinen sensor keine
 * Library existiert hat, mit der nan arbeiten konnte (außer in python)
 */

// Internsal declerations
void __bme280_16cmd(i2c_inst_t* i2c, uint16_t cmd);
void __bme280_reset(i2c_inst_t* i2c);
bme280_callib __bme280_read_callib(i2c_inst_t* i2c);
void __bme280_read_buf(i2c_inst_t* i2c, uint8_t what, uint8_t* buf,
                       size_t size);

// Struct functions

void update(struct bme280_ctx_s* self) {
  uint8_t data[8];
  __bme280_read_buf(self->i2c, 0xf7, data, 8);
  self->raw_temperature =
      (int32_t)((data[3] << 12) | (data[4] << 4) | (data[5] >> 4));
  self->raw_pressure =
      (int32_t)((data[0] << 12) | (data[1] << 4) | (data[2] >> 4));
  self->raw_humidity = (int32_t)((data[6] << 8) | data[7]);
}

// Berechnen der Temperatur anhand der Kallibrierungsdatan
float read_temp(struct bme280_ctx_s* self) {
  int32_t var1 =
      ((((self->raw_temperature >> 3) - ((int32_t)self->callib.dig_T1 << 1))) *
       ((int32_t)self->callib.dig_T2)) >>
      11;
  int32_t var2 =
      (((((self->raw_temperature >> 4) - ((int32_t)self->callib.dig_T1)) *
         ((self->raw_temperature >> 4) - ((int32_t)self->callib.dig_T1))) >>
        12) *
       ((int32_t)self->callib.dig_T3)) >>
      14;
  self->t_fine = var1 + var2;
  float T = (self->t_fine * 5 + 128) >> 8;
  return T / 100.0f;
}

// Berechnen der Luftfeuchtigkeit anhand der Kallibrierungsdatan
float read_humidity(struct bme280_ctx_s* self) {
  int32_t v_x1_u32r = self->t_fine - 76800;
  v_x1_u32r =
      (((((self->raw_humidity << 14) - (((int32_t)self->callib.dig_H4) << 20) -
          (((int32_t)self->callib.dig_H5) * v_x1_u32r)) +
         16384) >>
        15) *
       (((((((v_x1_u32r * ((int32_t)self->callib.dig_H6)) >> 10) *
            (((v_x1_u32r * ((int32_t)self->callib.dig_H3)) >> 11) + 32768)) >>
           10) +
          2097152) *
             ((int32_t)self->callib.dig_H2) +
         8192) >>
        14));
  v_x1_u32r = v_x1_u32r - (((((v_x1_u32r >> 15) * (v_x1_u32r >> 15)) >> 7) *
                            ((int32_t)self->callib.dig_H1)) >>
                           4);
  v_x1_u32r = (v_x1_u32r < 0) ? 0 : v_x1_u32r;
  v_x1_u32r = (v_x1_u32r > 419430400) ? 419430400 : v_x1_u32r;
  return (float)(v_x1_u32r >> 12) / 1024.0f;
}

// Berechnen des Luftdrucks? anhand der Kallibrierungsdatan
float read_pressure(struct bme280_ctx_s* self) {
  int64_t var1 = ((int64_t)self->t_fine) - 128000;
  int64_t var2 = var1 * var1 * (int64_t)self->callib.dig_P6;
  var2 = var2 + ((var1 * (int64_t)self->callib.dig_P5) << 17);
  var2 = var2 + (((int64_t)self->callib.dig_P4) << 35);
  var1 = ((var1 * var1 * (int64_t)self->callib.dig_P3) >> 8) +
         ((var1 * (int64_t)self->callib.dig_P2) << 12);
  var1 = (((((int64_t)1) << 47) + var1)) * ((int64_t)self->callib.dig_P1) >> 33;

  if (var1 == 0) {
    return 0;  // Vermeidung Division durch Null
  }
  int64_t p = 1048576 - self->raw_pressure;
  p = (((p << 31) - var2) * 3125) / var1;
  var1 = (((int64_t)self->callib.dig_P9) * (p >> 13) * (p >> 13)) >> 25;
  var2 = (((int64_t)self->callib.dig_P8) * p) >> 19;
  p = ((p + var1 + var2) >> 8) + (((int64_t)self->callib.dig_P7) << 4);
  return (float)p / 25600.0f;
}

// public functions
bme280_ctx* bme280_init(i2c_inst_t* i2c, uint sda, uint scl) {
  if (!i2c) return NULL;
  bme280_ctx* ret = (bme280_ctx*)malloc(sizeof(bme280_ctx));
  if (!ret) return NULL;
  // Init I2C and GPIO Pins
  i2c_init(i2c, 100 * 1000);
  gpio_set_function(sda, GPIO_FUNC_I2C);
  gpio_set_function(scl, GPIO_FUNC_I2C);
  gpio_pull_up(sda);
  gpio_pull_up(scl);
  // Setup the Sensor
  __bme280_reset(i2c);
  ret->callib = __bme280_read_callib(i2c);
  __bme280_16cmd(i2c, 0xf201);
  __bme280_16cmd(i2c, 0xf427);
  __bme280_16cmd(i2c, 0xf5a0);

  ret->update = update;
  ret->read_humidity = read_humidity;
  ret->read_temp = read_temp;
  ret->read_pressure = read_pressure;

  ret->i2c = i2c;
  return ret;
}

void bme280_deinit(bme280_ctx* ctx) { free(ctx); }

// Internal Functions

void __bme280_reset(i2c_inst_t* i2c) {
  // Reset Command
  uint8_t cmd[2] = {0xE0, 0xB6};
  i2c_write_blocking(i2c, BME280_ADDR, cmd, 2, false);
  sleep_ms(100);  // Wait
}

bme280_callib __bme280_read_callib(i2c_inst_t* i2c) {
  bme280_callib ret;
  // Read from sensor
  uint8_t buf[0x1a];  // Size 26
  __bme280_read_buf(i2c, 0x88, buf, 0x18);
  __bme280_read_buf(i2c, 0xa1, &ret.dig_H1, 1);
  uint8_t buf_h[0x7];
  __bme280_read_buf(i2c, 0xe1, buf_h, 7);

  // Get Temp Callib out of Data
  ret.dig_T1 = (uint16_t)(buf[1] << 8 | buf[0]);
  ret.dig_T2 = (int16_t)(buf[3] << 8 | buf[2]);
  ret.dig_T3 = (int16_t)(buf[5] << 8 | buf[4]);

  // Get pressure Data
  ret.dig_P1 = (uint16_t)(buf[7] << 8 | buf[6]);
  ret.dig_P2 = (int16_t)(buf[9] << 8 | buf[8]);
  ret.dig_P3 = (int16_t)(buf[11] << 8 | buf[10]);
  ret.dig_P4 = (int16_t)(buf[13] << 8 | buf[12]);
  ret.dig_P5 = (int16_t)(buf[15] << 8 | buf[14]);
  ret.dig_P6 = (int16_t)(buf[17] << 8 | buf[16]);
  ret.dig_P7 = (int16_t)(buf[19] << 8 | buf[18]);
  ret.dig_P8 = (int16_t)(buf[21] << 8 | buf[20]);
  ret.dig_P9 = (int16_t)(buf[23] << 8 | buf[22]);

  // Get humidity callib data
  ret.dig_H2 = (int16_t)(buf_h[1] << 8 | buf_h[0]);
  ret.dig_H3 = buf_h[2];
  ret.dig_H4 = (int16_t)((buf_h[3] << 4) | (buf_h[4] & 0x0F));
  ret.dig_H5 = (int16_t)((buf_h[5] << 4) | (buf_h[4] >> 4));
  ret.dig_H6 = (int8_t)buf_h[6];
  return ret;
}

void __bme280_16cmd(i2c_inst_t* i2c, uint16_t cmd) {
  // Could do this as well
  // i2c_write_blocking(i2c, BME280_ADDR, (uint8_t*)&cmd, 2, false);
  // Simply extracting into a 2byte buffer
  uint8_t _cmd[2] = {(cmd >> 8) & 0xFF, cmd & 0xff};
  i2c_write_blocking(i2c, BME280_ADDR, _cmd, 2, false);
}

void __bme280_read_buf(i2c_inst_t* i2c, uint8_t what, uint8_t* buf,
                       size_t size) {
  i2c_write_blocking(i2c, BME280_ADDR, &what, 1, true);
  i2c_read_blocking(i2c, BME280_ADDR, buf, size, false);
}