shortestPath function now performs AStar and returns path at once