Some refactoring and bugfixes
Add rudimentary unit test