//motor speed tem de ser entre 0 e 255

int enA = 2;                                       // Velocidade do motor A
int Motor_Esquerda_Tras = 6;                       // Quando ativado, o motor A anda para trás   IN1
int Motor_Esquerda_Frente = 7;                     // Quando ativado, o motor A anda para a frente   IN2

int enB = 3;                                       // Velocidade do motor B
int Motor_Direita_Frente = 11;                     // Quando ativado, o motor B anda para a frente   IN3
int Motor_Direita_Tras = 12;                       // Quando ativado, o motor B anda para trás   Motor_Direita_Tras

int sensor_Esquerda = A5;                          // Sensor da esquerda
int sensor_Meio = A7;                              // Sensor do meio
int sensor_Direita = A6;                           // Sensor da direita

long distanciaDireita;
long distanciaEsquerda;
long tempo1;
long tempo2;                                       // Tempo que demora a transmitir o sinal
int trigDireita = 10;
int trigEsquerda = 5;
int echoDireita = 9;
int echoEsquerda = 4;


const int encoder = 8;
const float diametro = 6.5;
const int pulsos_por_volta = 20;
const float aux = diametro * PI / pulsos_por_volta;
double Distancia_Percorrida;
volatile int pulsos = 0;

void setup() {
  Serial.begin(9600);
  pinMode(enA, OUTPUT);
  pinMode(enB, OUTPUT);
  pinMode(Motor_Esquerda_Tras, OUTPUT);
  pinMode(Motor_Esquerda_Frente, OUTPUT);
  pinMode(Motor_Direita_Frente, OUTPUT);
  pinMode(Motor_Direita_Tras, OUTPUT);

  pinMode(trigDireita, OUTPUT);       //trigDireita
  pinMode(echoDireita, INPUT);       //echoDireita
  pinMode(trigEsquerda, OUTPUT);      //trigEsquerda
  pinMode(echoEsquerda, INPUT);       //echoEsquerda

  pinMode(sensor_Esquerda, INPUT);
  pinMode(sensor_Meio, INPUT);
  pinMode(sensor_Direita, INPUT);

  pinMode(encoder, HIGH);
  attachInterrupt(digitalPinToInterrupt(encoder), contador, FALLING);
}

void FORWARD()                     // Ambos os motores andam para a frente
{
  digitalWrite(Motor_Esquerda_Tras, LOW);
  digitalWrite(Motor_Esquerda_Frente, HIGH);
  analogWrite(enA, 180);
  digitalWrite(Motor_Direita_Frente, HIGH);
  digitalWrite(Motor_Direita_Tras, LOW);
  analogWrite(enB, 165);
  Serial.print("...FORWARD...");
}

void FORWARD_RIGHT()                // O motor da direita anda para a frente
{
  digitalWrite(Motor_Esquerda_Tras, LOW);
  digitalWrite(Motor_Esquerda_Frente, HIGH);
  analogWrite(enA, 190);
  digitalWrite(Motor_Direita_Frente, LOW);
  digitalWrite(Motor_Direita_Tras, LOW);
  Serial.print("...FORWARD_RIGHT...");
}

void FORWARD_LEFT()               // O motor da esquerda anda para a frente
{
  digitalWrite(Motor_Direita_Frente, HIGH);
  digitalWrite(Motor_Direita_Tras, LOW);
  analogWrite(enB, 175);
  digitalWrite(Motor_Esquerda_Tras, LOW);
  digitalWrite(Motor_Esquerda_Frente, LOW);
  Serial.print("...FORWARD_LEFT...");
}

void BACKWARD_RIGHT()
{
  digitalWrite(Motor_Esquerda_Tras, HIGH);
  digitalWrite(Motor_Esquerda_Frente, LOW);
  analogWrite(enA, 160);
  digitalWrite(Motor_Direita_Frente, LOW);
  digitalWrite(Motor_Direita_Tras, LOW);
  Serial.print("...BACKWARD_RIGHT...");
}

void BACKWARD() {
  digitalWrite(Motor_Esquerda_Tras, HIGH);
  digitalWrite(Motor_Esquerda_Frente, LOW);
  analogWrite(enA, 160);
  digitalWrite(Motor_Direita_Frente, LOW);
  digitalWrite(Motor_Direita_Tras, HIGH);
  analogWrite(enB, 170);
  Serial.print("...BACKWARD...");
}

void HARD_TURN_LEFT() {
  digitalWrite(Motor_Esquerda_Tras, LOW);
  digitalWrite(Motor_Esquerda_Frente, HIGH);
  analogWrite(enA, 165);
  digitalWrite(Motor_Direita_Frente, LOW);
  digitalWrite(Motor_Direita_Tras, HIGH);
  analogWrite(enB, 165);
  Serial.print("...HTLEFT...");
}

