First part of Matlab code for 6c