clear all
% REFL in-vacuum path beam propagation, HAM1 drawing v10
% https://dcc.ligo.org/LIGO-D1000313-v10
% LisaBar, August 14, 2013

q_in = 1.03+13.1i;  % Beam on HAM1 calculated from CalculatePRM.m 
                    % Lisa: we don't have a measurement yet which confirms
                    % this number! 

% ROC of the curved mirrors in REFL path
R = [1.7 -0.6 1.7];
% Nominal distances (as in drawing v10) 
L = [0.831 0.854];
% LLO current distances (as in drawing v9) 
%L = [0.805 0.988];
    
    
reflWFS = beamPath;
qin = beamq;
qin.q =  1.03+13.1i;
reflWFS.seedq = qin;
reflWFS.seedz = 0;


% Curved mirrors in REFL path
reflWFS.addComponent(...
        component.curvedMirror(R(1), 0+1e-6, 'RM1'));
reflWFS.addComponent(...
        component.curvedMirror(R(2), L(1),'RM2'));
reflWFS.addComponent(...
        component.curvedMirror(R(3), sum(L),'M5'));


% Offset can be used to check that sled can be moved around     
offset =0e-2;

% LLO current tip-tilts distances (as in drawing v9)      
%z_L101 = reflWFS.component('M5').z + 22.5*25.4*2*1e-3 + 4.5*25.4*1e-3 + offset;

% Current version 10
% RM1 --> RM2: 832 mm
% RM1 --> RM5: 855 mm
% M5 --> M6: 733 mm
% M6 --> L1: 63 mm
% L1 --> LSC REFL diode: 198 mm
% M6 to edge of the sled: 326 mm 

dM5_M6 = 733*1e-3;
dM6_sled = 326*1e-3;

% LLO REFL sled as built at LLO - G1300837
% REFL WFS sled

f1 = 333e-3;      % r=150mm fused silica lens
f2 = -167e-3;     % r=-75mm fused silica lens

z_L101 = reflWFS.component('M5').z + (dM5_M6 + dM6_sled) + 4.5*25.4*1e-3 + offset;
z_L202 = z_L101 + 203e-3;
z_WFSA = z_L202 + (140+74+59+130)*1e-3 ;
z_WFSB = z_WFSA + 355*1e-3 ;
    
reflLSC = reflWFS.duplicate;
         
reflWFS.addComponent(...
        component.lens(f1, z_L101, 'L101'));
reflWFS.addComponent(...
        component.lens(f2, z_L202, 'L102'));
        
reflWFS.addComponent(...
        component.flatMirror(z_WFSA, 'ASC-REFL\_A'));
reflWFS.addComponent(...
        component.flatMirror(z_WFSB , 'ASC-REFL\_B'));
      
     
% LSC path

% M6 position
z_BS_LSC = sum(L) + dM5_M6*1e-3;
    
reflLSC.addComponent(...
        component.flatMirror(z_BS_LSC, 'M6'));

% L1 position
z_L1_LSC = z_BS_LSC + 63*1e-3;
    
reflLSC.addComponent(...
        component.lens(222.4e-3, z_L1_LSC, 'L1'));

% LSC REFL diode
z_REFL_Diode = z_L1_LSC + 198*1e-3;
    
     reflLSC.addComponent(...
        component.flatMirror(z_REFL_Diode, 'LSC-REFL\_A'));


comp = length(reflLSC.components);    

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

gouyLSC = reflLSC.gouyPhase([reflLSC.components.z]);
beamWLSC = 1e3.*[reflLSC.qPropagate([reflLSC.components.z]).beamWidth];

for k = 1 : comp
    
    leg1{k} = sprintf('%s ',num2str(gouyLSC(k)));
    leg2{k} = sprintf('%s ',num2str(beamWLSC(k)));
    
end
    
outputLSC = [topRow; lineBreak; [{reflLSC.components.label}.',{reflLSC.components.z}.',leg2.', leg1.']];
    
display(outputLSC) 
    
comp = length(reflWFS.components); 

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

