/* * MECRO KIT - PROYECTO 16: NIVEL DE BURBUJA DIGITAL
 * Objetivo: Lectura de IMU (Acelerómetro) y visualización gráfica
 * Hardware: ESP32 + MPU6050 + OLED SSD1306
 * Protocolo: I2C (Bus compartido en pines 21 y 22)
 */

#include <Adafruit_MPU6050.h>
#include <Adafruit_SSD1306.h>
#include <Wire.h>

// --- 1. CONFIGURACIÓN OLED ---
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// --- 2. CONFIGURACIÓN IMU ---
Adafruit_MPU6050 mpu;

void setup() {
  Serial.begin(115200);
  Serial.println("Iniciando Sistemas de Vuelo MECRO...");

  // Iniciar OLED
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println("Fallo OLED - Check cables I2C");
    for(;;);
  }

  // Iniciar MPU6050
  if (!mpu.begin()) {
    Serial.println("Fallo MPU6050 - Check cables I2C");
    while (1) { delay(10); }
  }
  
  // --- CALIBRACIÓN DE INGENIERÍA ---
  // Configuramos el rango y el filtro para estabilizar la lectura
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(30,25);
  display.print("CALIBRANDO...");
  display.display();
  delay(1000);
}

void loop() {
  // 1. Obtener eventos del sensor
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  // 2. Procesamiento de Datos (Física)
  // El acelerómetro mide en m/s^2. La gravedad es ~9.8 m/s^2.
  // Cuando inclinas el sensor, la gravedad se reparte en los ejes X e Y.
  
  // Mapear aceleración (-10 a 10 m/s^2) al tamaño de la pantalla
  // Centro de pantalla = (64, 32)
  // Invertimos signos para simular comportamiento de "burbuja" (flota hacia arriba)
  int x_pos = map((int)(a.acceleration.y * 10), -98, 98, 128, 0); 
  int y_pos = map((int)(a.acceleration.x * 10), -98, 98, 64, 0);
  
  // Limitar bordes (Clamping) para que la burbuja no salga de la pantalla
  if(x_pos < 4) x_pos = 4;
  if(x_pos > 124) x_pos = 124;
  if(y_pos < 4) y_pos = 4;
  if(y_pos > 60) y_pos = 60;

  // 3. Visualización (Renderizado)
  display.clearDisplay();
  
  // Dibujar Diana (Centro / Target)
  display.drawCircle(64, 32, 10, WHITE); // Círculo central
  display.drawLine(64, 20, 64, 44, WHITE); // Cruz vertical
  display.drawLine(52, 32, 76, 32, WHITE); // Cruz horizontal
  
  // Dibujar la "Burbuja"
  // Si está perfectamente nivelado, x_pos=64, y_pos=32 (Centro)
  display.fillCircle(x_pos, y_pos, 4, WHITE);
  
  // Telemetría en pantalla (Valores G)
  display.setCursor(0,0);
  display.print("X:"); display.print(a.acceleration.x, 1);
  display.setCursor(64,0);
  display.print("Y:"); display.print(a.acceleration.y, 1);
  
  display.display();
  
  // 40ms de delay para ~25 FPS (Fluidez visual)
  delay(40); 
}