#include "Wire.h"
#include "MPU6050.h"

MPU6050 sensor;

int16_t ax, ay, az; // Variables para el acelerómetro
int16_t gx, gy, gz; // Variables para el giroscopio

// Variables para almacenar los offsets
int16_t ax_offset = 0;
int16_t ay_offset = 0;
int16_t az_offset = 0; // Debería ser aproximadamente +16384 para +1g
int16_t gx_offset = 0;
int16_t gy_offset = 0;
int16_t gz_offset = 0;

void setup() {
    Serial.begin(115200);
    Wire.begin();
    sensor.initialize();

    if (sensor.testConnection()) {
        Serial.println("Sensor iniciado correctamente");
    } else {
        Serial.println("Error al iniciar el sensor");
        while (1); // Detener si no se conecta
    }

    // Calibración del giroscopio y acelerómetro
    Serial.println("Calibrando... Mantén el sensor inmóvil.");
    
    for (int i = 0; i < 3000; i++) {
        sensor.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
        ax_offset += ax;
        ay_offset += ay;
        az_offset += az; // Az debe ser +16384 cuando está en reposo (1g)
        gx_offset += gx;
        gy_offset += gy;
        gz_offset += gz;

        delay(5); // Pausa corta para estabilizar lecturas
    }

    // Promediar los offsets
    ax_offset /= 3000;
    ay_offset /= 3000;
    az_offset /= 3000;
    gx_offset /= 3000;
    gy_offset /= 3000;
    gz_offset /= 3000;

    // Mostrar resultados
    Serial.print("Offsets calculados:\n");
    Serial.print("Ax Offset: "); Serial.println(ax_offset);
    Serial.print("Ay Offset: "); Serial.println(ay_offset);
    Serial.print("Az Offset: "); Serial.println(az_offset);
    Serial.print("Gx Offset: "); Serial.println(gx_offset);
    Serial.print("Gy Offset: "); Serial.println(gy_offset);
    Serial.print("Gz Offset: "); Serial.println(gz_offset);

    // Opcional: Establecer offsets en el sensor (no siempre necesario)
    sensor.setXAccelOffset(ax_offset);
    sensor.setYAccelOffset(ay_offset);
    sensor.setZAccelOffset(az_offset);
    
    sensor.setXGyroOffset(gx_offset);
    sensor.setYGyroOffset(gy_offset);
    sensor.setZGyroOffset(gz_offset);

    // Mensaje indicando que la calibración ha terminado
    Serial.println("Calibración completa. Comenzando a medir...");
}

void loop() {
    // Leer datos del acelerómetro y giroscopio
    sensor.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    // Ajustar las lecturas con los offsets
    ax -= ax_offset; // Ajustar Ax
    ay -= ay_offset; // Ajustar Ay
    az -= az_offset; // Ajustar Az

    gx -= gx_offset; // Ajustar Gx
    gy -= gy_offset; // Ajustar Gy
    gz -= gz_offset; // Ajustar Gz

    // Imprimir datos ajustados
    Serial.print("AccX:"); Serial.print(ax); Serial.print(" ");
    Serial.print("AccY:"); Serial.print(ay); Serial.print(" ");
    Serial.print("AccZ:"); Serial.print(az); Serial.print(" ");
    
    Serial.print("GyroX:"); Serial.print(gx); Serial.print(" ");
    Serial.print("GyroY:"); Serial.print(gy); Serial.print(" ");
    Serial.print("GyroZ:"); Serial.println(gz);

    delay(100); // Espera 100 ms entre lecturas
}