Finished StateMachine class.

+Moved all code from main to StateMachine.