Refactoring + simple test

Writting of some simple tests, which pass. Some more to come.