Algorithm with c++