Convert calmanfp.c to C++