Finished making the C++ code match the Matlab script