Add preparation methods for A* search
Improve method naming
Change test code