Better organization, small fixes a bit of test code