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

// ==========================================
// CONFIGURACIÓN DE PARÁMETROS DEL TEST
// ==========================================
// Estas variables se recalibran automaticamente por la función calibraMotores.
// Corrigen la diferencia en torque entre los dos motores.
bool motores_calibrados = false;
int LPWM_MIN_TEST = 1;  
int RPWM_MIN_TEST = 135;  
int LPWM_MAX_TEST = 34;
int RPWM_MAX_TEST = 255; 

// Variables globales de calibración (consumo mínimo de RAM)
int16_t magX_min = 32767, magX_max = -32768;
int16_t magY_min = 32767, magY_max = -32768;
#define INT_PIN_CFG     0x37  // Registro para activar bypass I2C
#define MAG_CNTL1       0x0A  // Registro de control del magnetómetro
#define MPU6050_ADDRESS    0x68 // Dirección I2C del MPU-6050 y MPU-9250
#define INT_PIN_CFG     0x37  // Registro de configuración de interrupciones y bypass I2C
#define MPU9250_ADDRESS 0x69  // Dirección I2C típica del MPU-6050 y MPU-9250
#define PWR_MGMT_1  0x6B  // Registro de gestión de energía
#define AK8963_ADDRESS  0x0C  // ¡ESTA ES LA DIRECCIÓN REAL DE LA BRÚJULA!
#define AK8963_CNTL     0x0A  // Registro de control del magnetómetro
#define USER_CTRL       0x6A
bool mpu_ok = true;
const int TRIG_PIN = 4;
const int ECHO_PIN = 5;
const int PIN_MOTOR_ENA = 3;
const int PIN_MOTOR_IN1 = 2;
const int PIN_MOTOR_IN2 = 8;
const int PIN_MOTOR_IN3 = 12;
const int PIN_MOTOR_IN4 = 13;
const int PIN_MOTOR_ENB = 11;

#define ANCHO_OLED 128
#define ALTO_OLED  64
#define RESET_OLED    -1
#define DIRECCION_I2C 0x3C
Adafruit_SSD1306 pantalla_oled(ANCHO_OLED, ALTO_OLED, &Wire, RESET_OLED);

#define MAX_LINEAS 6        
#define LARGO_MAX_LINEA 22 
char bufferConsola[MAX_LINEAS][LARGO_MAX_LINEA]; 
int lineasActivas = 0;                           
bool tienePantallaOLED = false;
const long DISTANCIA_MIN_PISTA = 150; 
const unsigned long TIEMPO_MARCHA_1 = 600; 
const unsigned long TIEMPO_MARCHA_2 = 600; 

void robotReporta(const char* formato, ...) {
  if (!tienePantallaOLED) return;
  char lineaTemporal[LARGO_MAX_LINEA];
  va_list args;
  va_start(args, formato);
  vsnprintf(lineaTemporal, LARGO_MAX_LINEA, formato, args);
  va_end(args);

  if (lineasActivas < MAX_LINEAS) {
    strncpy(bufferConsola[lineasActivas], lineaTemporal, LARGO_MAX_LINEA - 1);
    bufferConsola[lineasActivas][LARGO_MAX_LINEA - 1] = '\0';
    lineasActivas++;
  } else {
    for (int i = 0; i < MAX_LINEAS - 1; i++) {
      strcpy(bufferConsola[i], bufferConsola[i + 1]);
    }
    strncpy(bufferConsola[MAX_LINEAS - 1], lineaTemporal, LARGO_MAX_LINEA - 1);
    bufferConsola[MAX_LINEAS - 1][LARGO_MAX_LINEA - 1] = '\0';
  }

  pantalla_oled.clearDisplay();
  pantalla_oled.setTextSize(1);
  pantalla_oled.setTextColor(SSD1306_WHITE);
  for (int i = 0; i < lineasActivas; i++) {
    pantalla_oled.setCursor(0, i * 10);
    pantalla_oled.print(bufferConsola[i]);
  }
  pantalla_oled.display();
}

