Updated BFS implementation to remove allocations