Rewrite algorithm in C++