add A* shortest path find function