close all
clear all
clc

%Create a beamPath object
InpPath = beamPath;
%Add components - for a first pass, ignore Faraday and HWPs, so only
%mirrors and lenses..
InpPath.addComponent(component.flatMirror(35e-3,'M1'));
InpPath.addComponent(component.flatMirror(75e-3,'M2'));
InpPath.addComponent(component.flatMirror(824e-3,'M3'));
InpPath.addComponent(component.flatMirror(997e-3,'M4'));
InpPath.addComponent(component.flatMirror(2243e-3,'IM1')); %~34 in from edge of PSL table to IM1, from CAD diagram
InpPath.addComponent(component.flatMirror(3317e-3,'MC1')); %~42.3in from IM1 to MC1 AR (from CAD diagram

InpPath.addComponent(component.lens(0.20,173e-3,'L1'));
InpPath.addComponent(component.lens(-0.150,671e-3,'L2'));
InpPath.addComponent(component.lens(0.4,934e-3,'L3'));

%Define a list of available lenses...
focalLengthList = (1e-3)*[-150, 400];
lensList = component.lens(focalLengthList);


InpPath.seedWaist(370e-6,0.);    %PMC waist
%Define the target (IR) beam: a waist of 50um at the doubling oven..
InpPath.targetWaist(1.78e-3,3.3173);  %%Accounting for offset from center of doubling oven...


% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % BLOCK FOR SENSITIVITY ANALYSIS
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 
% X = -0.1:0.002:0.1;
% Y = -0.1:0.002:0.1;
% MMeff = [];
% 
% for i = 1:length(X)
%     for j = 1:length(Y)
%         AUXXPath.component('L2').z = 671e-3 + X(i);
%         AUXXPath.component('L3').z = 934e-3 + Y(j);
%         MMeff(i,j) = AUXXPath.targetOverlap;
%     end
% end
% [c,h]=contour(X,Y,MMeff,[0.8:0.02:1.00]);
% set(gca,'FontSize',16,'FontWeight','bold');
% xlabel('Offset of Lens 2 from current position (m)','FontSize',16,'FontWeight','bold');
% ylabel('Offset of Lens 3 from current position (m)','FontSize',16,'FontWeight','bold');
% % h2=contourcmap('winter','Colorbar','on');
% % set(h2, 'CLim', [0.8, 1.0]);
% % set(h2,'ylim',[0.8,1]);
% % colorbar()
% 
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % END OF BLOCK FOR SENSITIVITY ANALYSIS
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % % BLOCK FOR MM OPTIMIZATION
% % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 
% 
% %Slide lenses around to optimize mode matching...
% %                          'L1',lensList,[0.08,0.29],...
% [pathList,overlapList] = AUXXPath.chooseComponents(...
%                          'L2',lensList,[0.48,0.8],...
%                          'L3',lensList,[0.83,0.97],...
%                          '-vt',0.1);
% pathList = pathList(overlapList >= 0.95);
% if size(pathList,1) > 1
%     sensitivityList = pathList.positionSensitivity;
%     [sensitivityList,sortIndex] = sort(sensitivityList);
%     pathList = pathList(sortIndex);
% end
% %Plot the "best" solution... Happens to be the 2nd one for us...
% figure
% hold on
% zplot = -0.1:.001:3.5;
% newhandle = pathList(1).plotBeamWidth(zplot,'r','LineWidth',3);
% pathList(1).plotComponents(zplot,'r*');
% pathList(1).plotBeams(zplot,'b');
% 
% display(pathList(1).components)
% 
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % END BLOCK FOR MM OPTIMIZATION
% % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% BLOCK FOR SINGLE LENS SLIDING
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

X = -0.1:0.002:0.1;
Y = 0:0.002:0.2;

MMeff1 = [];
MMeff2 = [];

for i = 1:length(X)
    InpPath.component('L2').z = 671e-3 + X(i);
    MMeff1(i) = InpPath.targetOverlap; 
end

for i = 1:length(X)
    InpPath.component('L3').z = 934e-3 + Y(i);
    MMeff2(i) = InpPath.targetOverlap; 
end

plot(X,MMeff1,'b',Y,MMeff2,'r','LineWidth',3);
xlabel('Distance lens is shifted from current position (m)','FontSize',16,'FontWeight','bold')
ylabel('Mode matching efficiency','FontSize',16,'FontWeight','bold')
set(gca,'LineWidth',3)
grid on


% h2=contourcmap('winter','Colorbar','on');
% set(h2, 'CLim', [0.8, 1.0]);
% set(h2,'ylim',[0.8,1]);
% colorbar()

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% END OF BLOCK FOR SINGLE LENS SLIDING
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%