C++ Algorithm