Added initial version of control FSM logic. Cleaned up the core code.