finished C++ implementation of BFS