#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <stdlib.h>

Adafruit_MPU6050 mpu;
const int XY_11 = 33;
const int XY_10 = 32;
const int XY_01 = 25;
const int XY_00 = 26;

#include <Wire.h>
#include <stdlib.h>
struct coeficients {
  float pressure;
  float cTempB;
  float P11;
  float P10;
  float P9;
  float P8;
  float P7;
  float P6;
  float P5;
  float P4;
  float P3;
  float P2;
  float P1;
  float T1;
  float T2;
  float T3;
  float g = 9.81054;
  float p = 1.29;
  float sealevelpress = 99991.5;
};

void writebyte(uint8_t mstadress, uint8_t slvadress, uint8_t wbyte) {
  Wire.beginTransmission(mstadress);
  Wire.write(slvadress);
  Wire.write(wbyte);
  Wire.endTransmission();
  delay(30);
}

struct coeficients *calccoef() {
  Wire.begin();


  writebyte(0x77, 0x1C, 0x03);
  writebyte(0x77, 0x17, 0x00);
  writebyte(0x77, 0x1F, 0x01);
  writebyte(0x77, 0x1D, 0x06);
  writebyte(0x77, 0x1B, 0x33);

  int b1[20];
  for (int i = 0; i <= 20; i++) {
    Wire.beginTransmission(0x77);
    Wire.write(69 - i);
    Wire.endTransmission();

    Wire.requestFrom(0x77, 1);
    if (Wire.available()) {
      b1[i] = Wire.read();
      //delay(30);
    }
  }
  struct coeficients *res = (coeficients *) malloc(sizeof(struct coeficients));
  // блок для обрахунку коефіцієнтів для барометра
  {
    res->P11 = ((float)b1[0]) / pow(2, 65);
    res->P10 = ((float)b1[1]) / pow(2, 48);
    res->P9 = (float)(b1[2] << 8 | b1[3]) / pow(2, 48);
    res->P8 = ((float)b1[4]) / pow(2, 15);
    res->P7 = ((float)b1[5]) / pow(2, 8);
    res->P6 = abs((float)(b1[6] << 8 | b1[7])) / pow(2, 6);
    res->P5 = abs((float)(b1[8] << 8 | b1[9])) / pow(2, -3);
    res->P4 = ((float)b1[10] / pow(2, 37));
    res->P3 = ((float)b1[11]) / pow(2, 32);
    res->P2 = ((float)(b1[12] << 8 | b1[13]) - pow(2, 14)) / pow(2, 29);
    res->P1 = ((float)(b1[14] << 8 | b1[15]) - pow(2, 14)) / pow(2, 20);
    res->T3 = ((float)b1[16]) / pow(2, 48);
    res->T2 = abs((float)(b1[17] << 8 | b1[18])) / pow(2, 30);
    res->T1 = abs((float)(b1[19] << 8 | b1[20])) / pow(2, -8);
  }
}

//result of this function will be fed into struct->pressure and struct->cTempB
void calcPT(struct coeficients *coefs) {

  uint8_t data[6];
  float PD1;
  float PD2;
  float PD3;
  float PO1;
  float PO2;
  float PD4;

  for (int i = 0; i < 6; i++) {
    Wire.beginTransmission(0x77);
    Wire.write(0x04 + i);
    Wire.endTransmission();
    Wire.requestFrom(0x77, 1);
    if (Wire.available()) {
      data[i] = Wire.read();
    }
  }

  uint32_t uncompT = ((data[5] << 16 | data[4] << 8) | data[3]);
  uint32_t uncompP = ((data[2] << 16 | data[1] << 8) | data[0]);
  PD1 = (float)(uncompT - coefs->T1);
  PD2 = (float)(PD1 * coefs->T2);
  float cTempB = PD2 - 3.1 + (PD1 * PD1) * coefs->T3;
  coefs->cTempB = cTempB;


  PD1 = (coefs->P6 * cTempB);
  PD2 = coefs->P7 * (cTempB * cTempB);
  PD3 = coefs->P8 * (cTempB * cTempB * cTempB);
  PO1 = (coefs->P5 + PD1 + PD2 + PD3);

  PD1 = coefs->P2 * cTempB;
  PD2 = coefs->P3 * (cTempB * cTempB);
  PD3 = coefs->P4 * (cTempB * cTempB * cTempB);
  PO2 = (float)uncompP * (coefs->P1 + PD1 + PD2 + PD3);
  PD1 = (float)uncompP * (float)uncompP;
  PD2 = coefs->P9 + coefs->P10 * cTempB;
  PD3 = PD1 * PD2;
  PD4 = PD3 + ((float)uncompP * (float)uncompP * (float)uncompP) * coefs->P11;
  coefs->pressure = PO1 + PO2 + PD4;
}