gouyASC = reflWFS.gouyPhase([reflWFS.components.z]);
beamWASC = 1e3.*[reflWFS.qPropagate([reflWFS.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; [{reflWFS.components.label}.',{reflWFS.components.z}.',leg3.', leg4.']];
    
display(outputASC) 
   
% %Plot REFL paths
% 
% figure(1)
% zz = -.5:1e-2:5;
% reflWFS.plotSummary(zz)
% shg
% display(reflWFS.components);
% %saveas(1, 'REFL/REFL_WFS_beampath.pdf', 'pdf');
% %saveas(1, 'REFL/REFL_WFS_beampath.fig', 'fig');
% 
% figure(2)
% 
% zz = -.5:1e-2:2.5;
% reflLSC.plotSummary(zz)
% shg
% display(reflLSC.components);
% %saveas(1, 'REFL/REFL_LSC_beampath.pdf', 'pdf');
% %saveas(1, 'REFL/REFL_LSC_beampath.fig', 'fig');

% POSITIONS of OPTICS and DIODES

% outputLSC = 
% 
%     'label'          'z (m)'         'BeamWidth (mm)'    'Gouy Phase (dg)'
%     '-----'          '-----'         '----'              '----'           
%     'RM1'            [1.0000e-06]    '2.1129 '           '4.3469e-06 '    
%     'RM2'            [    0.8310]    '0.14517 '          '66.5778 '       
%     'M5'             [    1.6850]    '1.9985 '           '161.041 '       
%     'M6'             [    1.6857]    '1.9985 '           '161.0445 '      
%     'L1'             [    1.7487]    '1.9987 '           '161.3506 '      
%     'LSC-REFL\_A'    [    1.9467]    '0.22244 '          '170.026 '       
% 
% outputASC
% 
% outputASC = 
% 
%     'label'          'z (m)'         'BeamWidth (mm)'    'Gouy Phase (dg)'
%     '-----'          '-----'         '----'              '----'           
%     'RM1'            [1.0000e-06]    '2.1129 '           '4.3469e-06 '    
%     'RM2'            [    0.8310]    '0.14517 '          '66.5778 '       
%     'M5'             [    1.6850]    '1.9985 '           '161.041 '       
%     'L101'           [    2.8583]    '2.0109 '           '166.7156 '      
%     'L102'           [    3.0613]    '0.78964 '          '169.1971 '      
%     'ASC-REFL\_A'    [    3.4643]    '0.32428 '          '201.4068 '      
%     'ASC-REFL\_B'    [    3.8193]    '0.37138 '          '288.1028 '      


% add our measurements from LHO
%measurement before TT
%mode master was at 40+sqrt(16^2+2^2) inches after M2
M2toModeMaster=(40+sqrt(16^2+2^2))*25.4e-3;
distM2toRM1=(26*2+1)*25.4e-3; %from counting holes on D1000313
RM1toModeMaster=M2toModeMaster-distM2toRM1;


%from Kieta's alog, as built distance
RM2distM=sqrt(32.25^2+1)*25.4e-3;
M5distM=RM2distM+sqrt(33.5^2+6.5^2)*25.4e-3;
L101distM=M5distM+(35+12)*25.4e-3;
L102distM=L101distM+0.1985; % T1200555
WFSAdist=L102distM+0.135+0.047+0.048+6*25.4e-3;
WFSBdist=L102distM+0.135+0.047+0.048+0.368+6*25.4e-3;
MeasXpos=RM1toModeMaster-2.387;
MeasYpos=RM1toModeMaster-3.578;
MeasXWo=3.588e-3/2;
MeasYWo=3.532e-3/2;

%for Aftter TT measurement, mode master was 66 and 3/8 " from M5
ModeMasterAfterPos=M5distM+(66+3/8)*25.4e-3;
MeasXAWo=1.38e-3;
MeasYAWo=1.15e-3;
MeasXApos=ModeMasterAfterPos-4.31;
MeasYApos=ModeMasterAfterPos-4.35;

measX=reflWFS.duplicate;
measY=reflWFS.duplicate;
measX.seedWaist(MeasXWo, MeasXpos);
measY.seedWaist(MeasYWo, MeasYpos);

Paths1=[measX, measY];
for ii=1:length(Paths1)
    Paths1(ii).moveComponent('RM2',RM2distM,'absolute');
    Paths1(ii).moveComponent('M5',M5distM,'absolute');
    Paths1(ii).moveComponent('L101', L101distM, 'absolute');
    Paths1(ii).moveComponent('L102', L102distM, 'absolute');
    Paths1(ii).moveComponent('ASC-REFL\_A' , WFSAdist, 'absolute');
    Paths1(ii).moveComponent('ASC-REFL\_B', WFSBdist, 'absolute');
end

%from our measurements before the TTS, what would we expect to measure
%after?
expX=measX.qPropagate(2);
expY=measY.qPropagate(2);


% make beam path starting with measurement after TTs
afterTT_X=measX.duplicate;
afterTT_Y=measY.duplicate;
%afterTT_X=beamPath;

%need to get seed right, the measurement was made around 2 m, set q to
%equal q there
dummy_X=beamPath;
dummy_Y=beamPath;
dummy_X.seedWaist(MeasXAWo, MeasXApos);
dummy_Y.seedWaist(MeasYAWo, MeasYApos);
dummyQ_X=dummy_X.qPropagate(2).q;
dummyQ_Y=dummy_Y.qPropagate(2).q;
afterTT_X.seedz=2;
afterTT_Y.seedz=2;
afterTT_X.seedq.q=dummyQ_X;
afterTT_Y.seedq.q=dummyQ_Y;

%overlap between before TT and after TT measurements
measX.targetz=2;
measY.targetz=2;
measX.targetq.q=dummyQ_X;
measY.targetq.q=dummyQ_Y;

OverlapX=measX.targetOverlap;
OverlapY=measY.targetOverlap;
%check luke said 27.7 inches from edge of HAM1 to Edge of HAM2
% Check overlap with what paul expects. 
HAM2HAM1=(-20122.0+22692.0)*1e-3; %center to center distance -24 inches from center of HAM1 to M1+22inches from M1 to M2
%to see what happens if we move tts, we need to move seedz for measurement
RM1toHAM2PO=HAM2HAM1+(22-24)*25.4e-3+distM2toRM1-18*25.4e-3;

PaulsBeamX=measX.duplicate;
PaulsBeamY=measX.duplicate;
PaulsBeamX.seedWaist(2.123e-3, -2.469);
PaulsBeamY.seedWaist(2.123e-3, -2.150);

% PathsX=[measX];%, afterTT_X];
% PathsY=[measY];%, afterTT_Y];
% 
% for ii=1:length(PathsX)
%     PathsNewX(ii)=beamPath;
%     PathsNewY(ii)=beamPath;
%     PathsNewX(ii).seedWaist(MeasXWo, MeasXpos);
%     PathsNewY(ii).seedWaist(MeasYWo, MeasYpos);
% %     PathsNewX(ii).seedq.q=PathsX(ii).qPropagate(-1*RM1toHAM2PO).q;
% %     PathsNewY(ii).seedq.q=PathsY(ii).qPropagate(-1*RM1toHAM2PO).q;
% %     PathsNewX(ii).targetz=-1*RM1toHAM2PO;
% %     PathsNewY(ii).targetz=-1*RM1toHAM2PO;
% %     PathsNewX(ii).targetq.q=-0.5479+13.336*i;
% %     PathsNewY(ii).targetq.q=-0.8969+13.0079*i;
%         PathsNewX(ii).targetWaist(2.1e-3, -1*RM1toHAM2PO-0.23);
%     PathsNewY(ii).targetWaist(2.1e-3, -1*RM1toHAM2PO-0.23);
% end
% 
% Paths=[measX, measY, afterTT_X, afterTT_Y];
% PathsNew=[PathsNewX, PathsNewY];
% 
% for ii=1:length(PathsNew)
%     OverlapsWExp(ii)=PathsNew(ii).targetOverlap();
% end

