#include <ESP32Servo.h> // Para el control del servo
#include <NewPing.h>    // Para el sensor ultrasónico

// Definiciones de pines para el Puente H L298N
#define IN1_MOTOR_DERECHO 17 // Pin de control Motor Derecho (adelante)
#define IN2_MOTOR_DERECHO 16 // Pin de control Motor Derecho (atrás)
#define IN3_MOTOR_IZQUIERDO 4 // Pin de control Motor Izquierdo (adelante)
#define IN4_MOTOR_IZQUIERDO 2 // Pin de control Motor Izquierdo (atrás)

// Pines de habilitación (ENA y ENB) para control de velocidad (PWM)
#define ENA_MOTOR_DERECHO  22 // GPIO para ENA (Motor Derecho)
#define ENB_MOTOR_IZQUIERDO 23  // GPIO para ENB (Motor Izquierdo)

// Definiciones de pines para el Sensor Ultrasónico HC-SR04
#define TRIG_PIN 19
#define ECHO_PIN 18
#define MAX_DISTANCE 200 // Máxima distancia a medir en cm

// Definición de pin para el Servo Motor
#define SERVO_PIN 13

// Creación de objetos
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE); // Objeto para el sensor ultrasónico
Servo myServo;                                  // Objeto para el servo motor

// Distancia de umbral para detectar obstáculos (en cm)
const int DISTANCIA_UMBRAL = 25; // Si el obstáculo está a menos de 25 cm, reaccionar

// Variables para control de PWM (velocidad)
const int freq = 30000; // Frecuencia del PWM (Hz)
const int resolution = 8; // Resolución del PWM (bits), 2^8 = 256 valores (0-255)
const int channel_ENA = 0; // Canal PWM para ENA
const int channel_ENB = 1; // Canal PWM para ENB

int velocidadMotor = 180; // Velocidad inicial de los motores (0-255)

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

  // Configuración de pines de motores como salida
  pinMode(IN1_MOTOR_DERECHO, OUTPUT);
  pinMode(IN2_MOTOR_DERECHO, OUTPUT);
  pinMode(IN3_MOTOR_IZQUIERDO, OUTPUT);
  pinMode(IN4_MOTOR_IZQUIERDO, OUTPUT);
  
  // Configuración de PWM para ENA y ENB
  ledcSetup(channel_ENA, freq, resolution);
  ledcAttachPin(ENA_MOTOR_DERECHO, channel_ENA);
  ledcSetup(channel_ENB, freq, resolution);
  ledcAttachPin(ENB_MOTOR_IZQUIERDO, channel_ENB);

  // Adjuntar el servo al pin
  myServo.attach(SERVO_PIN);
  myServo.write(90); // Centrar el servo al inicio
  delay(1000); // Dar tiempo para que el servo se centre
}

// Funciones para el control de los motores con velocidad (PWM)
void setVelocidad(int velocidad) {
  if (velocidad < 0) velocidad = 0;
  if (velocidad > 255) velocidad = 255;
  ledcWrite(channel_ENA, velocidad);
  ledcWrite(channel_ENB, velocidad);
}

void moverAdelante() {
  setVelocidad(velocidadMotor); // Aplicar velocidad
  digitalWrite(IN1_MOTOR_DERECHO, HIGH);
  digitalWrite(IN2_MOTOR_DERECHO, LOW);
  digitalWrite(IN3_MOTOR_IZQUIERDO, HIGH);
  digitalWrite(IN4_MOTOR_IZQUIERDO, LOW);
  Serial.println("Movimiento: Adelante");
}

void moverAtras() {
  setVelocidad(velocidadMotor); // Aplicar velocidad
  digitalWrite(IN1_MOTOR_DERECHO, LOW);
  digitalWrite(IN2_MOTOR_DERECHO, HIGH);
  digitalWrite(IN3_MOTOR_IZQUIERDO, LOW);
  digitalWrite(IN4_MOTOR_IZQUIERDO, HIGH);
  Serial.println("Movimiento: Atrás");
}

