/* * MECRO KIT - PROYECTO 11: RADAR DE EXPLORACIÓN
 * Objetivo: Integración de Mecánica (Servo), Sensores y Matemáticas Polares
 * Hardware: ESP32, Servo, HC-SR04, OLED
 * Librerías: ESP32Servo, Adafruit SSD1306
 */

#include <ESP32Servo.h>
#include <Adafruit_GFX.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 HARDWARE ---
Servo miServo;
const int PIN_SERVO = 13; // Salida PWM
const int PIN_TRIG  = 5;  // Salida Pulso
const int PIN_ECHO  = 18; // Entrada Eco

void setup() {
  Serial.begin(115200);
  
  // Configurar Servo
  // ESP32Servo se encarga de los timers PWM automáticamente
  miServo.attach(PIN_SERVO);
  
  // Configurar Sensor Ultrasonido
  pinMode(PIN_TRIG, OUTPUT);
  pinMode(PIN_ECHO, INPUT);
  
  // Iniciar OLED
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("Fallo OLED - Revisar I2C"));
    for(;;);
  }
  
  // Pantalla de inicio
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(30, 20);
  display.println("MECRO RADAR");
  display.setCursor(30, 35);
  display.println("Iniciando...");
  display.display();
  delay(2000);
}

// Función auxiliar para medir distancia (cm)
int obtenerDistancia() {
  digitalWrite(PIN_TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(PIN_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG, LOW);
  
  long duracion = pulseIn(PIN_ECHO, HIGH, 25000); // Timeout 25ms (~4m)
  int distanciaCm = duracion * 0.034 / 2;
  
  // Filtrar lecturas erróneas o muy lejanas para la escala de pantalla
  if (distanciaCm > 60 || distanciaCm == 0) distanciaCm = 60;
  
  return distanciaCm;
}

void loop() {
  // BARRIDO DE IDA (15° a 165°)
  // Evitamos 0° y 180° exactos para no forzar el tope mecánico del servo
  for(int angulo = 15; angulo <= 165; angulo += 3) {
    escanearSector(angulo);
  }
  
  // RETORNO RÁPIDO (Sin medir, para reiniciar barrido)
  for(int angulo = 165; angulo >= 15; angulo -= 5) {
     miServo.write(angulo);
     delay(15);
  }
}

void escanearSector(int angulo) {
    miServo.write(angulo);
    delay(40); // Espera mecánica vital (Settling time)
    
    int distancia = obtenerDistancia();
    
    // --- VISUALIZACIÓN ---
    display.clearDisplay();
    
    // 1. Dibujar líneas de referencia (Arcos de distancia)
    // El centro del radar está en (64, 64) -> Parte inferior central
    display.drawCircle(64, 64, 20, WHITE); // Zona 20cm
    display.drawCircle(64, 64, 40, WHITE); // Zona 40cm
    display.drawCircle(64, 64, 60, WHITE); // Zona 60cm (Límite visual)
    
    // 2. Calcular vector de barrido (La "Aguja")
    // Convertir Grados a Radianes para funciones trigonométricas
    float rad = angulo * PI / 180.0;
    
    // Transformación Polar a Cartesiana
    // x = r * cos(theta), y = r * sin(theta)
    // Restamos en Y porque en pantallas la coordenada 0 está arriba
    int lineaX = 64 - (60 * cos(rad));
    int lineaY = 64 - (60 * sin(rad));
    
    display.drawLine(64, 64, lineaX, lineaY, WHITE);
    
    // 3. Dibujar Objeto Detectado
    if(distancia < 60) {
       // Calcular posición exacta del punto en pantalla
       int objX = 64 - (distancia * cos(rad));
       int objY = 64 - (distancia * sin(rad));
       
       // Dibujar un "Blip" (Punto solido)
       display.fillCircle(objX, objY, 4, WHITE);
    }
    
    // Telemetría en pantalla
    display.setCursor(0,0);
    display.print("Ang:"); display.print(angulo);
    display.setCursor(80,0);
    display.print("Dst:"); display.print(distancia);
    
    display.display();
}