Add tests and some refactoring to make things work