#include <Wire.h>
#include <SPI.h>
#include <Adafruit_BMP280.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include "HX711.h"

#define BMP_SCK  (13)
#define BMP_MISO (12)
#define BMP_MOSI (11)
#define BMP_CS   (10)

const float sensorMax = 1023.0; // Valor máximo de leitura do ADC (10-bit)
const float pressureMax = 100.0; // Máxima pressão em kPa (ajuste conforme necessário)
const float fluidDensity = 1.225; // Densidade do ar em kg/m³

Adafruit_BMP280 bmp(BMP_CS); // hardware SPI
Adafruit_MPU6050 mpu;

HX711 scale;

float groundAltitude = 0; // Altitude em relação ao solo (referência)

void setup() {
  Serial.begin(9600);
  while (!Serial) delay(100);   // wait for native USB
  Serial.println(F("Teste dos sensores BMP280, MPU6050 e HX711"));

  // Inicializar BMP280
  unsigned status;
  status = bmp.begin();
  if (!status) {
    Serial.println(F("Sensor BMP280 não encontrado. Verifique conexões!"));
    while (1) delay(10);
  }

  bmp.setSampling(Adafruit_BMP280::MODE_NORMAL,
                  Adafruit_BMP280::SAMPLING_X2,
                  Adafruit_BMP280::SAMPLING_X16,
                  Adafruit_BMP280::FILTER_X16,
                  Adafruit_BMP280::STANDBY_MS_500);

  // Inicializar MPU6050
  if (!mpu.begin()) {
    Serial.println("Sensor MPU6050 não encontrado!");
    while (1) delay(10);
  }
  Serial.println("MPU6050 pronto.");

  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_2000_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  // Inicializar HX711
  scale.begin(A1, A0);
  scale.set_scale(); // Pressupondo calibrado
  scale.tare();

  // Capturar altitude inicial (referência do solo)
  float pressure = bmp.readPressure() / 100.0; // hPa
  float seaLevelPressure = 1013.25; // Pode ajustar manualmente
  groundAltitude = 44330.0 * (1.0 - pow(pressure / seaLevelPressure, 0.1903));

  Serial.print("Altitude inicial (nível do solo): ");
  Serial.print(groundAltitude);
  Serial.println(" m");

  delay(100);
}

void loop() {
  // -------- ALTITUDE RELATIVA --------
  float pressure = bmp.readPressure() / 100.0;
  float seaLevelPressure = 1013.25;
  float currentAltitude = 44330.0 * (1.0 - pow(pressure / seaLevelPressure, 0.1903));
  float relativeAltitude = currentAltitude - groundAltitude;

  Serial.print("Altitude atual (relativa ao solo): ");
  Serial.print(relativeAltitude);
  Serial.println(" m");

  // -------- ROTAÇÃO MPU6050 --------
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  Serial.print("Rotação X: ");
  Serial.print(g.gyro.x * 180.0 / PI);
  Serial.print(" °/s, Y: ");
  Serial.print(g.gyro.y * 180.0 / PI);
  Serial.print(" °/s, Z: ");
  Serial.print(g.gyro.z * 180.0 / PI);
  Serial.println(" °/s");

  // -------- PRESSÃO E VELOCIDADE (Bernoulli) --------
  float sensorValue = scale.get_units(10);
  if (sensorValue < 0) sensorValue = 0;

  float pressure_kPa = (sensorValue / sensorMax) * pressureMax;
  float pressure_Pa = pressure_kPa * 1000;
  float velocity = 0;

  if (pressure_Pa > 0) {
    velocity = sqrt(2 * pressure_Pa / fluidDensity);
  }

  Serial.print("Pressão: ");
  Serial.print(pressure_kPa);
  Serial.print(" kPa, Velocidade: ");
  Serial.print(velocity);
  Serial.println(" m/s");

  Serial.println();
  delay(2000);
}