Adding C++ implementation of an algorythm