Imprved fsm behaviour and code cleanup.