%% THis is the model to evaluate the as measured mode matching at LLO.  

close all;
clear all;

lambda = 1.064e-6;

% OMC output Telescope as built - from HAM6 drawing v13

dSRM_OM1 = 3.646;    
dOM1_OM2 = 1.395;                  
dOM2_OM3 = 0.708; 
R_OM1 = 4.6;
R_OM2 = 1.7;
nOM1 = 1.45;
tOM1 = 3/8 * 25.4 * 1e-3; % OM1 thickness
tSRM = 75e-3; % SRM thickness

% build beam path to AS port
 as = beamPath;

% Seed calculated with SRM mirror parameters
% as.seedq = beamq(beamQZ(R_SRM, w_SRM, lambda));
% as.seedz = 0;
% as.addComponent(component.thickLens(-R_SRM, inf, tSRM, nSRM, 1e-6, 'SRM'));
% ==> This is consistent with seed parameters right after the SRM of: as.seedq = beamq(-3.55 + i*1.09); 

%%%% Pick your favorite input beam %%%%

 %as.seedq = beamq(-3.55 + i*1.09); % Nominal seed X=Y
 %as.seedq = beamq(-4.42 + i*0.985); % Single bounce, ITMY, LLO log 8182
 as.seedq = beamq(-4.17 + i*0.993); % Single bounce, ITMX, LLO log 8182
 as.seedz = 0; 
 
as.addComponent(component.curvedMirror(R_OM1, dSRM_OM1-tSRM, 'OM1'));

%%%%%%%%% Mode matching to the OMC %%%%%%%%%%%%%%
as.addComponent(component.curvedMirror(R_OM2, as.component('OM1').z + dOM1_OM2, 'OM2'));
as.addComponent(component.flatMirror(as.component('OM2').z + dOM2_OM3, 'OM3'));


% Keita's parameters (with nominal seed)
zOMCw_nom = as.component('OM3').z + 0.268;
waistOMC_nom = 5.09e-4;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% As built OMC based on HAM6 solidworks drawings is:
% OM1-->OM2, nominal 1.395m
% OM2-->OM3: 633 mm
% OM3 --> OMC waist = 294 + 161 mm

% Difference between Keita's distances and actual distances
%dOM2_OMCdiff = (633+294+161)*1e-3 -(0.708+0.268);

dOM2_OMCdiff = (633+294+161)*1e-3 -(0.708+0.268);

zOMCw_real = as.component('OM3').z + 0.268 + dOM2_OMCdiff;
waistOMC_real = 4.90e-4;

as.addComponent(component.flatMirror(zOMCw_real, 'OMC waist'));


% Mode match calculation
as.targetWaist(waistOMC_real, zOMCw_real);


% Try to move tip-tilts to recover good mode-matching --> doesn't work
%as = as.optimizePath('OM1', as.component('OM1').z + [-0.5 0.5], 'OM2', as.component('OM2').z + [-0.5 0.5]);

modematch = sqrt(as.targetOverlap*as.targetOverlap);
disp(['modematching to OMC = ',num2str(modematch)])

f1 = 333.6e-3;
as.addComponent(component.lens(f1, as.component('OM3').z + 0.5, 'L101'));

as.addComponent(component.flatMirror(as.component('L101').z + 190e-3 , 'ASC-AS\_A'));
as.addComponent(component.flatMirror(as.component('L101').z + 475e-3, 'ASC-AS\_B'));


%as.targetWaist(250e-6, as.component('ASC-AS\_A').z + 0.5*(as.component('ASC-AS\_B').z - as.component('ASC-AS\_A').z));   
%as = as.optimizePath('L101', as.component('L101').z + [-0.5 0.5]);

%modTelesc = sqrt(as.targetOverlap*as.targetOverlap);
%disp(['modematching Telescope = ',num2str(modTelesc )])

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
zdomain = 3:0.01:8;

figure(1)
subplot(2,1,1)
hold on 

yplot = 1e3*[as.qPropagate(zdomain).beamWidth];
as.plotComponents(zdomain,0,'r*');
plot(zdomain, yplot, 'b', zdomain, -yplot, 'b');
axis tight

ylabel('Beam width (mm)')
grid on
hold off

subplot(2,1,2)
hold on
as.plotGouyPhase(zdomain,'wrap','b');
as.plotComponents(zdomain,180,'r*');
axis tight
grid on;
ylabel('Gouy Phase (degrees)')
xlabel('z Distance from SRM (m)');

comp = length(as.components);

