Implementing A*

* implementing A* (reference: wikipedia)