Tested breadth first path finder