Fixing unittests and finishing code