void inicializarMagnetometro() {
  // Configura el AK8963 a Modo de medición continua 2 (100Hz) y resolución de 16-bits
  Wire.beginTransmission(AK8963_ADDRESS);
  Wire.write(AK8963_CNTL);
  Wire.write(0x16); // 0x16 = Resolución 16-bit (bit 4) + Medición continua a 100Hz (0x06)
  Wire.endTransmission();
  delay(20);
}

/*
int leerBrujula() { // Google Gemini 20260620
  if (!mpu_ok) return -999;
  // Retraso para estabilización del bus I2C
  delay(150);
  Wire.beginTransmission(MPU9250_ADDRESS);
  Wire.write(0x03); 
  delay(150);
  
  int return_code = Wire.endTransmission(false);
  if (return_code != 0) {
    if (return_code == 2) {
      robotReporta("ERROR 2:");
      robotReporta("MPU-5060 No responde.");
    } else {
      robotReporta("ERROR ", return_code, ":");
      robotReporta("BRU FAIL: %d", return_code);
    }
    mpu_ok = false;
    return -999;
  } else {
    mpu_ok = true;
  }

  // Solicita los 7 bytes
  Wire.requestFrom(MPU9250_ADDRESS, 7, true);
  
  // Verifica si llegaron los bytes completos sin congelar el programa
  if (Wire.available() >= 7) {
    // Lectura segura byte por byte (Garantiza el orden LSB y MSB)
    uint8_t xl = Wire.read();
    uint8_t xh = Wire.read();
    int16_t mx = (int16_t)(xl | (xh << 8));

    uint8_t yl = Wire.read();
    uint8_t yh = Wire.read();
    int16_t my = (int16_t)(yl | (yh << 8));

    uint8_t zl = Wire.read();
    uint8_t zh = Wire.read();
    int16_t mz = (int16_t)(zl | (zh << 8));

    Wire.read(); // Registro ST2 obligatorio

    // Actualiza límites de calibración
    if (mx < magX_min) magX_min = mx;
    if (mx > magX_max) magX_max = mx;
    if (my < magY_min) magY_min = my;
    if (my > magY_max) magY_max = my;

    float mx_cal = 0.0;
    float my_cal = 0.0;

    // Evita división por cero sin romper la ejecución de la función
    if (magX_max != magX_min && magY_max != magY_min) {
      mx_cal = (float)(mx - magX_min) / (magX_max - magX_min) - 0.5;
      my_cal = (float)(my - magY_min) / (magY_max - magY_min) - 0.5;
    } else {
      mx_cal = (float)mx;
      my_cal = (float)my;
    }

    // Cálculo del rumbo
    float rumboRad = atan2(my_cal, mx_cal);
    int rumboGrados = rumboRad * 57.2957;
    
    if (rumboGrados < 0) rumboGrados += 360;

    robotReporta("RUMBO: %d DEG", rumboGrados);
    return rumboGrados;
  } else {
    robotReporta("ERROR: Faltan bytes en el buffer.");
    return -999;
  }
}
*/
/*
int leerBrujula() { 
  if (!mpu_ok) return -999;

  // Cambiado a AK8963_ADDRESS (0x0C) que es donde vive la brújula
  Wire.beginTransmission(AK8963_ADDRESS);
  Wire.write(0x03); // Registro HXL (Inicio de los datos del magnetómetro)
  
  int return_code = Wire.endTransmission(false);
  if (return_code != 0) {
    robotReporta("BRU FAIL I2C: %d", return_code);
    // Nota: No cambies mpu_ok a false permanentemente aquí para no romper el bucle de calibración
    return -999;
  }

  // Solicita los 7 bytes a la dirección de la brújula (0x0C)
  Wire.requestFrom(AK8963_ADDRESS, 7, true);
  
  if (Wire.available() >= 7) {
    // Al usar bypass, los ejes del AK8963 suelen venir invertidos o cambiados respecto al MPU.
    // Típicamente para el MPU-9250: el eje X magnético se lee en las primeras variables.
    uint8_t xl = Wire.read();
    uint8_t xh = Wire.read();
    int16_t mx = (int16_t)(xl | (xh << 8));

    uint8_t yl = Wire.read();
    uint8_t yh = Wire.read();
    int16_t my = (int16_t)(yl | (yh << 8));

    uint8_t zl = Wire.read();
    uint8_t zh = Wire.read();
    int16_t mz = (int16_t)(zl | (zh << 8));

    // LEER EL REGISTRO ST2 (Dirección 0x09) ES ESTRICTAMENTE OBLIGATORIO. 
    // Si no se lee, el chip bloquea las lecturas de los siguientes ciclos.
    uint8_t st2 = Wire.read(); 

    // Si el magnetómetro detecta un desbordamiento magnético, descartamos la lectura
    if (st2 & 0x08) {
      return -999; 
    }

    // Actualiza límites de calibración
    if (mx < magX_min) magX_min = mx;
    if (mx > magX_max) magX_max = mx;
    if (my < magY_min) magY_min = my;
    if (my > magY_max) magY_max = my;

    float mx_cal = 0.0;
    float my_cal = 0.0;

    if (magX_max != magX_min && magY_max != magY_min) {
      mx_cal = (float)(mx - magX_min) / (magX_max - magX_min) - 0.5;
      my_cal = (float)(my - magY_min) / (magY_max - magY_min) - 0.5;
    } else {
      mx_cal = (float)mx;
      my_cal = (float)my;
    }

    // En la brújula interna del MPU9250, la orientación del chip exige calcular el rumbo 
    // usando atan2(mx_cal, my_cal) o ajustar los signos debido a la disposición física del silicio.
    float rumboRad = atan2(mx_cal, my_cal); 
    int rumboGrados = rumboRad * 57.2957;
    
    if (rumboGrados < 0) rumboGrados += 360;

    robotReporta("RUMBO: %d DEG", rumboGrados);
    return rumboGrados;
  } else {
    robotReporta("ERROR: Buffer vacío");
    return -999;
  }
}
*/

