Code review (do not merge)

Here is my code review, if you upstream merge you should get only the review diffs otherwise just click on the code review commit.

First of all nice work! Lots of code and it looks really well documented with doxygen. It was a bit hard for me to understand what was going on because I haven't read the papers and you're still working on the final example (which I'll try and read when you finish it).

I made a few comments but it looks good to me!