Working version

Seems to quantitatively reproduce the results from the MATLAB
implementation. Fixed some bugs, and checked for type errors. Still some
errors in the last few steps of collecting the last samples.