close all
clear classes
addpath /Users/izumikiwamu/Documents/MATLAB/alm;

mm = 1e-3;%m
inch = 25.4*mm;

% Refractive Indices
n_fs_g = 1.4615;
% n_fs_ir = 1.4493;
n_bk7_g = 1.50663;      %check

lambda = 532e-9;    %wavelength [m]

%%% Set the beamsplitter where the doubled PSL and green X trans beam are
%%% combined, i.e. ISCT1-ALS-BS3, as the zero position.
z_isct1bs3 = 0;

%put a 1 if for HIFO, 0(or whatever) for aLIGO
hifo_solution = 1;


%% GREEN ARM TRANS (from HAM1 septum) to BS3 mode

% create beam path object
XARMtoISCT1path = beamPath;

%HAM1 Septum -> note septum is 17.7064m from curved mirror on HAM3
z_septum = z_isct1bs3 - 5.1347;
XARMtoISCT1path.addComponent(component.flatMirror(z_septum,'HAM1-Septum'));

%%% Seed Waist
% q parameter of ALS beams at HAM1 septum (see T1200470)
if hifo_solution == 1
    q_septum = 10.0283+4.6666*i;    % HIFO
else
    q_septum = 6.3446+9.598*i;   % aLIGO
end
% q_septum = 8.434+4.252*i;   % aLIGO bad
w0_septum = sqrt(lambda*imag(q_septum)/pi);     %waist radius
z0_septum = -real(q_septum);                    %waist position, not rayleigh
XARMtoISCT1path.seedWaist(w0_septum,z_septum+z0_septum, lambda);

%HAM1-M8 
z_ham1m8 = z_septum + 640*mm;       %Alberto T1200471
XARMtoISCT1path.addComponent(component.flatMirror(z_ham1m8,'HAM1-M8'));

%HAM1-M9
z_ham1m9 = z_ham1m8 + 313.6*mm;       %Alberto T1200471
XARMtoISCT1path.addComponent(component.flatMirror(z_ham1m9,'HAM1-M9'));

%HAM1-M11
z_ham1m11 = z_ham1m9 + 29*inch;     %D1000313 (T1200471 doc mistake)
XARMtoISCT1path.addComponent(component.flatMirror(z_ham1m11,'HAM1-M11'));

%ISCT1-ALS-M9
z_isct1m9 = z_ham1m11 + 11*inch + 1500*mm;  %D1000313 (T1200471 doc mistake)
XARMtoISCT1path.addComponent(component.flatMirror(z_isct1m9,'ISCT1-ALS-M9'));

%ISCT1-ALS-M8
z_isct1m8 = z_isct1m9 + 611*mm;
XARMtoISCT1path.addComponent(component.flatMirror(z_isct1m8,'ISCT1-ALS-M8'));

%ISCT1-ALS-PBS2
z_isct1pbs2 = z_isct1m8 + 13.5*inch;        %D1201103
XARMtoISCT1path.addComponent(component.flatMirror(z_isct1pbs2,'ISCT1-ALS-PBS2'));

%ISCT1-ALS-BS4
z_isct1bs4 = z_isct1pbs2+13*inch;
XARMtoISCT1path.addComponent(component.flatMirror(z_isct1bs4,'ISCT1-ALS-BS4'));

%ISCT1-ALS-L7 and L6 added by KI
if hifo_solution == 1
    
    %ISCT1-ALS-L7 and L6
    ROC_L7 = 100 * mm;
    ROC_L6 = -75 * mm;
    f_L6 = ROC_L6/(n_fs_g-1);
    f_L7 = ROC_L7/(n_fs_g-1);
    
    
    z_isct1l7 = z_isct1bs4 + 3*inch;          %D1201103
    extra = 0.0025;
    z_isct1l6 = z_isct1l7 + (f_L7 + f_L6) + extra;      % extra is a magic number
    
    XARMtoISCT1path.addComponent(component.lens(f_L7,z_isct1l7,'ISCT1-ALS-L7')); 
    XARMtoISCT1path.addComponent(component.lens(f_L6,z_isct1l6,'ISCT1-ALS-L6'));

end

%ISCT1-ALS-BS3
XARMtoISCT1path.addComponent(component.flatMirror(z_isct1bs3,'ISCT1-ALS-BS3'));

% The beam size and RoC at this point edited by KI
w_bs3 = XARMtoISCT1path.qPropagate(z_isct1bs3).beamWidth;
ROC_bs3 = XARMtoISCT1path.qPropagate(z_isct1bs3).radiusOfCurvature;


%% SHG to BS3 mode

% create beam path object
SHGtoALSCPDpath = beamPath;

%%% ISCT1-ALS-SHG:
z_shg = z_isct1bs3 - 55*inch;

% Waist of output mode of SHG (PPKTP) crystal
l_ktp = 10*mm;      %length of crystal 
n_ktp = 1.889;      %refractive index of ktp at 532nm
z0_shg = l_ktp/(5.68*n_ktp);        %rayleigh range of input mode
w0_shg = sqrt(z0_shg*lambda/pi);    %waist size of input mode
SHGtoALSCPDpath.seedWaist(w0_shg,z_shg, lambda);

%%% ISCT1-ALS-D1
z_isct1d1 = z_shg + 6*inch;
SHGtoALSCPDpath.addComponent(component.flatMirror(z_isct1d1,'ALS-D1'));

%%% ISCT1-ALS-L3 -> 
z_isct1l3 = -1.2344;    % to make the beam well-colimated
ROC_L3 = 75*mm; % edited by KI
f_L3 = ROC_L3/(n_fs_g-1);
SHGtoALSCPDpath.addComponent(component.lens(f_L3, z_isct1l3,'ALS-L3'));

%%% ISCT1-ALS-BS2
z_isct1bs2 = z_isct1d1+6*inch;
SHGtoALSCPDpath.addComponent(component.flatMirror(z_isct1bs2,'ALS-BS2'));

%%% ISCT1-ALS-HWP2
z_isct1hwp2 = z_isct1bs2 + 5*inch;
SHGtoALSCPDpath.addComponent(component.flatMirror(z_isct1hwp2,'ALS-HWP2'));

%%% ISCT1-ALS-PBS1
z_isct1pbs1 = z_isct1hwp2 + 4*inch;
SHGtoALSCPDpath.addComponent(component.flatMirror(z_isct1pbs1,'ALS-PBS1'));


%ISCT1-ALS-BS3
SHGtoALSCPDpath.addComponent(component.flatMirror(z_isct1bs3,'ALS-BS3'));


%% Calculation of Target Waist 
SHGtoALSCPDpath.targetWaistR(w_bs3, ROC_bs3, z_isct1bs3, lambda);
SHGtoALSCPDpath = SHGtoALSCPDpath.optimizePath('ALS-L3',[-1.4 -1.0]);
Overlap = SHGtoALSCPDpath.targetOverlap


%% Plotting
zdomain = linspace(-2, 10*inch,1000);  

figure(101)
    subplot(1,1,1)
    hold on
        xplot = XARMtoISCT1path.plotBeamWidth(zdomain,'b', 'LineWidth', 2);
        yplot = SHGtoALSCPDpath.plotBeamWidth(zdomain,'r', 'LineWidth', 2, 'LineStyle', '--');
        
        XARMtoISCT1path.plotComponents(zdomain,0,'b*');
        SHGtoALSCPDpath.plotComponents(zdomain,0,'r*');
    axis tight
    ylabel('Beam Width [m]')
    xlabel('Propagation distance [m]')
    grid on
    hold off