Cleaning up and fixing FSM