Implemented A* shortest path algorithm.