C code uptill matlab TESTED