// Variáveis globais

// Triggers
#define FTrig 2
#define ETrig 4
#define DTrig 6

// Echos
#define FEcho 3
#define EEcho 5
#define DEcho 7

// Conexão dos motores (pinos)
// Motor esquerdo - x, y
// Motor direito - w, z
#define MEA 8
#define MEB 9
#define MDA 10
#define MDB 11

// Distância
#define Max_Dist 100
#define Min_Dist 5

// LEDs
#define Red 13
#define Green 12

void setup() {
  // Definições
  Serial.begin(9600);
  Serial.println("Inicializando PROJETO - Stalker");

  // Triggers
  pinMode(ETrig, OUTPUT);
  pinMode(FTrig, OUTPUT);
  pinMode(DTrig, OUTPUT);

  // Echos
  pinMode(EEcho, INPUT);
  pinMode(FEcho, INPUT);
  pinMode(DEcho, INPUT);

  // Motores
  pinMode(MEA, OUTPUT);
  pinMode(MEB, OUTPUT);
  pinMode(MDA, OUTPUT);
  pinMode(MDB, OUTPUT);

  // LEDs
  pinMode(Red, OUTPUT);
  pinMode(Green, OUTPUT);
}

void loop() {
  // Código em loop
  int FDist = sensorF();
  int EDist = sensorE();
  int DDist = sensorD();

  Serial.print("Frente: ");
  Serial.print(FDist);
  Serial.print(" cm, Esquerda: ");
  Serial.print(EDist);
  Serial.print(" cm, Direita: ");
  Serial.print(DDist);
  Serial.println(" cm");

  if(FDist < Min_Dist) {
    tras();
    Serial.println("Alvo muito próximo - Trás");
    digitalWrite(Red, HIGH);
    digitalWrite(Green, LOW);
  }
  else if(FDist < EDist && FDist < DDist && FDist < Max_Dist) {
    frente();
    Serial.println("Seguindo alvo - Frente");
    digitalWrite(Red, LOW);
    digitalWrite(Green, HIGH);
  }
  else if(EDist < DDist && EDist < FDist && EDist < Max_Dist) {
    esquerda();
    Serial.println("Seguindo alvo - Esquerda");
    digitalWrite(Red, LOW);
    digitalWrite(Green, HIGH);
  }
  else if(DDist < EDist && DDist < FDist && DDist < Max_Dist) {
    direita();
    Serial.println("Seguindo alvo - Direita");
    digitalWrite(Red, LOW);
    digitalWrite(Green, HIGH);
  }
  else {
    parar();
    Serial.println("Distância segura - Parar");
    digitalWrite(Red, HIGH);
    digitalWrite(Green, LOW);
  }

  delay(100); // this speeds up the simulation
}

// Funções utilizadas

int sensorF() {
  digitalWrite(FTrig, LOW);
  delayMicroseconds(2);
  digitalWrite(FTrig, HIGH);
  delayMicroseconds(10);
  digitalWrite(FTrig, LOW);

  long t = pulseIn(FEcho, HIGH);
  int cm = t / 29 / 2;
  return cm;
}

int sensorE() {
  digitalWrite(ETrig, LOW);
  delayMicroseconds(2);
  digitalWrite(ETrig, HIGH);
  delayMicroseconds(10);
  digitalWrite(ETrig, LOW);

  long t = pulseIn(EEcho, HIGH);
  int cm = t / 29 / 2;
  return cm;
}

int sensorD() {
  digitalWrite(DTrig, LOW);
  delayMicroseconds(2);
  digitalWrite(DTrig, HIGH);
  delayMicroseconds(10);
  digitalWrite(DTrig, LOW);

  long t = pulseIn(DEcho, HIGH);
  int cm = t / 29 / 2;
  return cm;
}

int tras(){
    digitalWrite(MEA, LOW);
    digitalWrite(MEB, HIGH);
    digitalWrite(MDA, LOW);
    digitalWrite(MDB, HIGH);
}

int frente() {
    digitalWrite(MEA, HIGH);
    digitalWrite(MEB, LOW);
    digitalWrite(MDA, HIGH);
    digitalWrite(MDB, LOW);
}
int esquerda() {
    digitalWrite(MEA, HIGH);
    digitalWrite(MEB, LOW);
    digitalWrite(MDA, LOW);
    digitalWrite(MDB, HIGH);
}

int direita() {
    digitalWrite(MEA, LOW);
    digitalWrite(MEB, HIGH);
    digitalWrite(MDA, HIGH);
    digitalWrite(MDB, LOW);
}

int parar() {
    digitalWrite(MEA, LOW);
    digitalWrite(MEB, LOW);
    digitalWrite(MDA, LOW);
    digitalWrite(MDB, LOW);
}