Resturctured code to make it cleaner. Removed some debug code.