clear all
close all
lambda=1064e-9;
rayleigh2W=@(Rayleigh) Rayleigh*lambda/pi; % W=w0^2
W2rayleigh=@(W) lamba/pi*W;%W=w0^2
beamRadius=@(Rayleigh, z0, z) sqrt(rayleigh2W(Rayleigh)+(z-z0).^2/Rayleigh/pi*lambda);
beamRadiusForFit=@(beta, z) beamRadius(beta(1), beta(2), z);

%% B:L1 from TFP is nominally it's 8.75"
L1z_nominal=(36.122+.25-27.622)*25.4e-3; %8.75" from TFP
L1z=L1z_nominal; 
L1ROC=350e-3;
n=1.4498 ; %refractive index
L1f=L1ROC/(n-1);
pitPath=beamPath;
pitPath.addComponent(component.lens(L1f, L1z, 'B:L1'));
yawPath=pitPath.duplicate();

%% Measure the beam profile downstream of B:L1 (with the lens)
scanz_withLens=[599, 500, 400, 300, 200, 100]*1e-3; %meters according to the rail.
beamz_withLens=L1z+290e-3+scanz_withLens(1)-scanz_withLens ; %scanhead is 290mm away from L1z when it's at 599mm on rail. 

wpit_withLens=[1046, 1146, 1241, 1343, 1466, 1592]*1e-6/2;
wyaw_withLens=[1072, 1149, 1269, 1391, 1508, 1640]*1e-6/2;


betapit=nlinfit(beamz_withLens, wpit_withLens, beamRadiusForFit, [0.4e-3, 0.12]);
betayaw=nlinfit(beamz_withLens, wyaw_withLens, beamRadiusForFit, [0.4e-3, L1z_nominal]);

pitq=beamq.beamWaistAndZ(sqrt(rayleigh2W(betapit(1))), L1z+5.75*25.4e-3-betapit(2)); % One of Lee's "space" positions.
pitPath.seedq=pitq;
pitPath.seedz=L1z+5.75*25.4e-3;

yawq=beamq.beamWaistAndZ(sqrt(rayleigh2W(betayaw(1))), L1z+5.75*25.4e-3-betayaw(2));
yawPath.seedq=yawq;
yawPath.seedz=L1z+5.75*25.4e-3;

%% Sanity Check: Measure the beam profile w/o the lens
scanz_woLens=[610, 510, 410, 310, 210, 110]*1e-3; %meters
beamz_woLens=L1z+302e-3+scanz_woLens(1)-scanz_woLens ; %302mm instead of 290 as we moved the scan head

wpit_woLens=[1385, 1561, 1747, 1930, 2112, 2289]*1e-6/2;
wyaw_woLens=[1412, 1600, 1797, 1985, 2183, 2380]*1e-6/2;

betapit=nlinfit(beamz_woLens, wpit_woLens, beamRadiusForFit, [1, 1]);
betayaw=nlinfit(beamz_woLens, wyaw_woLens, beamRadiusForFit, [1, 1]);

pitq=beamq.beamWaistAndZ(sqrt(rayleigh2W( betapit(1))), -betapit(2)); % q at z=0
pitPath.targetq=pitq;
pitPath.targetz=0;

yawq=beamq.beamWaistAndZ(sqrt(rayleigh2W(betayaw(1))), -betayaw(2));
yawPath.targetq=yawq;
yawPath.targetz=0;

% just for plotting
pitPathWOLens=beamPath();
pitPathWOLens.seedq=pitq;
pitPathWOLens.seedz=0;
yawPathWOLens=beamPath();
yawPathWOLens.seedq=yawq;
yawPathWOLens.seedz=0;

zz=0:0.01:1.5;
figure
pitPathWOLens.plotBeamWidth(zz);
hold on
pitPath.plotBeamWidth(zz);
plot(beamz_withLens, wpit_withLens, 'o', beamz_woLens, wpit_woLens, 'x')
title('Sanity Check: B path beam radius with and without lens')
ylim([0, 1.3e-3])

disp(['pit overlap between measured down- and upstream of the lens: ', num2str(pitPath.targetOverlap)]);
if pitPath.targetOverlap < 0.99
    disp('pit overlap is poor');
end
disp(['yaw overlap is between measured down- and upstream of the lens: ', num2str(yawPath.targetOverlap)]);
if yawPath.targetOverlap < 0.99
    disp('yaw overlap is poor');
end
    
%%Calculate the beam profile upstream of B:L1
seedq=pitPath.qPropagate(0);
pitPath.seedq=seedq;
pitPath.seedz=0;
%%
% targetq based on Lee's caluculation for the beam downstream of B:L1.
% The distance from TFP to this beamq location is
% 36.372+5.75-27.622".
targetq=beamq();
targetq.q=5.218e-3+1i*508.6e-3;
pitPath.targetq=targetq;
pitPath.targetz=(36.372+5.75-27.622)*25.4e-3;
display(['pit target Overlap= ', num2str(pitPath.targetOverlap)])
nominalPath=beamPath();
nominalPath.seedq=targetq; %just for plotting
nominalPath.seedz=pitPath.targetz;

newpitPath=pitPath.optimizePath('B:L1', L1z_nominal+[-L1z_nominal, 0.2])
figure
zz=0:0.005:1.5;
newpitPath.plotBeamWidth(zz);
hold on
nominalPath.plotBeamWidth(zz);
plot(beamz_withLens, wpit_withLens, 'o') 
ylim([0, 1.3e-3])
text(0.1, 0.1e-3, ['Orange=Nominal.   q param at reference position= ', num2str(newpitPath.targetq.q)])
text(0.1, 0.2e-3, ['Circles=Measured. q param at reference position= ', num2str(pitPath.qPropagate((36.372+5.75-27.622)*25.4e-3).q)]);
text(0.1, 0.3e-3, ['Blue=After B:L1 pos optimization calculation. q= ', num2str(newpitPath.qPropagate( (36.372+5.75-27.622)*25.4e-3).q)]);
text(0.2, 0.26e-3, ['Overlap= ', num2str(newpitPath.targetOverlap)]);
