initial commit. Code imported from MATLAB implementation.