/*
  SEGUNDO CÓDIGO - 3° DESAFIO HÍBRIDO

  Equipe Elektro Maias - Etec Armando Pannunzio
  > Ana Luiza Marçal de Oliveira
  > Diogo Henrique Sampaio
  > Giovanna Santana Maia
  > Lucas de Oliveira Silva Rocha
  > Manuella Cristhyna Rodrigues Gonçalves
  > Prof° Clóvis Augusto Bismara

*/

// Inclusão de Bibliotecas
#include <NewPing.h>

// Mapeamento de Hardware
#define in1 5
#define in2 6
#define in3 11
#define in4 10

#define trigger 2
#define echo 3

// Variáveis Globais

#define max_distance 200
#define min_d 9
#define t_90 200
#define t_180 400

unsigned long int t, t_f, t_base = 8000;

bool flag;
int distance;  // Variável para armazenar distância

// Objetos da Bibliotca
NewPing hcsr04(trigger, echo, max_distance);

class defL298N {
    int pinA, pinB, pinC, pinD;

  public:
    void pinagem(int a, int b, int c, int d) {
      pinA = a;
      pinB = b;
      pinC = c;
      pinD = d;
      pinMode(pinA, OUTPUT);
      pinMode(pinB, OUTPUT);
      pinMode(pinC, OUTPUT);
      pinMode(pinD, OUTPUT);
    }

    void parar() {
      digitalWrite(pinA, LOW);
      digitalWrite(pinB, LOW);
      digitalWrite(pinC, LOW);
      digitalWrite(pinD, LOW);
    }

    void frente() {
      digitalWrite(pinA, LOW);
      digitalWrite(pinB, HIGH);
      digitalWrite(pinC, HIGH);
      digitalWrite(pinD, LOW);
    }

    void tras() {
      digitalWrite(pinA, HIGH);
      digitalWrite(pinB, LOW);
      digitalWrite(pinC, LOW);
      digitalWrite(pinD, HIGH);
    }

    void esquerda() {
      digitalWrite(pinA, LOW);
      digitalWrite(pinB, HIGH);
      digitalWrite(pinC, LOW);
      digitalWrite(pinD, HIGH);
    }

    void direita() {
      digitalWrite(pinA, HIGH);
      digitalWrite(pinB, LOW);
      digitalWrite(pinC, HIGH);
      digitalWrite(pinD, LOW);
    }
};

defL298N carro;

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

  carro.pinagem(in1, in2, in3, in4);

  pinMode(echo, INPUT);
  pinMode(trigger, OUTPUT);
}

void loop() {

  if (readPing() <= min_d) {
    carro.parar();
    delay(t_90 / 4);
    carro.tras();
    delay(t_90 / 4);
    carro.esquerda();
    
    t_f = millis();
    t = millis();
  }
  else {
  t = millis();
    if (t - t_f >= t_base) {
      carro.parar();
      delay(t_90 / 4);
      carro.tras();
      delay(t_90 / 4);
      carro.esquerda();
      t_f = millis();
    } else carro.frente();
  }

  /*
    // Se há barreira na frente
    if (readPing() <= min_d) {
      carro.parar();
      delay(t_90);
      carro.tras();
      delay(t_90);
      carro.parar();
      delay(t_90);

      if (flag) {
        carro.direita();
        delay(t_180);
        carro.parar();
        flag = 0x00;
      } else {
        carro.esquerda();
        delay(t_90);
        carro.parar();
        flag = 0x01;
      }
    } else {
      flag = 0x00;
      carro.frente();
    }
  */
}

int readPing() {
  delay(100);
  int measure = hcsr04.ping_cm();
  if (!measure) measure = 250;
  return measure;
}