Started cleaning up.

First algorithm cleaned up in main.cpp. Comments added to other files as
well.