Math functions

Convert quite a few math macros to functions.
This should make the code easier to use, debug and wrap externally. Additionally code size probably reduces since it's not all inline anymore...

Also cleaned up all the headers and added `extern "C"` to all the header files the math lib can easier be used in C++ applications.

So far the macros all still exist and call the respective functions for backwards compatibility.