Rewrote setNextGoal function. (There is BUG)