% %made after TTs to before TTs
% dummyQ2_X=afterTT_X.qPropagate(-1);
% dummyQ2_Y=afterTT_Y.qPropagate(-1);
% afterTT_X.seedq=dummyQ2_X;
% afterTT_Y.seedq=dummyQ2_Y;
% afterTT_X.seedz=-1;
% afterTT_Y.seedz=-1;

% afterTT_X.deleteComponent('RM1');
% afterTT_X.deleteComponent('RM2');
% afterTT_X.deleteComponent('M5');
% afterTT_Y.deleteComponent('RM1');
% afterTT_Y.deleteComponent('RM2');
% afterTT_Y.deleteComponent('M5');



% %gouy phase separation of WFSs based on measurement before TTs
% Gouy_WFS_X=measX.gouySeperation('ASC-REFL\_A', 'ASC-REFL\_B');
% Gouy_WFS_Y=measY.gouySeperation('ASC-REFL\_A', 'ASC-REFL\_B');
% Gouy_TT_X=measX.gouySeperation('RM1', 'RM2');
% Gouy_TT_Y=measY.gouySeperation('RM1', 'RM2');
% 
% %gouy phase separation of WFSs based on measurement after TTs
% Gouy_WFS_X_After=afterTT_X.gouySeperation('ASC-REFL\_A', 'ASC-REFL\_B');
% Gouy_WFS_Y_After=afterTT_Y.gouySeperation('ASC-REFL\_A', 'ASC-REFL\_B');
% Gouy_TT_X_After=afterTT_X.gouySeperation('RM1', 'RM2');
% Gouy_TT_Y_After=afterTT_Y.gouySeperation('RM1', 'RM2');
% 
% %gouy phase separation based on Pauls Beam
% P_Gouy_WFS_X=PaulsBeamX.gouySeperation('ASC-REFL\_A', 'ASC-REFL\_B');
% P_Gouy_WFS_Y=PaulsBeamY.gouySeperation('ASC-REFL\_A', 'ASC-REFL\_B');
% P_Gouy_TT_X=PaulsBeamX.gouySeperation('RM1', 'RM2')
% P_Gouy_TT_Y=PaulsBeamY.gouySeperation('RM1', 'RM2')

