Add tests and refactor a bit