lambda=532e-9;
%fiber from launcher
z=[195, 571, 588+950, 332, 585+352, 585+910+392]*1e-3;
%wP=[732, 781, 1201, 730, 860, 1451]/2*1e-6; % 1/e^2
%wY=[750, 792, 1267, 751, 907, 1513]/2*1e-6; % 1/e^2
wP=[824, 808, 1318, 801, 891, 1609]/2*1e-6; % D4sigma/2
wY=[839, 833, 1382, 829, 957, 1671]/2*1e-6; % D4sigma/2

[w0P, z0P, M2P]=fitMsquared(z, wP, lambda);
[w0Y, z0Y, M2Y]=fitMsquared(z, wY, lambda);

[w0PG, z0PG]=fitGaussianBeam(z, wP, lambda, [w0P, z0P]);
[w0YG, z0YG]=fitGaussianBeam(z, wY, lambda, [w0Y, z0Y]);

zz=0:0.01:(max(z)+0.01);
figure
plot(zz, w0P*sqrt(1+M2P^2*(lambda/pi/w0P^2)^2*(zz-z0P).^2), 'r', zz, w0Y*sqrt(1+M2Y^2*(lambda/pi/w0Y^2)^2*(zz-z0Y).^2), 'b');  
hold on
plot(zz, beamSizeGaussian(w0PG, z0PG, zz, lambda), 'm--', zz, beamSizeGaussian(w0YG, z0YG, zz, lambda), 'g--');
plot(z, wP, 'ro', z, wY, 'bx')
legend(['P fit, Msq=', num2str(M2P)], ['Y fit, Msq=', num2str(M2Y)], 'P Gaussian fit', 'Y Gaussian fit', 'P meas', 'Y meas');

function [w0_, z0_, msquared_]=fitMsquared(z_, w_, lambda_)
    % w_^2= w0^2+M^4*theta^2*(z-z0)^2,
    % theta=Gaussian divergence angle = lambda/pi/w0
    % Therefore, if w_^2=A*z^2+B*z+C,
    %   A=M^4*theta^2, B=-2*z0*A, C=A*z0^2+w0^2
    % i.e.
    % z0=-B/A/2
    % w0^2=C-A*z0^2
    % theta = lambda/pi/w0
    % M^4=A/theta^2
    P_=polyfit(z_, w_.^2, 2);
    z0_=-P_(2)/P_(1)/2;
    w0_=sqrt(P_(3)-P_(1)*z0_.^2);
    theta_=lambda_/pi/w0_;
    msquared_=sqrt(P_(1)/theta_^2);
    return;
end

function w_=beamSizeGaussian(w0_, z0_, z_, lambda_)
    w_= sqrt(w0_^2+((z_-z0_)*lambda_/pi/w0_).^2);
    return;
end

function [w0_, z0_]=fitGaussianBeam(z_, w_, lambda_, betaInitial_)
    fitfun=@(beta, z) beamSizeGaussian(beta(1), beta(2), z, lambda_);
    beta=nlinfit(z_, w_, fitfun, betaInitial_);
    w0_=beta(1);
    z0_=beta(2);
    return;
end
    
    

