C++ code and Matlab helper functions

Tested and run on Windows machines