Restructure of code, bugfixes, start adding tests