int leerBrujula() { 
  // Forzamos el bus para este intento
  Wire.beginTransmission(0x0C); // Dirección real de la brújula AK8963
  Wire.write(0x03); 
  
  int return_code = Wire.endTransmission(false);
  if (return_code != 0) {
    robotReporta("Error I2C: %d", return_code);
    return -999; // Si da error aquí, el puente físico del bypass sigue cerrado
  }

  Wire.requestFrom(0x0C, 7, true);
  
  if (Wire.available() >= 7) {
    uint8_t xl = Wire.read();  uint8_t xh = Wire.read();
    int16_t mx = (int16_t)(xl | (xh << 8));

    uint8_t yl = Wire.read();  uint8_t yh = Wire.read();
    int16_t my = (int16_t)(yl | (yh << 8));

    uint8_t zl = Wire.read();  uint8_t zh = Wire.read();
    int16_t mz = (int16_t)(zl | (zh << 8));

    uint8_t st2 = Wire.read(); // Registro obligatorio de cierre

    if (st2 & 0x08) return -999; // Desbordamiento magnético

    // Actualiza límites de calibración
    if (mx < magX_min) magX_min = mx;   if (mx > magX_max) magX_max = mx;
    if (my < magY_min) magY_min = my;   if (my > magY_max) magY_max = my;

    float mx_cal = 0.0, my_cal = 0.0;
    if (magX_max != magX_min && magY_max != magY_min) {
      mx_cal = (float)(mx - magX_min) / (magX_max - magX_min) - 0.5;
      my_cal = (float)(my - magY_min) / (magY_max - magY_min) - 0.5;
    } else {
      mx_cal = (float)mx;               my_cal = (float)my;
    }

    float rumboRad = atan2(mx_cal, my_cal); 
    int rumboGrados = rumboRad * 57.2957;
    if (rumboGrados < 0) rumboGrados += 360;

    return rumboGrados;
  } 
  return -999;
}

