Restructure and add doxygen documentation