Mainly added an A* planner