bool calibrarBrujulaEnGiro() { // Google Gemini 20260619
  // updated = "20260622";
  mpu_ok = true;
  int rumboGrados = 0;
  //if (!mpu_ok) return -999;
  robotReporta("--- TALLER: CALIBRAR ---");
  robotReporta("Gire el robot en 360...");
  delay(3000);

  // 1. Configurar sentido de giro (Pivotar sobre su eje)
  // Motor Izquierdo: Adelante (PWM 150)
  digitalWrite(2, HIGH);
  digitalWrite(8, LOW);
  analogWrite(3, 150);

  // Motor Derecho: Atrás (PWM 150)
  digitalWrite(12, LOW);
  digitalWrite(13, HIGH);
  analogWrite(11, 150);

  // 2. Ventana de muestreo dinámico (4 segundos de rotación)
  unsigned long tiempoInicio = millis();
  while (millis() - tiempoInicio < 4000) {
    rumboGrados = leerBrujula(); // Esta función actualiza magX_min/max y magY_min/max sobre la marcha
    if (rumboGrados == -999) {
      break;
    }
    delay(50);     // Muestreo a 20Hz para no saturar el bus
  }

  // 3. Frenar motores inmediatamente después de la calibración
  analogWrite(3, 0);   digitalWrite(2, LOW);
  analogWrite(11, 0);  digitalWrite(13, LOW);
  
  robotReporta("Frenando... Calibrado.");
  robotReporta("X:[%d,%d] Y:[%d,%d]", magX_min, magX_max, magY_min, magY_max);
  
  // 4. Mostrar rumbo final estático ya corregido
  delay(500);
  rumboGrados = leerBrujula();
  if (rumboGrados != -999) {
    robotReporta("-- CALIBRACION OK --");
    mpu_ok = true;
  } else {
    robotReporta("- CALIBRACION FALLO -");
    mpu_ok = false;
  }
  return mpu_ok;
}

long obtenerDistanciaCM(int muestras = 5) {
  float distancia_cm = 0;
  for (int i=1; i<muestras; i++) {
    digitalWrite(TRIG_PIN, LOW);
    delayMicroseconds(2);
    digitalWrite(TRIG_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG_PIN, LOW);
    long duracion = pulseIn(ECHO_PIN, HIGH, 30000); 
    if (duracion == 0) return 999; 
    distancia_cm += duracion * 0.0343 / 2;
    delay(32);
  }
  return long(distancia_cm/muestras);
}

void motores(int pwmIzq, int pwmDer) {
  if (pwmIzq == 0) {
    analogWrite(PIN_MOTOR_ENA, 0);
    digitalWrite(PIN_MOTOR_IN1, LOW);
    digitalWrite(PIN_MOTOR_IN2, LOW);
  } else {
    analogWrite(PIN_MOTOR_ENA, abs(pwmIzq));
    if (pwmIzq > 0) {
      digitalWrite(PIN_MOTOR_IN1, HIGH);
      digitalWrite(PIN_MOTOR_IN2, LOW);
    } else {
      digitalWrite(PIN_MOTOR_IN1, LOW);
      digitalWrite(PIN_MOTOR_IN2, HIGH);
    }
  }

  if (pwmDer == 0) {
    analogWrite(PIN_MOTOR_ENB, 0);
    digitalWrite(PIN_MOTOR_IN3, LOW);
    digitalWrite(PIN_MOTOR_IN4, LOW);
  } else {
    analogWrite(PIN_MOTOR_ENB, abs(pwmDer));
    if (pwmDer > 0) {
      digitalWrite(PIN_MOTOR_IN3, HIGH);
      digitalWrite(PIN_MOTOR_IN4, LOW);
    } else {
      digitalWrite(PIN_MOTOR_IN3, LOW);
      digitalWrite(PIN_MOTOR_IN4, HIGH);
    }
  }
}

