C code uptill matlab WORKING