Implemented better A* with heapq. Closes #58.