%% This is the model to evaluate the mode mismatch induced by the current ITMs in H1
%% Beam propagation for single bounce from ITMX (X path) and ITMY (Y path)
%% H1 interferometer, September 2014 (after ITM swap)

close all;
lambda = 1.064e-6;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Cross check of PRC, SRC and Schnupp Asy lengths

% Optical length PR3 --> ITMX AR
dPR3_ITXAR = dPR3_BSHR + dBSHR_BSAR * 1.45 + dBSAR_CPX1 + dCPX1_CPX2 *1.45 + dCPX2_ITXAR;
dPRM_ITXAR = dPRM_PR2 + dPR2_PR3 + dPR3_ITXAR;
dPRM_BSHR = dPRM_PR2 + dPR2_PR3 + dPR3_BSHR;

% Optical length BS --> ITMY HR / ITX HR
dBSHR_ITYHR = dBSHR_CPY1 + dCPY1_CPY2 * 1.45 + dCPY2_ITYAR + dTYAR_ITYHR * 1.45;
dBSHR_ITXHR =  dBSHR_BSAR * 1.45 + dBSAR_CPX1 + dCPX1_CPX2 * 1.45 + dCPX2_ITXAR + dITXAR_ITXHR * 1.45;

% Optical length SR3 --> ITMY AR
dSR3_BSHR = dSR3_BSAR + dBSAR_BSHR * 1.45;
dSR3_ITYAR = dSR3_BSHR + dBSHR_CPY1 + dCPY1_CPY2 *1.45 + dCPY2_ITYAR;


% Cross check optical length of signal recycling cavities
PRC = dPRM_PR2 + dPR2_PR3 + dPR3_BSHR + (dBSHR_ITXHR + dBSHR_ITYHR) / 2
SRC = dSRM_SR2 + dSR2_SR3 + dSR3_BSHR + (dBSHR_ITXHR + dBSHR_ITYHR) / 2

Schnupp = dBSHR_ITXHR - dBSHR_ITYHR;

% Build beam path to AS port
asX = beamPath;

%%% Start with beam perfectly matched on PRM
% Seed beam at PRM
%asX.seedq = beamq(beamQZ(-R_PRM, 2.24e-3, lambda));
%asX.seedz = 0;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Beam propagation from the IMC, based on Paul's measured IO parameters

% Input waist in IMC is in between MC1 and MC3

% Seed for X path
 asX.seedq = beamq(-0.2325 + 13.3377i); % q at MC3 HR - path for single bounce X
 asX.seedz = 0;
% Seed for Y path
 asY.seedq = beamq(-0.2325 + 13.3377i); % q at MC3 HR - path for single bounce Y
 asY.seedz = 0;

 
% From IMC to PRM
% MC3 substrate path length = 84.5mm; 122.6 MC3-AR surface to IM1 = 428.2mm
dWaist_SM1 = (122.6 + 428.2)*1e-3;
asX.addComponent(component.flatMirror(dWaist_SM1, 'SM1f'));
% IM1->IM2 = 1293.8mm, IM2 Rc = 12800mm, IM2 AOI = 7deg
asX.addComponent(component.curvedMirror(ROC_PMMT1*cos(7*pi/180), asX.component('SM1f').z + dSM1_PMMT1_eff, 'PMMT1f'));
%IM2->IM3 = 1170.4mm, IM3 Rc = -6240mm
asX.addComponent(component.curvedMirror(ROC_PMMT2*cos(7*pi/180), asX.component('PMMT1f').z + dPMMT2_PMMT1_eff, 'PMMT2f'));
%IM3 AOI = 7.1deg, IM3->IM4 = 1174.5mm
asX.addComponent(component.flatMirror(asX.component('PMMT2f').z + dSM2_PMMT2_eff, 'SM2f'));
% PRM lens, IM4->PRM-AR surface = 413.5mm PRM substrate path length = 73.7mm
asX.addComponent(component.thickLens(inf, R_PRM, tPRM, nCPX, asX.component('SM2f').z + dPRM_SM2_eff + tPRM/idxPRM, 'PRMHR'));


% X - Path from PRM to ITX HR
asX.addComponent(component.curvedMirror(R_PR2,  asX.component('PRMHR').z + dPRM_PR2, 'PR2'));
asX.addComponent(component.curvedMirror(R_PR3, asX.component('PR2').z + dPR2_PR3, 'PR3'));
asX.addComponent(component.thickLens(R_BS, inf, dBSHR_BSAR, nSRM, asX.component('PR3').z + dPR3_BSHR, 'BSHR'));

% Build Y Path
asY = asX.duplicate;  

asX.addComponent(component.thickLens(R_BS, inf, tCPX, nCPX, asX.component('BSHR').z + dBSAR_CPX1, 'CPXf'));
asX.addComponent(component.curvedMirror(-R_ITMX/nITMX, asX.component('CPXf').z + dCPX2_ITXAR + tITMX/nITMX, 'ITXHR'));

