#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BME280.h>
#include <Adafruit_HMC5883_U.h>
#include <Adafruit_MPU6050.h>

#define BrnoPressure 1025


// porovnani presnosti MPU a BME
/*
BME:  -40 - 85 deg, (0-65 - +-0.5deg) - str 11
MPU:  -40 - 85 deg, (+-1deg)          - str 14

*/
//nadefinovaný struktury
//HMC I2C adr = 0b00011110 = 0x1E.
Adafruit_HMC5883_Unified hmc = Adafruit_HMC5883_Unified(12345);
//MPU I2C adr = 0b0110100x = 0x68/0x69 (0x69 při připojení AD0 k VCC)
Adafruit_MPU6050 mpu;
//BME I2C adr = 0b0111011x = 0x76/0x77 (0x76 SDO připojeno na GND, 0x77 SDO připojeno na VDDIO)
Adafruit_BME280 bme;

//pomocne promenne
float t, t_last, t_print_last, t_print;
float bme_temperature, bme_preassure, bme_humidity;
sensors_event_t mpu_acceleration, mpu_gyrom, mpu_temperature, hmc_data;

void setup() {
  Serial.begin(115200);

  while(!hmc.begin()){
    delay(100);
    Serial.println("Chyba pripojeni HMC5883 senzoru");
  }
  Serial.println("Pripojeni k HMC5883 senzoru se podarilo");
  while(!mpu.begin()){
    delay(100);
    Serial.println("Chyba pripojeni MPU6050 senzoru");
  }
  Serial.println("Pripojeni k MPU6050 senzoru se podarilo");
  while(!bme.begin(0x76)){
    delay(100);
    Serial.println("Chyba pripojeni BME280 senzoru");
  }
  Serial.println("Pripojeni k BME280 senzoru se podarilo");


  //****************Nastavení rychlosti BME senzoru******************
//t_measure = 1+(2*samplTeploty) + (2*samplTlaku+0.5) + (2*samplVlhkost + 0.5)
//t_measure = 1+(2*4)+(2*1+0.5)+(2*4+0.5) = 20
//ODR = 1000/t_measure = 1000/(20) = 50 Hz
  bme.setSampling(Adafruit_BME280::MODE_FORCED,
                  Adafruit_BME280::SAMPLING_X4, // temperature
                  Adafruit_BME280::SAMPLING_X1, // pressure
                  Adafruit_BME280::SAMPLING_X4, // humidity
                  Adafruit_BME280::FILTER_OFF   
                  );

  /****************Nastavení rychlosti HMC senzoru******************
  //ze zakladu ma 15Hz
  //ODR registr jegistr A = 0|MA1(0)|MA0(0)|DO2(1)|DO1(0)|DO0(0)|MS1(0)|MS0(0) (default)
  
  DO2 DO1 DO0 ODR
  0   0   0   0.75
  0   0   1   1.5
  0   1   0   3
  0   1   1   7.5
  1   0   0   15 (default)
  1   0   1   30
  1   1   0   75
  1   1   1   reserved
  */
  Wire.begin();
  Wire.beginTransmission(HMC5883_ADDRESS_MAG);
  Wire.write(HMC5883_REGISTER_MAG_CRA_REG_M);
  Wire.write(0b00011000);
  Wire.endTransmission(true);

  //****************Nastavení rychlosti MPU senzoru******************
  //mpu má max rychlost cteni 1kHz - datasheet str 17 => nemusíme nic nastavovat

}

void loop() {
  t = millis();
  if(t-t_last > 20){
    //co se ma dit kazdych 20ms = 50Hz
    BME280_getData(true);
    HMC5883_getData(true);
    MPU6050_getData(true);
    t_last = t;
  }

  t_print = millis();
  if(t_print-t_print_last > 1000){
    //vypise to data kazdou sekundu
    BME280_printData();
    HMC5883_printData();
    MPU6050_printData();
    t_print_last = t_print;
  }

}

void BME280_printData() {
    Serial.print("Temperature = ");
    Serial.print(bme_temperature);
    Serial.println(" °C");

    Serial.print("Pressure = ");
    Serial.print(bme_preassure / 100.0F);
    Serial.println(" hPa");

    Serial.print("Humidity = ");
    Serial.print(bme_humidity);
    Serial.println(" %");

    Serial.println();
}
void BME280_getData(bool printStatus){
  if(printStatus){
    bme_temperature = bme.readTemperature();
    bme_preassure = bme.readPressure();
    bme_humidity = bme.readHumidity();
  }
}
void HMC5883_printData() {
  Serial.print("X: ");
  Serial.print(hmc_data.magnetic.x);
  Serial.print("  ");
  Serial.print("Y: ");
  Serial.print(hmc_data.magnetic.y);
  Serial.print("  ");
  Serial.print("Z: ");
  Serial.print(hmc_data.magnetic.z);
  Serial.print("  ");
  Serial.println("uT");
}
void HMC5883_getData(bool printStatus){
  if(printStatus){
    hmc.getEvent(&hmc_data);
  }
}
void MPU6050_printData() {
  Serial.print("Acceleration X: ");
  Serial.print(mpu_acceleration.acceleration.x);
  Serial.print(", Y: ");
  Serial.print(mpu_acceleration.acceleration.y);
  Serial.print(", Z: ");
  Serial.print(mpu_acceleration.acceleration.z);
  Serial.println(" m/s^2");

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

  Serial.print("Temperature: ");
  Serial.print(mpu_temperature.temperature);
  Serial.println(" degC");

  Serial.println("");
}
void MPU6050_getData(bool printStatus){
  if(printStatus){
    mpu.getEvent(&mpu_acceleration, &mpu_gyrom, &mpu_temperature);
  }
}