#include <stdio.h>
#include <math.h>
#include "pico/stdlib.h"

const int32_t dig_T1 = 0x6EA0;
const int32_t dig_T2 = (int16_t)0x68A7;
const int32_t dig_T3 = (int16_t)0x0032;

const int32_t dig_P1 = 0x928B; // Maybe negative
const int32_t dig_P2 = (int16_t)0xD66E;
const int32_t dig_P3 = (int16_t)0x0BD0;
const int32_t dig_P4 = (int16_t)0x248B;
const int32_t dig_P5 = (int16_t)0xFF55;
const int32_t dig_P6 = (int16_t)0xFFF9;
const int32_t dig_P7 = (int16_t)0x300C;
const int32_t dig_P8 = (int16_t)0xD120;
const int32_t dig_P9 = (int16_t)0x1388;

// 0x56, 0x01, 0x00, 0x16, 0x2F, 0x03, 0x1E, 0x66, 0x41, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
const int32_t dig_H1 = 0x4B;
const int32_t dig_H2 = (int16_t)0x0156;
const int32_t dig_H3 = 0x00; 
const int32_t dig_H4 = (int16_t)0x16F;
const int32_t dig_H5 = (int16_t)0x032;
const int32_t dig_H6 = 0x1e;

int32_t BME280_compensate_T_int32(int32_t adc_T)
{
  int32_t var1, var2, T;
  var1 = ((((adc_T >> 3) - ((int32_t)dig_T1 << 1))) * ((int32_t)dig_T2)) >> 11;
  var2 = (((((adc_T >> 4) - ((int32_t)dig_T1)) * ((adc_T >> 4) - ((int32_t)dig_T1)))
           >> 12) * ((int32_t)dig_T3)) >> 14;
  int32_t t_fine = var1 + var2;
  T = (t_fine * 5 + 128) >> 8;
  return T;
}

double compensate_temperature(uint32_t adc_T)
{
    double var1;
    double var2;
    double temperature;
    double temperature_min = -40;
    double temperature_max = 85;

    var1 = ((double)adc_T) / 16384.0 - ((double)dig_T1) / 1024.0;
    var1 = var1 * ((double)dig_T2);
    var2 = (((double)adc_T) / 131072.0 - ((double)dig_T2) / 8192.0);
    var2 = (var2 * var2) * ((double)dig_T3);
    double t_fine = (int32_t)(var1 + var2);
    temperature = (var1 + var2) / 5120.0;

    if (temperature < temperature_min)
    {
        temperature = temperature_min;
    } 
    else if (temperature > temperature_max)
    {
        temperature = temperature_max;
    }

    return temperature;
}

uint32_t reverse_compensate_temperature_old(double temperature) 
{
    double sum = temperature * 5120;
    return ((sum / (double)dig_T2) + ((double)dig_T1) / 1024.0) * 16384.0;
}

uint32_t reverse_compensate_temperature(double temperature) 
{
    double sum = temperature * 5120;
    double const2 = ((double)dig_T2) / 8192.0;
    double const3 = (double)dig_T3;
    double a = const3 / (131072.0 * 131072.0);
    double b = 1 / 16384.0 * (double)dig_T2 - 2 * const3 / 131072.0 * const2;
    double c = const3 * const2 * const2 - sum - (double)dig_T1 / 1024.0 * (double)dig_T2;

    double discriminant = b * b - 4 * a * c;
    double root = 
      discriminant > 0 
      ? fmax((-b + sqrt(discriminant)) / (2 * a), (-b - sqrt(discriminant)) / (2 * a))
      : -b / (2 * a);

    return round(root);
}

double tfine(uint32_t adc_T)
{
    double var1;
    double var2;
    double temperature;
    double temperature_min = -40;
    double temperature_max = 85;

    var1 = ((double)adc_T) / 16384.0 - ((double)dig_T1) / 1024.0;
    var1 = var1 * ((double)dig_T2);
    var2 = (((double)adc_T) / 131072.0 - ((double)dig_T2) / 8192.0);
    var2 = (var2 * var2) * ((double)dig_T3);
    return (int32_t)(var1 + var2);
}

uint32_t reverse_tfine(double temperature) 
{
    return temperature * 5120.0;
}

