#include <Wire.h>
#include <MPU9250_WE.h>
#include <Adafruit_BMP280.h>
#include <MadgwickAHRS.h>

#include <MPU9250_asukiaaa.h>

MPU9250_asukiaaa mySensor;

// Endereços típicos dos sensores
#define ADDR_MPU 0x68
#define ADDR_BMP 0x76

MPU9250_WE mpu = MPU9250_WE(ADDR_MPU);
Adafruit_BMP280 bmp;
Madgwick filter;

float AccX = 0, AccY = 0, AccZ = 0;
float GyroX = 0, GyroY = 0, GyroZ = 0;
float MagX = 0, MagY = 0, MagZ = 0;
float Temp = 0;
float Pressao = 0;
float Altitude = 0;
float Roll = 0;
float Pitch = 0;
float Yaw = 0;

void setup() {
  Serial.begin(115200);
  delay(500);
  Wire.begin(1, 2);

  Serial.println("🔍 Iniciando Scanner I2C...");
  for (byte address = 1; address < 127; address++) {
    Wire.beginTransmission(address);
    if (Wire.endTransmission() == 0) {
      Serial.print("📍 Dispositivo encontrado no endereco 0x");
      Serial.println(address, HEX);
    }
  }
  Serial.println("✅ Scanner finalizado.\n");

  Serial.println("🎯 Iniciando sensores...");

  Serial.print("WHO_AM_I: 0x");
  Serial.println(mpu.whoAmI(), HEX);

  mySensor.setWire(&Wire);
  mySensor.beginAccel();
  mySensor.beginGyro();
  mySensor.beginMag(); // tenta usar AK8963 se existir

  /*bool mpuIniciado = mpu.init();
  // MPU9250
  if (!mpuIniciado) {
    Serial.println("MPU9250 NAO encontrado!");
  } else {
    mpu.autoOffsets();
    mpu.setSampleRateDivider(5);
    mpu.setAccRange(MPU9250_ACC_RANGE_2G);
    mpu.enableAccDLPF(true);
    mpu.setAccDLPF(MPU9250_DLPF_6);
    Serial.println("MPU9250 OK");
  }*/

  // BMP280
  if (!bmp.begin(ADDR_BMP)) {
    Serial.println("BMP280 NAO encontrado!");
  } else {
    bmp.setSampling(Adafruit_BMP280::MODE_NORMAL,
                    Adafruit_BMP280::SAMPLING_X2,
                    Adafruit_BMP280::SAMPLING_X16,
                    Adafruit_BMP280::FILTER_X16,
                    Adafruit_BMP280::STANDBY_MS_500);
    Serial.println("BMP280 OK");
  }

  // Madgwick
  filter.begin(100); // 100 Hz
}

void loop() {
  // Atualizar dados do MPU
  /*xyzFloat acc = mpu.getGValues();
  xyzFloat gyr = mpu.getGyrValues();
  xyzFloat mag = mpu.getMagValues();

  AccX = acc.x;  AccY = acc.y;  AccZ = acc.z;
  GyroX = gyr.x; GyroY = gyr.y; GyroZ = gyr.z;
  MagX = mag.x;  MagY = mag.y;  MagZ = mag.z;*/

  mySensor.accelUpdate();
  mySensor.gyroUpdate();
  mySensor.magUpdate(); // pode falhar se não tiver magnetômetro

  AccX = mySensor.accelX(); AccY = mySensor.accelY(); AccZ = mySensor.accelZ();
  GyroX = mySensor.gyroX(); GyroY = mySensor.gyroY(); GyroZ = mySensor.gyroZ();
  MagX = mySensor.magX(); MagY = mySensor.magY(); MagZ = mySensor.magZ();

  // Atualizar dados do BMP
  Temp = bmp.readTemperature();
  Pressao = bmp.readPressure();
  Altitude = bmp.readAltitude();

  // Filtro Madgwick
  filter.update(GyroX, GyroY, GyroZ, AccX, AccY, AccZ, MagX, MagY, MagZ);
  Roll = filter.getRoll();
  Pitch = filter.getPitch();
  Yaw = filter.getYaw();

  // Exibir no Serial
  Serial.println("---- AMOSTRAGEM ----");
  Serial.print("Temp: "); Serial.println(Temp);
  Serial.print("Pressao: "); Serial.println(Pressao);
  Serial.print("Altitude: "); Serial.println(Altitude);

  Serial.print("Roll: "); Serial.println(Roll);
  Serial.print("Pitch: "); Serial.println(Pitch);
  Serial.print("Yaw: "); Serial.println(Yaw);
  Serial.println();

  delay(1000);
}