Algorithm for C++