double compensate_pressure(uint32_t adc_P, double t_fine) 
{
    double var1;
    double var2;
    double var3;
    double pressure;
    double pressure_min = 30000.0;
    double pressure_max = 110000.0;

    var1 = ((double)t_fine / 2.0) - 64000.0;
    var2 = var1 * var1 * ((double)dig_P6) / 32768.0;
    var2 = var2 + var1 * ((double)dig_P5) * 2.0;
    var2 = (var2 / 4.0) + (((double)dig_P4) * 65536.0);
    var3 = ((double)dig_P3) * var1 * var1 / 524288.0;
    var1 = (var3 + ((double)dig_P2) * var1) / 524288.0;
    var1 = (1.0 + var1 / 32768.0) * ((double)dig_P1);

    /* avoid exception caused by division by zero */
    if (var1 > (0.0))
    {
        double const1 = 6250.0 / var1;
        double const2 = var2 / 4096.0;
        double const9 = (double)dig_P9 / 2147483648.0;
        double const7 = (double)dig_P7;
        double const8 = (double)dig_P8 / 32768.0;

        double pressureA = 1048576.0 - (double) adc_P;
        double pressureB = (pressureA - const2) * const1;
        var1 = const9 * pressureB * pressureB;
        var2 = pressureB * const8;
        pressure = pressureB + (var1 + var2 + const7) / 16.0;

        // The above is a simplified version of the following code
        /*
        pressure = 1048576.0 - (double) adc_P;
        pressure = (pressure - (var2 / 4096.0)) * 6250.0 / var1;
        var1 = ((double)dig_P9) * pressure * pressure / 2147483648.0;
        var2 = pressure * ((double)dig_P8) / 32768.0;
        pressure = pressure + (var1 + var2 + ((double)dig_P7)) / 16.0;
        */

        if (pressure < pressure_min)
        {
            pressure = pressure_min;
        }
        else if (pressure > pressure_max)
        {
            pressure = pressure_max;
        }
    }
    else /* Invalid case */
    {
        pressure = pressure_min;
    }

    return pressure;
}

uint32_t reverse_compensate_pressure(double pressure, double t_fine) 
{
    double var1;
    double var2;
    double var3;

    var1 = ((double)t_fine / 2.0) - 64000.0;
    var2 = var1 * var1 * ((double)dig_P6) / 32768.0;
    var2 = var2 + var1 * ((double)dig_P5) * 2.0;
    var2 = (var2 / 4.0) + (((double)dig_P4) * 65536.0);
    var3 = ((double)dig_P3) * var1 * var1 / 524288.0;
    var1 = (var3 + ((double)dig_P2) * var1) / 524288.0;
    var1 = (1.0 + var1 / 32768.0) * ((double)dig_P1);

    double const1 = 6250.0 / var1;
    double const2 = var2 / 4096.0;
    double const9 = (double)dig_P9 / 2147483648.0;
    double const7 = (double)dig_P7;
    double const8 = (double)dig_P8 / 32768.0;

    // Solve `const9 / 16.0 * pressureB * pressureB  + ((const8 /16.0 + 1.0) * pressureB) + const7 / 16.0 - pressure = 0`
    // for pressureB:
    double a = const9 / 16.0;
    double b = const8 / 16.0 + 1.0;
    double c = const7 / 16.0 - pressure;

    double discriminant = b * b - 4 * a * c;
    double pressureB = 
      discriminant > 0 
      ? fmax((-b + sqrt(discriminant)) / (2 * a), (-b - sqrt(discriminant)) / (2 * a))
      : -b / (2 * a);
    double pressureA = pressureB / const1 + const2;
    return round(1048576.0 - pressureA);
}

double compensate_humidity(uint32_t adc_H, double t_fine)
{
    double humidity;
    double humidity_min = 0.0;
    double humidity_max = 100.0;
    double var1;
    double var2;
    double var3;
    double var4;
    double var5;
    double var6;

    var1 = ((double)t_fine) - 76800.0;
    var2 = (((double)dig_H4) * 64.0 + (((double)dig_H5) / 16384.0) * var1);
    var3 = adc_H - var2;
    var4 = ((double)dig_H2) / 65536.0;
    var5 = (1.0 + (((double)dig_H3) / 67108864.0) * var1);
    var6 = 1.0 + (((double)dig_H6) / 67108864.0) * var1 * var5;
    double var7 = var3 * var4 * (var5 * var6);
    humidity = var7 * (1.0 - ((double)dig_H1) * var7 / 524288.0);

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

    return humidity;
}

