clear all
close all
lambda=1064e-9;
beamRadiusForFit=@(beta, z) beamRadius(beta(1), beta(2), z);
n=1.4498 ; %refractive index of fused silica at 1064nm

%% Reference position: z=0 at B:M4.
%% Measurement data
d_rail = (19- [19,   17,   15,   13,   11,    9,    7,    5,    3,    1])*25.4e-3;
wp_4sigma = [1486, 1499, 1519, 1534, 1555, 1587, 1611, 1634, 1661, 1692]*1e-6/2;
wp_13p5 =   [1469, 1480, 1475, 1500, 1505, 1547, 1582, 1601, 1631, 1659]*1e-6/2;
wy_4sigma = [1507, 1528, 1541, 1558, 1580, 1611, 1636, 1660, 1682, 1701]*1e-6/2;
wy_13p5 =   [1467, 1510, 1520, 1539, 1552, 1590, 1613, 1628, 1647, 1677]*1e-6/2;

d_rail_at_19_to_SM = (118+6)*1e-3; 
%front of CCD casing, to off-platform Steering Mirror was 118mm, I assume
%6mm from the front to the sensor.
SM_to_BM4 = 194e-3;

z_rail=d_rail_at_19_to_SM + SM_to_BM4 + d_rail;

%% Ideal mode shape based on Lee's calculation
idealPath=beamPath();
idealPath.seedq.q=283.6e-3+1.291i; %this is q value at ZM4.
idealPath.seedz=0; %this is q value at ZM4.
PPath_w4s=idealPath.fitBeamWidth(z_rail, wp_4sigma);
PPath_w4s.targetq=idealPath.seedq;
PPath_w4s.targetz=idealPath.seedz;
YPath_w4s=idealPath.fitBeamWidth(z_rail, wy_4sigma);
YPath_w4s.targetq=idealPath.seedq;
YPath_w4s.targetz=idealPath.seedz;

PPath_13p5=idealPath.fitBeamWidth(z_rail, wp_13p5);
PPath_13p5.targetq=idealPath.seedq;
PPath_13p5.targetz=idealPath.seedz;
YPath_13p5=idealPath.fitBeamWidth(z_rail, wy_13p5);
YPath_13p5.targetq=idealPath.seedq;
YPath_13p5.targetz=idealPath.seedz;

%% Plot 4 sigma width
zz=-.4:0.01:1;
figure
plot(z_rail, wp_4sigma, 'ro');
hold on
plot(z_rail, -wy_4sigma, 'bx');
plot(zz, beamRadius(PPath_w4s.seedq.rayleighRange, PPath_w4s.seedz-PPath_w4s.seedq.waistZ, zz, lambda), 'r--');
plot(zz, -beamRadius(YPath_w4s.seedq.rayleighRange, YPath_w4s.seedz-YPath_w4s.seedq.waistZ, zz, lambda), 'b--');
idealPath.plotBeamWidth(zz, 'g');
legend('w4s P measured', 'w4s Y measured', 'P Gaussian fit', 'Y Gaussian fit', 'ideal mode', 'location', 'east')
xlabel('Distance from the last steering mirror in B path [m]')
ylabel('Beam radius [m]')
text(-0.2,  0.6e-3, ['P mode overlap: ', num2str(PPath_w4s.targetOverlap)]);
text(-0.2, -0.6e-3, ['Y mode overlap: ', num2str(YPath_w4s.targetOverlap)]);
grid on
title('SQZ-IFO MM estimate using 4 Sigma width')

%%Check M squared, but I couldn't get close enough to measure that in any
%%meaningful manner.
[w0P, z0P, msquaredP] = fitMsquared(z_rail, wp_4sigma, lambda);
[w0Y, z0Y, msquaredY] = fitMsquared(z_rail, wy_4sigma, lambda);

