Move multi-sample logic to C++ 

The C++ implementation is ugly and slow, but should be correct.