Somewhat optimized version of bfs