% figure(1)
% zz = -3:1e-2:4;
% measX.plotSummary(zz)
% shg
% 
% figure(2)
% measY.plotSummary(zz)
% shg
% %display(measY.components);
% 
% figure(3)
% PaulsBeamX.plotSummary(zz)
% shg
% 
% figure(4)
% PaulsBeamY.plotSummary(zz)
% shg

Paths=[PaulsBeamX, PaulsBeamY, measX, measY];
for ii=1:length(Paths)
    Gouy_Separation_TT_B4(ii)=Paths(ii).gouySeperation('RM1', 'RM2');
    Gouy_Separation_WFS_B4(ii)=Paths(ii).gouySeperation('ASC-REFL\_A', 'ASC-REFL\_B');
end

% try moving TTs

delta=1.6; %inches along table to move RM2 towards HAM2 (keeping RM1 and RM2 positions the same
delta2=1; %distance to move RM2 towards PSl 9 inches max (8)
changeRM2=(sqrt((32.25+delta)^2+1)-sqrt(32.25^2+1))*25.4e-3;
changeM5=(sqrt((33.5+delta+delta2)^2+6.5^2)*25.4e-3-sqrt(33.5^2+6.5^2)*25.4e-3)+changeRM2;
sledMove=delta2*25.4e-3; %+1 inch, -2 inches max

for ii=1:length(Paths)
    Paths(ii).moveComponent('RM2', changeRM2);
    Paths(ii).moveComponent('M5', changeM5);
    Paths(ii).moveComponent('L101', changeM5+sledMove);
    Paths(ii).moveComponent('L102', changeM5+sledMove);
    Paths(ii).moveComponent('ASC-REFL\_A', changeM5+sledMove);
    Paths(ii).moveComponent('ASC-REFL\_B', changeM5+sledMove);
end


for ii=1:length(Paths)
    Gouy_Separation_TT(ii)=Paths(ii).gouySeperation('RM1', 'RM2');
    Gouy_Separation_WFS(ii)=Paths(ii).gouySeperation('ASC-REFL\_A', 'ASC-REFL\_B');
end

figure(11)
zz = 2:1e-2:5;
PaulsBeamX.plotSummary(zz)
shg

figure(12)
PaulsBeamY.plotSummary(zz)
shg

% figure(1)
% zz = 1:1e-2:4;
% measX.plotSummary(zz)
% shg
% %display(measX.components);
% figure(11)
% afterTT_X.plotSummary(zz)
% 
% figure(2)
% measY.plotSummary(zz)
% shg
% %display(measY.components);
% 
% figure(12)
% afterTT_Y.plotSummary(zz)
% 
% figure(3)
% reflWFS.plotSummary(zz)
% shg
% %display(reflWFS.components);
% 
% 
% figure(1)
% zz = -3:1e-2:4;
% PathsNew(1).plotSummary(zz)
% shg