Reimplemented A* using heaps and hash tables