uint32_t reverse_compensate_humidity(double humidity, double t_fine) 
{
  double var1 = ((double)t_fine) - 76800.0;
  double var2 = (((double)dig_H4) * 64.0 + (((double)dig_H5) / 16384.0) * var1);
  double var4 = ((double)dig_H2) / 65536.0;
  double var5 = (1.0 + (((double)dig_H3) / 67108864.0) * var1);
  double var6 = 1.0 + (((double)dig_H6) / 67108864.0) * var1 * var5;
  
  double a = - ((double)dig_H1) / 524288.0;
  double b = 1.0;
  double c = -humidity;
  double discriminant = b * b - 4 * a * c;
  double var7 = 
      discriminant > 0 
      ? fmin((-b + sqrt(discriminant)) / (2 * a), (-b - sqrt(discriminant)) / (2 * a))
      : -b / (2 * a);  
  double var3 = var7 / (var4 * (var5 * var6));

  return round(var3 + var2);
}

uint32_t parse_pressure(uint8_t sample[]) {
  uint32_t data_msb = (uint32_t)sample[0] << 12;
  uint32_t data_lsb = (uint32_t)sample[1] << 4;
  uint32_t data_xlsb = (uint32_t)sample[2] >> 4;
  return data_msb | data_lsb | data_xlsb;
}

uint32_t parse_temperature(uint8_t sample[]) {
  uint32_t data_msb = (uint32_t)sample[3] << 12;
  uint32_t data_lsb = (uint32_t)sample[4] << 4;
  uint32_t data_xlsb = (uint32_t)sample[5] >> 4;
  return data_msb | data_lsb | data_xlsb;
}

uint32_t parse_humidity(uint8_t sample[]) {
  uint32_t data_msb = (uint32_t)sample[6] << 8;
  uint32_t data_lsb = (uint32_t)sample[7];
  return data_msb | data_lsb;
}

int main() {
  stdio_init_all();

  int32_t adc_T = 0x82435;
  printf("Input: %04x\n", adc_T);
  printf("Output: %d\n", BME280_compensate_T_int32(adc_T));
  printf("Output: %f\n", compensate_temperature(adc_T));
  printf("Output: %04x\n", reverse_compensate_temperature(25.69));

  // Test data recorded by Bonny:
  uint8_t sample[] = { 0x46, 0xAA, 0xA0, 0x82, 0x43, 0x50, 0x7A, 0x6C };
  uint32_t pressure_raw = parse_pressure(sample);
  uint32_t temperature_raw = parse_temperature(sample);
  uint32_t humidity_raw = parse_humidity(sample);

  double t_fine = tfine(temperature_raw);
  double temperature = compensate_temperature(temperature_raw);
  printf("Test: compensate_temperature(0X%x) -> %f\n", temperature_raw, temperature);
  printf("   -> reverse_compensature_temperature(%f) -> 0x%x\n", temperature, reverse_compensate_temperature(temperature));
  printf("   -> compensate again -> %f\n\n", compensate_temperature(reverse_compensate_temperature(temperature)));
  
  double pressure = compensate_pressure(pressure_raw, t_fine);
  double reverse_t_fine = reverse_tfine(temperature);
  printf("Test: compensate_pressure(0X%x, %f) -> %f\n", pressure_raw, t_fine, pressure);
  printf("   -> reverse_compensate_pressure(%f, %f) -> 0x%x\n", pressure, reverse_t_fine, reverse_compensate_pressure(pressure, reverse_t_fine));
  printf("   -> compensate again -> %f\n\n", compensate_pressure(reverse_compensate_pressure(pressure, reverse_t_fine), t_fine));

  double humidity = compensate_humidity(humidity_raw, t_fine);
  printf("Test: compensate_humidity(0X%x, %f) -> %f\n", humidity_raw, t_fine, humidity);
  printf("   -> reverse_compensate_humidity(%f, %f) -> 0x%x\n", humidity, reverse_t_fine, reverse_compensate_humidity(humidity, reverse_t_fine));
  printf("   -> compensate again -> %f\n", compensate_humidity(reverse_compensate_humidity(humidity, reverse_t_fine), t_fine));
  
  while (true) {
    sleep_ms(250);
  }
}