Clean up code and add doxygen documentation