First commit. Added all files and functionality. testmain.cpp added to test functionality.