Implemented algorithm in C++