Adding tests, refactoring a bit