// === Pines del L293D ===
// Motor A (izquierdo)
const int motorA1 = 3;
const int motorA2 = 4;
const int enableA = 5;  // Control de velocidad PWM

// Motor B (derecho)
const int motorB1 = 7;
const int motorB2 = 8;
const int enableB = 9;  // Control de velocidad PWM

// === Pines sensores Sharp ===
const int sharpIzq = A2;     // Sensor lateral izquierdo
const int sharpFront1 = A3;  // Sensor frontal izquierdo
const int sharpFront2 = A4;  // Sensor frontal derecho
const int sharpDer = A5;     // Sensor lateral derecho

// === Sensores de piso digitales ===
const int pisoDer = A0; // Sensor de piso derecho
const int pisoIzq = A1; // Sensor de piso izquierdo

// === Parámetros de movimiento ===
int velocidadMax = 200;       // Velocidad máxima del robot
int velocidadCurva = 160;     // Velocidad al girar
int umbralOponente = 400;     // Valor analógico que indica proximidad (menor = más cerca)

void setup() {
  // Configurar pines de motores como salida
  pinMode(motorA1, OUTPUT);
  pinMode(motorA2, OUTPUT);
  pinMode(enableA, OUTPUT);

  pinMode(motorB1, OUTPUT);
  pinMode(motorB2, OUTPUT);
  pinMode(enableB, OUTPUT);

  // Configurar sensores de piso como entrada
  pinMode(pisoDer, INPUT);
  pinMode(pisoIzq, INPUT);

  // No es necesario configurar los pines analógicos de los sensores Sharp
  // Puedes habilitar Serial.begin(9600); si deseas monitorear sensores
}

void loop() {
  // Leer sensores de piso (devuelven HIGH o LOW)
  int sueloDer = digitalRead(pisoDer);
  int sueloIzq = digitalRead(pisoIzq);

  // Leer sensores Sharp (valores entre 0 y 1023)
  int valIzq = analogRead(sharpIzq);
  int valFront1 = analogRead(sharpFront1);
  int valFront2 = analogRead(sharpFront2);
  int valDer = analogRead(sharpDer);

  // === 1. Evitar salirse del ring ===
  // Si algún sensor detecta el borde (LOW = borde blanco o sin línea)
  if (sueloIzq == LOW || sueloDer == LOW) {
    retroceder();         // Retrocede rápidamente
    delay(300);           // Espera un poco
    girarDerecha();       // Gira para cambiar de dirección
    delay(300);           
    detener();            // Pausa antes de seguir
    return;               // Finaliza esta iteración del loop
  }

  // === 2. Estrategia de ataque ===
  if (valFront1 < umbralOponente || valFront2 < umbralOponente) {
    avanzar();            // Oponente detectado al frente
  } else if (valIzq < umbralOponente) {
    girarIzquierda();     // Oponente a la izquierda
  } else if (valDer < umbralOponente) {
    girarDerecha();       // Oponente a la derecha
  } else {
    buscar();             // No se detecta nada, buscar girando
  }
}

// === Funciones de movimiento ===

// Avanza recto
void avanzar() {
  digitalWrite(motorA1, HIGH);
  digitalWrite(motorA2, LOW);
  analogWrite(enableA, velocidadMax);

  digitalWrite(motorB1, HIGH);
  digitalWrite(motorB2, LOW);
  analogWrite(enableB, velocidadMax);
}

// Retrocede
void retroceder() {
  digitalWrite(motorA1, LOW);
  digitalWrite(motorA2, HIGH);
  analogWrite(enableA, velocidadMax);

  digitalWrite(motorB1, LOW);
  digitalWrite(motorB2, HIGH);
  analogWrite(enableB, velocidadMax);
}

// Gira hacia la izquierda
void girarIzquierda() {
  digitalWrite(motorA1, LOW);
  digitalWrite(motorA2, HIGH);
  analogWrite(enableA, velocidadCurva);

  digitalWrite(motorB1, HIGH);
  digitalWrite(motorB2, LOW);
  analogWrite(enableB, velocidadCurva);
}

// Gira hacia la derecha
void girarDerecha() {
  digitalWrite(motorA1, HIGH);
  digitalWrite(motorA2, LOW);
  analogWrite(enableA, velocidadCurva);

  digitalWrite(motorB1, LOW);
  digitalWrite(motorB2, HIGH);
  analogWrite(enableB, velocidadCurva);
}

// Detiene los motores
void detener() {
  analogWrite(enableA, 0);
  analogWrite(enableB, 0);
}

// Búsqueda: gira hacia la izquierda buscando oponente
void buscar() {
  girarIzquierda();
  delay(400);
  detener();
}