void calibraMotores() {
  if (!motores_calibrados) {
    LPWM_MIN_TEST = 0;  
    RPWM_MIN_TEST = 0;  
    LPWM_MAX_TEST = 0;
    RPWM_MAX_TEST = 0; 
    delay(3000);
    unsigned long test_start_time = millis();
    delay(500); // <-- Pausa para estabilizar sensores.
    long distInicial = obtenerDistanciaCM(5);
    robotReporta("Pista: %ld cm", distInicial);
    delay(500); // <-- Pausa para estabilizar sensores.
    long distCalibre = obtenerDistanciaCM(5);
    robotReporta("TestI: %ld cm", distCalibre);
    int potenciaIzq = 0;
    int potenciaDer = 0;
    while ((potenciaIzq < 255) && (abs(distCalibre - distInicial) < 15 ) && ((millis() - test_start_time) < 400000)) {
      motores(potenciaIzq, 0);
      delay(300);
      motores(0, 0);   
      delay(200);
      distCalibre = obtenerDistanciaCM(5);
      potenciaIzq++;
    }
    delay(3000);
    test_start_time = millis();
    delay(500); // <-- Pausa para estabilizar sensores.
    distInicial = obtenerDistanciaCM(5);
    robotReporta("Pista: %ld cm", distInicial);
    delay(500); // <-- Pausa para estabilizar sensores.
    distCalibre = obtenerDistanciaCM(5);
    robotReporta("TestD: %ld cm", distCalibre);
    while ((potenciaDer < 255) && (abs(distCalibre - distInicial) < 15 ) && ((millis() - test_start_time) < 400000)) {
      motores(0, potenciaDer);
      delay(300);
      motores(0, 0);   
      delay(200);
      distCalibre = obtenerDistanciaCM(5);
      potenciaDer++;
    }
    pantalla_oled.clearDisplay();
    lineasActivas = 0;
    pantalla_oled.setTextSize(1);
    pantalla_oled.setCursor(0, 0);
    pantalla_oled.print(F("=== REPORTE MOTOR ==="));
    pantalla_oled.setCursor(0, 16);
    pantalla_oled.print(F("PWM:"));pantalla_oled.print(potenciaIzq);
    pantalla_oled.setCursor(0, 26);
    pantalla_oled.print(F("PWM:"));pantalla_oled.print(potenciaDer);
    pantalla_oled.display();
    // Calibra baja potencia:
    LPWM_MIN_TEST = potenciaIzq;  
    RPWM_MIN_TEST = potenciaDer; 
    // FALTA: Calibrar alta potencia ...
    LPWM_MAX_TEST = 34;
    RPWM_MAX_TEST = 255; 
    motores_calibrados = true;
    delay(10000);
    //while(true);
  }
}

void giroTrompo(bool rapido = false, unsigned long tiempo_ms = 315, bool lado = 0) {
  // updated = "20260622";
  // Potencia de motores: 20%
  int pwmIzq = ((LPWM_MAX_TEST - LPWM_MIN_TEST)/5) + LPWM_MIN_TEST;
  int pwmDer = -(((RPWM_MAX_TEST - RPWM_MIN_TEST)/5) + RPWM_MIN_TEST);
  if (rapido) {
    // Potencia de motores: 100%
    pwmIzq = LPWM_MAX_TEST;
    pwmDer = -RPWM_MAX_TEST;
  }
  unsigned long tiempo_f_ms = tiempo_ms / ((LPWM_MAX_TEST + abs(RPWM_MAX_TEST)) / (LPWM_MIN_TEST + abs(RPWM_MIN_TEST)));
  // El robot debe girar 180° sobre su propio eje:
  if (lado) {
    motores(pwmIzq, pwmDer);
  } else {
    motores(pwmDer, pwmIzq);
  }
  if (!rapido) {
    delay(tiempo_f_ms);// Girando. (<-- Se deberia ajustar automáticamente por potencia)
  } else {
    delay(tiempo_ms);// Girando. (<-- Se deberia ajustar automáticamente por potencia)
  }
  motores(0, 0);   
  delay(1000);
}

