Re-organised FSM's and data structures to reduce complexity of code