Adding unittests, restructored code