Rearrange code to be a bit more logical, add testing framework and remove garbage.