Refactor main.cpp into a test.cpp program.

Should really be using unit testing for this I think, but I don't know how to
do that in C++ yet.