     printheuristic();
     delay(1000);
     /* Caso seja o fim, finalize e imprima o resultado da computaçao */
-    if(isFim()){
-       /* Imprime caminho final */
-       defineWay();
-       delay(10000);
+    if (isFim()) {
+      /* Imprime caminho final */
+      defineWay();
+      delay(10000);
     }
     delay(2000);
     /* Descomente a linha a seguir para parar o código após uma interação */
     /* intervencao(); */
   }
-  
-  else{
-  /* Robo reexecuta o movimento correto ao apertar o botão */
-  /* TODO */
+
+  else {
+    /* Robo reexecuta o movimento correto ao apertar o botão */
+    /* TODO */
   }
   Serial.println("Fim do loop!");
 }
 
 /* Printa o caminho percorrido*/
-void printpath(){
+void printpath() {
   Serial.println("Este eh ao caminho percorrido ate aqui:");
-  for(i = 0; i < z; i++){
+  for (i = 0; i < z; i++) {
     Serial.print(pathRow[i]);
     Serial.print(" ");
     Serial.println(pathCol[i]);

      pathRow[z] = ActualRow;
      pathCol[z] = ActualCol;
      z += 1;
    } 
    return;
  }
  /* Caso não seja vizinho, devo regredir no grid, retrocendendo na lista percorrida até alcançar um vizinho */
  Serial.println("Nao e vizinho.............................");
  p -= 1;
  /* Se for,  eles estão na mesma linha, devo ir para direita ou esquerda */
  if(ActualRow == pathRow[p]){ 
     if(ActualCol < pathCol[p]){
      turnRight();
     }
     else{
      turnLeft(); 
     }
     ActualRow = pathRow[p];
     ActualCol = pathCol[p];
     pathRow[z] = ActualRow;
     pathCol[z] = ActualCol;
     z += 1;
     goAhead();
  }
  /* Se for,  eles estão na mesma linha, devo ir para frente ou trás */
  if(ActualCol == pathCol[p]){  
     if(ActualRow < pathRow[p]){
      goBackward();
     }
     else{
      goForward();
     }
     ActualRow = pathRow[p];
     ActualCol = pathCol[p];
     pathRow[z] = ActualRow;
     pathCol[z] = ActualCol;
     z += 1;
     goAhead();
  }
  goAhead(); 
}


void defineWay(){
  p = z;
  int lock = 1;
  Serial.println("Imprimindo caminho final...");
  /* Retrocede o indice da lista até encontrar a posição inicial */
  while(lock){
    if(pathRow[p] == StartRow && pathCol[p] == StartCol){
      lock = 0;  
    }
    else{  
      counter++;
      p--;
    }  
  }
  /* Imprime caminho final */
  for(i = 0; i < counter; i++){
    finalRow[i] = pathRow[p];
    finalCol[i] = pathCol[p];
    Serial.print("Passo ");
    Serial.print(i+1);
    Serial.print(" ");
    Serial.print(pathRow[p]);
    Serial.print(" ");

int aux2;

/* Funcao Heuristica calculada com a distância de Manhanttan */
void heuristicFunction() {
  Serial.println("Calculando matriz heuristica...");
  for (i = 0; i < NUM_LINHAS; i++) {
    for (j = 0; j < NUM_LINHAS; j++) {
      aux1 = abs(i - (GoalRow - 1));
      aux2 = abs(j - (GoalCol - 1));
      aux1 = aux1 + aux2;
      heuristic[i][j] = aux1;
      /*
        Serial.print("heuristic[");
        Serial.print(i);
        Serial.print("][");
        Serial.print(j);
        Serial.print("] = ");
        Serial.println(heuristic[i][j]);
      */
    }
  }
}

/* Funcao A Estrela (Junta funcao Heuristica com as variáveis de Custo) */
void a_star() {
  Serial.println("Aplicando A* em meus vizinhos...");

  /* Para evitar recalculos, seta o quadrado atual como 2000*/
  grid[ActualRow][ActualCol] = 2000;

  /* Calculo o A Estrela do quadrado da frente */
  setGrid(ActualRow - 1, ActualCol);
  /* Depois giro para a direita */
  turnAround();
  /* Calculo o A Estrela do quadrado da direita */
  setGrid(ActualRow, ActualCol + 1);
  /* Depois giro para a direita */
  turnAround();
  /* Calculo o A Estrela do quadrado de trás */
  setGrid(ActualRow + 1, ActualCol);
  /* Depois giro para a direita */
  turnAround();
  /* Calculo o A Estrela do quadrado da esquerda */
  setGrid(ActualRow, ActualCol - 1);
  /* Depois giro para a direita, para voltar a orientação inicial */
  turnAround();
}

/* Percorro minha matriz Grid em busca do menor valor, o primeiro menor valor será minha próxima posição */
void findPath() {
  Serial.println("Definindo rota...");
  p = z - 1;
  int padrao = 1000;
  for (i = 1; i < m; i++) {
    for (j = 1; j < m; j++) {
      if (grid[i][j] < padrao) {
        NextRow = i;
        NextCol = j;
        /*Serial.print(NextRow);
          Serial.println(NextCol);*/
        padrao = grid[i][j];
      }
    }
  }
}

/* Define o novo valor do quadrado observado (até então, desconhecido) */
void setGrid(int x, int y) {
  Serial.println("Calculando heuristica mais custo...");
  int q = x;
  int r = y;
  /* Se for obstaculo, valor 1000 */
  if (isObstacle(q, r)) {
    if (q >= 0 && r >= 0) {
      grid[x][y] = obstacle;
      return;
    }
  }
  /* Se já foi calculado, ignora */
  if (grid[x][y] == 2000) { //(grid[x][y] == 2000)
    return;
  }
  /* Calculo custo do caminho até chegar mais a heuritica */
  else {
    grid[x][y] = 0;
    grid[x][y] = 10 + heuristic[x - 1][y - 1];
    Serial.print("Este eh meu indice Z: ");
    Serial.println(z);
    p = z - 1;
    int lock2 = 1;
    printpath();
    while (lock2) {
      if (pathRow[p] == StartRow && pathCol[p] == StartCol) {
        lock2 = 0;
      }
      else {
        /*Serial.print(pathRow[p]);
          Serial.print("  ");
          Serial.println(pathCol[p]);*/
        grid[x][y] += heuristic[pathRow[p] - 1][pathCol[p] - 1];
        p -= 1;
      }
    }
  }
}

/* Encontra um caminho até a próxima posição */
void goAhead() {
  Serial.println("Indo para proxima posicao...");
  Serial.print("Linha: ");
  Serial.print(NextRow);
