Moved some code around for a better c port. added back doxygen comments.