Finished shortest path computation, but its really slowm TODO: implement A*