Partial implementation of AStar