// ================== MOTORES ==================
int motB0 = 2;
int motB1 = 3; // PWM
int motA1 = 4;
int motA0 = 5; // PWM

// ================== SENSORES DE LÍNEA ==================
int sensorleft   = A7;
int sensorcenter = A6;
int sensorright  = A5;

// Ajusta estos valores viendo el Monitor Serial
int umbralLeft   = 550;
int umbralCenter = 550;
int umbralRight  = 550;

// Para fondo blanco con borde negro:
// normalmente el negro da valores BAJOS, por eso está en false.
bool lineaConValorAlto = false;

// ================== BUZZER ==================
int buzzer = 6;

// ================== ULTRASÓNICO ==================
int echo = 8;
int triger = 7;

// Distancia para atacar al oponente
int distanciaAtaque = 25; // cm

// Variables de sensores
int lLeft = 0;
int lCenter = 0;
int lRight = 0;

unsigned long tiempoDebug = 0;

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

  pinMode(motA0, OUTPUT);
  pinMode(motA1, OUTPUT);
  pinMode(motB0, OUTPUT);
  pinMode(motB1, OUTPUT);

  pinMode(buzzer, OUTPUT);

  pinMode(echo, INPUT);
  pinMode(triger, OUTPUT);
  digitalWrite(triger, LOW);

  pinMode(sensorright, INPUT);
  pinMode(sensorcenter, INPUT);
  pinMode(sensorleft, INPUT);

  alto();

  Serial.println("Robot Sumo Fondo Blanco iniciado");
  Serial.println("Ajusta los umbrales si los sensores no detectan bien el borde.");
}

void loop()
{
  leerSensores();
  imprimirDebug();

  // Primero revisa si detecta el borde negro
  if (hayLinea())
  {
    escaparLinea();
    return;
  }

  // Después busca al oponente con el ultrasónico
  long d = distancia();

  if (d > 0 && d <= distanciaAtaque)
  {
    adelanteFuerte();
  }
  else
  {
    buscarOponente();
  }
}

// ================== LECTURA DE SENSORES ==================

void leerSensores()
{
  lLeft   = leerPromedio(sensorleft);
  lCenter = leerPromedio(sensorcenter);
  lRight  = leerPromedio(sensorright);
}

int leerPromedio(int pin)
{
  long suma = 0;

  for (int i = 0; i < 8; i++)
  {
    suma += analogRead(pin);
    delayMicroseconds(500);
  }

  return suma / 8;
}

bool detectaLinea(int valor, int umbral)
{
  if (lineaConValorAlto)
  {
    return valor > umbral;
  }
  else
  {
    return valor < umbral;
  }
}

bool hayLinea()
{
  bool izquierda = detectaLinea(lLeft, umbralLeft);
  bool centro    = detectaLinea(lCenter, umbralCenter);
  bool derecha   = detectaLinea(lRight, umbralRight);

  if (izquierda || centro || derecha)
  {
    return true;
  }

  return false;
}

// ================== ULTRASÓNICO ==================

long distancia()
{
  long tiempo;
  long distanciaCm;

  digitalWrite(triger, LOW);
  delayMicroseconds(2);

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

  tiempo = pulseIn(echo, HIGH, 25000UL);

  if (tiempo == 0)
  {
    return 999;
  }

  distanciaCm = tiempo / 58;

  if (distanciaCm <= 1 || distanciaCm > 150)
  {
    return 999;
  }

  return distanciaCm;
}

// ================== COMPORTAMIENTO ==================

void escaparLinea()
{
  alto();
  tone(buzzer, 1000, 100);
  delay(150);

  atras();
  delay(500);

  // Si el sensor izquierdo detecta borde, gira a la derecha
  if (detectaLinea(lLeft, umbralLeft))
  {
    derecha();
    delay(500);
  }
  // Si el sensor derecho detecta borde, gira a la izquierda
  else if (detectaLinea(lRight, umbralRight))
  {
    izquierda();
    delay(500);
  }
  // Si detecta el centro, retrocede y gira
  else
  {
    derecha();
    delay(500);
  }

  alto();
  delay(100);
}

void buscarOponente()
{
  // Giro de búsqueda
  derecha();
}

// ================== MOTORES ==================

void alto()
{
  digitalWrite(motA0, LOW);
  digitalWrite(motA1, LOW);
  digitalWrite(motB0, LOW);
  digitalWrite(motB1, LOW);
}

void adelanteFuerte()
{
  digitalWrite(motA1, HIGH);
  digitalWrite(motA0, LOW);

  digitalWrite(motB0, HIGH);
  digitalWrite(motB1, LOW);
}

void atras()
{
  digitalWrite(motA0, HIGH);
  digitalWrite(motA1, LOW);

  digitalWrite(motB1, HIGH);
  digitalWrite(motB0, LOW);
}

void izquierda()
{
  digitalWrite(motA0, LOW);
  digitalWrite(motA1, HIGH);

  digitalWrite(motB0, LOW);
  digitalWrite(motB1, HIGH);
}

void derecha()
{
  digitalWrite(motA1, LOW);
  digitalWrite(motA0, HIGH);

  digitalWrite(motB0, HIGH);
  digitalWrite(motB1, LOW);
}

// ================== DEBUG ==================

void imprimirDebug()
{
  if (millis() - tiempoDebug >= 250)
  {
    tiempoDebug = millis();

    Serial.print("Izq: ");
    Serial.print(lLeft);

    Serial.print(" | Centro: ");
    Serial.print(lCenter);

    Serial.print(" | Der: ");
    Serial.print(lRight);

    Serial.print(" | Distancia: ");
    Serial.println(distancia());
  }
}