Breadth First Search in C gets Loop

Question

I'm trying to implement the Breadth First Search algorithm in C, to search a path from the first (first column) white (255) point in a Binary Matrix until the last one (last column).

I've created an algorithm the binarize the image correctly based on a histogram, but now when I try to find the shortest path, I'm getting problems. Maybe I didn't understand very well how to do.

I've a grid, that is a matrix containing the binarizied values (0 or 255), where 255 means that I can "walk", and 0 means that it's a "wall". And I've the map, to know where I can go, and where I already went to. Both have same sizes.

I will paste the code of BFS Algorithm, and insert in Gist all the code if you wanna view, or for further references for someone.

Obs: Coordenada means Coordinate.

Coordenada bfs( unsigned char ** grid, Coordenada local, Queue queue, int8_t **map, unsigned long height, unsigned long width)
{
  /* Insere a coordenada atual na queue */
  queue.push(&queue, local);

  /* While queue are not empty */
  while ( queue.size != 0 )
  {
    /* Retrieve some point */
    Coordenada p = queue.pop(&queue);
    printf("Processando (%d, %d)\n", p.x, p.y);

    /* Check if it's the last point */
    if ( p.x == 33 - 1 && p.y == 16 - 1 )
    {
      printf("%s\n", "Chegamos onde queríamos");
      return p;
    }

    /* Try to move on */
    if ( isFree(grid, map, p.y + 1, p.x, height, width) )
    {
      map[p.y][p.x] = -1;
      Coordenada next_point;
      next_point.y = p.y + 1;
      next_point.x = p.x;
      queue.push(&queue, next_point);
    }

    if ( isFree(grid, map, p.y - 1, p.x, height, width) )
    {
      map[p.y][p.x] = -1;
      Coordenada next_point;
      next_point.y = p.y - 1;
      next_point.x = p.x;
      queue.push(&queue, next_point);
    }

    if ( isFree(grid, map, p.y + 1, p.x + 1, height, width) )
    {
      map[p.y][p.x] = -1;
      Coordenada next_point;
      next_point.y = p.y + 1;
      next_point.x = p.x + 1;
      queue.push(&queue, next_point);
    }

    if ( isFree(grid, map, p.y - 1, p.x + 1, height, width) )
    {
      map[p.y][p.x] = -1;
      Coordenada next_point;
      next_point.y = p.y - 1;
      next_point.x = p.x + 1;
      queue.push(&queue, next_point);
    }

    if ( isFree(grid, map, p.y + 1, p.x - 1, height, width) )
    {
      map[p.y][p.x] = -1;
      Coordenada next_point;
      next_point.y = p.y + 1;
      next_point.x = p.x - 1;
      queue.push(&queue, next_point);
    }

    if ( isFree(grid, map, p.y - 1, p.x - 1, height, width) )
    {
      map[p.y][p.x] = -1;
      Coordenada next_point;
      next_point.y = p.y - 1;
      next_point.x = p.x - 1;
      queue.push(&queue, next_point);
    }

    if ( isFree(grid, map, p.y, p.x - 1, height, width) )
    {
      map[p.y][p.x] = -1;
      Coordenada next_point;
      next_point.y = p.y;
      next_point.x = p.x - 1;
      queue.push(&queue, next_point);
    }

    if ( isFree(grid, map, p.y, p.x + 1, height, width) )
    {
      map[p.y][p.x] = -1;
      Coordenada next_point;
      next_point.y = p.y;
      next_point.x = p.x + 1;
      queue.push(&queue, next_point);
    }

  }

  Coordenada empty_coord;

  /* Otherwise */
  return empty_coord;
}

bool isFree( unsigned char ** grid, int8_t ** map, int y, int x, unsigned long height, unsigned long width )
{
  if((y >= 0 && y < height) && (x >= 0 && y < width) && (map[y][x] == 0) && (grid[y][x] == 255))
    return true;
  return false;
}

And the header:

/**
 * Point in Matrix
 *
 * We use this data type to represent a point in our
 * Euclidean Space Matrix.
 */
typedef struct
{
    int x;
    int y;
} Coordenada;

/**
 * The Node struct,
 * contains item and the pointer that point to next node.
 *
 * Ref: http://ben-bai.blogspot.com.br/2012/04/simple-queue-data-structure-in-ansi-c.html
 */
typedef struct Node {
    Coordenada item;
    struct Node* next;
} Node;

/**
 * The Queue struct, contains the pointers that
 * point to first node and last node, the size of the Queue,
 * and the function pointers.
 */
typedef struct Queue {
  Node* head;
  Node* tail;

  void (*push) (struct Queue*, Coordenada); // add item to tail
  // get item from head and remove it from queue
  Coordenada (*pop) (struct Queue*);
  // get item from head but keep it in queue
  Coordenada (*peek) (struct Queue*);
  // display all element in queue
  void (*display) (struct Queue*);
  // size of this queue
  int size;
} Queue;

Link in Gist: Gist containing Sources

Thanks for reading (:

Fore more reference, the Github Repository.