Implemented A* Search. Somehow the priority queue is not sorted out
well.