#include <ESP32Servo.h>

// Pines
const int servoPin = 13;
const int trigPin = 14;
const int echoPin = 35;

// Objeto Servo
Servo myServo;

// Umbral de detección de obstáculos
const int distanciaObstaculo = 20;  // cm

// Ángulos a escanear
const int angulos[] = {0, 45, 90, 135, 180};
const int numLecturas = sizeof(angulos) / sizeof(angulos[0]);
float distancias[numLecturas];

void setup() {
  Serial.begin(115200);

  myServo.setPeriodHertz(50);
  myServo.attach(servoPin, 500, 2400);

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  Serial.println("🚗 Escaneo panorámico activo...");
}

void loop() {
  // 1. Escaneo en los 5 ángulos
  for (int i = 0; i < numLecturas; i++) {
    myServo.write(angulos[i]);
    delay(200);  // Tiempo para que el servo llegue

    distancias[i] = medirDistancia();

    Serial.print("Ángulo ");
    Serial.print(angulos[i]);
    Serial.print("°: ");
    Serial.print(distancias[i]);
    Serial.println(" cm");
  }

  // 2. Análisis: ¿obstáculo al frente?
  if (distancias[2] <= distanciaObstaculo && distancias[2] > 0) {
    Serial.println("🚧 Obstáculo al frente");

    // Buscar el lado más despejado (izquierda o derecha)
    float izquierda = (distancias[0] + distancias[1]) / 2.0;
    float derecha = (distancias[3] + distancias[4]) / 2.0;

    if (izquierda > derecha) {
      Serial.println("🔁 Girar a la izquierda");
      // Aquí iría el código para girar a la izquierda
    } else {
      Serial.println("🔁 Girar a la derecha");
      // Aquí iría el código para girar a la derecha
    }

  } else {
    Serial.println("✅ Camino despejado. Avanzar.");
    // Aquí iría el código para avanzar
  }

  delay(100);  // Esperar antes del siguiente escaneo
}

float medirDistancia() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);

  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duracion = pulseIn(echoPin, HIGH, 30000);  // timeout de 30ms
  if (duracion == 0) return -1;

  return duracion * 0.0343 / 2.0;
}