struct coeficients *coefs;

void setup() {
  Serial.begin(115200);
  mpu.begin();
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_5_HZ);
  pinMode(XY_11, OUTPUT);
  pinMode(XY_10, OUTPUT);
  pinMode(XY_01, OUTPUT);
  pinMode(XY_00, OUTPUT);


  struct coeficients *coefs = calccoef();
  calcPT(coefs);
}
void loop() {
  calcPT(coefs);
  Serial.print("Temperature:");
  Serial.print(coefs->cTempB);
  Serial.print("*C\n");
  Serial.print("Pressure");
  Serial.print(coefs->pressure / 100);
  Serial.print("hPa\n");
  Serial.print("   ======>");
  Serial.print(coefs->sealevelpress / 100);
  Serial.print("hPa\n");
  Serial.print("Height: ");
  Serial.println((coefs->sealevelpress / (coefs->p * coefs->g) - coefs->pressure / (coefs->p * coefs->g)));

  delay(400);
}



/*
bool check_for_zero(float val){
  //if close to 0 return true
  if(abs(val)<=0.15){
    return true;
  }
  return false;
}

void setup(void) {
  Serial.begin(115200);
  mpu.begin();
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_5_HZ);
  pinMode(XY_11, OUTPUT);
  pinMode(XY_10, OUTPUT);
  pinMode(XY_01, OUTPUT);
  pinMode(XY_00, OUTPUT);
  delay(100);
}

void loop() {

  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);


  Serial.print("Acceleration X: ");
  Serial.print(a.acceleration.x);
  Serial.print(", Y: ");
  Serial.print(a.acceleration.y);
  Serial.print(", Z: ");
  Serial.print(a.acceleration.z);
  Serial.println(" m/s^2");

  Serial.print("Rotation X: ");
  Serial.print(g.gyro.x);
  Serial.print(", Y: ");
  Serial.print(g.gyro.y);
  Serial.print(", Z: ");
  Serial.print(g.gyro.z);
  Serial.println(" rad/s");

  if(check_for_zero(g.gyro.x) && !check_for_zero(g.gyro.y))
  {
    if(g.gyro.y > 0)
    {
      digitalWrite(XY_01, HIGH);
      digitalWrite(XY_11, HIGH);
    }
    else
    {
      digitalWrite(XY_10, HIGH);
      digitalWrite(XY_00, HIGH);
    }
  }
  else
  {
    if (!check_for_zero(g.gyro.x) && check_for_zero(g.gyro.y))
    {
      if(g.gyro.x > 0)
      {
        digitalWrite(XY_11, HIGH);
        digitalWrite(XY_10, HIGH);
      }
      else
      {
        digitalWrite(XY_01, HIGH);
        digitalWrite(XY_00, HIGH);
      }
    }
    else
    {
      if(!check_for_zero(g.gyro.x) && !check_for_zero(g.gyro.y) )
      {
        if(g.gyro.x > 0 && g.gyro.y > 0)
        {
          digitalWrite(XY_11, HIGH);
        }
        if(g.gyro.x > 0 && g.gyro.y < 0)
        {
          digitalWrite(XY_10, HIGH);
        }
        if(g.gyro.x < 0 && g.gyro.y > 0)
        {
          digitalWrite(XY_01, HIGH);
        }
        if(g.gyro.x < 0 && g.gyro.y < 0)
        {
          digitalWrite(XY_00, HIGH);
        }
      }
    }
  }
  delay(500);
  digitalWrite(XY_00, LOW);
  digitalWrite(XY_01, LOW);
  digitalWrite(XY_10, LOW);
  digitalWrite(XY_11, LOW);
}
*/