Add basic BFS
Use references wherever it makes sense