Updates to unittests and small codeoptimization
Finished unittests