Completed shortestPath but it probably aint right