fixed mex solver. Program now working

comparing code to matlab version still to be done