Fixed BFS implementation