Cleaned up implementation of shortest path. Pending cleanup of graph class.