Moved class declarations in header file

Clean up header and cpp file. Add a test main