Convert calmanp5.c to C++