%% 
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=(528 + 858 + 160)*1e-3;
f1=333e-3; % for red. E1000845-v1-10

% lens2
nominaldistance = 240e-3;
z2 = z1 + nominaldistance;
f2=-111e-3; % for red. E1000845-v1

% QPD positions
zQ1= z2+410e-3;
zQ2= z2+710e-3;

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

MM2L1holder = (851+416)*1e-3; % Measured distance from the downstream face of the L1 holder to the downstream MM face
zd=z1+MM2L1holder;
%% 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:(zd+.5);

    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