Subgoal planning working, almost (infinite loop)