Filled out implementation of shortest path, needs testing.