first AStar implementation