Code cleanup and C++ification

Updated to use C++. Also, fixed formatting