topRow =    {'label','z (m)','type', 'Gouy Phase'};
lineBreak = {'-----','-----','----', '----'};

topRow =    {'label','z (m)','BeamWidth (mm)', 'Gouy Phase (dg)'};
lineBreak = {'-----','-----','----', '----'};

gouyASC = as.gouyPhase([as.components.z]);
beamWASC = 1e3.*[as.qPropagate([as.components.z]).beamWidth];

for k = 1 : comp
    
    leg4{k} = sprintf('%s ',num2str(gouyASC(k)));
    leg3{k} = sprintf('%s ',num2str(beamWASC(k)));
    
end
    
outputASC = [topRow; lineBreak; [{as.components.label}.',{as.components.z}.',leg3.', leg4.']];
    
display(outputASC) 


display(sprintf('Gouy separation between OM1 and OM2 is %g', as.gouySeperation('OM1', 'OM2')))
display(sprintf('Gouy separation between OM3 and OM2 is %g', as.gouySeperation('OM3', 'OM2')))
display(sprintf('Gouy separation between OM1 and OM3 is %g', as.gouySeperation('OM1', 'OM3')))
display(sprintf('Gouy separation between WFS is %g', as.gouySeperation('ASC-AS\_A', 'ASC-AS\_B')))
display(sprintf('OM1 - OM2 distance is %g',as.component('OM2').z - as.component('OM1').z ))
display(sprintf('OM2 - OM3 distance is %g',as.component('OM3').z - as.component('OM2').z ))
display(sprintf('OM3 - L101 distance is %g',as.component('L101').z - as.component('OM3').z ))
display(sprintf('L101 - WFS A distance is %g',abs(as.component('L101').z - as.component('ASC-AS\_A').z )))
display(sprintf('L101 - WFS B distance is %g',abs(as.component('L101').z - as.component('ASC-AS\_B').z )))
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


% CalculateASpath_L1 - Y path
% modematching to OMC = 0.78701 ---> measured is really 0.71
% 
% outputASC = 
% 
%     'label'        'z (m)'     'BeamWidth (mm)'    'Gouy Phase (dg)'
%     '-----'        '-----'     '----'              '----'           
%     'OM1'          [3.5710]    '0.76252 '          '36.6779 '       
%     'OM2'          [4.9660]    '0.66232 '          '147.3689 '      
%     'OM3'          [5.6740]    '0.68672 '          '179.1855 '      
%     'OMC waist'    [6.0540]    '0.77619 '          '193.1577 '      
%     'L101'         [6.1740]    '0.81255 '          '196.8523 '      
%     'ASC-AS\_A'    [6.3640]    '0.41747 '          '207.7876 '      
%     'ASC-AS\_B'    [6.6490]    '0.27726 '          '331.2843 '      
% 
% Gouy separation between OM1 and OM2 is -69.309
% Gouy separation between OM3 and OM2 is -31.8166
% Gouy separation between OM1 and OM3 is -37.4924
% Gouy separation between WFS is -56.5033
% OM1 - OM2 distance is 1.395
% OM2 - OM3 distance is 0.708
% OM3 - L101 distance is 0.5
% L101 - WFS A distance is 0.19
% L101 - WFS B distance is 0.475


% CalculateASpath_L1 - X path
% modematching to OMC = 0.86809 ---> measured is really 0.8
% 
% outputASC = 
% 
%     'label'        'z (m)'     'BeamWidth (mm)'    'Gouy Phase (dg)'
%     '-----'        '-----'     '----'              '----'           
%     'OM1'          [3.5710]    '0.67726 '          '45.5063 '       
%     'OM2'          [4.9660]    '0.71447 '          '147.9802 '      
%     'OM3'          [5.6740]    '0.64962 '          '179.0867 '      
%     'OMC waist'    [6.0540]    '0.69864 '          '195.5601 '      
%     'L101'         [6.1740]    '0.72488 '          '200.1632 '      
%     'ASC-AS\_A'    [6.3640]    '0.36801 '          '214.1222 '      
%     'ASC-AS\_B'    [6.6490]    '0.29508 '          '331.39 '        
% 
% Gouy separation between OM1 and OM2 is -77.5262
% Gouy separation between OM3 and OM2 is -31.1066
% Gouy separation between OM1 and OM3 is -46.4196
% Gouy separation between WFS is -62.7322
% OM1 - OM2 distance is 1.395
% OM2 - OM3 distance is 0.708
% OM3 - L101 distance is 0.5
% L101 - WFS A distance is 0.19
% L101 - WFS B distance is 0.475
