#331 Fixed mode selection problem by reinitializing more. Reorganzied some more functions.