Code cleanup and unittests added