#include <Ultrasonic.h>

//Definicoes pinos Arduino ligados a entrada da Ponte H
int IN1 = 13;
int IN2 = 12;
int IN3 = 7;
int IN4 = 8;

//Define os pinos para o trigger e echo
#define pino_trigger 4
#define pino_echo 5

//Inicializa o sensor nos pinos definidos acima
Ultrasonic ultrasonic(pino_trigger, pino_echo);

int count = 0, aux;
bool pausado = 0;
float a[10];
float cmMsec;

void setup()
{
  //Define os pinos como saida
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);

  //Para o motor A
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);

  //Para o motor B
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);

  Serial.begin(9600);
  Serial.println("Lendo dados do sensor...");
}

void andaPraFrente()
{
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);

  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
}


void andaPraTras()
{
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);

  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}

void paraAsRodas ()
{
 //Para o motor A
 digitalWrite(IN1, HIGH);
 digitalWrite(IN2, HIGH);

 //Para o motor B
 digitalWrite(IN3, HIGH);
 digitalWrite(IN4, HIGH);
}


void giraCarroHorario()
{

  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);

  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
}

void selection_sort (float vetor[]) {
  int i, j, minimo, aux;

  for (i = 0; i < (10 - 1); i++) {
    /* O minimoimo é o primeiro número não ordenado ainda */
    minimo = i;
    for (j = i+1; j < 10; j++) {
      /* Caso tenha algum numero menor ele faz a troca do minimoimo*/
      if (vetor[j] < vetor[minimo]) {
        minimo = j;
      }
    }
    /* Se o minimoimo for diferente do primeiro numero não ordenado ele faz a troca para ordena-los*/
    if (i != minimo) {
      aux = vetor[i];
      vetor[i] = vetor[minimo];
      vetor[minimo] = aux;
    }
  }
  /* Imprime o vetor ordenado */
  for (i = 0; i < 10; i++) {
    Serial.println (vetor[i]);
  }
  Serial.println ("-----------------------------------------");
}

float findMin (float vetor[])
{
  int i;
  float minimum = vetor[0];
  for(i = 1; i < 10; i++)
  {
    if( minimum > vetor[i])
      minimum = vetor[i];
  }
  return minimum;
}



void loop()
{
  delay(50);
  if (count < 10)
  {
    //Le as informacoes do sensor em cm
    long microsec = ultrasonic.timing();
    cmMsec = ultrasonic.convert(microsec, Ultrasonic::CM);

    //Serial.print("Distancia em cm: ");
    //Serial.println(cmMsec);

    a[count] = cmMsec;
    count++;
  }
  else if(count == 10){
    count = 0;
    float distMin = findMin(a);
    Serial.println ("distancia lida:");
    Serial.println (distMin);
    Serial.println ("pausado: ");
    Serial.println (pausado);

    if(Serial.available() )
    {
      aux = Serial.read();
      if (aux == '0')
      {
        pausado = true;
      }
      else if(aux == '1')
      {
        pausado = false;
      }
    }
    if(pausado == true)
    {
      paraAsRodas();
    }
    else if( distMin > 20)
    {
      andaPraFrente();
    }
    else if (distMin <= 20)
    {
      paraAsRodas();
      delay(200);  // espere 1 segundo
      andaPraTras();
      delay(1000);
      giraCarroHorario ();  // gire por 0.5seg
      delay(500);  // espere 1/2 segundo
      paraAsRodas();
    }
  }
}