// Testing custom gy-63 breakout board (BMP280 chip)
//    with and ESP32.
// No libraries except standard arduino.h and wire.h
#include <Wire.h>
#include <Preferences.h>

#define GYRO_ID 0x68
// BMP-280 device id default is 0x77, if CSB is tied to Vcc the device id is 0x76.
#define BAROMETER_ID 0x77
#define SDA 21
#define SCL 22

Preferences preferences;

// Copy of the BMP280's status register
union
{
  struct
  {
    uint8_t im_update : 1;
    uint8_t : 2;
    uint8_t measuring : 1;
  } bit;
  uint8_t reg;
} status = {.reg = 0};
// The BMP280 compensation trim parameters (coefficients)
struct
{
  uint16_t dig_T1;
  int16_t dig_T2;
  int16_t dig_T3;
  uint16_t dig_P1;
  int16_t dig_P2;
  int16_t dig_P3;
  int16_t dig_P4;
  int16_t dig_P5;
  int16_t dig_P6;
  int16_t dig_P7;
  int16_t dig_P8;
  int16_t dig_P9;
} params;

uint8_t previous_measuring;             // Previous measuring state
uint32_t raw_pressure, raw_temperature; // Uncompensated measurement values.  Value is
                                        // always positive, even though the compensation
                                        // functions expect a signed integer as input.
int64_t t_fine;                         // t_fine carries fine temperature as global variable.
void barometer_setup(void);
void read_barometer(void);

void setup()
{
  // put your setup code here, to run once:
  Serial.begin(115200);
  while (!Serial)
    ;
  Serial.println("Hello from ESP32!");
  
  preferences.begin("my-app", false);
  Wire.begin(SDA, SCL, 400000U);
  delay(250);

  // Check if BMP280 is responding.
  Wire.beginTransmission(BAROMETER_ID);
  if (Wire.endTransmission() == 0)
  {
    Serial.println("BMP280 connection successful.");
    barometer_setup();
  }
  else
  {
    Serial.println("BMP280 connection failed.");
  }

  // Check if MPU650 is responding.
  Wire.beginTransmission(GYRO_ID);
  if (Wire.endTransmission() == 0)
  {
    Serial.println("MPU6050 connection successful.");
  }
  else
  {
    Serial.println("MPU6050 connection failed.");
  }
}

void loop()
{
  // put your main code here, to run repeatedly:
  read_barometer();
  delay(250); // this speeds up the simulation
}

void barometer_setup(void)
{
  // Reset BMP280
  Wire.beginTransmission(BAROMETER_ID); // Start communication with the BMP280.
  Wire.write(0xE0);                     // Reset register sub-address.
  Wire.write(0xB6);                     // Set the reset code.
  Wire.endTransmission();               // End the transmission with the BMP280.
  delay(10);

  // Get compensation parameters from BMP280 non-volatile memory.
  Wire.beginTransmission(BAROMETER_ID); // Start communication with the BMP280.
  Wire.write(0x88);                     // Compensation parameter register sub-address.
  Wire.endTransmission();               // End the transmission with the BMP280.
  Wire.requestFrom(BAROMETER_ID, 24U);  // Get 24 bytes from compensation parameter register.

  params.dig_T1 = Wire.read() | Wire.read() << 8;
  params.dig_T2 = Wire.read() | Wire.read() << 8;
  params.dig_T3 = Wire.read() | Wire.read() << 8;
  params.dig_P1 = Wire.read() | Wire.read() << 8;
  params.dig_P2 = Wire.read() | Wire.read() << 8;
  params.dig_P3 = Wire.read() | Wire.read() << 8;
  params.dig_P4 = Wire.read() | Wire.read() << 8;
  params.dig_P5 = Wire.read() | Wire.read() << 8;
  params.dig_P6 = Wire.read() | Wire.read() << 8;
  params.dig_P7 = Wire.read() | Wire.read() << 8;
  params.dig_P8 = Wire.read() | Wire.read() << 8;
  params.dig_P9 = Wire.read() | Wire.read() << 8;

  Serial.printf("T1: %d\n", params.dig_T1);
  Serial.printf("T2: %d\n", params.dig_T2);
  Serial.printf("T3: %d\n", params.dig_T3);
  Serial.printf("P1: %d\n", params.dig_P1);
  Serial.printf("P2: %d\n", params.dig_P2);
  Serial.printf("P3: %d\n", params.dig_P3);
  Serial.printf("P4: %d\n", params.dig_P4);
  Serial.printf("P5: %d\n", params.dig_P5);
  Serial.printf("P6: %d\n", params.dig_P6);
  Serial.printf("P7: %d\n", params.dig_P7);
  Serial.printf("P8: %d\n", params.dig_P8);
  Serial.printf("P9: %d\n", params.dig_P9);

  // Data output rate using normal mode, standard resolution(4x pressure os, 1x temp os),
  // IIR Filter @ 16, and standby time of 0.5ms is 83.3Hz or approximately every 12ms.

  // Set the Configuration Register
  // SPI or I2C        IIRFILTER        Standby Time
  // I2C   0x00        OFF   0x00        0.5MS   0x00
  // SPI   0x01         2    0x01       62.5MS   0x01
  //                    4    0x02        125MS   0x02
  //                    8    0x03        250MS   0x03
  //                   16    0x04        500MS   0x04
  //                                    1000MS   0x05
  //                                    2000MS   0x06
  //                                    3000MS   0x07
  Wire.beginTransmission(BAROMETER_ID);     // Start communication with BMP280.
  Wire.write(0xF5);                         // Configuration register sub-address.
  Wire.write(0x00 << 5 | 0x04 << 2 | 0x00); // Set spi3w_en,filter,t_sb
  Wire.endTransmission();

  // Set the Control Measure Register
  // Oversampling      Mode
  // NONE   0x00       SLEEP   0X00
  // X1     0X01       FORCED  0X01
  // X2     0X02       NORMAL  0X03
  // X4     0X03
  // X8     0X04
  // X16    0X05
  Wire.beginTransmission(BAROMETER_ID);     // Start communication with BMP280.
  Wire.write(0xF4);                         // Control Measure register sub-address.
  Wire.write(0x01 << 5 | 0x03 << 2 | 0x03); // Set osrs_t,osrs_p,mode
  Wire.endTransmission();
  // Take 1000 readings for barometer to stabilize.
  // for (int i = 0; i < 1000; i++)
  //{
  read_barometer();
  delay(4);
  //}
}

