% btwn opo input coupler and dichroic
% wincam avg=5
% rail mm, d13.5P um, d4sP, d13.5Y, d4sY
% 0, 1808, 1857, 2264, 2304
% 200, 1460, 1454, 1806, 1835
% 400, 969, 1085, 1392, 1403
% 530, 779, 872, 1110, 1112
% 
% rough distance from input coupler to steering mirror to the ccd casing front when rail=530
% 50+200+208 (mm)
% 
% after dichroic
% rotated the sensor by 180 deg, right<->left and up<->dn flipped
% 0, 1855, 1900, 2280, 2318
% 200, 1534, 1549, 1881, 1909
% 400, 1205, 1190, 1473, 1473
% 530, 856, 940, 1222, 1210
% 
% rough distance from input coupler to steering mirror to the ccd casing front when rail=530mm
% 375+120
% 
% After Lens (path on HAM7)
% rotated the sensor again by 180 deg.
% 0, 789, 877, 711, 768 <- no avg5
% 150, 633, 713, 636, 711 <- no avg5
% 300, 627, 656, 676, 716
% 450, 578, 622, 743, 760
% 530, 565, 610, 716, 738
% 150, 631, 696, 610, 685
% 0, 800, 868, 727, 787
% 
% Something is fishy in the above, why D4SY goes like
% z=[0, 150, 300, 450, 530]
% D4SY=[787, 685, 716, 760, 738]? 
% Why does it decrease from z=450 to z=530?
% Could it be averaging?
% increased to avg 20
% 150, 639, 699, 615, 679
% 0, 788, 858, 723, 781
% 300, 633, 659, 666, 705
% 450, 586, 623, 735, 757
% 530, 567, 608, 715, 746
% D4SY is still decreasing from z=450 to 530, so it's pretty consistent.
% 
% rough dist from sensor casing front to lens when rail is 530mm:  265
% lens to steering mirror G:M2: 60
% steering mirror to input coupler ~280?

%%
close all
clear all
lambda=532e-9;
nFS=1.4607; %fused silica refractive index at 532nm.
%%
OPO_ADC1_avg5=[... % inserted steering mirror between OPO M1 and dichroic.
    0, 1808, 1857, 2264, 2304;...
    200, 1460, 1454, 1806, 1835;...
    400, 969, 1085, 1392, 1403;...
    530, 779, 872, 1110, 1112];
OPO_ADC1_railOff=(50+200+208)*1e-3; %mm when the rail is at 530mm.
OPO_ADC1_z=OPO_ADC1_railOff+(530-OPO_ADC1_avg5(:,1))*1e-3;

ADC1_GM2_avg5=[... % inserted steering mirror between dichroic and G:M2.
    0, 1855, 1900, 2280, 2318;...
    200, 1534, 1549, 1881, 1909;...
    400, 1205, 1190, 1473, 1473;...
    530, 856, 940, 1222, 1210];
ADC1_GM2_railOff=(375+120)*1e-3;
ADC1_GM2_z=ADC1_GM2_railOff+(530-ADC1_GM2_avg5(:,1))*1e-3;

onHAM7_avg20=[... % this is the HAM7 path downstream of G:L3
 150, 639, 699, 615, 679; ...
 0, 788, 858, 723, 781; ...
 300, 633, 659, 666, 705; ...
 450, 586, 623, 735, 757; ...
 530, 567, 608, 715, 746];
onHAM7_railOff=(265+60+280)*1e-3;
onHAM7_z=onHAM7_railOff+(530-onHAM7_avg20(:,1))*1e-3;
GL3_z=(60+280)*1e-3; % distance from the opo input coupler to the lens.

[w0Y, z0Y, msqY]=fitMsquared(onHAM7_z, onHAM7_avg20(:, 3)*1e-6/2, lambda);
[w0P, z0P, msqP]=fitMsquared(onHAM7_z, onHAM7_avg20(:, 5)*1e-6/2, lambda);

figure
plot(onHAM7_z, onHAM7_avg20(:, 3)*1e-6/2, 'or', onHAM7_z, onHAM7_avg20(:, 5)*1e-6/2, 'xb', ...
    'markersize', 12)
grid on
xlabel('approximate distance from the OPO input coupler [m]')
ylabel('beam radius based on D4S diameter using WinCam [m]')
hold on
plot(ADC1_GM2_z, ADC1_GM2_avg5(:,3)*1e-6/2, 'om', ADC1_GM2_z, ADC1_GM2_avg5(:,5)*1e-6/2, 'xg', ...
    'markersize', 12);
legend(['hor. after lens, M2=', num2str(msqY)], ['vert. after lens, M2=', num2str(msqP)], 'hor. picked off upstream of the lens', 'vert. picked off upstream of the lens', 'location', 'northwest')
xlim([0, 1200e-3])
%ylim([300, 1200])
%plot(GL3_z, 300, '*', 'markersize', 10)
%text(GL3_z, 350, 'G:L3 position')

lensROC=150e-3;
lensf=lensROC/(nFS-1);

temppath=beamPath();
temppath.seedWaist(200e-6, 0, lambda);
pickOffPathY=temppath.fitBeamWidth(ADC1_GM2_z, ADC1_GM2_avg5(:, 3)/2*1e-6);
z1=(0:100)/100*GL3_z;
z2=GL3_z+(0:100)/100*(1200e-3-GL3_z);
figure
subplot(211)
pickOffPathY.plotBeamWidth(z1);
hold on
pickOffPathY.plotBeamWidth(z2);

pickOffPathY.addComponent(component.lens(lensf, GL3_z, 'G:L3'));
pickOffPathY.plotBeamWidth(z2);
plot(ADC1_GM2_z, ADC1_GM2_avg5(:,3)*1e-6/2, 'bx', ...
    'markersize', 12);
plot(onHAM7_z, onHAM7_avg20(:, 3)*1e-6/2, 'or', 'markersize', 12);
grid on
ylim([0, 1.4e-3])
ylabel('YAW radius based on D4S [m]')
title('YAW beam profile of SK path (red circle) and picked off upstream of the lens (blue cross)');
text(0.7, 0.3e-3, ['SK path: Hrizontal M^2=', num2str(msqY), ', w0=', num2str(w0Y*1e3), '[um]'])

%%
pickOffPathP=temppath.fitBeamWidth(ADC1_GM2_z, ADC1_GM2_avg5(:, 5)/2*1e-6);
subplot(212)
pickOffPathP.plotBeamWidth(z1);
hold on
pickOffPathP.plotBeamWidth(z2);

pickOffPathP.addComponent(component.lens(lensf, GL3_z, 'G:L3'));
pickOffPathP.plotBeamWidth(z2);
plot(ADC1_GM2_z, ADC1_GM2_avg5(:,5)*1e-6/2, 'bx', ...
    'markersize', 12);
plot(onHAM7_z, onHAM7_avg20(:, 5)*1e-6/2, 'or', 'markersize', 12);
grid on
ylim([0, 1.4e-3])
text(0.7, 0.3e-3, ['SK path: Vertical M^2=', num2str(msqP), ', w0=', num2str(w0P*1e3), '[um]'])
ylabel('PIT radius based on D4S [m]')
xlabel('distance from the input coupler of OPO [m]');
title('PIT beam profile of SK path (red circle) and picked off upstream of the lens (blue cross)');


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