AStar Fully implemented