C++ with simple algorithm