Modified AStar algorithm to return path coordinates