Working code of AStar