% X - Path back from ITXHR to BS HR
asX.addComponent(component.thickLens(inf, inf, 100e-3, nCPX, asX.component('ITXHR').z + dCPX2_ITXAR, 'CPXb'));
asX.addComponent(component.thickLens(inf, -R_BS, 2*dBSHR_BSAR, nSRM, asX.component('CPXb').z + dBSAR_CPX1, 'BSAR'));
% X - Path back from BS AR to SRM
asX.addComponent(component.curvedMirror(R_SR3, asX.component('BSAR').z + dSR3_BSAR, 'SR3'));
asX.addComponent(component.curvedMirror(R_SR2, asX.component('SR3').z + dSR2_SR3 , 'SR2'));
asX.addComponent(component.thickLens(-R_SRM, inf, tSRM, nSRM, asX.component('SR2').z + dSRM_SR2, 'SRM'));

% 
% % X - Overlap with target beam right after SRM - We don't have these measurements for H1 yet
% 
% %asX.targetq = beamq(-3.55 + i*1.09); % Nonimal parameters right after SRM
% asX.targetq = beamq(-4.17 + i*0.993); % Single bounce, ITMX, LLO log 8182
% 
% asX.targetz = asX.component('SRM').z + 1e-6;
% modematch = sqrt(asX.targetOverlap*asX.targetOverlap);
% disp(['modematching asX with measured beam after SRM = ',num2str(modematch)])



% Y - Path from BSHR to ITYHR   
asY.addComponent(component.thickLens(R_CPY, inf, tCPY, nCPY, asY.component('BSHR').z + dBSHR_CPY1, 'CPYf'));
asY.addComponent(component.curvedMirror(-R_ITMY/nITMY, asY.component('CPYf').z + dCPY2_ITYAR + tITMY/nITMY, 'ITYHR'));
% Y - Path back from ITYHR to BS AR
asY.addComponent(component.thickLens(inf, -R_CPY, 100e-3, nCPX, asY.component('ITYHR').z + dCPY2_ITYAR, 'CPYb'));
asY.addComponent(component.thickLens(inf, -R_BS, dBSHR_BSAR, nSRM, asY.component('CPYb').z + dBSHR_CPY1, 'BSAR')); 
% Y - Path back from BS AR to SRM
asY.addComponent(component.curvedMirror(R_SR3, asY.component('BSAR').z + dSR3_BSAR, 'SR3'));
asY.addComponent(component.curvedMirror(R_SR2, asY.component('SR3').z + dSR2_SR3 , 'SR2'));
asY.addComponent(component.thickLens(-R_SRM, inf, tSRM, nSRM, asY.component('SR2').z + dSRM_SR2, 'SRM'));


% Overlap between beams after SRM
qX = asX.qPropagate([asX.component('SRM').z + 100e-2]).q ;
qY = asY.qPropagate([asY.component('SRM').z + 100e-2]).q ;

overlapXY = overlap(beamq(qX), beamq(qY));
disp(['overlap of Qs after SRM = ',num2str(overlapXY)])



% % Y - Overlap with target beam right after SRM - - We don't have these measurements for H1 yet
% asY.targetq = beamq(-4.42 + i*0.985); % Single bounce, ITMY, LLO log 8182
% asY.targetz = asY.component('SRM').z + 1e-6;
% 
% modematch = sqrt(asY.targetOverlap*asY.targetOverlap);
% disp(['modematching asY with measured beam after SRM = ',num2str(modematch)])
% 
% 
% % OutputTelescope - X path
 asX.addComponent(component.curvedMirror(R_OM1, asX.component('SRM').z + dSRM_OM1-tSRM, 'OM1'));
 asX.addComponent(component.curvedMirror(R_OM2, asX.component('OM1').z + dOM1_OM2, 'OM2'));
 asX.addComponent(component.flatMirror(asX.component('OM2').z + dOM2_OM3, 'OM3'));
% 
% % OutputTelescope - Y path
 asY.addComponent(component.curvedMirror(R_OM1, asY.component('SRM').z + dSRM_OM1-tSRM, 'OM1'));
 asY.addComponent(component.curvedMirror(R_OM2, asY.component('OM1').z + dOM1_OM2, 'OM2'));
 asY.addComponent(component.flatMirror(asY.component('OM2').z + dOM2_OM3, 'OM3'));
% 
% 
% 
% % Keita's parameters (with nominal seed)
% zOMCw_nomX = asX.component('OM3').z + 0.268;
% zOMCw_nomY = asY.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);
 
 zOMCw_realX = asX.component('OM3').z + 0.268 + dOM2_OMCdiff;
 zOMCw_realY = asY.component('OM3').z + 0.268 + dOM2_OMCdiff;
 waistOMC_real = 4.90e-4;
 
 asX.addComponent(component.flatMirror(zOMCw_realX, 'OMC waist'));
 asY.addComponent(component.flatMirror(zOMCw_realX, 'OMC waist'));

