Basic 'goal'/FSM stuff working