%%
n=1.4496;
IM2_ROC=12.8; % FDR T0900386, also T0900407
w0_IMC=2.124e-3;% T0900407
zR_IMC=pi/1064e-9*w0_IMC^2;
zIMCOutWaist=0;
z_IMCOut=1j*zR_IMC; % Waist is 174.2mm downstream of MC3AR according to T0900407
zMC3AR= zIMCOutWaist-0.1742; 

dIMCOutWaist_IM2=1.5506;
zIM2=zIMCOutWaist+dIMCOutWaist_IM2;

dIM2_IM3=1.1703;
zIM3=zIM2+dIM2_IM3;

dIM3_IM4=1175.4e-3;
zIM4=zIM3+dIM3_IM4;

%%
IMC_IFO_path=beamPath();
IMC_IFO_path.seedWaist(w0_IMC, zIMCOutWaist)
IMC_IFO_path.addComponent(component.flatMirror(zMC3AR, 'MC3AR'))
IMC_IFO_path.addComponent(component.curvedMirror(IM2_ROC, zIM2, 'IM2'))
IMC_IFO_path.addComponent(component.curvedMirror(IM3_ROC, zIM3, 'IM3'))
IMC_IFO_path.addComponent(component.flatMirror(zIM4, 'IM4'))

%% according to D1200693
pico1= [-848.5 -180 -107.0]*1e-3;
pico2= [-709.5 561 -107.0]*1e-3;
dpico1_pico2 = norm(pico1-pico2); %this is 753.9, 

ROM_RH3= [-903.5 -180 -107.0]*1e-3;
ROM_LH1= [-603.5 -750 -107.0]*1e-3;
dLH1_RH3=norm(ROM_RH3-ROM_LH1)
dRH3_pico1=norm(pico1-ROM_RH3)

dLH1_IM4=5*25.4e-3; % this is just a very rough guesstimate from D0901083.

%% according to Hanford alog 12537 by MattH
w0=233.44e-6; %according to their "final" plot
f1=343.6e-3;
f2=-171.9e-3;
dL1_L2 = (-0.255-(-0.49));
dL2_PD=(0.4-(-0.255));
dpico2_PD=16*25.4e-3;
dL2_pico2=-dpico2_PD+dL2_PD;

%%
dpico1_L1=dpico1_pico2-dL2_pico2-dL1_L2;
dIM4_IM4T=0.358;
zIM4T=zIM4+dIM4_IM4T;
zpico1=zIM4+dLH1_IM4+dLH1_RH3+dRH3_pico1;
zL1=zpico1+dpico1_L1;
zL2=zL1+dL1_L2;
zpico2=zL2+dL2_pico2;
zPD=zpico2+dpico2_PD;
zIM4=zpico1 - dLH1_IM4-dLH1_RH3-dRH3_pico1;

ISS_Heintzepath=beamPath();
ISS_Heintzepath.addComponent(component.flatMirror(zpico1, 'pico1'));
ISS_Heintzepath.addComponent(component.lens(f1, zL1, 'L1'));
ISS_Heintzepath.addComponent(component.lens(f2, zL2, 'L2'));
ISS_Heintzepath.addComponent(component.flatMirror(zpico2, 'pico2'));
ISS_Heintzepath.addComponent(component.flatMirror(zPD, 'PD'));
ISS_Heintzepath.addComponent(component.flatMirror(zIM4, 'IM4'))
ISS_Heintzepath.seedWaist(w0, zPD)
figure
ISS_Heintzepath.plotSummary()
ISS_Heintzepath.qPropagate(zIM4)
ABCD_pico1_PD=ISS_Heintzepath.getTransferMatrix(zpico1, zPD)
ABCD_pico2_PD=ISS_Heintzepath.getTransferMatrix(zpico2, zPD)
gouy_pico1_pico2=ISS_Heintzepath.gouySeparation('pico1', 'pico2')


ISS_IMCpath=IMC_IFO_path.duplicate()
ISS_IMCpath.addComponent(component.flatMirror(zpico1, 'pico1'));
ISS_IMCpath.addComponent(component.lens(f1, zL1, 'L1'));
ISS_IMCpath.addComponent(component.lens(f2, zL2, 'L2'));
ISS_IMCpath.addComponent(component.flatMirror(zpico2, 'pico2'));
ISS_IMCpath.addComponent(component.flatMirror(zPD, 'PD'));
ISS_IMCpath.addComponent(component.flatMirror(zIM4T, '(IM4 TRANS)'))
figure
ISS_IMCpath.plotSummary()
ABCD_MC3_PD=ISS_IMCpath.getTransferMatrix(zMC3AR, zPD)
ABCD_IM3_IM4T=ISS_IMCpath.getTransferMatrix(zIM3, zIM4T)
ABCD_IM4T_PD=ISS_IMCpath.getTransferMatrix(zIM4T, zPD)
ABCD_MC3_IM3=ISS_IMCpath.getTransferMatrix(zMC3AR, zIM3)
gouy_pico1_pico2_ISS_IMCpath=ISS_IMCpath.gouySeparation('pico1', 'pico2')
qPD_IMCpath=ISS_IMCpath.qPropagate(zPD)