Second algorithm, does not work

We will now convert the code to CPP.

first full implementation

First full implementation of the interpolation/sampling idea that we had. This does not work.