reorganized matlab code