Clean up code and add doxgen-style comments