long avanza(int distancia, int potencia = 1, unsigned long tiempo_max_ms = 5000, int correccion_rumbo = 0) {
  potencia = min(max(potencia, 1), 5);
  unsigned long internal_delay = 200;
  long distancia_inicial = obtenerDistanciaCM(5);
  long distancia_final = min(distancia_inicial - distancia, 25);
  long distancia_actual = distancia_inicial;
  int pwmIzq = min((((LPWM_MAX_TEST - LPWM_MIN_TEST)/5)*potencia) + LPWM_MIN_TEST, 255);
  int pwmDer = min((((RPWM_MAX_TEST - RPWM_MIN_TEST)/5)*potencia) + RPWM_MIN_TEST, 255);
  /*
  if (distancia < 0) { // ¿reversa?
    distancia = min(distancia, -25);
    pwmIzq = -pwmIzq;
    pwmDer = =pwmDer;
  }
  */
    unsigned long tiempo_inicial = millis();
    unsigned long tiempo_transcurrido = millis() - tiempo_inicial;
    long distancia_recorrida = 0; 
    motores(pwmIzq, pwmDer);
    while ((distancia_actual > distancia_final) && (tiempo_transcurrido < tiempo_max_ms)) {
      tiempo_transcurrido = millis() - tiempo_inicial;
      distancia_actual = obtenerDistanciaCM(1);
      distancia_recorrida += distancia_inicial - distancia_actual;
      delay(internal_delay);
      if (correccion_rumbo != 0) {
        if (correccion_rumbo < 0) {
          motores(0, 220);
          delay(internal_delay);
          motores(pwmIzq, pwmDer);
        } else {
          motores(220, 0);
          delay(internal_delay);
          motores(pwmIzq, pwmDer);
        }
      } else {
        delay(internal_delay);
      }
    }
    motores(0, 0); 
    return distancia_recorrida;
}

