Preparing for rewrite of mathtools
Rewrote mathtools.c in C++