% zero position is the IMC waist between MC3 and IM1.
zMC3AR= -0.1742; 
% zMC3AR: Waist is 174.2mm downstream of MC3AR according to T0900407
dIMCOutWaist_IM2=1.5506; % T0900407
zIM2=dIMCOutWaist_IM2; % because I set zIMCOutWaist as zero.

% next use T1700227 to figure out RH9 position.
dIM2_RH9= 775e-3;
zRH9= zIM2-dIM2_RH9;% RH9 is the IFO refl pickoff mirror
dRH9_RM1=(1194+221.5+600+1369)*1e-3; % T1700227
zRM1=zRH9+dRH9_RM1;
zRM2=zRM1+838e-3;% T1700227
zM5=zRM2+910e-3;% T1700227
rRM1=1.7;%https://dcc.ligo.org/LIGO-T1000247
rRM2=-0.6;%https://dcc.ligo.org/LIGO-T1000247
rM5=1.7;% we don't use this but this is also from https://dcc.ligo.org/LIGO-T1000247
zL1=zM5+1220e-3;
fL1=333.6e-3;
zL2=zL1+203e-3;
fL2=-166.8e-3;
zREFLB=zL2+230e-3+119e-3;
zREFLA=zL2+230e-3+368e-3;
REFLpath=beamPath();
REFLpath.addComponent(component.flatMirror(zRH9, 'RH9'));
REFLpath.addComponent(component.curvedMirror(rRM1, zRM1, 'RM1'));
REFLpath.addComponent(component.curvedMirror(rRM2, zRM2, 'RM2'));
REFLpath.addComponent(component.curvedMirror(rM5, zM5, 'M5'));
REFLpath.addComponent(component.lens(fL1, zL1, 'L101'));
REFLpath.addComponent(component.lens(fL2, zL2, 'L102'));
REFLpath.addComponent(component.flatMirror(zREFLB, 'REFLB'))
REFLpath.addComponent(component.flatMirror(zREFLA, 'REFLA'))

M_RM1_RM2=REFLpath.getTransferMatrix(zRM1-1e-6, zRM2+1e-6);
M_RM2_RM2=REFLpath.getTransferMatrix(zRM2-1e-6, zRM2+1e-6);
M_RM1_REFLA=REFLpath.getTransferMatrix(zRM1-1e-6, zREFLA+1e-6);
M_RM1_REFLB=REFLpath.getTransferMatrix(zRM1-1e-6, zREFLB+1e-6);
M_RM2_REFLA=REFLpath.getTransferMatrix(zRM2-1e-6, zREFLA+1e-6);
M_RM2_REFLB=REFLpath.getTransferMatrix(zRM2-1e-6, zREFLB+1e-6);

M_RH9_RM1=REFLpath.getTransferMatrix(zRH9, zRM1+1e-6);
M_RH9_RM2=REFLpath.getTransferMatrix(zRH9, zRM2+1e-6);
M_RH9_M5=REFLpath.getTransferMatrix(zRH9, zM5+1e-6);
% M_RM2_M5=REFLpath.getTransferMatrix(zRM2, zM5);

%% What's the combination of RM1 and RM2 YAW rotation that is the 
% equivalent of 0.5mrad YAW rotation of RH9?
RH9injection_theta=0.5e-3; % Rotate RH9 by 0.5 mrad (beam rotation 1mrad)
outRM2_theta=M_RH9_RM2*[0;2*RH9injection_theta];
M=[M_RM1_RM2(1, 2), M_RM2_RM2(1, 2); M_RM1_RM2(2, 2), M_RM2_RM2(2, 2)];
RM1_RM2_inj_theta=(M\outRM2_theta)/2
% = [0.53mrad, -2.0mrad] this is the combination of rotation of RM1 and RM2 
% that would cause the same effect as 1mrad rotation of RH9.
% Note that each reflection switches the "polarity" of mirror rotation in
% YAW. M1 (the mirror that comes directly after RH9) has the opposite 
% polalrity to RH9, next one (M2) has the same polarity, the next one (RM1)
% the opposite and the next one (RM2) the same polarity as RH9.
%
% That is, positive rotation of 1mrad of RH9 is equivalent of 
% -1*0.53mrad rotation of RM1 and
% +1*(-2.0mrad) rotation of RM2.

outRM1_theta=M_RH9_RM1*[0;2*RH9injection_theta];
outM5_theta=M_RH9_M5*[0;2*RH9injection_theta];
xRM1=outRM1_theta(1,1)
xRM2=outRM2_theta(1,1)
xM5=outM5_theta(1,1)
thetaM5=outM5_theta(2,1)

%% What is the combination of RM1 and RM2 YAW rotation that is the equivalent
% of negative 0.5mrad rotation of IM1?
IM1injection_theta=-0.5e-3;
%from T1700227,
%distance(IM1-to-cpl)-distance(RM9-to-cpl)~sqrt(10^2+4^2)*25.4mm
d=sqrt(10^2+4^2)*25.4e-3;
invec_RH9_IM1inj=[d;-1]*2*IM1injection_theta;% note the sign
outvec_IM1inj=M_RH9_RM2*invec_RH9_IM1inj;
M=[M_RM1_RM2(1, 2), M_RM2_RM2(1, 2); M_RM1_RM2(2, 2), M_RM2_RM2(2, 2)];
RM1_RM2_IM1inj=(M\outvec_IM1inj)/2
% = [ 0.5262; -1.8562] mrad.
% Thinking about polarity of things again,
% negative rotation of 0.5mrad of IM1 is equivalent of 
% -1*(0.53)mrad rotation of RM1 and
% +1*(-1.9mrad) rotation of RM2.
