Add a unit test for sssp_bfs()