Re-arranging C++ code