void girarDerecha() {
  setVelocidad(velocidadMotor); // Aplicar velocidad de giro si se desea diferente, o usar la misma
  digitalWrite(IN1_MOTOR_DERECHO, LOW);
  digitalWrite(IN2_MOTOR_DERECHO, LOW); // Detener motor derecho (o invertir para giro sobre el eje)
  digitalWrite(IN3_MOTOR_IZQUIERDO, HIGH);
  digitalWrite(IN4_MOTOR_IZQUIERDO, LOW);
  Serial.println("Movimiento: Girar Derecha");
}

void girarIzquierda() {
  setVelocidad(velocidadMotor); // Aplicar velocidad de giro
  digitalWrite(IN1_MOTOR_DERECHO, HIGH);
  digitalWrite(IN2_MOTOR_DERECHO, LOW);
  digitalWrite(IN3_MOTOR_IZQUIERDO, LOW); // Detener motor izquierdo (o invertir)
  digitalWrite(IN4_MOTOR_IZQUIERDO, LOW);
  Serial.println("Movimiento: Girar Izquierda");
}

void detener() {
  setVelocidad(0); // Detener la velocidad (0 PWM)
  digitalWrite(IN1_MOTOR_DERECHO, LOW);
  digitalWrite(IN2_MOTOR_DERECHO, LOW);
  digitalWrite(IN3_MOTOR_IZQUIERDO, LOW);
  digitalWrite(IN4_MOTOR_IZQUIERDO, LOW);
  Serial.println("Movimiento: Detenido");
}

// Función para leer la distancia del sensor ultrasónico
int leerDistancia() {
  delay(50); // Pequeña pausa entre mediciones
  unsigned int uS = sonar.ping(); // Realiza un ping y devuelve la duración en microsegundos
  int distanciaCm = uS / US_ROUNDTRIP_CM; // Convierte microsegundos a cm

  if (distanciaCm == 0) { // Si el sensor no detecta nada, o está fuera de rango, devuelve un valor alto
    return MAX_DISTANCE;
  }
  return distanciaCm;
}

void loop() {
  int distanciaFrente = leerDistancia();
  Serial.print("Distancia al frente: ");
  Serial.print(distanciaFrente);
  Serial.println(" cm");

  if (distanciaFrente < DISTANCIA_UMBRAL) {
    Serial.println("¡Obstáculo detectado! Iniciando maniobra de evasión...");
    detener(); // Detener el carro
    delay(500); // Esperar un momento

    moverAtras(); // Retroceder un poco
    delay(700);
    detener();
    delay(300);

    // Escanear para ver la mejor dirección
    int distanciaIzquierda, distanciaDerecha;

    // Mirar a la izquierda (por ejemplo, a 15 grados)
    myServo.write(15);
    delay(700); // Esperar que el servo se mueva
    distanciaIzquierda = leerDistancia();
    Serial.print("Distancia izquierda: ");
    Serial.print(distanciaIzquierda);
    Serial.println(" cm");

    // Mirar a la derecha (por ejemplo, a 165 grados)
    myServo.write(165);
    delay(700); // Esperar que el servo se mueva
    distanciaDerecha = leerDistancia();
    Serial.print("Distancia derecha: ");
    Serial.print(distanciaDerecha);
    Serial.println(" cm");

    // Volver el servo al centro
    myServo.write(90);
    delay(700); // Esperar que el servo se centre

    // Decidir la dirección de giro
    if (distanciaIzquierda > distanciaDerecha) {
      Serial.println("Girando a la izquierda...");
      girarIzquierda();
      delay(1000); // Tiempo de giro
    } else {
      Serial.println("Girando a la derecha...");
      girarDerecha();
      delay(1000); // Tiempo de giro
    }
    detener(); // Detener después del giro
    delay(500); // Pequeña pausa
  } else {
    // No hay obstáculos, seguir adelante
    moverAdelante();
  }

  delay(50); // Pequeña pausa para evitar mediciones y acciones muy rápidas
}