Added draft C port of Matlab code