// Sensor infrarojo
int pinInfraIzq = 12;
int pinInfraDer= 13;

int valorInfraIzq = 0;
int valorInfraDer = 0;

// MOTOR IZQUIERDO
const int pinIN1 = 5;
const int pinIN2 = 4;
const int pinENA = 3;

// MOTOR DERECHO
const int pinIN3 = 9;
const int pinIN4 = 10;
const int pinENB = 11;

const int velIzq = 70; // 0-255
const int velDer = 75; // 0-255

const int intEsperaAvanzaIzqDer = 200; // 110
const int intEsperaAvanza = 40; // 50
const int intEsperaAlto = 15; // 20

void setup() {
  Serial.begin(9600);
  //delay(1000);

  pinMode(pinInfraIzq, INPUT);
  pinMode(pinInfraDer, INPUT);

  pinMode(pinIN1, OUTPUT);
  pinMode(pinIN2, OUTPUT);
  pinMode(pinENA, OUTPUT);

  pinMode(pinIN3, OUTPUT);
  pinMode(pinIN4, OUTPUT);
  pinMode(pinENB, OUTPUT);

  // VCC SENSORES INFRAROJOS
  pinMode(A0, OUTPUT);
  pinMode(A1, OUTPUT);
  analogWrite(A0, 255);
  analogWrite(A1, 255);

  Alto();

  for(int i = 0; i < 5; i++){
    digitalWrite(LED_BUILTIN, HIGH);
    delay(300);
    digitalWrite(LED_BUILTIN, LOW);
    delay(100);
  }

}

void loop() {

  valorInfraIzq = digitalRead(pinInfraIzq);
  valorInfraDer = digitalRead(pinInfraDer);
  //Serial.println("Izq: " + String(valorInfraIzq) + " Der: " + String(valorInfraDer));

  if(valorInfraIzq == 0 && valorInfraDer == 0){
    Serial.println("Ninguno en linea");

    Alto();
    Avanza();
  }

  if(valorInfraIzq == 0 && valorInfraDer == 1){
    Serial.println("Derecho en linea");

    Alto();
    AvanzaIzquierdo();
  }

  if(valorInfraIzq == 1 && valorInfraDer == 0){
    Serial.println("Izquierdo en linea");

    Alto();
    AvanzaDerecho();
  }

  if(valorInfraDer == 1 && valorInfraIzq == 1){
    Serial.println("Ambos en linea");

    // SENTIDO DE GIRO
    Alto();
  }


}



void AvanzaIzquierdo(){
    // SENTIDO DE GIRO
    analogWrite (pinENA, 0.8*velDer);
    digitalWrite(pinIN1, HIGH);
    delay(intEsperaAvanzaIzqDer); // 200
}

void AvanzaDerecho(){
  // SENTIDO DE GIRO
  analogWrite (pinENB, 0.8*velIzq);
  digitalWrite(pinIN4, HIGH);
  delay(intEsperaAvanzaIzqDer); // 200
}

void Avanza(){

  // AVANZA IZQUIERDO
  analogWrite (pinENA, velIzq);
  digitalWrite(pinIN1, HIGH);

  // AVANZA DERECHO
  analogWrite (pinENB, velDer);
  digitalWrite(pinIN4, HIGH);

  delay(intEsperaAvanza); // 50
}


void Alto(){
  // SENTIDO DE GIRO
  digitalWrite(pinIN1,LOW);
  digitalWrite(pinIN2,LOW);
  digitalWrite(pinIN3,LOW);
  digitalWrite(pinIN4,LOW);

  // VELOCIDAD DE MOTORES
  analogWrite (pinENA, 0);
  analogWrite (pinENB, 0);
  delay(intEsperaAlto);
}