% This is a green path
lambda=532e-9;

%% measurement data
%first placed the nanoscan rail at a convenient location and made
%measurements. The larger z1, the smaller the distance from the launcher to
%the nanoscan.
launcher2BS=18e-3;
BS2rail=501e-3; %when z1=590 
z1=[90, 190, 290, 390, 490, 590 ]*1e-3; %reading on the rail.

%wanted to go closer, so moved the rail by 302mm closer to the opo BS.
% again, the larger z2, the smaller the distance from the launcher to the
% nanoscan.
railofst=302e-3;
z2=[610, 510, 410, 310, 210]*1e-3+railofst;

z12=[z1, z2];
zz12=590e-3 -z12 +BS2rail+launcher2BS; 
%zz12 is the distance from the launcher to the scan head.
% zz12=BS2rail+launcher2BS when z12=590mm.
% the larger z12, the smaller zz12.

zz3=(773+364+775+766)*1e-3+launcher2BS; %far point, folded path using a mirror.

zz=[zz12, zz3];
wPD4S=[1042,  993,  948,  910,  888,  862, 862,  850,  849,  858,  875, 2331]*1e-6/2;
wYD4S=[1413, 1339, 1269, 1214, 1151, 1106, 968, 1000, 1038, 1086, 1145, 2852]*1e-6/2;
wP=   [ 991,  946,  906,  873,  841,  812, 769,  766,  780,  804,  831, 2150]*1e-6/2;
wY=   [1255, 1196, 1139, 1087, 1033,  993, 853,  877,  923,  968, 1024, 2538]*1e-6/2;

[w0P, z0P, M2P]=fitMsquared(zz, wPD4S, lambda);
[w0Y, z0Y, M2Y]=fitMsquared(zz, wYD4S, lambda);

beamSize=@(w0, z0, z) sqrt(w0^2+((z-z0)*lambda/pi/w0).^2);
fitfun=@(beta, z) beamSize(beta(1), beta(2), z);

betapit=nlinfit(zz, wPD4S, fitfun, [1e-3, 1]);
betayaw=nlinfit(zz, wYD4S, fitfun, [1e-3, 1]);

zzz=min(zz):0.01:max(zz);
figure
plot(zz, wPD4S, 'ro', zz, wYD4S, 'bx')
hold on
plot(zzz, w0P*sqrt(1+M2P^2*(lambda/pi/w0P^2)^2*(zzz-z0P).^2), 'm', zzz, w0Y*sqrt(1+M2Y^2*(lambda/pi/w0Y^2)^2*(zzz-z0Y).^2), 'g');
grid on
plot(zzz, beamSize(betapit(1), betapit(2), zzz), 'm--', zzz, beamSize(betayaw(1), betayaw(2), zzz), 'g--')
legend('P D4S/2', 'Y D4S/2', ['P fit: M2=', num2str(M2P)], ['Y fit: M2=', num2str(M2Y)], 'P Gaussian fit', 'Y Gaussian fit')  

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

