Move stuff from test.cpp out in to its own files. Some refactoring and commenting