Breadth first search path finding - first dev version
bfs fix