#include <MPU6050_6Axis_MotionApps20.h>
#include <SD.h>
#include <SPI.h>

MPU6050 mpu;
const int chipSelect = 27; // Pino de seleção do chip SD (pode variar dependendo do seu hardware)
File dataFile;

void setup() {
    Serial.begin(115200);
    Wire.begin();
    delay(2000);

    // Inicializar o MPU9250
    if (!mpu.setup(0x68)) {  // change to your own address
        while (1) {
            Serial.println("MPU connection failed. Please check your connection with connection_check example.");
            delay(5000);
        }
    }

    // Calibrar o MPU9250
    Serial.println("Accel Gyro calibration will start in 5sec.");
    Serial.println("Please leave the device still on the flat plane.");
    mpu.verbose(true);
    delay(5000);
    mpu.calibrateAccelGyro();

    Serial.println("Mag calibration will start in 5sec.");
    Serial.println("Please wave the device in a figure eight until done.");
    delay(5000);
    mpu.calibrateMag();

    print_calibration();
    mpu.verbose(false);

    // Inicializar o cartão SD
    if (!SD.begin(chipSelect)) {
        Serial.println("SD card initialization failed!");
        while (1);
    }
    Serial.println("SD card initialized.");

    // Criar ou abrir o arquivo de dados
    dataFile = SD.open("dataFile.txt", FILE_WRITE);
    if (!dataFile) {
        Serial.println("Error opening dataFile.txt");
    } else {
        Serial.println("dataFile.txt opened successfully.");
        // Escrever cabeçalho no arquivo
        dataFile.println("Time (ms), Accel X (m/s^2), Accel Y (m/s^2), Accel Z (m/s^2), Gyro X (deg/s), Gyro Y (deg/s), Gyro Z (deg/s), Mag X (uT), Mag Y (uT), Mag Z (uT)");
        dataFile.close();
    }
}

void loop() {
    // Coletar dados do MPU9250
    mpu.update();

    // Obter os dados dos sensores
    float ax = mpu.getAccX();  // Aceleração em X (m/s^2)
    float ay = mpu.getAccY();  // Aceleração em Y (m/s^2)
    float az = mpu.getAccZ();  // Aceleração em Z (m/s^2)
    float gx = mpu.getGyroX(); // Velocidade angular em X (deg/s)
    float gy = mpu.getGyroY(); // Velocidade angular em Y (deg/s)
    float gz = mpu.getGyroZ(); // Velocidade angular em Z (deg/s)
    float mx = mpu.getMagX();  // Magnetômetro em X (uT)
    float my = mpu.getMagY();  // Magnetômetro em Y (uT)
    float mz = mpu.getMagZ();  // Magnetômetro em Z (uT)

    // Imprimir os dados no monitor serial
    Serial.print("Time: "); Serial.print(millis()); Serial.print(" ms, ");
    Serial.print("Accel X: "); Serial.print(ax); Serial.print(" m/s^2, ");
    Serial.print("Accel Y: "); Serial.print(ay); Serial.print(" m/s^2, ");
    Serial.print("Accel Z: "); Serial.print(az); Serial.print(" m/s^2, ");
    Serial.print("Gyro X: "); Serial.print(gx); Serial.print(" deg/s, ");
    Serial.print("Gyro Y: "); Serial.print(gy); Serial.print(" deg/s, ");
    Serial.print("Gyro Z: "); Serial.print(gz); Serial.print(" deg/s, ");
    Serial.print("Mag X: "); Serial.print(mx); Serial.print(" uT, ");
    Serial.print("Mag Y: "); Serial.print(my); Serial.print(" uT, ");
    Serial.print("Mag Z: "); Serial.println(mz); Serial.print(" uT");

    // Escrever os dados no arquivo SD
    dataFile = SD.open("dataFile.txt", FILE_WRITE);
    if (dataFile) {
        dataFile.print(millis()); dataFile.print(", ");
        dataFile.print(ax); dataFile.print(", ");
        dataFile.print(ay); dataFile.print(", ");
        dataFile.print(az); dataFile.print(", ");
        dataFile.print(gx); dataFile.print(", ");
        dataFile.print(gy); dataFile.print(", ");
        dataFile.print(gz); dataFile.print(", ");
        dataFile.print(mx); dataFile.print(", ");
        dataFile.print(my); dataFile.print(", ");
        dataFile.println(mz);
        dataFile.close();
    } else {
        Serial.println("Error opening dataFile.txt for writing.");
    }

    delay(100); // Intervalo de 100 ms entre leituras
}

void print_calibration() {
    Serial.println("< calibration parameters >");
    Serial.println("accel bias [g]: ");
    Serial.print(mpu.getAccBiasX() * 1000.f / (float)MPU9250::CALIB_ACCEL_SENSITIVITY);
    Serial.print(", ");
    Serial.print(mpu.getAccBiasY() * 1000.f / (float)MPU9250::CALIB_ACCEL_SENSITIVITY);
    Serial.print(", ");
    Serial.print(mpu.getAccBiasZ() * 1000.f / (float)MPU9250::CALIB_ACCEL_SENSITIVITY);
    Serial.println();
    Serial.println("gyro bias [deg/s]: ");
    Serial.print(mpu.getGyroBiasX() / (float)MPU9250::CALIB_GYRO_SENSITIVITY);
    Serial.print(", ");
    Serial.print(mpu.getGyroBiasY() / (float)MPU9250::CALIB_GYRO_SENSITIVITY);
    Serial.print(", ");
    Serial.print(mpu.getGyroBiasZ() / (float)MPU9250::CALIB_GYRO_SENSITIVITY);
    Serial.println();
    Serial.println("mag bias [mG]: ");
    Serial.print(mpu.getMagBiasX());
    Serial.print(", ");
    Serial.print(mpu.getMagBiasY());
    Serial.print(", ");
    Serial.print(mpu.getMagBiasZ());
    Serial.println();
    Serial.println("mag scale []: ");
    Serial.print(mpu.getMagScaleX());
    Serial.print(", ");
    Serial.print(mpu.getMagScaleY());
    Serial.print(", ");
    Serial.print(mpu.getMagScaleZ());
    Serial.println();
}