void read_barometer(void)
{
  // Check status bit to see if data is ready.
  // If it is not, return.
  // If it is, get the data and process.
  Wire.beginTransmission(BAROMETER_ID);
  Wire.write(0xF3);
  Wire.endTransmission();
  Wire.requestFrom(BAROMETER_ID, 1U);
  status.reg = Wire.read();
  if (status.bit.measuring ^ previous_measuring)
    previous_measuring = status.bit.measuring;
  else
  {
    Wire.beginTransmission(BAROMETER_ID);
    Wire.write(0xF7);
    Wire.endTransmission();
    Wire.requestFrom(BAROMETER_ID, 6U);
    raw_pressure = Wire.read() << 12 | Wire.read() << 4 | Wire.read() >> 4;
    raw_temperature = Wire.read() << 12 | Wire.read() << 4 | Wire.read() >> 4;
    // Serial.printf("Raw pressure: %d\n", raw_pressure);
    // Serial.printf("Raw temperature: %d\n", raw_temperature);

    int64_t var1, var2, T, P;
    var1 = ((((raw_temperature >> 3) - ((int64_t)params.dig_T1 << 1))) * ((int64_t)params.dig_T2)) >> 11;
    var2 = (((((raw_temperature >> 4) - ((int64_t)params.dig_T1)) * ((raw_temperature >> 4) - ((int64_t)params.dig_T1))) >> 12) * ((int64_t)params.dig_T3)) >> 14;
    t_fine = var1 + var2;
    T = (t_fine * 5 + 128) >> 8;

    var1 = ((int64_t)t_fine) - 128000;
    var2 = var1 * var1 * (int64_t)params.dig_P6;
    var2 = var2 + ((var1 * (int64_t)params.dig_P5) << 17);
    var2 = var2 + (((int64_t)params.dig_P4) << 35);
    var1 = ((var1 * var1 * (int64_t)params.dig_P3) >> 8) + ((var1 * (int64_t)params.dig_P2) << 12);
    var1 = (((((int64_t)1) << 47) + var1)) * ((int64_t)params.dig_P1) >> 33;
    if (var1 != 0)
    {
      P = 1048576 - raw_pressure;
      P = (((P << 31) - var2) * 3125) / var1;
      var1 = (((int64_t)params.dig_P9) * (P >> 13) * (P >> 13)) >> 25;
      var2 = (((int64_t)params.dig_P8) * P) >> 19;
      P = (((P + var1 + var2) >> 8) + (((int64_t)params.dig_P7) << 4)) / 256;
    }
    // Serial.printf("Compensated Pressure: %4.2f\n", P / 100.0);
    // Serial.printf("Compensated Temperature: %4.2f\n", T / 100.0 * 1.8 + 32.0);
    // Serial.printf("%4.2f, %4.2f\n", P / 100.0, T / 100.0 * 1.8 + 32.0);
    Serial.printf("%4.2f\n", T / 100.0 * 1.8 + 32.0);
  }
}