Java - BFS java pacman

Hola a todos!

Como proyecto en la uni tengo que programar un BFS( breadth first searrch) para que el thread Depredador encuentre la ruta optima al jugador. No consigo programarlo siguiendo la estructura del javadoc http://www.dit.upm.es/~pepe/doc/adsw/proyecto1/javadoc/index.html en el que hacen un bfs boolean y luego ya un metodo primerPaso que devuelve Casilla.

Yo he hecho este codigo pero no me funciona :

mport java.util.ArrayList;
import java.util.HashMap;
import java.util.HashSet;
import java.util.List;
import java.util.Map;
import java.util.Random;
import java.util.Set;

public class Depredador extends Movil implements Runnable {

private List<Casilla> pendientes = new ArrayList<Casilla>();
private Map<Casilla,Casilla> visitadasmap = new HashMap<Casilla,Casilla>();
private Set<Casilla> visitadas = new HashSet<Casilla>();


private Jugador jugador;
private Casilla casilla;
private Terreno terreno;
private boolean vivo;
Random random = new Random();
Direccion[] direcciones = Direccion.values();


public Depredador(Terreno terreno) {
this.terreno = terreno;
setImage("Anibal.png");
vivo = true;
}


@Override
public void run() {
try {
while (vivo) {
Casilla origen = jugador.getCasilla();
Casilla destino = this.getCasilla();
Casilla siguiente = primerPaso(destino,origen);
Direccion direccion = destino.direccion(destino, siguiente);
terreno.move(this,direccion);
Thread.sleep(100);

}
} catch (InterruptedException ie) {
return;
}
}


public boolean bfs( Casilla destino, List<Casilla> pendientes, Set<Casilla> visitadas, Map<Casilla,Casilla> visitadasmap ){

while(true){
if(pendientes.isEmpty()) return false;
Casilla c1 = pendientes.remove(0);
visitadas.add(c1);
if(c1==destino) return true;
for(Direccion direccion: Direccion.values()){
Casilla casilla = c1.siguiente(direccion);
if( casilla!=null && !terreno.hayPared(casilla, direccion) && !visitadas.contains(casilla) ){
visitadasmap.put(casilla,c1);
pendientes.add(casilla);
}
}
}


}

public Casilla primerPaso(Casilla origen, Casilla destino){
List<Casilla> pendientes = new ArrayList<Casilla>();
Set<Casilla> visitadas = new HashSet<Casilla>();
Map<Casilla,Casilla> visitadasmap = new HashMap<Casilla,Casilla>();
pendientes.add(origen);
if(this.bfs(destino,pendientes,visitadas,visitadasmap)){
return visitadasmap.get(destino);
}
return null;
}

El codigo completo esta aqui : https://github.com/adsw-upm/Laboratorio1/tree/master/Pacman1

Si alguien me echa una mano se lo agradecería mucho!

Un saludo