A* (a star) implementation (working version)