C algorithm to C++