BFS Algorithm in C++