void HARD_TURN_RIGHT() {
  digitalWrite(Motor_Esquerda_Tras, HIGH);
  digitalWrite(Motor_Esquerda_Frente, LOW);
  analogWrite(enA, 165);
  digitalWrite(Motor_Direita_Frente, HIGH);
  digitalWrite(Motor_Direita_Tras, LOW);
  analogWrite(enB, 165);
  Serial.print("...HTRIGHT...");
}

void STOP_RIGHT() {                 // Pára o motor da esquerda
  digitalWrite(Motor_Esquerda_Tras, LOW);
  digitalWrite(Motor_Esquerda_Frente, LOW);
  Serial.print("...STOP_RIGHT...");
}

void STOP_LEFT() {                // Pára o motor da direita
  digitalWrite(Motor_Direita_Frente, LOW);
  digitalWrite(Motor_Direita_Tras, LOW);
  Serial.print("...STOP_LEFT...");
}

void contador()
{
  pulsos++;
}

void loop() {

 digitalWrite(trigDireita,LOW);
 delayMicroseconds(5);
 digitalWrite(trigDireita, HIGH);
 delayMicroseconds(10);
 tempo1 = pulseIn(echoDireita, HIGH);
 distanciaDireita= int(0.017*tempo1);

 digitalWrite(trigEsquerda,LOW);
 delayMicroseconds(5);
 digitalWrite(trigEsquerda, HIGH);
 delayMicroseconds(10);
 tempo2 = pulseIn(echoEsquerda, HIGH);
 distanciaEsquerda= int(0.017*tempo2);

 int LED_Esquerda = analogRead(sensor_Esquerda);
 int LED_Meio = analogRead(sensor_Meio);
 int LED_Direita = analogRead(sensor_Direita);

 Serial.print("Distancia sensor esquerda: ");
 Serial.print(distanciaEsquerda);
 Serial.println();

 Serial.print("Distancia sensor direita: ");
 Serial.print(distanciaDireita);
 Serial.println();

 delay(1000);
 while (LED_Esquerda > 500 || LED_Meio > 500  || LED_Direita > 500)
 {
  if (LED_Esquerda < 500 && LED_Meio > 500 && LED_Direita < 500)    //Está tudo bem, anda para a frente     010
  {
   FORWARD();
  }
  else if (LED_Esquerda < 500 && LED_Meio < 500 && LED_Direita < 500)   //Está fora do percurso, anda para trás até encontrar a linha preta     000
  {
   BACKWARD();
   delay(100);
  }
  else if (LED_Esquerda < 500 && LED_Meio < 500 && LED_Direita > 500)    //Compensa o movimento para a direita     001
  {
   FORWARD_LEFT();
  }
  else if (LED_Esquerda > 500 && LED_Meio < 500 && LED_Direita < 500)     //Compensa o movimento para a esquerda     100
  {
   FORWARD_RIGHT();
  }
  else if (LED_Esquerda < 500 && LED_Meio > 500 && LED_Direita > 500)     // 011
  {
   HARD_TURN_RIGHT();
  }
  else if (LED_Esquerda > 500 && LED_Meio > 500 && LED_Direita < 500)     // 110
  {
   HARD_TURN_LEFT();
  }
  else if (LED_Esquerda > 500 && LED_Meio < 500 && LED_Direita > 500)     //Encontra 2 trajetórias     101
  {
   FORWARD_RIGHT();
  }
  else if (LED_Esquerda > 500 && LED_Meio > 500 && LED_Direita > 500)     //Se todos os sensores tiverem em cima da trajetória     111
  {
    FORWARD_LEFT();
    FORWARD_RIGHT();
    delay(100);

    while(LED_Esquerda > 500 && LED_Meio > 500 && LED_Direita > 500)     //Se todos os sensores tiverem em cima da trajetória     111
    {
     STOP_RIGHT();
     STOP_LEFT();
    }
  }
 }
 if (distanciaDireita == distanciaEsquerda)
 {
  FORWARD();
 }

 else if (distanciaDireita > distanciaEsquerda && distanciaEsquerda >= 6)
 {
  FORWARD_LEFT();
  FORWARD_RIGHT();
 }

 else if (distanciaDireita < distanciaEsquerda && distanciaDireita >= 4)
 {
  FORWARD_LEFT();
  FORWARD_RIGHT();
 }

 else if (distanciaDireita < distanciaEsquerda && 4 <= distanciaDireita < 6)
 {
  FORWARD_RIGHT();
 }

 else if (distanciaDireita > distanciaEsquerda && 4 <= distanciaEsquerda < 6)
 {
  FORWARD_LEFT();
 }

 else if (distanciaDireita < distanciaEsquerda && distanciaDireita < 4)
 {
  BACKWARD();
 }

 else if (distanciaDireita > distanciaEsquerda && distanciaEsquerda < 4)
 {
  BACKWARD();
 }


 Distancia_Percorrida = pulsos * aux;
 Serial.print("Distancia percorrida: ");
 Serial.print(Distancia_Percorrida);
 Serial.println();
}