Modify to enable compiling with mex