This version works as well as the MATLAB code