% % Mode match calculation to real / nominal OMC
 disp('As built OMC')
 asX.targetWaist(waistOMC_real, zOMCw_realX);
 asY.targetWaist(waistOMC_real, zOMCw_realX);

 
%  %disp('Nominal OMC')
%  %asX.targetWaist(waistOMC_nom, zOMCw_nomX);
%  %asY.targetWaist(waistOMC_nom, zOMCw_nomY);
% 
% 
% % % Try to move tip-tilts to recover good mode-matching
% %   disp(['OM1 = ',num2str(asX.component('OM1').z) ])
% %   disp(['OM2 = ',num2str(asX.component('OM2').z) ])
% %   disp(['OM3 = ',num2str(asX.component('OM3').z) ])
% % 
% %   asX = asX.optimizePath('OM2', asX.component('OM2').z + [-0.1 0.1], 'OM3', asX.component('OM3').z + [-0.1 0.1]);
% % 
% %  disp(['OM1 = ',num2str(asX.component('OM1').z) ])
% %  disp(['OM2 = ',num2str(asX.component('OM2').z) ])
% %  disp(['OM3 = ',num2str(asX.component('OM3').z) ])
% % 
% 
  modematch = sqrt(asX.targetOverlap*asX.targetOverlap);
  disp(['modematching asX with OMC = ',num2str(modematch)])

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

  
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
zdomain = 0:1:130;

% figure(1)
% subplot(2,1,1)
% hold on 
% 
% yplot = 1e3*[asX.qPropagate(zdomain).beamWidth];
% asX.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
% asX.plotGouyPhase(zdomain,'wrap','b');
% asX.plotComponents(zdomain,180,'r*');
% axis tight
% grid on;
% ylabel('Gouy Phase (degrees)')
% xlabel('z Distance from IMC waist (m)');
% 
% 
% % X components
% comp = length(asX.components);
% topRow =    {'label','z (m)','BeamWidth (mm)', 'Gouy Phase (degree)'};
% lineBreak = {'-----','-----','-----', '-----'};
% gouy = asX.gouyPhase([asX.components.z]);
% beamW = 1e3.*[asX.qPropagate([asX.components.z]).beamWidth];
% paramQ = [asX.qPropagate([asX.components.z]).q];
% for k = 1 : comp
%     
%     leg1{k} = sprintf('%s ',num2str(gouy(k)));
%     leg2{k} = sprintf('%s ',num2str(beamW(k)));
%     leg3{k} = sprintf('%s ',num2str(paramQ(k)));
% end
% disp('X Parameters')
% output = [topRow; lineBreak; [{asX.components.label}.',{asX.components.z}.',leg2.', leg1.']];
% display(output)
%  
%  
% 
% figure(2)
% subplot(2,1,1)
% hold on 
% 
% yplot = 1e3*[asY.qPropagate(zdomain).beamWidth];
% asY.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
% asY.plotGouyPhase(zdomain,'wrap','b');
% asY.plotComponents(zdomain,180,'r*');
% axis tight
% grid on;
% ylabel('Gouy Phase (degrees)')
% xlabel('z Distance from IMC waist (m)');
% 
% 
% % % Y components
% comp = length(asY.components);
% topRow =    {'label','z (m)','BeamWidth (mm)', 'Gouy Phase (degree)'};
% lineBreak = {'-----','-----','-----', '-----'};
% gouy = asY.gouyPhase([asY.components.z]);
% beamW = 1e3.*[asY.qPropagate([asY.components.z]).beamWidth];
% paramQ = [asY.qPropagate([asY.components.z]).q];
% for k = 1 : comp
%     
%     leg1{k} = sprintf('%s ',num2str(gouy(k)));
%     leg2{k} = sprintf('%s ',num2str(beamW(k)));
%     leg3{k} = sprintf('%s ',num2str(paramQ(k)));
% end
% disp('Y Parameters')
% output = [topRow; lineBreak; [{asY.components.label}.',{asY.components.z}.',leg2.', leg1.']];
% display(output)



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 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); 

%as.seedq = beamq(-3.55 + i*1.09); % Nominal seed
%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;

% From Aidan
% The defocus values are the inverses of the RoC values and can be added linearly. 
% Therefore, if you have an initial lens of ROC = R_static and you have a final lens with RoC of R_final, you need to solve the following equation:
% 
% 1/R_final = 1/R_static + (dS/dP_RH)*P_RH
% 
% where P is the power in the ring heater. I tend not to bother putting things into RoC, but generally leave them as defocii (in diopters = m^-1), 
% so the above equation simply becomes:
% 
% S_final = S_static + (dS/dP_RH)*P_RH
% 
%                 
% 
% P_RH = (1/R_final-1/R_static)/alfa

% Ring heater power needed to match ITMX to ITMY

% R_final =
% 
%                 1907.47912
% 
% R_static
% 
% R_static =
% 
%                 1943.74584
% 
% alfa = 1.36E-5

%P_RH =

       %    0.7192338210469