%% 
clear all
close all
%% Initialization
% Measured upstream (source) parameterxs, measured from the launcher lens mount.
wu=[2.487, 2.619]*1e-3;
zu=[7.020, 10.847]; % waist is 7 or 10 m upstream of the last launcher lens. Positive means that the waist is upstream.
dimensions={'X', 'Y'};

% Nominal lens placement
% lens1
z1=1.4; % about 1m.
f1=333.6e-3; % for red. E1000845-v1-10

% lens2
nominaldistance = 203e-3;
z2 = z1 + nominaldistance;
f2=-166.8e-3; % for red. E1000845-v1–08

% QPD positions
zQ1= z2+367e-3;
zQ2= zQ1+369e-3;

% Measured downstream parameters. This is used as a target.
wd=[.371 .356]*1e-3/2;
zd_MM=[.364 .370]; %waist position from the modemaster head, positive means downstream of the modemaster

MM2L2holder = 276.0e-3; % Measured distance from the downstream face of the L2 holder to the downstream MM face
ROC2=75e-3; % lens2 ROC, note that this is concave.
diam2 = 25e-3; % diameter of lens2.
thick2=6.35e-3; % lens thickness at the edge.
thickMt = 25.4e-3/2; % mount thickness
lipMt=1.6e-3; % thickness of the lip (MEASURED).

depth2= thickMt -lipMt -thick2 + ROC2*(1-cos(asin(diam2/2/ROC2)));
% Distance from the center of the curved surface of L2 lens to the downstream face of the L2 holder. 

MM2L2 = MM2L2holder+depth2; % Distance from the center of the curved surface of L2 lens to the MM head.
zd=z2 + MM2L2; % Position of MM

%% Don't modify things from here

colors={'r', 'b'};
for ii=1:length(dimensions)
    % First we propagate the upstream beam to the downstream using nominal lens
    % distance, just for plotting.

    nominalPath=beamPath;
    nominalPath.seedq = beamq.beamWaistAndZ(wu(ii), zu(ii));
    nominalPath.seedz = -1e-6;

    nominalPath.addComponent(component.lens(f1, z1, 'L1'));
    nominalPath.addComponent(component.lens(f2, z2, 'L2'));

    nominalPath.addComponent(component.flatMirror(zQ1, 'QPD1'));
    nominalPath.addComponent(component.flatMirror(zQ2, 'QPD2'));
    nominalPath.addComponent(component.flatMirror(zd, 'MM'));
    z=(z1-1):0.01:(z2+1);

    figure(101)
    title('Ideal propagation profile')
    hold on;
    nominalPath.plotBeamWidth(z, colors{ii});
    tmpQ = nominalPath.qPropagate(zd); % beamq object at the MM head
    hh=text(z2, (-1)^ii * wu(ii), [dimensions{ii} ': w0=' num2str(tmpQ.waistSize * 1000) 'mm at ' num2str(-tmpQ.waistZ*1000) 'mm downstream of MM'], ...
        'Color', colors{ii})

    
    % Set the target.
    nominalPath.targetz = zd; % this is the position of the MM head
    nominalPath.targetq = beamq.beamWaistAndZ(wd(ii), -zd_MM);
    % The sign of zd_MM should be flipped because, if zd is positive (i.e. the
    % waist is downstream of the MM), from ALM's point of view, at MM position, 
    % the beam is not yet reached the waist.

    % Now match the upstream and downstream. Ideally we want to do this by moving L2 and MM together, but ALM cannot do it.
    % We'll just move L1. This should be good enough.

    measuredPathWithFit = nominalPath.optimizePath('L1', [z1-0.02, z1+0.02]);
    
    figure
    measuredPathWithFit.plotSummary(z)
    title([dimensions{ii} ' L1 position optimized for the measured data'])
    subplot 211
    spacing=measuredPathWithFit.component('L2').z-measuredPathWithFit.component('L1').z;
    spacingDiff = spacing-nominaldistance;
    if spacingDiff > 0
        spacingStr=['+' num2str(spacingDiff*1e3)];
    else
        spacingStr=num2str(spacingDiff*1e3);
    end
    text(0, -2e-3, [dimensions{ii} ' spacing: ' num2str(spacing*1e3) ' = nominal' spacingStr 'mm'])
end

%%
figure(101)
nominalPath.plotComponents(z);
grid on