%% Plot 1/e^2 width
zz=-.4:0.01:1;
figure
plot(z_rail, wp_13p5, 'ro');
hold on
plot(z_rail, -wy_13p5, 'bx');
plot(zz, beamRadius(PPath_13p5.seedq.rayleighRange, PPath_13p5.seedz-PPath_13p5.seedq.waistZ, zz, lambda), 'r--');
plot(zz, -beamRadius(YPath_13p5.seedq.rayleighRange, YPath_13p5.seedz-YPath_13p5.seedq.waistZ, zz, lambda), 'b--');
idealPath.plotBeamWidth(zz, 'g');
legend('1/e^2 width P measured', '1/e^2 width Y measured', 'P Gaussian fit', 'Y Gaussian fit', 'ideal mode', 'location', 'east')
xlabel('Distance from the last steering mirror in B path [m]')
ylabel('Beam radius [m]')
text(-0.2,  0.6e-3, ['P mode overlap: ', num2str(PPath_13p5.targetOverlap)]);
%text(-0.2, 0.53e-3, ['Msquared ; ', num2str(msquaredP)]);
text(-0.2, -0.6e-3, ['Y mode overlap: ', num2str(YPath_13p5.targetOverlap)]);
%text(-0.2, -0.67e-3, ['Msquared ; ', num2str(msquaredY)]);
grid on
title('SQZ-IFO MM estimate using 1/e^2 beam width')

%% Add B:L2
L2ROC=500e-3;
L2f=L2ROC/(n-1);
d_BM1_to_BL1 = 87e-3;

d_BBS1_to_BM4 = 110e-3;
z_BBS1=-d_BBS1_to_BM4;
d_BM3_to_BBS1 = 204e-3;
z_BM3=z_BBS1-d_BM3_to_BBS1;
d_BM3_to_BL2_mm = 148e-3;
z_L2=z_BM3+d_BM3_to_BL2_mm;

PPath_w4s.addComponent(component.lens(L2f, z_L2, 'B:L2'));
newseedq=PPath_w4s.qPropagate(z_BM3);
newPPath_w4s=PPath_w4s.duplicate();
newPPath_w4s.seedq=newseedq;
newPPath_w4s.seedz=z_BM3;
optPPath_w4s=newPPath_w4s.optimizePath('B:L2', [z_BM3, z_BBS1]);

YPath_w4s.addComponent(component.lens(L2f, z_L2, 'B:L2'));
newseedq=YPath_w4s.qPropagate(z_BM3);
newYPath_w4s=YPath_w4s.duplicate();
newYPath_w4s.seedq=newseedq;
newYPath_w4s.seedz=z_BM3;
optYPath_w4s=newYPath_w4s.optimizePath('B:L2', [z_BM3, z_BBS1]);

figure
optPPath_w4s.plotSummary();
title('P optimization')
text(optPPath_w4s.component('B:L2').z, 0.3e-3, ['L2 move by ', num2str((optPPath_w4s.component('B:L2').z-PPath_w4s.component('B:L2').z)*1e3), 'mm']);
text(optPPath_w4s.component('B:L2').z, 0.2e-3, ['new overlap: ', num2str(optPPath_w4s.targetOverlap())]);
grid on

figure
optYPath_w4s.plotSummary();
title('Y optimization')
text(optYPath_w4s.component('B:L2').z, 0.3e-3, ['L2 move by ', num2str((optYPath_w4s.component('B:L2').z-YPath_w4s.component('B:L2').z)*1e3), 'mm']);
text(optYPath_w4s.component('B:L2').z, 0.2e-3, ['new overlap: ', num2str(optYPath_w4s.targetOverlap())]);
grid on

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_=beamRadius(Rayleigh, z0, z, lambda) 
    w_=sqrt(rayleigh2W(Rayleigh, lambda)+(z-z0).^2/Rayleigh/pi*lambda);
    return;
end

function wsq_=rayleigh2W(Rayleigh, lambda)
    wsq_=Rayleigh*lambda/pi; % W=w0^2
    return;
end

function rayleigh_=W2rayleigh(W, lambda)
    rayleigh_=lambda/pi*W;%W=w0^2
    return;
end
