Adding initial version of control FSM. Almost complete RTL pre debug.