Done initial refactoring. Added simple tests. Does not fully work yet but getting there.