Added unittests and fixed bugs