void setup() {
  //Serial.begin(9600);
  //while (!Serial); 
  Wire.begin();
  
  // 1. Configuración de Pines de Motores y Ultrasonido
  pinMode(TRIG_PIN, OUTPUT); pinMode(ECHO_PIN, INPUT);
  pinMode(PIN_MOTOR_ENA, OUTPUT); pinMode(PIN_MOTOR_IN1, OUTPUT); pinMode(PIN_MOTOR_IN2, OUTPUT);
  pinMode(PIN_MOTOR_ENB, OUTPUT); pinMode(PIN_MOTOR_IN3, OUTPUT); pinMode(PIN_MOTOR_IN4, OUTPUT);
  motores(0, 0); 

  delay(100); // Pausa de hardware para estabilizar voltajes de los sensores

  // 1. Despertar MPU-9250 en la dirección 0x69
  Wire.beginTransmission(MPU9250_ADDRESS); // 0x69
  Wire.write(PWR_MGMT_1); 
  Wire.write(0x00);
  Wire.endTransmission();
  delay(20);

  // 2. Desactivar maestro interno en la dirección 0x69
  Wire.beginTransmission(MPU9250_ADDRESS); // 0x69
  Wire.write(USER_CTRL); 
  Wire.write(0x00);
  Wire.endTransmission();
  delay(10);

  // 3. Activar Bypass I2C en la dirección 0x69
  Wire.beginTransmission(MPU9250_ADDRESS); // 0x69
  Wire.write(INT_PIN_CFG); 
  Wire.write(0x02); // Abre el puente físico hacia la brújula
  Wire.endTransmission();
  delay(50); // Pausa clave para que la brújula reciba energía del bus

  // 4. Configurar la Brújula interna (apunta a la dirección 0x0C)
  inicializarMagnetometro();

  // 4. INICIALIZAR LA PANTALLA OLED (Ahora que el bus está listo)
  if (pantalla_oled.begin(SSD1306_SWITCHCAPVCC, DIRECCION_I2C)) {
    tienePantallaOLED = true;
    pantalla_oled.clearDisplay();
    pantalla_oled.display();
  }

  // 5. PROCESO DE LA BRÚJULA
  // Nota: Asegúrate de que dentro de 'calibrarBrujulaEnGiro()' y 'leerBrujula()' 
  // estés apuntando a la dirección I2C de la brújula, que es la 0x0C (no la 0x68).
  bool mpu_ok = calibrarBrujulaEnGiro();
  delay(100); // Reduje tiempos muertos innecesarios de 3000ms a 1000ms
  leerBrujula();
  delay(100); // <-- Pausa para estabilizar sensores.

  robotReporta("== CALIBRA MOTORES ==");
  calibraMotores();
  long distInicial = obtenerDistanciaCM(5);
  robotReporta("Pista: %ld cm", distInicial);
/* 
  if (distInicial < DISTANCIA_MIN_PISTA) {
    robotReporta("ERROR: Pista corta");
    while(true); 
  }
*/
  robotReporta("Suelo en 3 seg...");
  delay(3000);

  // Busco minima potencia:

  // 1. EJECUCIÓN SÍLENCIO: TEST 1 (MÍNIMO)
  pantalla_oled.clearDisplay();
  lineasActivas = 0;
  robotReporta("Corriendo P1..."); 
  
  long p1_Inicio = obtenerDistanciaCM(5);
  motores(LPWM_MIN_TEST, RPWM_MIN_TEST); 
  delay(TIEMPO_MARCHA_1);
  motores(0, 0);
  delay(100); // <-- Pausa para estabilizar sensores.
  long p1_Fin = obtenerDistanciaCM(5);
  long deltaP1 = abs(p1_Fin - p1_Inicio);
  
  delay(3000); 

  // 2. EJECUCIÓN SÍLENCIO: TEST 2 (MÁXIMO)
  pantalla_oled.clearDisplay();
  lineasActivas = 0;
  robotReporta("Corriendo P2...");
  
  delay(100); // <-- Pausa para estabilizar sensores.
  long p2_Inicio = obtenerDistanciaCM(5);
  motores(LPWM_MAX_TEST, RPWM_MAX_TEST); 
  delay(TIEMPO_MARCHA_2);
  motores(0, 0);   
  delay(100); // <-- Pausa para estabilizar sensores.
  long p2_Fin = obtenerDistanciaCM(5);
  long deltaP2 = abs(p2_Fin - p2_Inicio);

  // 
  long rumboGrados = leerBrujula();

  // 3. PANTALLA FINAL DETALLADA FIJA (Estilo reporte de laboratorio)
  pantalla_oled.clearDisplay();
  pantalla_oled.setTextColor(SSD1306_WHITE);
  pantalla_oled.setTextSize(1);
  
  pantalla_oled.setCursor(0, 0);
  pantalla_oled.print(F("=== REPORTE MOTOR ==="));
  
  // Bloque Test 1
  pantalla_oled.setCursor(0, 16);
  pantalla_oled.print(F("PRUEBA 1 (MINIMA):"));
  pantalla_oled.setCursor(0, 26);
  pantalla_oled.print(F("PWM:")); pantalla_oled.print(LPWM_MIN_TEST);
  pantalla_oled.print(F(",")); pantalla_oled.print(RPWM_MIN_TEST);
  pantalla_oled.print(F(" Dist:")); pantalla_oled.print(deltaP1); pantalla_oled.print(F("cm"));

  // Bloque Test 2
  pantalla_oled.setCursor(0, 36);
  pantalla_oled.print(F("PRUEBA 2 (MAXIMA):"));
  pantalla_oled.setCursor(0, 46);
  pantalla_oled.print(F("PWM:")); pantalla_oled.print(LPWM_MAX_TEST);
  pantalla_oled.print(F(",")); pantalla_oled.print(RPWM_MAX_TEST);
  pantalla_oled.print(F(" Dist:")); pantalla_oled.print(deltaP2); pantalla_oled.print(F("cm"));
  
  pantalla_oled.setCursor(0, 56);
  pantalla_oled.print(F("Grados:")); pantalla_oled.print(rumboGrados);

  pantalla_oled.display();
  delay(3000);
  giroTrompo(false, 315, 0);
  long distancia_recorrida = avanza(100, 1, 5000, -1);
}

void loop() {
  // Se mantiene estático mostrando los resultados numéricos de las constantes y los deltas.
}