% 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, lambda) sqrt(w0^2+((z-z0)*lambda/pi/w0).^2);

tempPath=beamPath;
tempPath.seedWaist(300e-6,-0.5, lambda);
tempPath.targetWaist(300e-6, 0, lambda);
PFitPathD4S=tempPath.fitBeamWidth(zz, wPD4S);
YFitPathD4S=tempPath.fitBeamWidth(zz, wYD4S);

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(PFitPathD4S.seedq.waistSize, -PFitPathD4S.seedq.waistZ+PFitPathD4S.seedz, zzz, lambda), 'm--', zzz, beamSize(YFitPathD4S.seedq.waistSize, -YFitPathD4S.seedq.waistZ+YFitPathD4S.seedz, zzz, lambda), 'g--')
legend('P D4S/2', 'Y D4S/2', ['P fit: M2=', num2str(M2P)], ['Y fit: M2=', num2str(M2Y)], 'P Gaussian fit', 'Y Gaussian fit')  


%% Sheila's measurement, see alog 57906 and specifically
% https://alog.ligo-wa.caltech.edu/aLOG/uploads/57906_20210210142810_uLaser_collimator_to_OPO_532.m
zSD = ((22-[2, 5, 9, 14, 18, 22])+13.25)*25.4e-3;
hSD = [898, 832, 772, 712, 673, 640]*1e-6; %these seem to be 13.5% radius according to one of her plot captions.
vSD = [810, 748, 680, 620, 592, 568]*1e-6;  %second value was written as 1390um in my notebook, probably a trascription error

tempPath.seedWaist(300e-6,-0.5, lambda);
tempPath.targetWaist(330e-6, 0, lambda);
PFitPathSD=tempPath.fitBeamWidth(zSD, vSD);
YFitPathSD=tempPath.fitBeamWidth(zSD, hSD);
PFitPath=tempPath.fitBeamWidth(zz, wP);
YFitPath=tempPath.fitBeamWidth(zz, wY);

figure
plot(zSD, vSD, 'ro', zSD, hSD, 'rx', zz, wP, 'go', zz, wY, 'gx')
title('Beam radius VS distance from G path launcher')
xlabel('m')
ylabel('m')
hold on
zzz=-1:0.01:1.2;
plot(zzz, beamSize(YFitPath.targetq.waistSize, -YFitPath.targetq.waistZ+YFitPath.targetz, zzz, lambda), 'k');
plot(zzz, beamSize(PFitPathSD.seedq.waistSize, -PFitPathSD.seedq.waistZ+PFitPathSD.seedz, zzz, lambda), 'r');
plot(zzz, beamSize(YFitPathSD.seedq.waistSize, -YFitPathSD.seedq.waistZ+YFitPathSD.seedz, zzz, lambda), 'r--');
plot(zzz, beamSize(PFitPath.seedq.waistSize, -PFitPath.seedq.waistZ+PFitPath.seedz, zzz, lambda), 'b');
plot(zzz, beamSize(YFitPath.seedq.waistSize, -YFitPath.seedq.waistZ+YFitPath.seedz, zzz, lambda), 'b--');
legend(['Sheila P(1/e^2): ', num2str(PFitPathSD.targetOverlap)], ['Sheila Y(1/e^2): ', num2str(YFitPathSD.targetOverlap)], ...
    ['P (1/e^2): ', num2str(PFitPath.targetOverlap)], ['Y (1/e^2): ', num2str(YFitPath.targetOverlap)],...
    'ideal beam', 'Gaussian fit', 'Gaussian fit', 'Gaussian fit', 'Gaussian fit', 'Location', 'NorthWest')
grid on
xlim([-1, 1.2])
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

