Original code in matlab