Cleaned code a bit.
Added a mex file for MATLAB.