Passes all tests except for 1 instance on bfs graph (330767681 17882616). Needs documentation and osm outfile writing statements.