Add tests

Includes some refactoring.