function LCGT_LSC()
% Construct and simulate length sensing and control of LCGT
%
% == Usage ==
%
% Before running this function, review and edit the following files.
%
% (1) paramLCGT.m
%
% This function returns a structure p, which contains all the parameters
% necessary for the LSC simulation.
% Read the comments at the beginning of the file for details.
% Usually you only have to edit this file and files included herein.
%
% (2) modelLCGT.m
%
% This function constructs an Optickle model of LCGT taking the parameter
% structure p as an argument.
% If you want to modify the optical configuration of LCGT, edit this file.
%
% (3) determineDemodPhase.m
%
% This function is used to optimize the demodulation phases for various
% probes. It also generates vector sensing matrix for each probe.
% Edit this file if you want to change how the demodulation phases are
% optimized.
%

%{{{ ==== Initialization ====

%{{{ Path setup

%% Path setup
clear all

baseDir = pwd;
addpath(genpath([baseDir,'/Optickle']));
%addpath(genpath([baseDir,'/Optickle2']));
addpath(genpath([baseDir,'/tools']));

%}}}

%{{{ Read parameters

%% Read parameters
p=paramLCGT();

%}}}

%{{{ Flags

%% Flags

%Whether to generate auxiliary plots
osaplots = p.osaplots;
sweepplots = p.sweepplots;

%}}}

%{{{ Units constants

%% Units constants
nm=1e-9;
pm=1e-12;
MHz=1e6;
ppm=1e-6;

c = 299792458;
hp = 6.626068e-34;

%}}}

%{{{ Close all figures

%% Close all figures
close all

%}}}

%}}}

%{{{ ==== Create result directory ====

%% Create result directory
if p.DRSE
    resultDir = 'DRSE';
else
    resultDir = 'BRSE';
end
%resultDir = [baseDir,'/results/',p.Name,'_',num2str(round(rem(now*86400,100000))),'/'];
resultDir = [baseDir,'/results/',p.Name,'/'];
if p.useMZI
    if p.MidOffset == 0 
        resultDir = [baseDir,'/results/MZI_new/',p.Name,'_',num2str(p.Phi1),'_',num2str(p.Phi3),'_130%f2/'];
    else
        if p.MZIM2
            resultDir = [baseDir,'/results/MZI_new/',p.Name,'_',num2str(p.Phi1),'_',num2str(p.Phi3),'_', num2str(p.MidOffset),'_Pattern4prime/'];
        else
            resultDir = [baseDir,'/results/MZI_new/',p.Name,'_',num2str(p.Phi1),'_',num2str(p.Phi3),'_', num2str(p.MidOffset),'_test/'];
        end
    end
end

if p.RAM
    resultDir = [baseDir,'/results/BRSE_AM/',num2str(p.g1AM),'_PMRAMIndexModified_SecondOrder/'];
    if p.ParaRAM
        resultDir = [baseDir,'/results/BRSE_AM_Parallel/',num2str(p.g1AM),'_f2test/'];
    end
    
end
%}}}


%{{{ Load saved results
if 0

%{{{ Saved directory

savedDir = '/home/aso/exp/LCGT/MIF/ISC/CoreModel/IFOmodel/results/17_45/BRSE_17_45_SDM_BAE_R1_56212/';

%savedDir = '/home/aso/exp/LCGT/MIF/ISC/CoreModel/IFOmodel/results/17_45/DRSE_17_45_SDM_BAE_R2/';

%resultDir = savedDir;

%}}}

%{{{ Load saved parameters
load([savedDir, 'Parameters.mat']);
%}}}

%{{{ Load the saved tickle results
load([savedDir, 'TickleResults.mat']);
%load([savedDir, 'TickleResultsSCL.mat']);
%}}}

%{{{ Load the saved fixed tickle results
%load([savedDir, 'TickleResultsFixed.mat']);
%}}}

%{{{ Rename fixed results to normal results

% DC = fDCf;
% sigDC = sigDCf;
% sigAC = sigACf;
% mMech = mMechf;
% noiseAC = noiseACf;
% noiseMech = noiseMechf;

%}}}

end
%}}}

%{{{ Disable SCL injection

p.do_SCL = false;

%Scattered light amplitudes
p.EsETMX = 0;
p.EsPR2 = 0;
p.EsPRMAR = 0;
p.EsSR2 = 0;
p.EsSRMAR = 0;
p.EsITMtoBS = 0;
p.EsITMAR = 0;
p.EsREFL = 0;

%}}}

%{{{ ===== Adjust IFO parameters =====

%{{{ Construct an Optickle model


%% Construct an Optickle model
[opt,n,l,pr,dr] = modelLCGT(p);

% == Extract some useful model information ==
drvNames=getDriveNames(opt);
Ndrive=opt.Ndrive;
Nlink=opt.Nlink;
lambda=opt.lambda;
c=opt.c;

%}}}
%% ********* Mirror Sweeps *********

%{{{ mkdir

mkdir([resultDir,'Sweep']);

%% SRCL Sweep

fprintf('p.f3EOM %d \n', p.f3EOM);

if sweepplots
    Nstep = 500;  %increased # for more presice measurement of p.dTune
    range=20*nm;
    dl=linspace(-range,range,Nstep);
    pos=zeros(Ndrive, Nstep);
    pos(getDriveNumbers(opt,'SRM'),:) = dl;
    [fDC, sigDC]=sweep(opt,pos);

    h1=figure(1);
    plot(dl, squeeze(sigDC(pr.REFL_1DmI,:)),dl, squeeze(sigDC(pr.REFL_1DpI,:)),dl, squeeze(sigDC(pr.REFL_1I,:))...
        ,dl, squeeze(sigDC(pr.POP_1I,:)));
    
        
    xlabel('Displacement [m]','FontSize',16);
    ylabel('Signal [W]','FontSize',16);
    title('SRCL Sweep','FontSize',16);
    grid on
    YLim = get(gca, 'YLim');
    hold on
    plot([10*nm,10*nm],YLim,'color','r');
    legend('REFL_1DmI','REFL_1DpI','REFL_1I','POP_1I','Location','Best');
    print(h1, '-dpdf', [resultDir,'Sweep/', 'SweepSRCL.pdf']);
    saveas(h1, [resultDir,'Sweep/', 'SweepSRCL.fig']);
    
    if p.useMZI
        % calculate detune displacement corresponding to phase difference b/w EOMs
        
        [M, I] = min(abs(sigDC(pr.POP_1I,:)))
        p.tmp_dTune = dl(I);
        p.tmp_detunePhase = 90-(p.tmp_dTune*360/lambda);
        
        
        %{
        p.dTune = p.tmp_dTune;
        p.detunePhase = p.tmp_detunePhase;
        p.posOffsetSRM = p.posOffsetSRM+ p.dTune;
        %}
        fprintf('Phase difference b/w f1 [degree]: %e \n', p.Phi1);
        fprintf('Displacement of SRM [m]: %e \n', p.dTune);
        fprintf('Detune Phase [degree]: %e \n', p.detunePhase);
        %{
        % reconstruct the model
        if p.Phi1 ~= 0
            p.DRSE = 1;
        end
        %}
        
        %[opt,n,l,pr,dr] = modelLCGT(p);
        fprintf('p.DRSE : %d \n', p.DRSE);
        fprintf('p.posOffsetSRM : %d \n', p.posOffsetSRM);
    end
    
    close(h1);
    
    %Save data
    M = [dl(:),squeeze(sigDC(pr.POP_1I,:))'];
    dlmwrite([resultDir, 'Sweep/SweepSRCL.dat'], M)
    
    
end

%{{{ *********** Check DC fields ***************

%{{{ Sweep

%% Sweep
pos=zeros(Ndrive,1);
[fDC, sigDC]=sweep(opt,pos);

%}}}

%{{{ Scale the input laser power to make the carrier power at the BS constant

%% Scale the input laser power to make the carrier power at the BS constant

%Carrier power at the BS
P_BS = abs(fDC(l.PR3toBS,find(p.vMod==0)))^2;

%Scale the input laser power
p.Pin = p.Pin * p.Pbs/P_BS;

%Reconstruct the Optickle Model
[opt,n,l,pr,dr] = modelLCGT(p);

%% Check power at each field, each probe
%{
[fDC, sigDC] = tickle(opt, [], []);

fprintf('DC fields (fDC matrix):\n');
showfDC(opt, fDC);
fprintf('|nProbes (sigDC matrix):\n');
showsigDC(opt, sigDC);
%}
%{
fprintf('p.DRSE : %d \n', p.DRSE);
Text1 = 'sigDC at MZI Dark DC'
fprintf('sigDC at MZI Dark DC: %d \n',sigDC(pr.MZIDARKDC));
%}
%% Check power at each field, each probe

[fDC, sigDC] = tickle(opt, [], []);
fprintf('Source power: %d \n', p.Pin);
if p.useMZI
    fprintf('sigDC at MZI Dark DC: %d \n',sigDC(pr.MZIDARKDC));
end

% Power at detection ports
%P_REFL1 = abs(fDC(l.REFL1,:)*fDC(l.REFL1,:)');


%Recalculate the power
[fDC, sigDC]=sweep(opt,pos);
P_BS = abs(fDC(l.PR3toBS,find(p.vMod==0)))^2;

%Arm power
Parm = abs(fDC(l.ITMXtoETMX,find(p.vMod==0)))^2;

%}}}

%{{{ Adjust Homodyne Phase

%% Adjust Homodyne Phase

%{{{ Measure the AS power by arm mismatch

% No offset
opt = setPosOffset(opt, n.ETMX, 0); 
opt = setPosOffset(opt, n.ETMY, 0);
pos=zeros(Ndrive,1);
[fDC, sigDC]=sweep(opt,pos);
% Carrier power leaking out to the AS port by reflectivity mismatch
Pmismatch = abs(fDC(l.OMCTR, find(p.vMod==0))).^2;

%}}}

%{{{ Measure the AS power with arm offset

% Add offset
testArmOffset = p.armOffsetPolarity*0.5*pm;
opt = setPosOffset(opt, n.ETMX, testArmOffset); 
opt = setPosOffset(opt, n.ETMY, -testArmOffset);

pos=zeros(Ndrive,1);
[fDC, sigDC]=sweep(opt,pos);

% Carrier power leaking out to the AS by the DARM offset
Pdc = abs(fDC(l.OMCTR, find(p.vMod==0))).^2 - Pmismatch;

%}}}

%{{{ Adjust the arm offset

% Necessary Pdc to achieve the target HD phase
PdcTarget = Pmismatch * tan(p.HDphase*pi/180)^2;

% Arm offset necessary to realize the target HD phase
p.armOffset = p.armOffsetPolarity * p.DCReadout * sqrt(PdcTarget/(Pdc/(testArmOffset^2)));

%}}}

%}}}

%{{{ Check HD Phase

%% Check HD Phase
% Add offset
opt = setPosOffset(opt, n.ETMX, p.armOffset); 
opt = setPosOffset(opt, n.ETMY, -p.armOffset);

pos=zeros(Ndrive,1);
[fDC, sigDC]=sweep(opt,pos);

% Carrier power leaking out to the AS by the DARM offset
Pdc = abs(fDC(l.OMCTR, find(p.vMod==0))).^2 - Pmismatch;
% Homodyne phase
HDphase = atan(sqrt(Pdc/Pmismatch))*180/pi;

%}}}

%{{{ Power at detection ports

%% Power at detection ports

P_REFL1 = abs(fDC(l.REFL1,:)*fDC(l.REFL1,:)');
P_REFL2 = abs(fDC(l.REFL2,:)*fDC(l.REFL2,:)');
P_REFL3 = abs(fDC(l.REFL3,:)*fDC(l.REFL3,:)');
P_REFL4 = abs(fDC(l.REFL4,:)*fDC(l.REFL4,:)');

P_POP1 = abs(fDC(l.POP1,:)*fDC(l.POP1,:)');
P_POP2 = abs(fDC(l.POP2,:)*fDC(l.POP2,:)');

P_ASPO = abs(fDC(l.ASPO,:)*fDC(l.ASPO,:)');
P_OMCREFL = abs(fDC(l.OMCREFL,:)*fDC(l.OMCREFL,:)');
P_AS = abs(fDC(l.OMCTR,:)*fDC(l.OMCTR,:)');
P_POX = abs(fDC(l.POX,:)*fDC(l.POX,:)');
P_POY = abs(fDC(l.POY,:)*fDC(l.POY,:)');

%}}}

%{{{ Adjust attenuators

%% Adjust attenuators

P_REFL_Max = max([P_REFL1,P_REFL2,P_REFL3,P_REFL4]);

if P_REFL_Max > p.PpdMax
   p.REFL_ATTN = 1 - p.PpdMax/P_REFL_Max; 
end

P_POP_Max = max([P_POP1,P_POP2]);
if P_POP_Max > p.PpdMax
   p.POP_ATTN = 1 - p.PpdMax/P_POP_Max; 
end

%Reconstruct the Optickle Model
[opt,n,l,pr,dr] = modelLCGT(p);

%}}}

%{{{ Check power at detection ports again

%% Check power at detection ports again

pos=zeros(Ndrive,1);
[fDC, sigDC]=sweep(opt,pos);

P_REFL1 = abs(fDC(l.REFL1,:)*fDC(l.REFL1,:)');
P_REFL2 = abs(fDC(l.REFL2,:)*fDC(l.REFL2,:)');
P_REFL3 = abs(fDC(l.REFL3,:)*fDC(l.REFL3,:)');
P_REFL4 = abs(fDC(l.REFL4,:)*fDC(l.REFL4,:)');

P_POP1 = abs(fDC(l.POP1,:)*fDC(l.POP1,:)');
P_POP2 = abs(fDC(l.POP2,:)*fDC(l.POP2,:)');

P_ASPO = abs(fDC(l.ASPO,:)*fDC(l.ASPO,:)');
P_OMCREFL = abs(fDC(l.OMCREFL,:)*fDC(l.OMCREFL,:)');
P_AS = abs(fDC(l.OMCTR,:)*fDC(l.OMCTR,:)');
P_POX = abs(fDC(l.POX,:)*fDC(l.POX,:)');
P_POY = abs(fDC(l.POY,:)*fDC(l.POY,:)');

%}}}

%{{{ PRG

%% PRG

PRG=abs(fDC(l.PRMtoPR2, find(p.vMod==0))).^2/(abs(fDC(l.PRMbk, find(p.vMod==0))).^2);
Rarm=abs(fDC(l.ITMXtoMICHX_PO, find(p.vMod==0))).^2/(abs(fDC(l.MICHX_POtoITMX, ...
                                                  find(p.vMod==0))).^2);
Rarm=abs(fDC(l.BStoPR3, find(p.vMod==0))).^2/(abs(fDC(l.PR3toBS, find(p.vMod==0))).^2);

%}}}

%{{{ Light Power in the PRC, SRC, ARMs

XARMCarrierPower = abs(fDC(l.ITMXtoETMX, find(p.vMod==0))).^2;
XARMf1Power =sum(abs(fDC(l.ITMXtoETMX, find(p.vMod==p.fmod1|p.vMod==-p.fmod1))).^2);
XARMf2Power =sum(abs(fDC(l.ITMXtoETMX, find(p.vMod==p.fmod2|p.vMod==-p.fmod2))).^2);

YARMCarrierPower = abs(fDC(l.ITMYtoETMY, find(p.vMod==0))).^2;
YARMf1Power =sum(abs(fDC(l.ITMYtoETMY, find(p.vMod==p.fmod1|p.vMod==-p.fmod1))).^2);
YARMf2Power =sum(abs(fDC(l.ITMYtoETMY, find(p.vMod==p.fmod2|p.vMod==-p.fmod2))).^2);


PRCCarrierPower =abs(fDC(l.PRMtoPR2, find(p.vMod==0))).^2;
SRCCarrierPower =abs(fDC(l.SR2toSRM, find(p.vMod==0))).^2;
AS_CarrierPower =abs(fDC(l.SRMtoASPOM, find(p.vMod==0))).^2;
REFLCarrierPower = abs(fDC(l.REFL_ATTN, find(p.vMod==0))).^2;

PRCf1Power =sum(abs(fDC(l.PRMtoPR2, find(p.vMod==p.fmod1|p.vMod==- ...
                                         p.fmod1))).^2);
SRCf1Power =sum(abs(fDC(l.SR2toSRM, find(p.vMod==p.fmod1|p.vMod==- ...
                                         p.fmod1))).^2);
AS_f1Power =sum(abs(fDC(l.SRMtoASPOM, find(p.vMod==p.fmod1|p.vMod==- ...
                                           p.fmod1))).^2);
REFL_f1Power =sum(abs(fDC(l.REFL_ATTN, find(p.vMod==p.fmod1|p.vMod==- ...
                                           p.fmod1))).^2);
PRCf2Power =sum(abs(fDC(l.PRMtoPR2, find(p.vMod==p.fmod2|p.vMod==- ...
                                         p.fmod2))).^2);
SRCf2Power =sum(abs(fDC(l.SR2toSRM, find(p.vMod==p.fmod2|p.vMod==- ...
                                         p.fmod2))).^2);
AS_f2Power =sum(abs(fDC(l.SRMtoASPOM, find(p.vMod==p.fmod2|p.vMod==- ...
                                           p.fmod2))).^2);
REFL_f2Power =sum(abs(fDC(l.REFL_ATTN, find(p.vMod==p.fmod2|p.vMod==- ...
                                           p.fmod2))).^2);

%}}}

%}}}

%{{{ ********** OSA Plots **************

%{{{ OSA plot in the PRC

%% OSA plot in the PRC
mkdir([resultDir,'OSAPlots']);

if osaplots
    h1=figure(1);
    h=osaplot(fDC, p.vMod,l.PRMbk,1,1,'k');
    hold(h(1),'on')
    hold(h(2),'on')
    h=osaplot(fDC, p.vMod,l.PRMtoPR2,1,1,'r');

    title(h(1), 'Fields in the PRC', 'FontSize', 16);
    print(h1, '-dpdf', [resultDir, 'OSAPlots/','OSA_Plot_PRC.pdf']);
    saveas(h1,  [resultDir, 'OSAPlots/','OSA_Plot_PRC.fig']);
    %close(h1)
end

%}}}

%{{{ OSA plot in the SRC

%% OSA plot in the SRC
if osaplots
    h1=figure(1);
    h=osaplot(fDC, p.vMod,l.PRMbk,1,1,'k');
    hold(h(1),'on')
    hold(h(2),'on')
    h=osaplot(fDC, p.vMod,l.SRMtoSR2,1,1,'r');

    title(h(1), 'Fields in the SRC', 'FontSize', 16);
    print(h1, '-dpdf', [resultDir,'OSAPlots/', 'OSA_Plot_SRC.pdf']);
    saveas(h1,  [resultDir,'OSAPlots/', 'OSA_Plot_SRC.fig']);
    %close(h1)
end

%}}}

%{{{ OSA plot in the X arm

%% OSA plot in the X arm
if osaplots
    h1=figure(1);
    h=osaplot(fDC, p.vMod,l.PRMbk,1,1,'k');
    hold(h(1),'on')
    hold(h(2),'on')
    h=osaplot(fDC, p.vMod,l.ITMXtoETMX,1,1,'r');

    title(h(1), 'Fields in the X-arm', 'FontSize', 16);
    print(h1, '-dpdf', [resultDir,'OSAPlots/', 'OSA_Plot_XARM.pdf']);
    saveas(h1,  [resultDir,'OSAPlots/', 'OSA_Plot_XARM.fig']);
    close(h1)
end

%}}}

%{{{ OSA plot in the Y arm

%% OSA plot in the Y arm
if osaplots
    h1=figure(1);
    h=osaplot(fDC, p.vMod,l.PRMbk,1,1,'k');
    hold(h(1),'on')
    hold(h(2),'on')
    h=osaplot(fDC, p.vMod,l.ITMYtoETMY,1,1,'r');

    title(h(1), 'Fields in the Y-arm', 'FontSize', 16);
    print(h1, '-dpdf', [resultDir,'OSAPlots/', 'OSA_Plot_YARM.pdf']);
    saveas(h1,  [resultDir,'OSAPlots/', 'OSA_Plot_YARM.fig']);
    close(h1)
end

%}}}

%{{{ OSA plot at REFL

%% OSA plot at REFL
if osaplots
    h1=figure(1);
    h=osaplot(fDC, p.vMod,l.PRMbk,1,1,'k');
    hold(h(1),'on')
    hold(h(2),'on')
    h=osaplot(fDC, p.vMod,l.REFL_ATTN,1,1,'r');


    title(h(1), 'Fields at REFL', 'FontSize', 16);
    print(h1, '-dpdf', [resultDir,'OSAPlots/', 'OSA_Plot_REFL.pdf']);
    saveas(h1,  [resultDir,'OSAPlots/', 'OSA_Plot_REFL.fig']);
    close(h1)
end

%}}}

%{{{ OSA plot at OMC_TR

%% OSA plot at OMC_TR

if osaplots
    h1=figure(1);
    h=osaplot(fDC, p.vMod,l.OMCREFL,1,1,'k');
    hold(h(1),'on')
    hold(h(2),'on')
    h=osaplot(fDC, p.vMod,l.OMCTR,1,1,'r');


    title(h(1), 'Fields at OMCTR', 'FontSize', 16);
    print(h1, '-dpdf', [resultDir,'OSAPlots/', 'OSA_Plot_OMCTR.pdf']);
    saveas(h1,  [resultDir,'OSAPlots/', 'OSA_Plot_OMCTR.fig']);
    close(h1)
end

%}}}

%{{{ close figures

%% close figures
close all

%}}}

%}}}

%{{{ ********** Optimize the demodulation phase **********

%{{{ Do optimization

%% Do optimization
mkdir([resultDir,'VectorSensingMatrices']);
determineDemodPhase(p.vectorSensM, [resultDir,'VectorSensingMatrices/'], p);

%}}}

%{{{ Load the saved demodulation phases 

%% Load the saved demodulation phases 
demodPhaseLCGT;

%}}}

%{{{ Reconstruct the Optickle model

%% Reconstruct the Optickle model
% % Construct the model with the optimized demodulation phase.
[opt,n,l,pr,dr] = modelLCGT(p);

% == Extract some useful model information ==
drvNames=getDriveNames(opt);
Ndrive=opt.Ndrive;
Nlink=opt.Nlink;
lambda=opt.lambda;
c=opt.c;

opt2dot(opt, [resultDir,'LCGT_LSC.dot']);

%pause

%}}}

%}}}

%{{{ Save simulation parameters

%% Save simulation parameters
save([resultDir,'Parameters.mat'],'p','opt','l','n','pr','dr');

%}}}

%}}}

%{{{ === Create Sensing Matrix Plots ===

%{{{ Plots

%% Plots

[fDC, sigDC, sigAC, mMech, noiseAC, noiseMech]=tickle(opt,[],p.ftickle);

probeNames = {'AS_DC','ASPO_1I','ASPO_1Q','REFL_2I','REFL_2Q','REFL_1I','REFL_1Q','POP_1I','POP_1Q','POP_2I', ...
    'POP_2Q','REFL_1DmI','REFL_1DmQ','REFL_2DmI', 'REFL_2DmQ'};
cPrbNames = probeNames;
mPrb = eye(length(probeNames));
driveNames = {'ETMX','ETMY','ITMX','ITMY','BS','PRM','SRM'};
cDrvNames = {'DARM','CARM','MICH','PRC','SRC'};
mDrv = [1,-1,0,0,0,0,0; %DARM
        1,1,0,0,0,0,0;  %CARM
        1,-1,-1,1,0,0,0;%MICH
        0,0,0,0,0,1,0;  %PRC
        0,0,0,0,0,0,1]; %SRC

if p.MICHBS
    mDrv = [1,-1,0,0,0,0,0; %DARM
            1,1,0,0,0,0,0;  %CARM
            0,0,0,0,1,-1/sqrt(2),1/sqrt(2);%MICH
            0,0,0,0,0,1,0;  %PRC
            0,0,0,0,0,0,1]; %SRC
end

h1=figure(1);
plotSensingMatrix(opt, sigAC, probeNames, mPrb, cPrbNames, driveNames, ...
                      mDrv, cDrvNames, noiseAC);
                  
h2=figure(2);    
plotSensingMatrix(opt, sigAC, probeNames, mPrb, cPrbNames, driveNames, ...
                      mDrv, cDrvNames);
h3=figure(3);                  
plotOffsetMatrix(opt, sigDC, probeNames, mPrb, cPrbNames);

h4 = figure(4);
plotOffsetMatrixInMeter(opt,sigAC,sigDC,probeNames, mPrb, cPrbNames, ...
    driveNames, mDrv, cDrvNames);

mkdir([resultDir,'SensingMatrices']);
print(h1, '-dpdf', [resultDir,'SensingMatrices/', 'SensingMatrixNoiseNormalized.pdf']);
saveas(h1,  [resultDir,'SensingMatrices/', 'SensingMatrixNoiseNormalized.fig']);
print(h2, '-dpdf', [resultDir, 'SensingMatrices/','SensingMatrix.pdf']);
saveas(h2,  [resultDir, 'SensingMatrices/','SensingMatrix.fig']);
print(h3, '-dpdf', [resultDir, 'SensingMatrices/','OffsetMatrix.pdf']);
saveas(h3,  [resultDir, 'SensingMatrices/','OffsetMatrix.fig']);
print(h4, '-dpdf', [resultDir, 'SensingMatrices/','OffsetMatrixInMeter.pdf']);
saveas(h4,  [resultDir, 'SensingMatrices/','OffsetMatrixInMeter.fig']);

%}}}

%{{{ Close figures

%% Close figures
close all;

%}}}

%{{{ Sensing Matrix of actually used signal ports

%% Sensing Matrix of actually used signal ports

probeNames = p.signalPorts;
cPrbNames = probeNames;
mPrb = eye(length(probeNames));
driveNames = {'ETMX','ETMY','ITMX','ITMY','BS','PRM','SRM'};
cDrvNames = {'DARM','CARM','MICH','PRC','SRC'};
mDrv = [1,-1,0,0,0,0,0; %DARM
        1,1,0,0,0,0,0;  %CARM
        1,-1,-1,1,0,0,0;%MICH
        0,0,0,0,0,1,0;  %PRC
        0,0,0,0,0,0,1]; %SRC

if p.MICHBS
    mDrv = [1,-1,0,0,0,0,0; %DARM
            1,1,0,0,0,0,0;  %CARM
            0,0,0,0,1,-1/sqrt(2),1/sqrt(2);%MICH
            0,0,0,0,0,1,0;  %PRC
            0,0,0,0,0,0,1]; %SRC
end

[mSens, mSensNorm, mOfs, mOfsMeter] = getSensingMatrix(opt, sigAC, sigDC,...
    probeNames, mPrb, cPrbNames, driveNames, mDrv, cDrvNames, noiseAC);

plotSensingMatrixWithSign(opt, sigAC, probeNames, mPrb, cPrbNames, driveNames, mDrv, cDrvNames);
print('-dpdf', [resultDir, 'SensingMatrices/','SensingMatrixSelected.pdf']);

%}}}

%}}}

%{{{ === RF Power at each PD ===

%% RF Power at each PD

DCsigPower = zeros(9,1);

DCsigPower(1) = abs(sigDC(cellfun(@(x)getProbeNum(opt,x),{'AS_DC'})));

v = sigDC(cellfun(@(x)getProbeNum(opt,x),{'REFL_1I','REFL_1Q'}));
DCsigPower(2) = sqrt(v'*v);
REFL1_RFPower = DCsigPower(2);

v = sigDC(cellfun(@(x)getProbeNum(opt,x),{'REFL_2I','REFL_2Q'}));
DCsigPower(3) = sqrt(v'*v);
REFL2_RFPower = DCsigPower(3);

v = sigDC(cellfun(@(x)getProbeNum(opt,x),{'POP_1I','POP_1Q'}));
DCsigPower(4) = sqrt(v'*v);
POP1_RFPower = DCsigPower(4);

v = sigDC(cellfun(@(x)getProbeNum(opt,x),{'POP_2I','POP_2Q'}));
DCsigPower(5) = sqrt(v'*v);
POP2_RFPower = DCsigPower(5);

v = sigDC(cellfun(@(x)getProbeNum(opt,x),{'REFL_1DmI','REFL_1DmQ'}));
DCsigPower(6) = sqrt(v'*v);
REFL1Dm_RFPower = DCsigPower(6);

v = sigDC(cellfun(@(x)getProbeNum(opt,x),{'REFL_1DpI','REFL_1DpQ'}));
DCsigPower(7) = sqrt(v'*v);
REFL1Dp_RFPower = DCsigPower(7);

v = sigDC(cellfun(@(x)getProbeNum(opt,x),{'REFL_2DmI','REFL_2DmQ'}));
DCsigPower(8) = sqrt(v'*v);
REFL2Dm_RFPower = DCsigPower(8);

v = sigDC(cellfun(@(x)getProbeNum(opt,x),{'REFL_2DpI','REFL_2DpQ'}));
DCsigPower(9) = sqrt(v'*v);
REFL2Dp_RFPower = DCsigPower(9);

%}}}

%{{{ ********* Mirror Sweeps *********

%% ********* Mirror Sweeps *********

%{{{ mkdir

%mkdir([resultDir,'Sweep']);

%}}}

%{{{ DARM Sweep

%% DARM Sweep

if sweepplots
    
    Nstep = 100;
    range=0.001*nm;
    dl=linspace(-range,range,Nstep);
    pos=zeros(Ndrive, Nstep);
    pos(getDriveNumbers(opt,'ETMX'),:) = dl;
    pos(getDriveNumbers(opt,'ETMY'),:) = -dl;
    [fDC, sigDC]=sweep(opt,pos);

    h1=figure(1);
    plot(dl, squeeze(sigDC(pr.AS_DC,:)),dl, squeeze(sigDC(pr.ASPO_1I,:)));
    xlabel('Displacement [m]','FontSize',16);
    ylabel('Signal [W]','FontSize',16);
    title('DARM Sweep','FontSize',16);
    grid on
    legend('AS_DC','ASPO_1I');

    print(h1, '-dpdf', [resultDir,'Sweep/', 'SweepDARM.pdf']);
    saveas(h1, [resultDir,'Sweep/', 'SweepDARM.fig']);

    %close(h1);
end
%}}}

%{{{ CARM Sweep

%% CARM Sweep

if sweepplots
    Nstep = 100;
    range=1*pm;
    dl=linspace(-range,range,Nstep);
    pos=zeros(Ndrive, Nstep);
    pos(getDriveNumbers(opt,'ETMX'),:) = dl;
    pos(getDriveNumbers(opt,'ETMY'),:) = dl;
    [fDC, sigDC]=sweep(opt,pos);

    h1=figure(1);
    plot(dl, squeeze(sigDC(pr.REFL_2I,:)),dl, squeeze(sigDC(pr.REFL_1I,:)));
    xlabel('Displacement [m]','FontSize',16);
    ylabel('Signal [W]','FontSize',16);
    title('CARM Sweep','FontSize',16);
    grid on
    legend('REFL_2I','REFL_1I');
    print(h1, '-dpdf', [resultDir,'Sweep/', 'SweepCARM.pdf']);
    saveas(h1, [resultDir,'Sweep/', 'SweepCARM.fig']);
    
    close(h1);
end
%}}}

%{{{ MICH Sweep

%% MICH Sweep

if sweepplots & ~p.MICHBS
    Nstep = 100;
    range=1*nm;
    dl=linspace(-range,range,Nstep);
    pos=zeros(Ndrive, Nstep);
    pos(getDriveNumbers(opt,'ETMX'),:) = dl;
    pos(getDriveNumbers(opt,'ETMY'),:) = -dl;
    pos(getDriveNumbers(opt,'ITMX'),:) = -dl;
    pos(getDriveNumbers(opt,'ITMY'),:) = dl;
    [fDC, sigDC]=sweep(opt,pos);

    h1=figure(1);
    plot(dl, squeeze(sigDC(pr.REFL_1DmQ,:)),dl, squeeze(sigDC(pr.REFL_1DpQ,:))...
         ,dl, squeeze(sigDC(pr.REFL_1Q,:)),...
        dl, squeeze(sigDC(pr.POP_1Q,:)));
    xlabel('Displacement [m]','FontSize',16);
    ylabel('Signal [W]','FontSize',16);
    title('MICH Sweep','FontSize',16);
    grid on
    legend('REFL_1DmQ','REFL_1DpQ','REFL_1Q', 'POP_1Q');
    print(h1, '-dpdf', [resultDir, 'Sweep/','SweepMICH.pdf']);
    saveas(h1, [resultDir, 'Sweep/','SweepMICH.fig']);
    
    %    close(h1);
end
%}}}

%{{{ MICH BS Sweep

%% MICH BS Sweep

if sweepplots & p.MICHBS
    Nstep = 100;
    range=1*nm;
    dl=linspace(-range,range,Nstep);
    pos=zeros(Ndrive, Nstep);
    pos(getDriveNumbers(opt,'BS'),:) = dl;
    pos(getDriveNumbers(opt,'PRM'),:) = -1*dl/sqrt(2);
    pos(getDriveNumbers(opt,'SRM'),:) = 1*dl/sqrt(2);
    [fDC, sigDC]=sweep(opt,pos);      

    h1=figure(1);
    plot(dl, squeeze(sigDC(pr.REFL_1DmQ,:)),dl, squeeze(sigDC(pr.REFL_1DpQ,:))...
         ,dl, squeeze(sigDC(pr.REFL_1Q,:)),...
        dl, squeeze(sigDC(pr.POP_1Q,:)));
    xlabel('Displacement [m]','FontSize',16);
    ylabel('Signal [W]','FontSize',16);
    title('MICH Sweep','FontSize',16);
    grid on
    legend('REFL_1DmQ','REFL_1DpQ','REFL_1Q', 'POP_1Q');    
    print(h1, '-dpdf', [resultDir, 'Sweep/','SweepMICH.pdf']);
    saveas(h1, [resultDir, 'Sweep/','SweepMICH.fig']);
    
    close(h1);
end
%}}}

%{{{ PRCL Sweep

%% PRCL Sweep

if sweepplots
    Nstep = 100;
    range=1*nm;
    dl=linspace(-range,range,Nstep);
    pos=zeros(Ndrive, Nstep);
    pos(getDriveNumbers(opt,'PRM'),:) = dl;
    [fDC, sigDC]=sweep(opt,pos);

    h1=figure(1);
    plot(dl, squeeze(sigDC(pr.REFL_2DmI,:)),dl, squeeze(sigDC(pr.REFL_2DpI,:)),dl, squeeze(sigDC(pr.REFL_2I,:)),...
        dl, squeeze(sigDC(pr.POP_2I,:)));
        
    xlabel('Displacement [m]','FontSize',16);
    ylabel('Signal [W]','FontSize',16);
    title('PRCL Sweep','FontSize',16);
    grid on
    legend('REFL_2DmI','REFL_2DpI','REFL_2I', 'POP_2I');
    print(h1, '-dpdf', [resultDir, 'Sweep/','SweepPRCL.pdf']);
    saveas(h1, [resultDir, 'Sweep/','SweepPRCL.fig']);
    
    close(h1);
end
%}}}

%{{{ SRCL Sweep

%% SRCL Sweep
%{

if sweepplots
    Nstep = 500;  %increased # for more presice measurement of p.dTune
    range=20*nm;
    dl=linspace(-range,range,Nstep);
    pos=zeros(Ndrive, Nstep);
    pos(getDriveNumbers(opt,'SRM'),:) = dl;
    [fDC, sigDC]=sweep(opt,pos);

    h1=figure(1);
    plot(dl, squeeze(sigDC(pr.REFL_1DmI,:)),dl, squeeze(sigDC(pr.REFL_1DpI,:)),dl, squeeze(sigDC(pr.REFL_1I,:))...
        ,dl, squeeze(sigDC(pr.POP_1I,:)));
    
        
    xlabel('Displacement [m]','FontSize',16);
    ylabel('Signal [W]','FontSize',16);
    title('SRCL Sweep','FontSize',16);
    grid on
    YLim = get(gca, 'YLim');
    hold on
    plot([10*nm,10*nm],YLim,'color','r');
    legend('REFL_1DmI','REFL_1DpI','REFL_1I','POP_1I','Location','Best');
    print(h1, '-dpdf', [resultDir,'Sweep/', 'SweepSRCL.pdf']);
    saveas(h1, [resultDir,'Sweep/', 'SweepSRCL.fig']);
    
    % calculate detune displacement corresponding to phase difference b/w EOMs
    [M, I] = min(abs(sigDC(pr.POP_1I,:)))
    p.tmp_dTune = dl(I);
    p.tmp_detunePhase = 90-(p.tmp_dTune*360/lambda)
    p.dTune = p.tmp_dTune;
    p.detunePhase = p.tmp_detunePhase;
    fprintf('Displacement of SRM [m]: %e \n', p.dTune);
    fprintf('Detune Phase [degree]: %e \n', p.detunePhase);

    % reconstruct the model
    BRSEthreshold = 5e-11; %threshold for decideing p.DRSE=0. later better to get this dependent on Nstep)
    if p.dTune>BRSEthreshold
        p.DRSE = 1;
    end
    [opt,n,l,pr,dr] = modelLCGT(p);
    
    close(h1);
    
    %Save data
    M = [dl(:),squeeze(sigDC(pr.POP_1I,:))'];
    dlmwrite([resultDir, 'Sweep/SweepSRCL.dat'], M)
    
    
end
%}
%}}}

%{{{ Close figures

%% Close figures
close all;

%}}}

%}}}

%{{{ ******* RF Power at each PD with realistic RMS ******

if p.calc_RFPD_requirements

%{{{ Add Probes

rfList = p.vMod(p.vMod >=0);
idx=1;
REFL1_Probes_I = zeros(length(rfList),1);
REFL1_Probes_Q = zeros(length(rfList),1);
REFL2_Probes_I = zeros(length(rfList),1);
REFL2_Probes_Q = zeros(length(rfList),1);
POP1_Probes_I = zeros(length(rfList),1);
POP1_Probes_Q = zeros(length(rfList),1);
POP2_Probes_I = zeros(length(rfList),1);
POP2_Probes_Q = zeros(length(rfList),1);
ASPO_Probes_I = zeros(length(rfList),1);
ASPO_Probes_Q = zeros(length(rfList),1);

for fmod = rfList'
    [opt, REFL1_Probes_I(idx)] = addProbeIn(opt, sprintf('REFL1_%.2eI',fmod), n.REFL1, ...
                                   'in',  fmod, 0);
    [opt, REFL1_Probes_Q(idx)] = addProbeIn(opt, sprintf('REFL1_%.2eI',fmod), n.REFL1, ...
                                   'in',  fmod, 90*(fmod ~= 0));    
    [opt, REFL2_Probes_I(idx)] = addProbeIn(opt, sprintf('REFL2_%.2eI',fmod), n.REFL2, ...
                                   'in',  fmod, 0);
    [opt, REFL2_Probes_Q(idx)] = addProbeIn(opt, sprintf('REFL2_%.2eI',fmod), n.REFL2, ...
                                   'in',  fmod, 90*(fmod ~= 0));    
    [opt, POP1_Probes_I(idx)] = addProbeIn(opt, sprintf('POP1_%.2eI',fmod), n.POP1, ...
                                   'in',  fmod, 0);
    [opt, POP1_Probes_Q(idx)] = addProbeIn(opt, sprintf('POP1_%.2eI',fmod), n.POP1, ...
                                   'in',  fmod, 90*(fmod ~= 0));    
    [opt, POP2_Probes_I(idx)] = addProbeIn(opt, sprintf('POP2_%.2eI',fmod), n.POP2, ...
                                   'in',  fmod, 0);
    [opt, POP2_Probes_Q(idx)] = addProbeIn(opt, sprintf('POP2_%.2eI',fmod), n.POP2, ...
                                   'in',  fmod, 90*(fmod ~= 0));
    [opt, ASPO_Probes_I(idx)] = addProbeIn(opt, sprintf('ASPO_%.2eI',fmod), n.ASPO, ...
                                   'in',  fmod, 0);
    [opt, ASPO_Probes_Q(idx)] = addProbeIn(opt, sprintf('ASPO_%.2eI',fmod), n.ASPO, ...
                                   'in',  fmod, 90*(fmod ~= 0));

    idx=idx+1;

end

%}}}    

%{{{ Prepare pos

pos=zeros(Ndrive, 2^5);
nn = 1;    
for ii = [1,-1]
    for jj = [1,-1]
        for kk = [1,-1]
            for ll = [1,-1]
                for mm = [1,-1]
                    pos(getDriveNumbers(opt,'ETMX'),nn) = ii*p.DARM_RMS;
                    pos(getDriveNumbers(opt,'ETMY'),nn) = -ii*p.DARM_RMS;

                    pos(getDriveNumbers(opt,'ETMX'),nn) = ...
                        pos(getDriveNumbers(opt,'ETMX'),nn) + jj*p.CARM_RMS;                    

                    pos(getDriveNumbers(opt,'ETMY'),nn) = ...
                        pos(getDriveNumbers(opt,'ETMY'),nn) + jj*p.CARM_RMS;

                    pos(getDriveNumbers(opt,'BS'),nn) = sqrt(2)*kk*p.MICH_RMS;

                    pos(getDriveNumbers(opt,'PRM'),nn) = ll*p.PRCL_RMS;                    

                    pos(getDriveNumbers(opt,'SRM'),nn) = mm*p.SRCL_RMS;                    
                    nn = nn + 1;
                end
            end
        end
    end
end

%}}}

%{{{ Do sweep

[fDC, sigDC]=sweep(opt,pos);

%}}}

%{{{ Get RF Power

REFL1_RFPower = zeros(length(rfList),1);
REFL2_RFPower = zeros(length(rfList),1);
POP1_RFPower = zeros(length(rfList),1);
POP2_RFPower = zeros(length(rfList),1);
ASPO_RFPower = zeros(length(rfList),1);

% ** Important Note **
% The signal amplitude (sigDC), given by Optickle is
% the one after the demodulation. Since the demodulation
% process reduces the amplitude by a factor of 2, the actual
% amplitude of the RF power modulation at the PD surface
% is twice the output of Optickle. (Consider, cos(w*t)^2 and
% expand it to cos(2*w*t) and cos(0). You get 1/2 in front.)
for idx = 1:length(rfList)
    a  = sqrt(sigDC(REFL1_Probes_I(idx), :).^2 ...
                    + sigDC(REFL1_Probes_Q(idx),:).^2);
    REFL1_RFPower(idx) = 2*max(a);

    a  = sqrt(sigDC(REFL2_Probes_I(idx), :).^2 ...
                    + sigDC(REFL2_Probes_Q(idx),:).^2);
    REFL2_RFPower(idx) = 2*max(a);

    a  = sqrt(sigDC(POP1_Probes_I(idx), :).^2 ...
                    + sigDC(POP1_Probes_Q(idx),:).^2);
    POP1_RFPower(idx) = 2*max(a);

    a  = sqrt(sigDC(POP2_Probes_I(idx), :).^2 ...
                    + sigDC(POP2_Probes_Q(idx),:).^2);
    POP2_RFPower(idx) = 2*max(a);
    
    a  = sqrt(sigDC(ASPO_Probes_I(idx), :).^2 ...
                    + sigDC(ASPO_Probes_Q(idx),:).^2);
    ASPO_RFPower(idx) = 2*max(a);

end

%}}}

%{{{ Compute DC Power

%REFL1
a = squeeze(fDC(l.REFL1,:,:));
b = a'*a;
PdcREFL1 = max(abs(diag(b)));

%REFL2
a = squeeze(fDC(l.REFL2,:,:));
b = a'*a;
PdcREFL2 = max(abs(diag(b)));

%POP1
a = squeeze(fDC(l.POP1,:,:));
b = a'*a;
PdcPOP1 = max(abs(diag(b)));

%POP2
a = squeeze(fDC(l.POP2,:,:));
b = a'*a;
PdcPOP2 = max(abs(diag(b)));

%ASPO
a = squeeze(fDC(l.ASPO,:,:));
b = a'*a;
PdcASPO = max(abs(diag(b)));


%}}}

%{{{ Save to file
File = fopen([resultDir, 'RFPower.txt'],'w');
fprintf(File, '####### RF Power at each PD #######\n\n');

fprintf(File, '--- REFL1 PD: target = %fMHz ---\n', p.fmod1/MHz);
Pdc = PdcREFL1;
fprintf(File, 'DC = %e W\n', Pdc);
fprintf(File, 'Shot noise = %e W/rtHz\n\n', sqrt(2*Pdc*hp*c/p.lambda) );
for idx = 2:length(rfList)
    fprintf(File, '%fMHz = %e W\n', rfList(idx)/MHz, REFL1_RFPower(idx));
end

fprintf(File, '\n--- REFL2 PD: target = %fMHz ---\n', p.fmod2/MHz);
Pdc = PdcREFL2;
fprintf(File, 'DC = %e W\n', Pdc);
fprintf(File, 'Shot noise = %e W/rtHz\n\n', sqrt(2*Pdc*hp*c/p.lambda) );
for idx = 2:length(rfList)
    fprintf(File, '%fMHz = %e W\n', rfList(idx)/MHz, REFL2_RFPower(idx));
end

fprintf(File, '\n--- POP1 PD: target = %fMHz ---\n', p.fmod1/MHz);
Pdc = PdcPOP1;
fprintf(File, 'DC = %e W\n', Pdc);
fprintf(File, 'Shot noise = %e W/rtHz\n\n', sqrt(2*Pdc*hp*c/p.lambda) );

for idx = 2:length(rfList)
    fprintf(File, '%fMHz = %e W\n', rfList(idx)/MHz, POP1_RFPower(idx));
end

fprintf(File, '\n--- POP2 PD: target = %fMHz ---\n', p.fmod2/MHz);
Pdc = PdcPOP2;
fprintf(File, 'DC = %e W\n', Pdc);
fprintf(File, 'Shot noise = %e W/rtHz\n\n', sqrt(2*Pdc*hp*c/p.lambda) );

for idx = 2:length(rfList)
    fprintf(File, '%fMHz = %e W\n', rfList(idx)/MHz, POP2_RFPower(idx));
end

fprintf(File, '\n--- ASPO PD: target = %fMHz ---\n', p.fmod1/MHz);
Pdc = PdcASPO;
fprintf(File, 'DC = %e W\n', Pdc);
fprintf(File, 'Shot noise = %e W/rtHz\n\n', sqrt(2*Pdc*hp*c/p.lambda) );

for idx = 2:length(rfList)
    fprintf(File, '%fMHz = %e W\n', rfList(idx)/MHz, ASPO_RFPower(idx));
end

fclose(File);
    
%}}}

%{{{ Reconstruct the Optickle model

%% Reconstruct the Optickle model
% % Construct the model with the optimized demodulation phase.
[opt,n,l,pr,dr] = modelLCGT(p);

% == Extract some useful model information ==
drvNames=getDriveNames(opt);
Ndrive=opt.Ndrive;
Nlink=opt.Nlink;
lambda=opt.lambda;
c=opt.c;

%}}}

end

%}}}

%{{{ ******** Tickle ************

if p.doTickle
    
%{{{ tickle

%% Tickle

display('Start Tickling ...')
%f=logspace(-1,5,1000);
%f=logspace(-3,4,500);
f=logspace(0,4,500);

tic
[fDC, sigDC, sigAC, mMech, noiseAC, noiseMech] = ptickle(opt, [], f);
toc
display('Finished.')

%}}}

%{{{ Save the tickle results

%% Save the tickle results
save([resultDir,'TickleResults.mat'],'fDC','sigDC','sigAC','mMech','noiseAC','noiseMech','f','DCsigPower');

%}}}

end %if p.doTickle
%}}}

%{{{ ******** Fixed Tickle ***********

if p.doFixedTickle

%{{{ Tickle

display('Start Fixed Optics Tickling ...')

p.fixedOptics = 1;
[opt,n,l,pr,dr] = modelLCGT(p);

f=logspace(0,4,500);
tic
[fDCf, sigDCf, sigACf, mMechf, noiseACf, noiseMechf] = ptickle(opt, [], f);
toc
display('Finished.')
%}}}

%{{{ Save the tickle results

%% Save the tickle results
save([resultDir,'TickleResultsFixed.mat'],'fDCf','sigDCf','sigACf','mMechf','noiseACf','noiseMechf','f');

%}}}

end

%}}}

%{{{ Plot Pure Quantum Sensitivity

%% Plot Pure Quantum Sensitivity

%Angular freq
w = 2*pi*f;

sigDARM = [0.5,-0.5]*squeeze(sigAC(pr.AS_DC, getDriveNumbers(opt, {'ETMX','ETMY'}),:));
% for ii = [1:length(w)]
%     sigDARM = sigAC(pr.AS_DC, getDriveNumbers(opt, {'ETMX','ETMY'}),ii)* ...
%               [mMech(dr.ETMX,dr.ETMX,ii),-mMech(dr.ETMY,dr.ETMY,ii)]*0.5;
% end
sigDARM = sigDARM(:);

noiseDARM = noiseAC(pr.AS_DC, :);
noiseDARM = noiseDARM(:);

pureQN = abs(noiseDARM./sigDARM/p.Larm);

h1=figure(1);
h=loglog(f, abs(noiseDARM./sigDARM/p.Larm));
set(gca,'FontSize',16);
grid on
title('Pure Quantum Strain Sensitivity','FontSize',16);
xlabel('Hz','FontSize',16);
h=ylabel('1/sqrt(Hz)','FontSize',16);
print(h1, '-dpdf', [resultDir, 'PureQuantumNoise.pdf']);
saveas(h1, [resultDir, 'PureQuantumNoise.fig']);
close(h1);

%Save
M = [f(:),abs(noiseDARM./sigDARM/p.Larm)];
dlmwrite([resultDir, 'PureQuantumNoise.dat'], M);

%}}}

if p.doTickle
if p.LoopNoise
    
%{{{ ****** Loop Noise Calculation **********

%% ****** Loop Noise Calculation **********
%******************************************
%
% Matirces for Loop Noise
%
% DOF Vector: (DARM, CARM, MICH, PRC, SRC)
%
%
% Probe Vector: Probes for [DARM, CARM, MICH, PRCL, SRCL]
%
%

%{{{ Prepare

%Angular freq
w = 2*pi*f;

%Drive name cell array
drvNames = {'ETMX', 'ETMY', 'ITMX', 'ITMY', 'BS', 'PRM', 'SRM', 'PM', ...
                    'AM',  'Mod1.amp', 'Mod1.phase', 'Mod2.amp', ...
            'Mod2.phase', 'Mod3.amp', 'Mod3.phase'};
if p.useMZI
    if p.MZIM2
        if p.f3EOM
            drvNames = {'ETMX', 'ETMY', 'ITMX', 'ITMY', 'BS', 'PRM', 'SRM', 'PM', ...
                'AM',  'Mod1.amp', 'Mod1.phase', 'Mod2.amp', ...
                'Mod2.phase', 'Mod3.amp', 'Mod3.phase',...
                'Mod1A.amp', 'Mod1A.phase', 'Mod3A.amp',...
                'Mod3A.phase', 'MZIM1', 'MZIM2', 'MZIM4'};
        else
            drvNames = {'ETMX', 'ETMY', 'ITMX', 'ITMY', 'BS', 'PRM', 'SRM', 'PM', ...
                'AM',  'Mod1.amp', 'Mod1.phase', 'Mod2.amp', ...
                'Mod2.phase','Mod3.amp', 'Mod3.phase',...
                'Mod1A.amp','Mod1A.phase', ...
                'MZIM1', 'MZIM2', 'MZIM4'};
        end
    else
        if p.f3EOM
            drvNames = {'ETMX', 'ETMY', 'ITMX', 'ITMY', 'BS', 'PRM', 'SRM', 'PM', ...
                'AM',  'Mod1.amp', 'Mod1.phase', 'Mod2.amp', ...
                'Mod2.phase', 'Mod3.amp', 'Mod3.phase',...
                'Mod1A.amp', 'Mod1A.phase', 'Mod3A.amp',...
                'Mod3A.phase', 'MZIM1', 'MZIM4'};
        else
            drvNames = {'ETMX', 'ETMY', 'ITMX', 'ITMY', 'BS', 'PRM', 'SRM', 'PM', ...
                'AM',  'Mod1.amp', 'Mod1.phase', 'Mod2.amp', ...
                'Mod2.phase', 'Mod3.amp', 'Mod3.phase',...
                'Mod1A.amp', 'Mod1A.phase', ...
                'MZIM1', 'MZIM4'};
        end
    end
end

if p.RAM
    drvNames = {'ETMX', 'ETMY', 'ITMX', 'ITMY', 'BS', 'PRM', 'SRM', 'PM', ...
                    'AM',  'Mod1.amp', 'Mod1.phase', 'Mod2.amp', ...
            'Mod2.phase', 'Mod3.amp', 'Mod3.phase', ...
            'Mod1AM1.amp', 'Mod1AM1.phase','Mod2AM1.amp', 'Mod2AM1.phase'};
end

%Map object to return a drive index given a drive name
actMap = containers.Map(drvNames, [1:length(drvNames)]);

%Drive indices array
iDrv = getDriveNumbers(opt, drvNames);

% DOF to Actuator conversion matrix
Mconv = zeros(length(iDrv),5);
%DARM
Mconv(1,1)=1/2; Mconv(2,1)=-1/2;
%CARM
Mconv(1,2)=0; Mconv(2,2)=0;
Mconv(8,2)=1; 
%MICH
if p.MICHBS
    Mconv(5,3)=1; Mconv(6,3)=-1/sqrt(2);Mconv(7,3)=1/sqrt(2);
else
    Mconv(1,3)=1; Mconv(2,3)=-1;Mconv(3,3)=-1; Mconv(4,3)=1;    
end
%PRCL
Mconv(6,4)=1;
%SRCL
Mconv(7,5)=1;

%CARM ETM
Mconv2 = Mconv;
Mconv2(1,2)=1/2; Mconv2(2,2)=1/2;
Mconv2(8,2)=0; 

%Actuator vector for individual mirror
vActETMX = zeros(length(iDrv),1);
vActETMX(actMap('ETMX')) = 1;
vActETMY = zeros(length(iDrv),1);
vActETMY(actMap('ETMY')) = 1;
vActITMX = zeros(length(iDrv),1);
vActITMX(actMap('ITMX')) = 1;
vActITMY = zeros(length(iDrv),1);
vActITMY(actMap('ITMY')) = 1;
vActBS = zeros(length(iDrv),1);
vActBS(actMap('BS')) = 1;
vActPRM = zeros(length(iDrv),1);
vActPRM(actMap('PRM')) = 1;
vActSRM = zeros(length(iDrv),1);
vActSRM(actMap('SRM')) = 1;
vActPM = zeros(length(iDrv),1);
vActPM(actMap('PM')) = 1;
vActAM = zeros(length(iDrv),1);
vActAM(actMap('AM')) = 1;
vActf1AM = zeros(length(iDrv),1);
vActf1AM(actMap('Mod1.amp')) = 1;
vActf1PM = zeros(length(iDrv),1);
vActf1PM(actMap('Mod1.phase')) = 1;
vActf2AM = zeros(length(iDrv),1);
vActf2AM(actMap('Mod2.amp')) = 1;
vActf2PM = zeros(length(iDrv),1);
vActf2PM(actMap('Mod2.phase')) = 1;
vActf3AM = zeros(length(iDrv),1);
vActf3AM(actMap('Mod3.amp')) = 1;
vActf3PM = zeros(length(iDrv),1);
vActf3PM(actMap('Mod3.phase')) = 1;
if p.useMZI
    vActf1APM = zeros(length(iDrv),1);
    vActf1APM(actMap('Mod1A.phase')) = 1;
    vActf1AAM = zeros(length(iDrv),1);
    vActf1AAM(actMap('Mod1A.amp')) = 1;
    if p.f3EOM
        vActf3APM = zeros(length(iDrv),1);
        vActf3APM(actMap('Mod3A.phase')) = 1;
        vActf3AAM = zeros(length(iDrv),1);
        vActf3AAM(actMap('Mod3A.amp')) = 1;
    end
    vActMZIM1 = zeros(length(iDrv),1);
    vActMZIM1(actMap('MZIM1')) = 1;
    vActMZIM4 = zeros(length(iDrv),1);
    vActMZIM4(actMap('MZIM4')) = 1;
    
    if p.MZIM2
        vActMZIM2 = zeros(length(iDrv),1);
        vActMZIM2(actMap('MZIM2')) = 1;
    end
    
end

if p.RAM
    vActf1AM1AM = zeros(length(iDrv),1);
    vActf1AM1AM(actMap('Mod1AM1.amp')) = 1;
    vActf1AM1PM = zeros(length(iDrv),1);
    vActf1AM1PM(actMap('Mod1AM1.phase')) = 1;
    vActf2AM1AM = zeros(length(iDrv),1);
    vActf2AM1AM(actMap('Mod2AM1.amp')) = 1;
    vActf2AM1PM = zeros(length(iDrv),1);
    vActf2AM1PM(actMap('Mod2AM1.phase')) = 1;
end

%Probe indices
iPrb = [getProbeNum(opt,p.signalPorts{1}), getProbeNum(opt,p.signalPorts{2}),...
        getProbeNum(opt,p.signalPorts{3}), getProbeNum(opt,p.signalPorts{4}),...
        getProbeNum(opt,p.signalPorts{5})];


%}}}

%{{{ Actuation Matrix (DOF to Mirrors) A 

%% Actuation Matrix (DOF to Mirrors) A 

% Mechanical TF matrix
Mtf = repmat(eye(length(iDrv), length(iDrv)),[1,1,length(w)]);
Mtf(1,1,:) = freqresp(p.tfTM,w);
Mtf(2,2,:) = freqresp(p.tfTM,w);
Mtf(3,3,:) = freqresp(p.tfTM,w);
Mtf(4,4,:) = freqresp(p.tfTM,w);
Mtf(5,5,:) = freqresp(p.tfBS,w);
Mtf(6,6,:) = freqresp(p.tfPRM,w);
Mtf(7,7,:) = freqresp(p.tfSRM,w);
% Mtf(8,8,:) is the transfer function from laser frequency feedback
% signal to the phase modulation.
% It is 1/(2*pi*f) because  f = d phi/dt
Mtf(8,8,:) = 1./(i*w);
% Mtf(9,9,:) is the transfer function from the laser power actuation
% to the power variation of the laser. It is 1/2 because this actuator
% is actually an amplitude modulator. Note that dP/P = 2*dE/E.
Mtf(9,9,:) = 1/2;

A = zeros(length(iDrv),5,length(w));
%Loop for the frequency points
for ii=[1:length(w)]
    %Mmod = diag(diag(mMech(iDrv, iDrv,ii)));
    %Mmod = mMech(iDrv, iDrv,ii);
    Mmod=eye(length(iDrv), length(iDrv));
    A(:,:,ii) = Mmod*Mtf(:,:,ii)*Mconv;
end

%CARM ETM
A2 = zeros(length(iDrv),5,length(w));
%Loop for the frequency points
for ii=[1:length(w)]
    %Mmod = diag(diag(mMech(iDrv, iDrv,ii)));
    %Mmod = mMech(iDrv, iDrv,ii);
    Mmod=eye(length(iDrv), length(iDrv));
    A2(:,:,ii) = Mmod*Mtf(:,:,ii)*Mconv2;
end

%}}}

%{{{ Detector Matrix D

%% Detector Matrix D
D=sigAC(iPrb, iDrv, :);

%}}}

%{{{ Sensing Matrix S

%% Sensing Matrix S
S = eye(5);

%}}}

%{{{ Check the optical gain

%% Check the raw loop gain
mkdir([resultDir,'OptGains']);

OPTG = zeros(5,5,length(w));
CARM_ETM_OPTG = zeros([1, length(w)]);
for ii=[1:length(w)]
    OPTG(:,:,ii) = S*D(:,:,ii)*Mconv;
    v1 = S*D(:,:,ii)*(vActETMX-vActETMY);
    CARM_ETM_OPTG(ii) = v1(2);
end

close all;

% DARM
h1=figure(1);
subplot(2,1,1);
semilogx(f, 20*log10(squeeze(abs(OPTG(1,1,:)))));
title('DARM Optical Gain','FontSize',12);
ylabel('Gain [dB]')
grid on
subplot(2,1,2);
semilogx(f,squeeze(angle(OPTG(1,1,:)))*180/pi);
ylabel('Phase [deg.]')
xlabel('Frequency [Hz]');
grid on

print(h1, '-dpdf', [resultDir, 'OptGains/', 'OptGainDARM.pdf']);
saveas(h1, [resultDir, 'OptGains/', 'OptGainDARM.fig']);
close(h1);

%Save data
Resp = squeeze(OPTG(1,1,:));
filename = [resultDir, 'OptGains/', 'OptGainDARM.dat'];
fid = fopen(filename,'w');
fprintf(fid,'#f, real, imag\n');
fclose(fid);
M = [f(:), real(Resp(:)), imag(Resp(:))];
dlmwrite(filename, M, '-append');


% CARM
Resp = squeeze(OPTG(2,2,:))./(i*w(:));
%Resp = squeeze(OPTG(2,2,:));
h1=figure(1);
subplot(2,1,1);
semilogx(f, 20*log10(abs(Resp)));
title('CARM Optical Gain','FontSize',12);
ylabel('Gain [dB]')
grid on
subplot(2,1,2);
semilogx(f,angle(Resp)*180/pi);
ylabel('Phase [deg.]')
xlabel('Frequency [Hz]');
grid on

print(h1, '-dpdf', [resultDir, 'OptGains/','OptGainCARM.pdf']);
saveas(h1,  [resultDir, 'OptGains/','OptGainCARM.fig']);
close(h1);

%Save data
filename = [resultDir, 'OptGains/', 'OptGainCARM.dat'];
fid = fopen(filename,'w');
fprintf(fid,'#f, real, imag\n');
fclose(fid);
M = [f(:), real(Resp(:)), imag(Resp(:))];
dlmwrite(filename, M, '-append');

% CARM ETM
Resp = CARM_ETM_OPTG;
h1=figure(1);
subplot(2,1,1);
semilogx(f, 20*log10(abs(Resp)));
title('CARM ETM Optical Gain','FontSize',12);
ylabel('Gain [dB]')
grid on
subplot(2,1,2);
semilogx(f,angle(Resp)*180/pi);
ylabel('Phase [deg.]')
xlabel('Frequency [Hz]');
grid on

print(h1, '-dpdf', [resultDir, 'OptGains/','OptGainCARM_ETM.pdf']);
saveas(h1,  [resultDir, 'OptGains/','OptGainCARM_ETM.fig']);
close(h1);

%Save data
filename = [resultDir, 'OptGains/', 'OptGainCARM_ETM.dat'];
fid = fopen(filename,'w');
fprintf(fid,'#f, real, imag\n');
fclose(fid);
M = [f(:), real(Resp(:)), imag(Resp(:))];
dlmwrite(filename, M, '-append');

% MICH
h1=figure(1);
subplot(2,1,1);
semilogx(f, 20*log10(squeeze(abs(OPTG(3,3,:)))));
title('MICH Optical Gain','FontSize',12);
ylabel('Gain [dB]')
grid on
subplot(2,1,2);
semilogx(f,squeeze(angle(OPTG(3,3,:)))*180/pi);
ylabel('Phase [deg.]')
xlabel('Frequency [Hz]');
grid on

print(h1, '-dpdf', [resultDir, 'OptGains/','OptGainMICH.pdf']);
saveas(h1,  [resultDir, 'OptGains/','OptGainMICH.fig']);
close(h1);

%Save data
Resp = squeeze(OPTG(3,3,:));
filename = [resultDir, 'OptGains/', 'OptGainMICH.dat'];
fid = fopen(filename,'w');
fprintf(fid,'#f, real, imag\n');
fclose(fid);
M = [f(:), real(Resp(:)), imag(Resp(:))];
dlmwrite(filename, M, '-append');

% PRCL
h1=figure(1);
subplot(2,1,1);
semilogx(f, 20*log10(squeeze(abs(OPTG(4,4,:)))));
title('PRCL Optical Gain','FontSize',12);
ylabel('Gain [dB]')
grid on
subplot(2,1,2);
semilogx(f,squeeze(angle(OPTG(4,4,:)))*180/pi);
ylabel('Phase [deg.]');
xlabel('Frequency [Hz]');
grid on

print(h1, '-dpdf', [resultDir, 'OptGains/','OptGainPRCL.pdf']);
saveas(h1,  [resultDir, 'OptGains/','OptGainPRCL.fig']);
close(h1);

%Save data
Resp = squeeze(OPTG(4,4,:));
filename = [resultDir, 'OptGains/', 'OptGainPRCL.dat'];
fid = fopen(filename,'w');
fprintf(fid,'#f, real, imag\n');
fclose(fid);
M = [f(:), real(Resp(:)), imag(Resp(:))];
dlmwrite(filename, M, '-append');

% SRCL
h1=figure(1);
subplot(2,1,1);
semilogx(f, 20*log10(squeeze(abs(OPTG(5,5,:)))));
title('SRCL Optical Gain','FontSize',12);
ylabel('Gain [dB]')
grid on
subplot(2,1,2);
semilogx(f,squeeze(angle(OPTG(5,5,:)))*180/pi);
ylabel('Phase [deg.]')
xlabel('Frequency [Hz]');
grid on

print(h1, '-dpdf', [resultDir, 'OptGains/','OptGainSRCL.pdf']);
saveas(h1,  [resultDir, 'OptGains/','OptGainSRCL.fig']);
close(h1);

%Save data
Resp = squeeze(OPTG(5,5,:));
filename = [resultDir, 'OptGains/', 'OptGainSRCL.dat'];
fid = fopen(filename,'w');
fprintf(fid,'#f, real, imag\n');
fclose(fid);
M = [f(:), real(Resp(:)), imag(Resp(:))];
dlmwrite(filename, M, '-append');

%}}}

%{{{ Check the raw loop gain

%% Check the raw loop gain
mkdir([resultDir,'RawGains']);

G = zeros(5,5,length(w));
for ii=[1:length(w)]
    G(:,:,ii) = S*D(:,:,ii)*A(:,:,ii);
end

close all;

% DARM
h1=figure(1);
subplot(2,1,1);
semilogx(f, 20*log10(squeeze(abs(G(1,1,:)))));
title('DARM Raw Gain (without feedback filter)','FontSize',12);
ylabel('Gain [dB]')
grid on
subplot(2,1,2);
semilogx(f,squeeze(angle(G(1,1,:)))*180/pi);
ylabel('Phase [deg.]')
xlabel('Frequency [Hz]');
grid on

print(h1, '-dpdf', [resultDir, 'RawGains/', 'RawGainDARM.pdf']);
saveas(h1, [resultDir, 'RawGains/', 'RawGainDARM.fig']);
close(h1);

%Save data
Resp = squeeze(G(1,1,:));
filename = [resultDir, 'RawGains/', 'RawGainDARM.dat'];
fid = fopen(filename,'w');
fprintf(fid,'#f, real, imag\n');
fclose(fid);
M = [f(:), real(Resp(:)), imag(Resp(:))];
dlmwrite(filename, M, '-append');


% CARM
h1=figure(1);
subplot(2,1,1);
semilogx(f, 20*log10(squeeze(abs(G(2,2,:)))));
title('CARM Raw Gain (without feedback filter)','FontSize',12);
ylabel('Gain [dB]')
grid on
subplot(2,1,2);
semilogx(f,squeeze(angle(G(2,2,:)))*180/pi);
ylabel('Phase [deg.]')
xlabel('Frequency [Hz]');
grid on

print(h1, '-dpdf', [resultDir, 'RawGains/','RawGainCARM.pdf']);
saveas(h1,  [resultDir, 'RawGains/','RawGainCARM.fig']);
close(h1);

%Save data
Resp = squeeze(G(2,2,:));
filename = [resultDir, 'RawGains/', 'RawGainCARM.dat'];
fid = fopen(filename,'w');
fprintf(fid,'#f, real, imag\n');
fclose(fid);
M = [f(:), real(Resp(:)), imag(Resp(:))];
dlmwrite(filename, M, '-append');

% MICH
h1=figure(1);
subplot(2,1,1);
semilogx(f, 20*log10(squeeze(abs(G(3,3,:)))));
title('MICH Raw Gain (without feedback filter)','FontSize',12);
ylabel('Gain [dB]')
grid on
subplot(2,1,2);
semilogx(f,squeeze(angle(G(3,3,:)))*180/pi);
ylabel('Phase [deg.]')
xlabel('Frequency [Hz]');
grid on

print(h1, '-dpdf', [resultDir, 'RawGains/','RawGainMICH.pdf']);
saveas(h1,  [resultDir, 'RawGains/','RawGainMICH.fig']);
close(h1);

%Save data
Resp = squeeze(G(3,3,:));
filename = [resultDir, 'RawGains/', 'RawGainMICH.dat'];
fid = fopen(filename,'w');
fprintf(fid,'#f, real, imag\n');
fclose(fid);
M = [f(:), real(Resp(:)), imag(Resp(:))];
dlmwrite(filename, M, '-append');

% PRCL
h1=figure(1);
subplot(2,1,1);
semilogx(f, 20*log10(squeeze(abs(G(4,4,:)))));
title('PRCL Raw Gain (without feedback filter)','FontSize',12);
ylabel('Gain [dB]')
grid on
subplot(2,1,2);
semilogx(f,squeeze(angle(G(4,4,:)))*180/pi);
ylabel('Phase [deg.]')
xlabel('Frequency [Hz]');
grid on

print(h1, '-dpdf', [resultDir, 'RawGains/','RawGainPRCL.pdf']);
saveas(h1,  [resultDir, 'RawGains/','RawGainPRCL.fig']);
close(h1);

%Save data
Resp = squeeze(G(4,4,:));
filename = [resultDir, 'RawGains/', 'RawGainPRCL.dat'];
fid = fopen(filename,'w');
fprintf(fid,'#f, real, imag\n');
fclose(fid);
M = [f(:), real(Resp(:)), imag(Resp(:))];
dlmwrite(filename, M, '-append');

% SRCL
h1=figure(1);
subplot(2,1,1);
semilogx(f, 20*log10(squeeze(abs(G(5,5,:)))));
title('SRCL Raw Gain (without feedback filter)','FontSize',12);
ylabel('Gain [dB]')
grid on
subplot(2,1,2);
semilogx(f,squeeze(angle(G(5,5,:)))*180/pi);
ylabel('Phase [deg.]')
xlabel('Frequency [Hz]');
grid on

print(h1, '-dpdf', [resultDir, 'RawGains/','RawGainSRCL.pdf']);
saveas(h1,  [resultDir, 'RawGains/','RawGainSRCL.fig']);
close(h1);

%Save data
Resp = squeeze(G(5,5,:));
filename = [resultDir, 'RawGains/', 'RawGainSRCL.dat'];
fid = fopen(filename,'w');
fprintf(fid,'#f, real, imag\n');
fclose(fid);
M = [f(:), real(Resp(:)), imag(Resp(:))];
dlmwrite(filename, M, '-append');

%}}}

%{{{ Cross Couplings

%{{{ Coupling Matrix

Dcn = zeros(5,5,length(w));
for ii=[1:length(w)]
    Dcn(:,:,ii) = D(:,:,ii)*Mconv;
end

%}}}

%{{{ MICH/ DARM

% MICH to DARM
h1=figure(1);
subplot(2,1,1);
semilogx(f, 20*log10(squeeze(abs(Dcn(1,3,:))./abs(Dcn(1,1,:)))));
title('MICH to DARM Coupling','FontSize',12);
ylabel('Gain [dB]')
grid on
subplot(2,1,2);
semilogx(f,squeeze(angle(Dcn(1,3,:)))*180/pi);
ylabel('Phase [deg.]')
xlabel('Frequency [Hz]');
grid on

%}}}

%{{{ MICH to DARM

% MICH to DARM
h1=figure(1);
subplot(2,1,1);
semilogx(f, 20*log10(squeeze(abs(Dcn(1,3,:)))));
title('MICH to DARM Coupling','FontSize',12);
ylabel('Gain [dB]')
grid on
subplot(2,1,2);
semilogx(f,squeeze(angle(Dcn(1,3,:)))*180/pi);
ylabel('Phase [deg.]')
xlabel('Frequency [Hz]');
grid on

%}}}

%{{{ DARM to DARM

% MICH to DARM
h1=figure(1);
subplot(2,1,1);
semilogx(f, 20*log10(squeeze(abs(Dcn(1,1,:)))));
title('MICH to DARM Coupling','FontSize',12);
ylabel('Gain [dB]')
grid on
subplot(2,1,2);
semilogx(f,squeeze(angle(Dcn(1,3,:)))*180/pi);
ylabel('Phase [deg.]')
xlabel('Frequency [Hz]');
grid on

%}}}

%{{{ MICH to MICH

% MICH to DARM
h1=figure(1);
subplot(2,1,1);
semilogx(f, 20*log10(squeeze(abs(Dcn(3,3,:)))));
title('MICH to DARM Coupling','FontSize',12);
ylabel('Gain [dB]')
grid on
subplot(2,1,2);
semilogx(f,squeeze(angle(Dcn(3,3,:)))*180/pi);
ylabel('Phase [deg.]')
xlabel('Frequency [Hz]');
grid on

%}}}

close all

%}}}

%{{{ ====== Feedback Matrix F ========

%{{{ mkdir

mkdir([resultDir,'OpenLoopGains']);

%}}}

%{{{ DARM

%% DARM

%Fdarm = zpk([-2*pi*20,-2*pi*100, -2*pi*100],[-2*pi*1,-2*pi*800, -2*pi*800, -2*pi*1500],1);
z0 = -2*pi*20;
z1 = -2*pi*p.DARMUGF/2;
p0 = -2*pi*1;
p1 = -2*pi*4*p.DARMUGF;
p2 = -2*pi*8*p.DARMUGF;
Fdarm = zpk([z0,z1,z1],[p0,p1,p1,p2],1);
%  h = bodeplot(Fdarm);
%  p1 = getoptions(h);
%  p1.FreqUnits = 'Hz';
%  p1.Grid = 'on';
%  setoptions(h,p1);

% Set dc gain
ii = find(f >= p.DARMUGF, 1);
G0 = -sign(angle(G(1,1,ii)))/abs(G(1,1,ii)*freqresp(Fdarm,w(ii)));

set(Fdarm, 'k', G0);


% Open Loop Gain
OPLG = squeeze(G(1,1,:)).*squeeze(freqresp(Fdarm,w));
%OPLG=squeeze(G(1,1,:));
% Plot
h1=figure(1);
subplot(2,1,1);
semilogx(f, 20*log10(abs(OPLG)));
title('DARM Open Loop Gain','FontSize',12);
ylabel('Gain [dB]')
grid on
subplot(2,1,2);
semilogx(f,squeeze(angle(OPLG))*180/pi);
ylabel('Phase [deg.]')
xlabel('Frequency [Hz]');
grid on
print(h1, '-dpdf', [resultDir, 'OpenLoopGains/','OpenLoopGainDARM.pdf']);
saveas(h1,  [resultDir, 'OpenLoopGains/','OpenLoopGainDARM.fig']);
close(h1);

%Save OPLG data
filename = [resultDir, 'OpenLoopGains/', 'OpenLoopGainDARM.dat'];
fid = fopen(filename,'w');
fprintf(fid,'#f, real, imag\n');
fclose(fid);
M = [f(:), real(OPLG(:)), imag(OPLG(:))];
dlmwrite(filename, M, '-append');

% Plot Fdarm
Resp = squeeze(freqresp(Fdarm,w));
h1=figure(1);
subplot(2,1,1);
semilogx(f, 20*log10(abs(Resp)));
title('DARM Filter','FontSize',12);
ylabel('Gain [dB]')
grid on
subplot(2,1,2);
semilogx(f,squeeze(angle(Resp))*180/pi);
ylabel('Phase [deg.]')
xlabel('Frequency [Hz]');
grid on
print(h1, '-dpdf', [resultDir, 'OpenLoopGains/','FilterTF_DARM.pdf']);
saveas(h1,  [resultDir, 'OpenLoopGains/','FilterTF_DARM.fig']);
close(h1);

%Save filter response data
filename = [resultDir, 'OpenLoopGains/', 'FilterTF_DARM.dat'];
fid = fopen(filename,'w');
fprintf(fid,'#f, real, imag\n');
fclose(fid);

M = [f(:), real(Resp(:)), imag(Resp(:))];

dlmwrite(filename, M, '-append');

%}}}

%{{{ CARM

%% CARM
%Fcarm = zpk([-2*pi*2e3, -2*pi*100],[-2*pi*150e5, -2*pi*200e3, -2*pi*1], 1);
Fcarm = zpk([-2*pi*2e3],[-2*pi*500e3, -2*pi*200e3, ...
                    -2*pi*0.1, -2*pi*0.1],1);
% h = bodeplot(Fcarm);
% p1 = getoptions(h);
% p1.FreqUnits = 'Hz';
% p1.Grid = 'on';
% setoptions(h,p1);

% Set dc gain
UGF = 10000;
ii = find(f >= UGF, 1);
G0 = -sign(angle(G(2,2,ii)))/abs(G(2,2,ii)*freqresp(Fcarm,w(ii)));
set(Fcarm, 'k', G0);

% Open Loop Gain
OPLG = squeeze(G(2,2,:)).*squeeze(freqresp(Fcarm,w));

% Plot
h1=figure(1);
subplot(2,1,1);
semilogx(f, 20*log10(abs(OPLG)));
title('CARM Open Loop Gain','FontSize',12);
ylabel('Gain [dB]')
grid on
subplot(2,1,2);
semilogx(f,squeeze(angle(OPLG))*180/pi);
ylabel('Phase [deg.]')
xlabel('Frequency [Hz]');
grid on
print(h1, '-dpdf', [resultDir, 'OpenLoopGains/','OpenLoopGainCARM.pdf']);
saveas(h1,  [resultDir, 'OpenLoopGains/','OpenLoopGainCARM.fig']);
close(h1);

%Save OPLG data
filename = [resultDir, 'OpenLoopGains/', 'OpenLoopGainCARM.dat'];
fid = fopen(filename,'w');
fprintf(fid,'#f, real, imag\n');
fclose(fid);
M = [f(:), real(OPLG(:)), imag(OPLG(:))];
dlmwrite(filename, M, '-append');

% Plot Fcarm
Resp = squeeze(freqresp(Fcarm,w));
h1=figure(1);
subplot(2,1,1);
semilogx(f, 20*log10(abs(Resp)));
title('CARM Filter','FontSize',12);
ylabel('Gain [dB]')
grid on
subplot(2,1,2);
semilogx(f,squeeze(angle(Resp))*180/pi);
ylabel('Phase [deg.]')
xlabel('Frequency [Hz]');
grid on
print(h1, '-dpdf', [resultDir, 'OpenLoopGains/','FilterTF_CARM.pdf']);
saveas(h1,  [resultDir, 'OpenLoopGains/','FilterTF_CARM.fig']);
close(h1);

%Save filter response data
filename = [resultDir, 'OpenLoopGains/', 'FilterTF_CARM.dat'];
fid = fopen(filename,'w');
fprintf(fid,'#f, real, imag\n');
fclose(fid);
M = [f(:), real(Resp(:)), imag(Resp(:))];
dlmwrite(filename, M, '-append');

%}}}

%{{{ MICH

%% MICH
z1 = -2*pi*p.MICHUGF/2;
p1 = -2*pi*4*p.MICHUGF;
p2 = -2*pi*8*p.MICHUGF;

if p.DRSE
    Fmich = zpk([z1],[p1,p2],-1);
else
    Fmich = zpk([z1,z1],[p1,p1,p2],-1);
end

% h = bodeplot(Fmich);
% p1 = getoptions(h);
% p1.FreqUnits = 'Hz';
% p1.Grid = 'on';
% setoptions(h,p1);

% Set dc gain
ii = find(f >= p.MICHUGF, 1);
G0 = -sign(angle(G(3,3,ii)))/abs(G(3,3,ii)*freqresp(Fmich,w(ii)));
set(Fmich, 'k', G0);

% Open Loop Gain
OPLG = squeeze(G(3,3,:)).*squeeze(freqresp(Fmich,w));

% Plot
h1=figure(1);
subplot(2,1,1);
semilogx(f, 20*log10(abs(OPLG)));
title('MICH Open Loop Gain','FontSize',12);
ylabel('Gain [dB]')
grid on
subplot(2,1,2);
semilogx(f,squeeze(angle(OPLG))*180/pi);
ylabel('Phase [deg.]')
xlabel('Frequency [Hz]');
grid on
print(h1, '-dpdf', [resultDir,'OpenLoopGains/', 'OpenLoopGainMICH.pdf']);
saveas(h1,  [resultDir,'OpenLoopGains/', 'OpenLoopGainMICH.fig']);
close(h1);

%Save OPLG data
filename = [resultDir, 'OpenLoopGains/', 'OpenLoopGainMICH.dat'];
fid = fopen(filename,'w');
fprintf(fid,'#f, real, imag\n');
fclose(fid);
M = [f(:), real(OPLG(:)), imag(OPLG(:))];
dlmwrite(filename, M, '-append');

% Plot Fmich
Resp = squeeze(freqresp(Fmich,w));
h1=figure(1);
subplot(2,1,1);
semilogx(f, 20*log10(abs(Resp)));
title('MICH Filter','FontSize',12);
ylabel('Gain [dB]')
grid on
subplot(2,1,2);
semilogx(f,squeeze(angle(Resp))*180/pi);
ylabel('Phase [deg.]')
xlabel('Frequency [Hz]');
grid on
print(h1, '-dpdf', [resultDir, 'OpenLoopGains/','FilterTF_MICH.pdf']);
saveas(h1,  [resultDir, 'OpenLoopGains/','FilterTF_MICH.fig']);
close(h1);

%Save filter response data
filename = [resultDir, 'OpenLoopGains/', 'FilterTF_MICH.dat'];
fid = fopen(filename,'w');
fprintf(fid,'#f, real, imag\n');
fclose(fid);
M = [f(:), real(Resp(:)), imag(Resp(:))];
dlmwrite(filename, M, '-append');

%}}}

%{{{ PRCL

%% PRCL
z1 = -2*pi*p.PRCLUGF/2;
p1 = -2*pi*4*p.PRCLUGF;
p2 = -2*pi*8*p.PRCLUGF;
Fprc = zpk([z1,z1],[p1,p1,p2],1);
% h = bodeplot(Fprc);
% p1 = getoptions(h);
% p1.FreqUnits = 'Hz';
% p1.Grid = 'on';
% setoptions(h,p1);

% Set dc gain
ii = find(f >= p.PRCLUGF, 1);
G0 = -sign(angle(G(4,4,ii)))/abs(G(4,4,ii)*freqresp(Fprc,w(ii)));
set(Fprc, 'k', G0);

% Open Loop Gain
OPLG = squeeze(G(4,4,:)).*squeeze(freqresp(Fprc,w));

% Plot
h1=figure(1);
subplot(2,1,1);
semilogx(f, 20*log10(abs(OPLG)));
title('PRC Open Loop Gain','FontSize',12);
ylabel('Gain [dB]')
grid on
subplot(2,1,2);
semilogx(f,squeeze(angle(OPLG))*180/pi);
ylabel('Phase [deg.]')
xlabel('Frequency [Hz]');
grid on
print(h1, '-dpdf', [resultDir,'OpenLoopGains/', 'OpenLoopGainPRC.pdf']);
saveas(h1,  [resultDir,'OpenLoopGains/', 'OpenLoopGainPRC.fig']);
close(h1);

%Save OPLG data
filename = [resultDir, 'OpenLoopGains/', 'OpenLoopGainPRC.dat'];
fid = fopen(filename,'w');
fprintf(fid,'#f, real, imag\n');
fclose(fid);
M = [f(:), real(OPLG(:)), imag(OPLG(:))];
dlmwrite(filename, M, '-append');

% Plot Fprc
Resp = squeeze(freqresp(Fprc,w));
h1=figure(1);
subplot(2,1,1);
semilogx(f, 20*log10(abs(Resp)));
title('PRC Filter','FontSize',12);
ylabel('Gain [dB]')
grid on
subplot(2,1,2);
semilogx(f,squeeze(angle(Resp))*180/pi);
ylabel('Phase [deg.]')
xlabel('Frequency [Hz]');
grid on
print(h1, '-dpdf', [resultDir, 'OpenLoopGains/','FilterTF_PRC.pdf']);
saveas(h1,  [resultDir, 'OpenLoopGains/','FilterTF_PRC.fig']);
close(h1);

%Save filter response data
filename = [resultDir, 'OpenLoopGains/', 'FilterTF_PRC.dat'];
fid = fopen(filename,'w');
fprintf(fid,'#f, real, imag\n');
fclose(fid);
M = [f(:), real(Resp(:)), imag(Resp(:))];
dlmwrite(filename, M, '-append');


%}}}

%{{{ SRCL

%% SRCL
z1 = -2*pi*p.SRCLUGF/2;
p1 = -2*pi*4*p.SRCLUGF;
p2 = -2*pi*8*p.SRCLUGF;
Fsrc = zpk([z1,z1],[p1,p1,p2],-1);

h = bodeplot(Fsrc);
p1 = getoptions(h);
p1.FreqUnits = 'Hz';
p1.Grid = 'on';
setoptions(h,p1);

% Set dc gain
ii = find(f >= p.SRCLUGF, 1);
G0 = -sign(angle(G(5,5,ii)))/abs(G(5,5,ii)*freqresp(Fsrc,w(ii)));
set(Fsrc, 'k', G0);

% Open Loop Gain
OPLG = squeeze(G(5,5,:)).*squeeze(freqresp(Fsrc,w));

% Plot
h1=figure(1);
subplot(2,1,1);
semilogx(f, 20*log10(abs(OPLG)));
title('SRC Open Loop Gain','FontSize',12);
ylabel('Gain [dB]')
grid on
subplot(2,1,2);
semilogx(f,squeeze(angle(OPLG))*180/pi);
ylabel('Phase [deg.]')
xlabel('Frequency [Hz]');
grid on
print(h1, '-dpdf', [resultDir,'OpenLoopGains/', 'OpenLoopGainSRC.pdf']);
saveas(h1,  [resultDir,'OpenLoopGains/', 'OpenLoopGainSRC.fig']);
close(h1);

%Save OPLG data
filename = [resultDir, 'OpenLoopGains/', 'OpenLoopGainSRC.dat'];
fid = fopen(filename,'w');
fprintf(fid,'#f, real, imag\n');
fclose(fid);
M = [f(:), real(OPLG(:)), imag(OPLG(:))];
dlmwrite(filename, M, '-append');

% Plot Fprc
Resp = squeeze(freqresp(Fsrc,w));
h1=figure(1);
subplot(2,1,1);
semilogx(f, 20*log10(abs(Resp)));
title('SRC Filter','FontSize',12);
ylabel('Gain [dB]')
grid on
subplot(2,1,2);
semilogx(f,squeeze(angle(Resp))*180/pi);
ylabel('Phase [deg.]')
xlabel('Frequency [Hz]');
grid on
print(h1, '-dpdf', [resultDir, 'OpenLoopGains/','FilterTF_SRC.pdf']);
saveas(h1,  [resultDir, 'OpenLoopGains/','FilterTF_SRC.fig']);
close(h1);

%Save filter response data
filename = [resultDir, 'OpenLoopGains/', 'FilterTF_SRC.dat'];
fid = fopen(filename,'w');
fprintf(fid,'#f, real, imag\n');
fclose(fid);
M = [f(:), real(Resp(:)), imag(Resp(:))];
dlmwrite(filename, M, '-append');

%}}}

%{{{ F matrix

%% F matrix

F = zeros(5,5,length(w));
F(1,1,:) = squeeze(freqresp(Fdarm, w));
F(2,2,:) = squeeze(freqresp(Fcarm, w));
F(3,3,:) = squeeze(freqresp(Fmich, w));
F(4,4,:) = squeeze(freqresp(Fprc, w));
F(5,5,:) = squeeze(freqresp(Fsrc, w));

%}}}

%}}}

%{{{ Shot Noise Plot

%% Shot Noise Plot
mkdir([resultDir,'LoopNoise']);

% Shot noise vector
NSall = noiseAC(iPrb, :);
NSdarm = zeros(5, length(w));
NSdarm(1,:) = NSall(1,:);
NScarm = zeros(5, length(w));
NScarm(2,:) = NSall(2,:);
NSmich = zeros(5, length(w));
NSmich(3,:) = NSall(3,:);
NSprc = zeros(5, length(w));
NSprc(4,:) = NSall(4,:);
NSsrc = zeros(5, length(w));
NSsrc(5,:) = NSall(5,:);

h1 = figure(1);
loglog(f,NSall(1,:), f,NSall(2,:),f,NSall(3,:),f,NSall(4,:),f,NSall(5,:));
legend('DARM','CARM','MICH','PRC','SRC');
title('Shot Noise at each signal port','FontSize',16);
xlabel('Hz','FontSize',16);
h=ylabel('W/sqrt(Hz)','FontSize',16);
grid on
print(h1, '-dpdf', [resultDir, 'LoopNoise/', 'ShotNoises.pdf']);
saveas(h1,  [resultDir, 'LoopNoise/', 'ShotNoises.fig']);
close(h1);

%}}}

%{{{ ===== Displacement Equivalent Quantum Noises  ====

%% ===== Displacement Equivalent Quantum Noises  ====

XqnDARM=zeros(length(w),1);
OptgDARM=zeros(length(w),1);
XqnCARM=zeros(length(w),1);
OptgCARM=zeros(length(w),1);
XqnMICH=zeros(length(w),1);
OptgMICH=zeros(length(w),1);
XqnPRCL=zeros(length(w),1);
OptgPRCL=zeros(length(w),1);
XqnSRCL=zeros(length(w),1);
OptgSRCL=zeros(length(w),1);
for ii = 1:length(w)
    OptgDARM(ii) = D(1,:,ii)*Mconv2*[1;0;0;0;0];
    XqnDARM(ii) = NSall(1,ii)./OptgDARM(ii);
    OptgCARM(ii) = D(2,:,ii)*Mconv2*[0;1;0;0;0];
    XqnCARM(ii) = NSall(2,ii)./OptgCARM(ii);
    OptgMICH(ii) = D(3,:,ii)*Mconv2*[0;0;1;0;0];
    XqnMICH(ii) = NSall(3,ii)./OptgMICH(ii);
    OptgPRCL(ii) = D(4,:,ii)*Mconv2*[0;0;0;1;0];
    XqnPRCL(ii) = NSall(4,ii)./OptgPRCL(ii);
    OptgSRCL(ii) = D(5,:,ii)*Mconv2*[0;0;0;0;1];
    XqnSRCL(ii) = NSall(5,ii)./OptgSRCL(ii);
end

%Plot
h1 = figure(1);
loglog(f,abs(XqnDARM),f,abs(XqnCARM),f,abs(XqnMICH),f,abs(XqnPRCL),f,abs(XqnSRCL));
legend('DARM','CARM','MICH','PRC','SRC');
title('Displacement Equivalent Shot Noise','FontSize',16);
xlabel('Hz','FontSize',16);
h=ylabel('m/sqrt(Hz)','FontSize',16);
grid on
print(h1, '-dpdf', [resultDir, 'LoopNoise/', 'DisplacementEquivalentQuantumNoises.pdf']);
saveas(h1,  [resultDir, 'LoopNoise/', 'DisplacementEquivalentQuantumNoises.fig']);
close(h1);

%Save
fid = fopen([resultDir, 'LoopNoise/', 'DisplacementEquivalentQuantumNoises.dat'],'w');
fprintf(fid,'#f, DARM, CARM, MICH, PRC, SRC\n');
fclose(fid);

M = [f', XqnDARM, XqnCARM, XqnMICH, XqnPRCL, XqnSRCL];
dlmwrite([resultDir, 'LoopNoise/', 'DisplacementEquivalentQuantumNoises.dat'], M, '-append');

%}}}

%{{{ === Shot noise coupling ===

%% === Shot noise coupling ===

% Loop for w
Eall = zeros(5,length(w));
Edarm = zeros(5,length(w));
Ecarm = zeros(5,length(w));
Emich = zeros(5,length(w));
Eprc = zeros(5,length(w));
Esrc = zeros(5,length(w));
calib = zeros(1,length(w));
Hnf = zeros(5,5,length(w));
Gnf = zeros(5,5,length(w));

for ii = [1:length(w)]
    H = inv(eye(5)+S*D(:,:,ii)*A(:,:,ii)*F(:,:,ii));
    Hnf(:,:,ii) = H;
    Gnf(:,:,ii) = eye(5)+S*D(:,:,ii)*A(:,:,ii)*F(:,:,ii);
    Edarm(:,ii) = H*S*NSdarm(:,ii);
    Ecarm(:,ii) = H*S*NScarm(:,ii);
    Emich(:,ii) = H*S*NSmich(:,ii);
    Eprc(:,ii) = H*S*NSprc(:,ii);
    Esrc(:,ii) = H*S*NSsrc(:,ii);
    Eall(:,ii) = sqrt(abs(Edarm(:,ii)).^2+abs(Ecarm(:,ii)).^2+abs(Emich(:,ii)).^2+...
                      abs(Eprc(:,ii)).^2+abs(Esrc(:,ii)).^2);
    calib(ii) = 3000*[1,0,0,0,0]*H*S*D(:,:,ii)*Mconv*[1;0;0;0;0];
end

h1 = figure(1);
loglog(f, abs(Eall(1,:))./abs(calib), f, abs(Edarm(1,:))./abs(calib),...
    f, abs(Ecarm(1,:))./abs(calib), f, abs(Emich(1,:))./abs(calib),...
    f, abs(Eprc(1,:))./abs(calib),f, abs(Esrc(1,:))./abs(calib));
legend('Total','DARM','CARM','MICH','PRC','SRC');
title('Shot Noise Coupling','FontSize',16);
xlabel('Hz','FontSize',16);
h=ylabel('1/sqrt(Hz)','FontSize',16);
grid on
print(h1, '-dpdf', [resultDir, 'LoopNoise/', 'ShotNoiseCoupling.pdf']);
saveas(h1,  [resultDir, 'LoopNoise/', 'ShotNoiseCoupling.fig']);
close(h1);

fid = fopen([resultDir, 'LoopNoise/', 'ShotNoiseCoupling.dat'],'w');
fprintf(fid,'#f,Total, DARM, CARM, MICH, PRC, SRC\n');
fclose(fid);

M = [f(:), (abs(Eall(1,:))./abs(calib))', (abs(Edarm(1,:))./abs(calib))',...
    (abs(Ecarm(1,:))./abs(calib))', (abs(Emich(1,:))./abs(calib))',...
    (abs(Eprc(1,:))./abs(calib))', (abs(Esrc(1,:))./abs(calib))'];

dlmwrite([resultDir, 'LoopNoise/', 'ShotNoiseCoupling.dat'],M, '-append');

%}}}

%{{{ ==== PD noise ===

%% ==== PD noise ===
% 1. ASDC, 2. REFL1, 3. REFL2, 4. POP1, 5. POP2
% 6. REFL1Dm, 7. REFL1Dp, 8. REFL2Dm, 9. REFL2Dp

PDNdarm = zeros(5, length(w));
[name,idx] = sigPortToPD(p.signalPorts{1});
PDNdarm(1,:) = DCsigPower(idx)/p.DCPDDrange * ones(length(w),1); % in W

PDNcarm = zeros(5, length(w));
[name,idx] = sigPortToPD(p.signalPorts{2});
PDNcarm(2,:) = DCsigPower(idx)/p.RFPDDrange * ones(length(w),1); % in W

PDNmich = zeros(5, length(w));
[name,idx] = sigPortToPD(p.signalPorts{3});
PDNmich(3,:) = DCsigPower(idx)/p.RFPDDrange * ones(length(w),1); % in W

PDNprc = zeros(5, length(w));
[name,idx] = sigPortToPD(p.signalPorts{4});
PDNprc(4,:) = DCsigPower(idx)/p.RFPDDrange * ones(length(w),1); % in W

PDNsrc = zeros(5, length(w));
[name,idx] = sigPortToPD(p.signalPorts{5});
PDNsrc(5,:) = DCsigPower(idx)/p.RFPDDrange * ones(length(w),1); % in W


%}}}

%{{{ ==== PD Noise Coupling ===

%% ==== PD Noise Coupling ===

% Loop for w
Eall = zeros(5,length(w));
Edarm = zeros(5,length(w));
Ecarm = zeros(5,length(w));
Emich = zeros(5,length(w));
Eprc = zeros(5,length(w));
Esrc = zeros(5,length(w));
calib = zeros(1,length(w));

for ii = [1:length(w)]
    H = inv(eye(5)+S*D(:,:,ii)*A(:,:,ii)*F(:,:,ii));
    Edarm(:,ii) = H*S*PDNdarm(:,ii);
    Ecarm(:,ii) = H*S*PDNcarm(:,ii);
    Emich(:,ii) = H*S*PDNmich(:,ii);
    Eprc(:,ii) = H*S*PDNprc(:,ii);
    Esrc(:,ii) = H*S*PDNsrc(:,ii);
    Eall(:,ii) = sqrt(abs(Edarm(:,ii)).^2+abs(Ecarm(:,ii)).^2+abs(Emich(:,ii)).^2+...
                      abs(Eprc(:,ii)).^2+abs(Esrc(:,ii)).^2);
    calib(ii) = 3000*[1,0,0,0,0]*H*S*D(:,:,ii)*Mconv*[1;0;0;0;0];
end

h1 = figure(1);
loglog(f, pureQN, f, abs(Eall(1,:))./abs(calib), f, abs(Edarm(1,:))./abs(calib),...
    f, abs(Ecarm(1,:))./abs(calib), f, abs(Emich(1,:))./abs(calib),...
    f, abs(Eprc(1,:))./abs(calib),f, abs(Esrc(1,:))./abs(calib));
legend('Target QN','Total','DARM','CARM','MICH','PRC','SRC');
title('PD Noise Coupling','FontSize',16);
xlabel('Hz','FontSize',16);
h=ylabel('1/sqrt(Hz)','FontSize',16);
grid on
print(h1, '-dpdf', [resultDir, 'LoopNoise/', 'PDNoiseCoupling.pdf']);
saveas(h1,  [resultDir, 'LoopNoise/', 'PDNoiseCoupling.fig']);
close(h1);

fid = fopen([resultDir, 'LoopNoise/', 'PDNoiseCoupling.dat'],'w');
fprintf(fid,'#f,Total, DARM, CARM, MICH, PRC, SRC\n');
fclose(fid);

M = [f(:), (abs(Eall(1,:))./abs(calib))', (abs(Edarm(1,:))./abs(calib))',...
    (abs(Ecarm(1,:))./abs(calib))', (abs(Emich(1,:))./abs(calib))',...
    (abs(Eprc(1,:))./abs(calib))', (abs(Esrc(1,:))./abs(calib))'];

dlmwrite([resultDir, 'LoopNoise/', 'PDNoiseCoupling.dat'],M, '-append');

%}}}

%{{{ Feed forward

%% Feed forward


F2 = zeros(5,5,length(w));

%Measure the TFs
H = zeros(5,5,length(w));

for ii = [1:length(w)]
    G = S*D(:,:,ii)*A(:,:,ii)*F(:,:,ii);
    H(:,:,ii) = inv(eye(5)+G);
    
    %TF from MICH error to DARM error
    Vtmp = G*[0;0;1;0;0];
    TFmich = Vtmp(1);
    Vtmp = G*[0;0;0;1;0];
    TFprc = Vtmp(1);
    Vtmp = G*[0;0;0;0;1];
    TFsrc = Vtmp(1);
    
    %TF from DARM fb to DARM error
    Vtmp = S*D(:,:,ii)*A(:,:,ii)*[1;0;0;0;0];
    TFdarm = Vtmp(1);
    
    %Feed forward matrix
    
    FF = zeros(5,5);
    FF(1,3) = -TFmich/TFdarm;
    FF(1,4) = -TFprc/TFdarm;
    FF(1,5) = -TFsrc/TFdarm;
    
    % Update the filter matrix F
    a=p.FeedForwardError;%*(2*rand-1);
    b=0*p.FeedForwardError;%*(2*rand-1);
    F2(:,:,ii) = F(:,:,ii) + FF*(1+a+i*b);
end

%}}}

%{{{ Loop Noise with FF

%% Loop Noise with FF
% Calculate the loop noise again
% Loop for w
Eall = zeros(5,length(w));
Edarm = zeros(5,length(w));
Ecarm = zeros(5,length(w));
Emich = zeros(5,length(w));
Eprc = zeros(5,length(w));
Esrc = zeros(5,length(w));
calib = zeros(1,length(w));

for ii = [1:length(w)]
    H(:,:,ii) = inv(eye(5)+S*D(:,:,ii)*A(:,:,ii)*F2(:,:,ii));
    
    Edarm(:,ii) = H(:,:,ii)*S*NSdarm(:,ii);
    Ecarm(:,ii) = H(:,:,ii)*S*NScarm(:,ii);
    Emich(:,ii) = H(:,:,ii)*S*NSmich(:,ii);
    Eprc(:,ii) = H(:,:,ii)*S*NSprc(:,ii);
    Esrc(:,ii) = H(:,:,ii)*S*NSsrc(:,ii);
    Eall(:,ii) = sqrt(abs(Edarm(:,ii)).^2+abs(Ecarm(:,ii)).^2+abs(Emich(:,ii)).^2+...
                      abs(Eprc(:,ii)).^2+abs(Esrc(:,ii)).^2);
    calib(ii) = 3000*[1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*Mconv*[1;0;0;0;0];
end

%
h1 = figure(1);
loglog(f, abs(Eall(1,:))./abs(calib), f, abs(Edarm(1,:))./abs(calib),...
    f, abs(Ecarm(1,:))./abs(calib), f, abs(Emich(1,:))./abs(calib),...
    f, abs(Eprc(1,:))./abs(calib),f, abs(Esrc(1,:))./abs(calib));
legend('Total','DARM','CARM','MICH','PRC','SRC');
title('Shot Noise Coupling with Feed Forward','FontSize',16);
xlabel('Hz','FontSize',16);
h=ylabel('1/sqrt(Hz)','FontSize',16);
grid on
print(h1, '-dpdf', [resultDir, 'LoopNoise/', 'ShotNoiseCouplingWithFF.pdf']);
saveas(h1,  [resultDir, 'LoopNoise/', 'ShotNoiseCouplingWithFF.fig']);
close(h1);

fid = fopen([resultDir, 'LoopNoise/', 'ShotNoiseCouplingWithFF.dat'],'w');
fprintf(fid,'#Total, DARM, CARM, MICH, PRC, SRC\n');
fclose(fid);

M = [f(:), (abs(Eall(1,:))./abs(calib))', (abs(Edarm(1,:))./abs(calib))',...
    (abs(Ecarm(1,:))./abs(calib))', (abs(Emich(1,:))./abs(calib))',...
    (abs(Eprc(1,:))./abs(calib))', (abs(Esrc(1,:))./abs(calib))'];

dlmwrite([resultDir, 'LoopNoise/', 'ShotNoiseCouplingWithFF.dat'],M, '-append');

 
%}}}

%{{{ PD Noise with FF

%% PD Noise with FF
% Calculate the loop noise again
% Loop for w
QNdarm = zeros(5,length(w));
Eall = zeros(5,length(w));
Edarm = zeros(5,length(w));
Ecarm = zeros(5,length(w));
Emich = zeros(5,length(w));
Eprc = zeros(5,length(w));
Esrc = zeros(5,length(w));
calib = zeros(1,length(w));

for ii = [1:length(w)]
    H(:,:,ii) = inv(eye(5)+S*D(:,:,ii)*A(:,:,ii)*F2(:,:,ii));
    
    QNdarm(:,ii) = H(:,:,ii)*S*NSdarm(:,ii);
    Edarm(:,ii) = H(:,:,ii)*S*PDNdarm(:,ii);
    Ecarm(:,ii) = H(:,:,ii)*S*PDNcarm(:,ii);
    Emich(:,ii) = H(:,:,ii)*S*PDNmich(:,ii);
    Eprc(:,ii) = H(:,:,ii)*S*PDNprc(:,ii);
    Esrc(:,ii) = H(:,:,ii)*S*PDNsrc(:,ii);
    Eall(:,ii) = sqrt(abs(Edarm(:,ii)).^2+abs(Ecarm(:,ii)).^2+abs(Emich(:,ii)).^2+...
                      abs(Eprc(:,ii)).^2+abs(Esrc(:,ii)).^2+abs(QNdarm(:,ii)).^2);
    calib(ii) = 3000*[1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*Mconv*[1;0;0;0;0];
end


h1 = figure(1);
loglog(f, pureQN, f, abs(Eall(1,:))./abs(calib), f, abs(Edarm(1,:))./abs(calib),...
    f, abs(Ecarm(1,:))./abs(calib), f, abs(Emich(1,:))./abs(calib),...
    f, abs(Eprc(1,:))./abs(calib),f, abs(Esrc(1,:))./abs(calib));
legend('Target QN','Total','DARM','CARM','MICH','PRC','SRC');
title('PD Noise Coupling with Feed Forward','FontSize',16);
xlabel('Hz','FontSize',16);
h=ylabel('1/sqrt(Hz)','FontSize',16);
grid on
set(gca, 'YLim',[1e-30,1e-21])
print(h1, '-dpdf', [resultDir, 'LoopNoise/', 'PDNoiseCouplingWithFF.pdf']);
saveas(h1,  [resultDir, 'LoopNoise/', 'PDNoiseCouplingWithFF.fig']);
close(h1);

fid = fopen([resultDir, 'LoopNoise/', 'PDNoiseCouplingWithFF.dat'],'w');
fprintf(fid,'#Total, DARM, CARM, MICH, PRC, SRC\n');
fclose(fid);

M = [f(:), (abs(Eall(1,:))./abs(calib))', (abs(Edarm(1,:))./abs(calib))',...
    (abs(Ecarm(1,:))./abs(calib))', (abs(Emich(1,:))./abs(calib))',...
    (abs(Eprc(1,:))./abs(calib))', (abs(Esrc(1,:))./abs(calib))'];

dlmwrite([resultDir, 'LoopNoise/', 'PDNoiseCouplingWithFF.dat'],M, '-append');


%}}}

%{{{ CARM Response with FF

Ecarm = zeros(5,length(w));
EcarmETM = zeros(5,length(w));
Ecarmnfb = zeros(5,length(w));
EcarmnfbETM = zeros(5,length(w));
TEMP = zeros(15,length(w));
F3 = F2;
F3(2,2,:) = 0;
calib = zeros(length(w),1);
for ii = [1:length(w)]
    H = inv(eye(5)+S*D(:,:,ii)*A(:,:,ii)*F3(:,:,ii));
    H2 = inv(eye(5)+S*D(:,:,ii)*A2(:,:,ii)*F3(:,:,ii));    
    Ecarm(:,ii) = H*S*D(:,:,ii)*A(:,:,ii)*[0,1,0,0,0]';
    EcarmETM(:,ii) = H2*S*D(:,:,ii)*A2(:,:,ii)*[0,1,0,0,0]';    
    Ecarmnfb(:,ii) = S*D(:,:,ii)*A(:,:,ii)*[0,1,0,0,0]';    
    EcarmnfbETM(:,ii) = S*D(:,:,ii)*A2(:,:,ii)*[0,1,0,0,0]';    
    TEMP(:,ii) = D(1,1,ii);
    calib(ii) = 3000*[1,0,0,0,0]*H*S*D(:,:,ii)*Mconv*[1;0;0;0;0];    
end

TSUSP = squeeze(Mtf(1,1,:));

save(['CARM-Resp.mat'],'f','w','Ecarm','Ecarmnfb', 'EcarmETM', 'EcarmnfbETM', ...
     'TSUSP', 'calib');
%save(['CARM-Resp-Simple-Low.mat'],'f','w','Ecarmnfb',  'EcarmnfbETM')


%}}}

%{{{ plot

figure(1)
loglog(f, abs(Ecarmnfb(2,:)))
figure(2)
semilogx(f, 180/pi*angle(Ecarmnfb(2,:))) 

%}}}

%}}}

%{{{ ===== Noise Coupling =====

%{{{ Mirror Displacement Noise
mkdir([resultDir,'DisplacementNoise']);

Getmx = zeros(length(w), 1);
Getmy = zeros(length(w), 1);
Gitmx = zeros(length(w), 1);
Gitmy = zeros(length(w), 1);
Gbs = zeros(length(w), 1);
Gprm = zeros(length(w), 1);
Gsrm = zeros(length(w), 1);

for ii = [1:length(w)]
    H(:,:,ii) = inv(eye(5)+S*D(:,:,ii)*A(:,:,ii)*F2(:,:,ii));
    C = 3000*[1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*Mconv*[1;0;0;0;0];
    Getmx(ii) = abs([1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*vActETMX/C);
    Getmy(ii) = abs([1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*vActETMY/C);
    Gitmx(ii) = abs([1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*vActITMX/C);
    Gitmy(ii) = abs([1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*vActITMY/C);
    Gbs(ii) = abs([1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*vActBS/C);
    Gprm(ii) = abs([1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*vActPRM/C);
    Gsrm(ii) = abs([1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*vActSRM/C);
end

%
h1 = figure(1);
loglog(f, Getmx, f, Getmy, f, Gitmx, f, Gitmy, f, Gbs, f, Gprm, f, Gsrm);

legend('ETMX','ETMY','ITMX','ITMY','BS','PRM','SRM');
title('Displacement Noise Coupling','FontSize',16);
xlabel('Hz','FontSize',16);
h=ylabel('G [1/m]','FontSize',16);
grid on
print(h1, '-dpdf', [resultDir, 'DisplacementNoise/', 'DisplacementNoiseCouplingFF.pdf']);
saveas(h1,  [resultDir, 'DisplacementNoise/', 'DisplacementNoiseCouplingFF.fig']);
close(h1);

fid = fopen([resultDir, 'DisplacementNoise/', ...
             'DisplacementNoiseCouplingFF.dat'],'w');
fprintf(fid,['#Displacement noise coupling G: Multiply G with the displacement ' ...
             'noise spectrum of each mirror in m/sqrt(Hz) to get the ' ...
             'strain equivalent DARM noise.\n']);
fprintf(fid,'#freq,ETMX,ETMY,ITMX,ITMY,BS,PRM,SRM\n');
fclose(fid);

M = [f(:), Getmx, Getmy, Gitmx, Gitmy, Gbs, Gprm, Gsrm];

dlmwrite([resultDir, 'DisplacementNoise/', 'DisplacementNoiseCouplingFF.dat'],M, '-append');

if p.useMZI
    
    if p.MZIM2
        mkdir([resultDir,'MZI']);
        
        Gmzim1 = zeros(length(w), 1);
        Gmzim2 = zeros(length(w), 1);
        Gmzim4 = zeros(length(w), 1);
        for ii = [1:length(w)]
            Gmzim1(ii) = abs([1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*vActMZIM1/C);
            Gmzim2(ii) = abs([1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*vActMZIM2/C);
            Gmzim4(ii) = abs([1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*vActMZIM4/C);
        end
        
        h1 = figure(1);
        loglog(f, Gmzim1, f, Gmzim2, f, Gmzim4);
        
        legend('MZIM1(mid-fringe)', 'MZIM2(mid-fringe)', 'MZIM4(dark-fringe)');
        title('MZI-Displacement Noise Coupling', 'FontSize', 16);
        xlabel('Hz', 'FontSize', 16);
        h=ylabel('G [1/m]', 'FontSize',16);
        grid on
        print(h1, '-dpdf', [resultDir, 'MZI/', 'DisplacementNoiseCouplingFF.pdf']);
        saveas(h1,  [resultDir, 'MZI/', 'DisplacementNoiseCouplingFF.fig']);
        close(h1);
        
        fid = fopen([resultDir, 'MZI/', ...
                 'DisplacementNoiseCouplingFF.dat'],'w');
        fprintf(fid,['#Displacement noise coupling G: Multiply G with the displacement ' ...
                 'noise spectrum of each mirror in m/sqrt(Hz) to get the ' ...
                 'strain equivalent DARM noise.\n']);
        fprintf(fid,'#freq,MZIM1(mid),MZIM2(mid), MZIM4(dark)\n');
        fclose(fid);
    
        M = [f(:), Gmzim1, Gmzim2, Gmzim4];
    
        dlmwrite([resultDir, 'MZI/', 'DisplacementNoiseCouplingFF.dat'],M, '-append');
    else
    
        mkdir([resultDir,'MZI']);
        
        Gmzim1 = zeros(length(w), 1);
        Gmzim4 = zeros(length(w), 1);
        for ii = [1:length(w)]
            Gmzim1(ii) = abs([1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*vActMZIM1/C);
            Gmzim4(ii) = abs([1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*vActMZIM4/C);
        end
        
        h1 = figure(1);
        loglog(f, Gmzim1, f, Gmzim4);
        
        legend('MZIM1(mid-fringe)', 'MZIM4(dark-fringe)');
        title('MZI-Displacement Noise Coupling', 'FontSize', 16);
        xlabel('Hz', 'FontSize', 16);
        h=ylabel('G [1/m]', 'FontSize',16);
        grid on
        print(h1, '-dpdf', [resultDir, 'MZI/', 'DisplacementNoiseCouplingFF.pdf']);
        saveas(h1,  [resultDir, 'MZI/', 'DisplacementNoiseCouplingFF.fig']);
        close(h1);
        
        fid = fopen([resultDir, 'MZI/', ...
                 'DisplacementNoiseCouplingFF.dat'],'w');
        fprintf(fid,['#Displacement noise coupling G: Multiply G with the displacement ' ...
              'noise spectrum of each mirror in m/sqrt(Hz) to get the ' ...
              'strain equivalent DARM noise.\n']);
        fprintf(fid,'#freq,MZIM1(mid),MZIM4(dark)\n');
        fclose(fid);
    
        M = [f(:), Gmzim1, Gmzim4];
    
        dlmwrite([resultDir, 'MZI/', 'DisplacementNoiseCouplingFF.dat'],M, '-append');
    end
end

%}}}

%{{{ Laser Noise Couplings
mkdir([resultDir,'LaserNoise']);

Gfreq = zeros(length(w),1);
Grin = zeros(length(w),1);


for ii = [1:length(w)]
    H(:,:,ii) = inv(eye(5)+1*S*D(:,:,ii)*A(:,:,ii)*F2(:,:,ii));
    C = 3000*[1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*Mconv*[1;0;0;0;0];
    Gfreq(ii) = abs([1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*vActPM/f(ii)/C);
    Grin(ii) = abs([1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*vActAM/2/C);
end
%
h1 = figure(1);
loglog(f, Gfreq);

title('Laser Frequency Noise Coupling','FontSize',16);
xlabel('Hz', 'FontSize', 16);
h=ylabel('|G| [1/Hz]','FontSize',16);
grid on
print(h1, '-dpdf', [resultDir, 'LaserNoise/', 'LaserFreqNoiseCouplingFF.pdf']);
saveas(h1,  [resultDir, 'LaserNoise/', 'LaserFreqNoiseCouplingFF.fig']);
close(h1);

h1 = figure(1);
loglog(f, Grin);

title('Laser Intensity Noise Coupling','FontSize',16);
xlabel('Hz', 'FontSize', 16);
h=ylabel('|G|','FontSize',16);
grid on
print(h1, '-dpdf', [resultDir, 'LaserNoise/', 'LaserIntNoiseCouplingFF.pdf']);
saveas(h1,  [resultDir, 'LaserNoise/', 'LaserIntNoiseCouplingFF.fig']);
close(h1);

fid = fopen([resultDir, 'LaserNoise/', 'LaserNoiseCouplingFF.dat'],'w');
fprintf(fid,['#Laser noise coupling G: Multiply G with the noise spectra ' ...
             'of laser noises to get the equivalent DARM strain noise in 1/sqrt(Hz).\n']);
fprintf(fid,'#freq, Freq. Noise [1/Hz], RIN\n');
fclose(fid);

M = [f(:), Gfreq, Grin];

dlmwrite([resultDir, 'LaserNoise/', 'LaserNoiseCouplingFF.dat'],M, '-append');


%}}}

%{{{ Oscillator Noise
mkdir([resultDir,'OscillatorNoise']);

Gf1PM = zeros(length(w),1);
Gf1AM = zeros(length(w),1);
Gf2PM = zeros(length(w),1);
Gf2AM = zeros(length(w),1);
Gf3PM = zeros(length(w),1);
Gf3AM = zeros(length(w),1);
if and(~p.useMZI,~p.RAM)
    for ii = [1:length(w)]
        H(:,:,ii) = inv(eye(5)+1*S*D(:,:,ii)*A(:,:,ii)*F2(:,:,ii));
        C = 3000*[1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*Mconv*[1;0;0;0;0];
        Gf1PM(ii) = abs([1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*vActf1PM/C);
        Gf1AM(ii) = abs([1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*vActf1AM/C);
        Gf2PM(ii) = abs([1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*vActf2PM/C);
        Gf2AM(ii) = abs([1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*vActf2AM/C);
        Gf3PM(ii) = abs([1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*vActf3PM/C);
        Gf3AM(ii) = abs([1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*vActf3AM/C);
    end
    %
    
    %{{{ Phase noise
    
    h1 = figure(1);
    loglog(f, Gf1PM,f, Gf2PM,f, Gf3PM);
    
    legend('f1','f2', 'f3');
    title('Oscillator Phase Noise Coupling','FontSize',16);
    xlabel('Hz', 'FontSize', 16);
    h=ylabel('|G| 1/rad','FontSize',16);
    grid on
    print(h1, '-dpdf', [resultDir, 'OscillatorNoise/', 'OscillatorPhaseNoiseCouplingFF.pdf']);
    saveas(h1,  [resultDir, 'OscillatorNoise/', 'OscillatorPhaseNoiseCouplingFF.fig']);
    close(h1);
    
    fid = fopen([resultDir, 'OscillatorNoise/', ...
                 'OscillatorPhaseNoiseCouplingFF.dat'],'w');
    fprintf(fid,['#Oscillator phase noise coupling G: Multiply G with the ' ...
                 'phase noise of the RF oscillators in rad/sqrt(Hz)  to get the ' ...
                 'strain equivalent DARM noise\n']);
    fprintf(fid,'#freq, f1 [1/rad], f2 [1/rad], f3 [1/rad]\n');
    fclose(fid);
    
    M = [f(:), Gf1PM, Gf2PM,Gf3PM ];
    
    dlmwrite([resultDir, 'OscillatorNoise/', 'OscillatorPhaseNoiseCouplingFF.dat'],M, '-append');
    
    %}}}
    
    %{{{ Amplitude noise
    
    h1 = figure(1);
    loglog(f, Gf1AM,f, Gf2AM,f, Gf3AM);
    
    legend('f1','f2', 'f3');
    title('Oscillator Amplitude Noise Coupling','FontSize',16);
    xlabel('Hz', 'FontSize', 16);
    h=ylabel('|G| ','FontSize',16);
    grid on
    print(h1, '-dpdf', [resultDir, 'OscillatorNoise/', 'OscillatorAmplitudeNoiseCouplingFF.pdf']);
    saveas(h1,  [resultDir, 'OscillatorNoise/', 'OscillatorAmplitudeNoiseCouplingFF.fig']);
    close(h1);
    
    fid = fopen([resultDir, 'OscillatorNoise/', ...
                 'OscillatorAmplitudeNoiseCouplingFF.dat'],'w');
    fprintf(fid,['#Oscillator amplitude noise coupling G: Multiply G with the ' ...
                 'relative amplitude noise of the RF oscillators to get the ' ...
                 'strain equivalent DARM noise\n']);
    fprintf(fid,'#freq, f1, f2, f3\n');
    fclose(fid);
    
    M = [f(:), Gf1AM, Gf2AM, Gf3AM ];
    
    dlmwrite([resultDir, 'OscillatorNoise/', 'OscillatorAmplitudeNoiseCouplingFF.dat'],M, '-append');
end

if p.useMZI
    Gf1APM = zeros(length(w),1);
    Gf1AAM = zeros(length(w),1);
    if p.f3EOM
        Gf3APM = zeros(length(w),1);
        Gf3AAM = zeros(length(w),1);
    end
    for ii = [1:length(w)]
        H(:,:,ii) = inv(eye(5)+1*S*D(:,:,ii)*A(:,:,ii)*F2(:,:,ii));
        Gf1APM(ii) = abs([1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*vActf1APM/C);
        Gf1AAM(ii) = abs([1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*vActf1AAM/C);
        Gf2PM(ii) = abs([1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*vActf2PM/C);
        Gf2AM(ii) = abs([1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*vActf2AM/C);
        if p.f3EOM
            Gf3APM(ii) = abs([1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*vActf3APM/C);
            Gf3AAM(ii) = abs([1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*vActf3AAM/C);
        end
    end
    
    if p.f3EOM
        %Phase noise
        h1 = figure(1);
        loglog(f,Gf1APM,f,Gf2PM,f,Gf3APM);
        
        legend('f1A','f2','f3A');
        title('Oscillator Phase Noise Coupling(only typeA for f1&f3)','FontSize',16);
        xlabel('Hz', 'FontSize', 16);
        h=ylabel('|G| ','FontSize',16);
        grid on
        print(h1, '-dpdf', [resultDir, 'MZI/', 'OscillatorPhaseNoiseCouplingFF.pdf']);
        saveas(h1,  [resultDir, 'MZI/', 'OscillatorPhaseNoiseCouplingFF.fig']);
        close(h1);
        
        fid = fopen([resultDir, 'MZI/', ...
            'OscillatorPhaseNoiseCouplingFF.dat'],'w');
        fprintf(fid,['#Oscillator phase noise coupling G: Multiply G with the ' ...
            'relative phase noise of the RF oscillators to get the ' ...
            'strain equivalent DARM noise\n'...
            '# results of f1&f3 correspond to phase difference b/w EOMs\n']);
        fprintf(fid,'#freq, f1, f2, f3\n');
        fclose(fid);
        
        M = [f(:), Gf1APM, Gf2PM, Gf3APM];
        
        dlmwrite([resultDir, 'MZI/', 'OscillatorPhaseNoiseCouplingFF.dat'],M, '-append');
        
        %Amplitude noise 
        
        h1 = figure(1);
        loglog(f,Gf1AAM,f,Gf2AM,f,Gf3AAM);
        
        legend('f1A','f2','f3A');
        title('Oscillator Amplitude Noise Coupling(only typeA for f1&f3)','FontSize',16);
        xlabel('Hz', 'FontSize', 16);
        h=ylabel('|G| ','FontSize',16);
        grid on
        print(h1, '-dpdf', [resultDir, 'MZI/', 'OscillatorAmplitudeNoiseCouplingFF.pdf']);
        saveas(h1,  [resultDir, 'MZI/', 'OscillatorAmplitudeNoiseCouplingFF.fig']);
        close(h1);
        
        fid = fopen([resultDir, 'MZI/', ...
            'OscillatorAmplitudeNoiseCouplingFF.dat'],'w');
        fprintf(fid,['#Oscillator Amplitude noise coupling G: Multiply G with the ' ...
            'relative amplitude noise of the RF oscillators to get the ' ...
            'strain equivalent DARM noise\n'...
            '# results of f1&f3 correspond to difference in converwsion rate b/w EOMs\n']);
        fprintf(fid,'#freq, f1, f2, f3\n');
        fclose(fid);
        
        M = [f(:), Gf1APM, Gf2PM, Gf3APM];
        
        dlmwrite([resultDir, 'MZI/', 'OscillatorAmplitudeNoiseCouplingFF.dat'],M, '-append');
    else
        %Phase noise
        h1 = figure(1);
        loglog(f,Gf1APM,f,Gf2PM);
        
        legend('f1A','f2');
        title('Oscillator Phase Noise Coupling(only typeA for f1)','FontSize',16);
        xlabel('Hz', 'FontSize', 16);
        h=ylabel('|G| ','FontSize',16);
        grid on
        print(h1, '-dpdf', [resultDir, 'MZI/', 'OscillatorPhaseNoiseCouplingFF.pdf']);
        saveas(h1,  [resultDir, 'MZI/', 'OscillatorPhaseNoiseCouplingFF.fig']);
        close(h1);
        
        fid = fopen([resultDir, 'MZI/', ...
            'OscillatorPhaseNoiseCouplingFF.dat'],'w');
        fprintf(fid,['#Oscillator phase noise coupling G: Multiply G with the ' ...
            'relative phase noise of the RF oscillators to get the ' ...
            'strain equivalent DARM noise\n'...
            '# results of f1 correspond to phase difference b/w EOMs\n']);
        fprintf(fid,'#freq, f1, f2\n');
        fclose(fid);
        
        M = [f(:), Gf1APM, Gf2PM];
        
        dlmwrite([resultDir, 'MZI/', 'OscillatorPhaseNoiseCouplingFF.dat'],M, '-append');
        
        %Amplitude noise 
        
        h1 = figure(1);
        loglog(f,Gf1AAM,f,Gf2AM);
        
        legend('f1A','f2');
        title('Oscillator Amplitude Noise Coupling(only typeA for f1)','FontSize',16);
        xlabel('Hz', 'FontSize', 16);
        h=ylabel('|G| ','FontSize',16);
        grid on
        print(h1, '-dpdf', [resultDir, 'MZI/', 'OscillatorAmplitudeNoiseCouplingFF.pdf']);
        saveas(h1,  [resultDir, 'MZI/', 'OscillatorAmplitudeNoiseCouplingFF.fig']);
        close(h1);
        
        fid = fopen([resultDir, 'MZI/', ...
            'OscillatorAmplitudeNoiseCouplingFF.dat'],'w');
        fprintf(fid,['#Oscillator Amplitude noise coupling G: Multiply G with the ' ...
            'relative amplitude noise of the RF oscillators to get the ' ...
            'strain equivalent DARM noise\n'...
            '# results of f1 correspond to difference in converwsion rate b/w EOMs\n']);
        fprintf(fid,'#freq, f1, f2\n');
        fclose(fid);
        
        M = [f(:), Gf1APM, Gf2PM];
        
        dlmwrite([resultDir, 'MZI/', 'OscillatorAmplitudeNoiseCouplingFF.dat'],M, '-append');
        
    end
    
end

if p.RAM
    Gf1AM1PM = zeros(length(w),1);
    Gf1AM1AM = zeros(length(w),1);
    Gf2AM1PM = zeros(length(w),1);
    Gf2AM1AM = zeros(length(w),1);
     for ii = [1:length(w)]
        H(:,:,ii) = inv(eye(5)+1*S*D(:,:,ii)*A(:,:,ii)*F2(:,:,ii));
        C = 3000*[1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*Mconv*[1;0;0;0;0];
        Gf1PM(ii) = abs([1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*vActf1PM/C);
        Gf1AM(ii) = abs([1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*vActf1AM/C);
        Gf2PM(ii) = abs([1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*vActf2PM/C);
        Gf2AM(ii) = abs([1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*vActf2AM/C);
        Gf1AM1PM(ii) = abs([1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*vActf1AM1PM/C);
        Gf1AM1AM(ii) = abs([1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*vActf1AM1AM/C);
        Gf2AM1PM(ii) = abs([1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*vActf2AM1PM/C);
        Gf2AM1AM(ii) = abs([1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*vActf2AM1AM/C);
    end
    %
    
    %{{{ Phase noise
    
    h1 = figure(1);
    loglog(f, Gf1PM,f, Gf2PM,f, Gf1AM1PM,f, Gf2AM1PM);
    
    legend('f1','f2', 'f1_RAM','f2_RAM');
    title('Oscillator Phase Noise Coupling','FontSize',16);
    xlabel('Hz', 'FontSize', 16);
    h=ylabel('|G| 1/rad','FontSize',16);
    grid on
    print(h1, '-dpdf', [resultDir, 'OscillatorNoise/', 'OscillatorPhaseNoiseCouplingFF.pdf']);
    saveas(h1,  [resultDir, 'OscillatorNoise/', 'OscillatorPhaseNoiseCouplingFF.fig']);
    close(h1);
    
    fid = fopen([resultDir, 'OscillatorNoise/', ...
                 'OscillatorPhaseNoiseCouplingFF.dat'],'w');
    fprintf(fid,['#Oscillator phase noise coupling G: Multiply G with the ' ...
                 'phase noise of the RF oscillators in rad/sqrt(Hz)  to get the ' ...
                 'strain equivalent DARM noise\n']);
    fprintf(fid,'#freq, f1 [1/rad], f2 [1/rad], f1_RAM [1/rad], f2_RAM [1/rad]\n');
    fclose(fid);
    
    M = [f(:), Gf1PM, Gf2PM,Gf1AM1PM,Gf2AM1PM ];
    
    dlmwrite([resultDir, 'OscillatorNoise/', 'OscillatorPhaseNoiseCouplingFF.dat'],M, '-append');
    
    %}}}
    
    %{{{ Amplitude noise
    
    h1 = figure(1);
    loglog(f, Gf1AM,f, Gf2AM,f, Gf1AM1AM,f, Gf2AM1AM);
    
    legend('f1','f2', 'f1_RAM', 'f2_RAM');
    title('Oscillator Amplitude Noise Coupling','FontSize',16);
    xlabel('Hz', 'FontSize', 16);
    h=ylabel('|G| ','FontSize',16);
    grid on
    print(h1, '-dpdf', [resultDir, 'OscillatorNoise/', 'OscillatorAmplitudeNoiseCouplingFF.pdf']);
    saveas(h1,  [resultDir, 'OscillatorNoise/', 'OscillatorAmplitudeNoiseCouplingFF.fig']);
    close(h1);
    
    fid = fopen([resultDir, 'OscillatorNoise/', ...
                 'OscillatorAmplitudeNoiseCouplingFF.dat'],'w');
    fprintf(fid,['#Oscillator amplitude noise coupling G: Multiply G with the ' ...
                 'relative amplitude noise of the RF oscillators to get the ' ...
                 'strain equivalent DARM noise\n']);
    fprintf(fid,'#freq, f1, f2, f1_RAM, f2_RAM\n');
    fclose(fid);
    
    M = [f(:), Gf1AM, Gf2AM, Gf1AM1AM, Gf2AM1AM ];
    
    dlmwrite([resultDir, 'OscillatorNoise/', 'OscillatorAmplitudeNoiseCouplingFF.dat'],M, '-append');
    
end


%}}}

%}}}

%}}}

%{{{ === Noise Requirements ===

%{{{ Load TM Displacement Noises
%%
X = dlmread('TMDisp.dat', ',');
%loglog(X(:,1),X(:,2));


%%
xseis = interp1(X(:,1),X(:,2),f);

%}}}

%{{{ Compute Xreq

Xreq = zeros(length(iDrv),length(w));
TotalNoise = zeros(length(w),1);
for ii = [1:length(w)]
    H(:,:,ii) = inv(eye(5)+S*D(:,:,ii)*A(:,:,ii)*F2(:,:,ii));
    C = 3000*[1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*Mconv*[1;0;0;0;0];
    QN = [1,0,0,0,0]*H(:,:,ii)*S*NSdarm(:,ii)/C;
    DN = xseis(ii);  %displacement noise in DARM with strain unit(i.e. error signal)
    TotalNoise(ii) = sqrt(QN^2+DN^2);
    T = [1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)/C;  %L.1439 D=sigAC(iPrb, iDrv, :)->T=T(iDrv, freq)
    Xreq(:,ii) = abs((TotalNoise(ii)./T/p.DispNoiseSafetyFactor))';
end

%}}}

%{{{ Mirror Displacement Requirements

h1 = figure(1);
loglog(f, xseis, f, Xreq(actMap('ETMX'),:),f, Xreq(actMap('ETMY'),:),f, ...
       Xreq(actMap('ITMX'),:), f, Xreq(actMap('ITMY'),:), f, ...
       Xreq(actMap('BS'),:), f, Xreq(actMap('PRM'),:),f, Xreq(actMap('SRM'),:));

legend('SAS Noise','ETMX','ETMY','ITMX','ITMY','BS','PRM','SRM');
title(['Displacement Requirement (Safety Factor',num2str(p.DispNoiseSafetyFactor),')'],'FontSize',16);
xlabel('Frequency [Hz]','FontSize',16);
h=ylabel('m/sqrt(Hz)','FontSize',16);
grid on
print(h1, '-dpdf', [resultDir, 'DisplacementNoise/', 'DisplacementNoiseRequirement.pdf']);
saveas(h1,  [resultDir, 'DisplacementNoise/', 'DisplacementNoiseRequirement.fig']);
close(h1);

fid = fopen([resultDir, 'DisplacementNoise/', 'DisplacementNoiseRequirement.dat'],'w');
fprintf(fid,['#Mirror displacement noise requirements in [m/sqrt(Hz). The ' ...
             'mirror vibration levels have to be below these values in ' ...
             'order to keep the contribution of these noises to DARM below ' ...
             'the target sensitivity by the safety factor ' ...
             num2str(p.DispNoiseSafetyFactor) '.\n']);
fprintf(fid,'#freq, SAS Noise,ETMX,ETMY,ITMX,ITMY,BS,PRM,SRM\n');
fclose(fid);

M = [f(:), xseis(:), Xreq(actMap('ETMX'),:)', Xreq(actMap('ETMY'),:)', ...
       Xreq(actMap('ITMX'),:)', Xreq(actMap('ITMY'),:)', ...
       Xreq(actMap('BS'),:)', Xreq(actMap('PRM'),:)', Xreq(actMap('SRM'),:)'];


dlmwrite([resultDir, 'DisplacementNoise/', 'DisplacementNoiseRequirement.dat'],M, '-append');

if p.useMZI
    
   	if p.MZIM2
        h1 = figure(1);
        loglog(f,Xreq(actMap('MZIM1'),:),f, Xreq(actMap('MZIM2'),:),f,Xreq(actMap('MZIM4'),:));
        
        legend('MZIM1(mid)','MZIM2(mid)','MZIM4(dark)');
        title(['MZI-Displacement Requirement (Safety Factor',num2str(p.DispNoiseSafetyFactor),')'],'FontSize',16);
        xlabel('Frequency [Hz]','FontSize',16);
        h=ylabel('m/sqrt(Hz)','FontSize',16);
        grid on
        print(h1, '-dpdf', [resultDir, 'MZI/', 'DisplacementNoiseRequirement.pdf']);
        saveas(h1,  [resultDir, 'MZI/', 'DisplacementNoiseRequirement.fig']);
        close(h1);
    
        fid = fopen([resultDir, 'MZI/', 'DisplacementNoiseRequirement.dat'],'w');
        fprintf(fid,['#Mirror displacement noise requirements in [m/sqrt(Hz). The ' ...
                 'mirror vibration levels have to be below these values in ' ...
                 'order to keep the contribution of these noises to DARM below ' ...
                'the target sensitivity by the safety factor ' ...
                num2str(p.DispNoiseSafetyFactor) '.\n']);
        fprintf(fid,'#freq, MZIM1(mid), MZIM2(mid),MZIM4(dark)\n');
        fclose(fid);
        
        M = [f(:), Xreq(actMap('MZIM1'),:)', Xreq(actMap('MZIM2'),:)', Xreq(actMap('MZIM4'),:)'];
        
        dlmwrite([resultDir, 'MZI/', 'DisplacementNoiseRequirement.dat'],M, '-append');
        
    else
    
        h1 = figure(1);
        loglog(f,Xreq(actMap('MZIM1'),:),f,Xreq(actMap('MZIM4'),:));
        
        legend('MZIM1(mid)','MZIM4(dark)');
        title(['MZI-Displacement Requirement (Safety Factor',num2str(p.DispNoiseSafetyFactor),')'],'FontSize',16);
        xlabel('Frequency [Hz]','FontSize',16);
        h=ylabel('m/sqrt(Hz)','FontSize',16);
        grid on
        print(h1, '-dpdf', [resultDir, 'MZI/', 'DisplacementNoiseRequirement.pdf']);
        saveas(h1,  [resultDir, 'MZI/', 'DisplacementNoiseRequirement.fig']);
        close(h1);
    
        fid = fopen([resultDir, 'MZI/', 'DisplacementNoiseRequirement.dat'],'w');
        fprintf(fid,['#Mirror displacement noise requirements in [m/sqrt(Hz). The ' ...
                 'mirror vibration levels have to be below these values in ' ...
                 'order to keep the contribution of these noises to DARM below ' ...
                'the target sensitivity by the safety factor ' ...
                num2str(p.DispNoiseSafetyFactor) '.\n']);
        fprintf(fid,'#freq, MZIM1(mid), MZIM4(dark)\n');
        fclose(fid);
        
        M = [f(:), Xreq(actMap('MZIM1'),:)', Xreq(actMap('MZIM4'),:)'];
        
        dlmwrite([resultDir, 'MZI/', 'DisplacementNoiseRequirement.dat'],M, '-append');
    end
end

%}}}

%{{{ Laser Noise Requirements

%{{{ Frequency noise requirements

h1 = figure(1);
loglog(f, f.*Xreq(actMap('PM'),:));

legend('Free run frequency noise');
title(['Frequency Noise Requirement (Safety Factor',num2str(p.DispNoiseSafetyFactor),')'],'FontSize',16);
xlabel('Frequency [Hz]','FontSize',16);
h=ylabel('Hz/sqrt(Hz)','FontSize',16);
grid on
print(h1, '-dpdf', [resultDir, 'LaserNoise/', 'MCFrequencyNoiseRequirement.pdf']);
saveas(h1,  [resultDir, 'LaserNoise/', 'MCFrequencyNoiseRequirement.fig']);
close(h1);

fid = fopen([resultDir, 'LaserNoise/', 'MCFrequencyNoiseRequirement.dat'],'w');
fprintf(fid,'#f, Hz/sqrt(Hz)\n');
fclose(fid);

M = [f(:), (f.*Xreq(actMap('PM'),:))'];

dlmwrite([resultDir, 'LaserNoise/', 'MCFrequencyNoiseRequirement.dat'],M, '-append');

%}}}

%{{{ MC displacement requirement

h1 = figure(1);
Lmc = 26.6;
dLmc = Lmc*f.*Xreq(actMap('PM'),:)/(opt.c/opt.lambda);
loglog(f,dLmc)
title(['Frequency Noise Requirement (Safety Factor',num2str(p.DispNoiseSafetyFactor),')'],'FontSize',16);
xlabel('Frequency [Hz]','FontSize',16);
h=ylabel('m/sqrt(Hz)','FontSize',16);
grid on
print(h1, '-dpdf', [resultDir, 'LaserNoise/', 'MCDisplacementNoiseRequirement.pdf']);
saveas(h1,  [resultDir, 'LaserNoise/', 'MCDisplacementNoiseRequirement.fig']);
close(h1);

fid = fopen([resultDir, 'LaserNoise/', 'MCDisplacementNoiseRequirement.dat'],'w');
fprintf(fid,'#f, m/sqrt(Hz)\n');
fclose(fid);

M = [f(:), dLmc'];

dlmwrite([resultDir, 'LaserNoise/', 'MCDisplacementNoiseRequirement.dat'],M, '-append');

%}}}

%{{{ Inensity noise requirements

h1 = figure(1);
loglog(f, 2*Xreq(actMap('AM'),:));

legend('Intensity noise');
title(['Intensity Noise Requirement (Safety Factor',num2str(p.DispNoiseSafetyFactor),')'],'FontSize',16);
xlabel('Frequency [Hz]','FontSize',16);
h=ylabel('dP/P/sqrt(Hz)','FontSize',16);
grid on
print(h1, '-dpdf', [resultDir, 'LaserNoise/', 'IntensityNoiseRequirement.pdf']);
saveas(h1,  [resultDir, 'LaserNoise/', 'IntensityNoiseRequirement.fig']);
close(h1);

fid = fopen([resultDir, 'LaserNoise/', 'IntensityNoiseRequirement.dat'],'w');
fprintf(fid,'#f, FreeRunLaser\n');
fclose(fid);

M = [f(:), (2*Xreq(actMap('AM'),:))'];

dlmwrite([resultDir, 'LaserNoise/', 'IntensityNoiseRequirement.dat'],M, '-append');

%}}}

%}}}

%{{{ Oscillator Noise Requirements

%{{{ f1 Phase Noise
% The SSB phase noise (Nssb) and the phase noise amplitude (Npn)
% is related by the following formulae.  
% exp(-i*(Omega*t + Npn*sin(w*t))) ~ J0(Npn) + J1(Npn)
% Nssb = |J1(Npn)/J0(Npn)|^2 ~ (Npn/2)^2.
% It is a convension to show Nssb in dB (10*log10(Nssb)).
% The safety factor 10 is for the phase noise amplitude.
% Since the SSB PN is the power ratio, the safety factor is 100, i.e. 20dBc.
if ~p.useMZI
    h1 = figure(1);
    semilogx(f, 20*log10(Xreq(actMap('Mod1.phase'),:)/2));
    
    legend('Intensity noise');
    title(['f1 SSB Phase Noise Requirement (Safety Factor ',num2str(20* ...
                                                      log10(p.DispNoiseSafetyFactor)),' dBc)'],'FontSize',16);
    xlabel('Frequency [Hz]','FontSize',16);
    h=ylabel('SSB Phase Noise [dBc]','FontSize',16);
    grid on
    print(h1, '-dpdf', [resultDir, 'OscillatorNoise/', 'f1SSBPhaseNoiseRequirement.pdf']);
    saveas(h1,  [resultDir, 'OscillatorNoise/', 'f1SSBPhaseNoiseRequirement.fig']);
    close(h1);
    
    fid = fopen([resultDir, 'OscillatorNoise/', 'f1SSBPhaseNoiseRequirement.dat'],'w');
    fprintf(fid,'#f, FreeRunLaser\n');
    fclose(fid);
    
    M = [f(:), ((Xreq(actMap('Mod1.phase'),:)/2).^2)'];
    
    dlmwrite([resultDir, 'OscillatorNoise/', 'f1SSBPhaseNoiseRequirement.dat'],M, '-append');
end
%}}}

%{{{ f2 Phase Noise
% The SSB phase noise (Nssb) and the phase noise amplitude (Npn)
% is related by the following formulae.  
% exp(-i*(Omega*t + Npn*sin(w*t))) ~ J0(Npn) + J1(Npn)
% Nssb = |J1(Npn)/J0(Npn)|^2 ~ (Npn/2)^2.
% It is a convension to show Nssb in dB (10*log10(Nssb)).
% The safety factor 10 is for the phase noise amplitude.
% Since the SSB PN is the power ratio, the safety factor is 100, i.e. 20dBc.

h1 = figure(1);
semilogx(f, 20*log10(Xreq(actMap('Mod2.phase'),:)/2));

legend('Intensity noise');
title(['f2 SSB Phase Noise Requirement (Safety Factor ',num2str(20* ...
                                                  log10(p.DispNoiseSafetyFactor)),' dBc)'],'FontSize',16);
xlabel('Frequency [Hz]','FontSize',16);
h=ylabel('SSB Phase Noise [dBc]','FontSize',16);
grid on
if p.useMZI
    print(h1, '-dpdf', [resultDir, 'MZI/', 'f2SSBPhaseNoiseRequirement.pdf']);
    saveas(h1,  [resultDir, 'MZI/', 'f2SSBPhaseNoiseRequirement.fig']);
else
    print(h1, '-dpdf', [resultDir, 'OscillatorNoise/', 'f2SSBPhaseNoiseRequirement.pdf']);
    saveas(h1,  [resultDir, 'OscillatorNoise/', 'f2SSBPhaseNoiseRequirement.fig']);
end
close(h1);
if p.useMZI
    fid = fopen([resultDir, 'MZI/', 'f2SSBPhaseNoiseRequirement.dat'],'w');
else
    fid = fopen([resultDir, 'OscillatorNoise/', 'f2SSBPhaseNoiseRequirement.dat'],'w');
end
fprintf(fid,'#f, FreeRunLaser\n');
fclose(fid);

M = [f(:), ((Xreq(actMap('Mod2.phase'),:)/2).^2)'];
if p.useMZI
    dlmwrite([resultDir, 'MZI/', 'f2SSBPhaseNoiseRequirement.dat'],M, '-append');
else
    dlmwrite([resultDir, 'OscillatorNoise/', 'f2SSBPhaseNoiseRequirement.dat'],M, '-append');
end
%}}}

%{{{ f3 Phase Noise
% The SSB phase noise (Nssb) and the phase noise amplitude (Npn)
% is related by the following formulae.  
% exp(-i*(Omega*t + Npn*sin(w*t))) ~ J0(Npn) + J1(Npn)
% Nssb = |J1(Npn)/J0(Npn)|^2 ~ (Npn/2)^2.
% It is a convension to show Nssb in dB (10*log10(Nssb)).
% The safety factor 10 is for the phase noise amplitude.
% Since the SSB PN is the power ratio, the safety factor is 100, i.e. 20dBc.
if and(~p.useMZI,~p.RAM)
    h1 = figure(1);
    semilogx(f, 20*log10(Xreq(actMap('Mod3.phase'),:)/2));
    
    legend('Intensity noise');
    title(['f3 SSB Phase Noise Requirement (Safety Factor ',num2str(20* ...
        log10(p.DispNoiseSafetyFactor)),' dBc)'],'FontSize',16);
    xlabel('Frequency [Hz]','FontSize',16);
    h=ylabel('SSB Phase Noise [dBc]','FontSize',16);
    grid on
    print(h1, '-dpdf', [resultDir, 'OscillatorNoise/', 'f3SSBPhaseNoiseRequirement.pdf']);
    saveas(h1,  [resultDir, 'OscillatorNoise/', 'f3SSBPhaseNoiseRequirement.fig']);
    close(h1);
    
    fid = fopen([resultDir, 'OscillatorNoise/', 'f3SSBPhaseNoiseRequirement.dat'],'w');
    fprintf(fid,'#f, FreeRunLaser\n');
    fclose(fid);
    
    M = [f(:), ((Xreq(actMap('Mod3.phase'),:)/2).^2)'];
    
    dlmwrite([resultDir, 'OscillatorNoise/', 'f3SSBPhaseNoiseRequirement.dat'],M, '-append');
end
%fprintf('p.DRSE : %d \n', p.DRSE);
%disp(p.DRSE);
%}}}

% f1&f3 typeA phase noise corresponding to phase difference b/w EOMs
if p.useMZI
    %f1 typeA
    h1 = figure(1);
    semilogx(f, 20*log10(Xreq(actMap('Mod1A.phase'),:)/2));
    
    legend('Intensity noise');
    title(['f1 typeA SSB Phase Noise Requirement (Safety Factor ',num2str(20* ...
                                                      log10(p.DispNoiseSafetyFactor)),' dBc)'],'FontSize',16);
    xlabel('Frequency [Hz]','FontSize',16);
    h=ylabel('SSB Phase Noise [dBc]','FontSize',16);
    grid on
    print(h1, '-dpdf', [resultDir, 'MZI/', 'f1A_SSBPhaseNoiseRequirement.pdf']);
    saveas(h1,  [resultDir, 'MZI/', 'f1A_SSBPhaseNoiseRequirement.fig']);
    close(h1);
    
    fid = fopen([resultDir, 'MZI/', 'f1A_SSBPhaseNoiseRequirement.dat'],'w');
    fprintf(fid,'#f, FreeRunLaser\n');
    fclose(fid);
    
    M = [f(:), ((Xreq(actMap('Mod1A.phase'),:)/2).^2)'];
    
    dlmwrite([resultDir, 'MZI/', 'f1A_SSBPhaseNoiseRequirement.dat'],M, '-append');
    
    if p.f3EOM
        %f3 typeA
        h1 = figure(1);
        semilogx(f, 20*log10(Xreq(actMap('Mod3A.phase'),:)/2));
        
        legend('Intensity noise');
        title(['f3 typeA SSB Phase Noise Requirement (Safety Factor ',num2str(20* ...
            log10(p.DispNoiseSafetyFactor)),' dBc)'],'FontSize',16);
        xlabel('Frequency [Hz]','FontSize',16);
        h=ylabel('SSB Phase Noise [dBc]','FontSize',16);
        grid on
        print(h1, '-dpdf', [resultDir, 'MZI/', 'f3A_SSBPhaseNoiseRequirement.pdf']);
        saveas(h1,  [resultDir, 'MZI/', 'f3A_SSBPhaseNoiseRequirement.fig']);
        close(h1);
        
        fid = fopen([resultDir, 'MZI/', 'f3A_SSBPhaseNoiseRequirement.dat'],'w');
        fprintf(fid,'#f, FreeRunLaser\n');
        fclose(fid);
        
        M = [f(:), ((Xreq(actMap('Mod3A.phase'),:)/2).^2)'];
        
        dlmwrite([resultDir, 'MZI/', 'f3SSBPhaseNoiseRequirement.dat'],M, '-append');
    end
end


% f1&f2 for RAM phase noise
if p.RAM
    %f1
    h1 = figure(1);
    semilogx(f, 20*log10(Xreq(actMap('Mod1AM1.phase'),:)/2));
    
    legend('Intensity noise');
    title(['f1_RAM SSB Phase Noise Requirement (Safety Factor ',num2str(20* ...
                                                      log10(p.DispNoiseSafetyFactor)),' dBc)'],'FontSize',16);
    xlabel('Frequency [Hz]','FontSize',16);
    h=ylabel('SSB Phase Noise [dBc]','FontSize',16);
    grid on
    print(h1, '-dpdf', [resultDir, 'OscillatorNoise/', 'f1RAMSSBPhaseNoiseRequirement.pdf']);
    saveas(h1,  [resultDir, 'OscillatorNoise/', 'f1RAMSSBPhaseNoiseRequirement.fig']);
    close(h1);
    
    fid = fopen([resultDir, 'OscillatorNoise/', 'f1RAMSSBPhaseNoiseRequirement.dat'],'w');
    fprintf(fid,'#f, FreeRunLaser\n');
    fclose(fid);
    
    M = [f(:), ((Xreq(actMap('Mod1AM1.phase'),:)/2).^2)'];
    
    dlmwrite([resultDir, 'OscillatorNoise/', 'f1RAMSSBPhaseNoiseRequirement.dat'],M, '-append');
    
    %f2
    h1 = figure(1);
    semilogx(f, 20*log10(Xreq(actMap('Mod2AM1.phase'),:)/2));
    
    legend('Intensity noise');
    title(['f2_RAM SSB Phase Noise Requirement (Safety Factor ',num2str(20* ...
                                                      log10(p.DispNoiseSafetyFactor)),' dBc)'],'FontSize',16);
    xlabel('Frequency [Hz]','FontSize',16);
    h=ylabel('SSB Phase Noise [dBc]','FontSize',16);
    grid on
    print(h1, '-dpdf', [resultDir, 'OscillatorNoise/', 'f2RAMSSBPhaseNoiseRequirement.pdf']);
    saveas(h1,  [resultDir, 'OscillatorNoise/', 'f2RAMSSBPhaseNoiseRequirement.fig']);
    close(h1);
    
    fid = fopen([resultDir, 'OscillatorNoise/', 'f2RAMSSBPhaseNoiseRequirement.dat'],'w');
    fprintf(fid,'#f, FreeRunLaser\n');
    fclose(fid);
    
    M = [f(:), ((Xreq(actMap('Mod2AM1.phase'),:)/2).^2)'];
    
    dlmwrite([resultDir, 'OscillatorNoise/', 'f2RAMSSBPhaseNoiseRequirement.dat'],M, '-append');
    
end




%{{{ f1 Amplitude Noise
if ~p.useMZI
    h1 = figure(1);
    loglog(f, Xreq(actMap('Mod1.amp'),:));
    
    legend('Intensity noise');
    title(['f1 Amplitude Noise Requirement (Safety Factor ',num2str(p.DispNoiseSafetyFactor),')'],'FontSize',16);
    xlabel('Frequency [Hz]','FontSize',16);
    h=ylabel('Relative Amplitude Noise [1/sqrt(Hz)]','FontSize',16);
    grid on
    print(h1, '-dpdf', [resultDir, 'OscillatorNoise/', 'f1AmplitudeNoiseRequirement.pdf']);
    saveas(h1,  [resultDir, 'OscillatorNoise/', 'f1AmplitudeNoiseRequirement.fig']);
    close(h1);
    
    fid = fopen([resultDir, 'OscillatorNoise/', 'f1AmplitudeNoiseRequirement.dat'],'w');
    fprintf(fid,'#f, FreeRunLaser\n');
    fclose(fid);
    
    M = [f(:), (Xreq(actMap('Mod1.amp'),:))'];
    
    dlmwrite([resultDir, 'OscillatorNoise/', 'f1AmplitudeNoiseRequirement.dat'],M, '-append');
end

%}}}

%{{{ f2 Amplitude Noise

h1 = figure(1);
loglog(f, Xreq(actMap('Mod2.amp'),:));

legend('Intensity noise');
title(['f2 Amplitude Noise Requirement (Safety Factor ',num2str(p.DispNoiseSafetyFactor),')'],'FontSize',16);
xlabel('Frequency [Hz]','FontSize',16);
h=ylabel('Relative Amplitude Noise [1/sqrt(Hz)]','FontSize',16);
grid on
if p.useMZI
    print(h1, '-dpdf', [resultDir, 'MZI/', 'f2AmplitudeNoiseRequirement.pdf']);
    saveas(h1,  [resultDir, 'MZI/', 'f2AmplitudeNoiseRequirement.fig']);
else
    print(h1, '-dpdf', [resultDir, 'OscillatorNoise/', 'f2AmplitudeNoiseRequirement.pdf']);
    saveas(h1,  [resultDir, 'OscillatorNoise/', 'f2AmplitudeNoiseRequirement.fig']);
end
close(h1);
if p.useMZI
    fid = fopen([resultDir, 'MZI/', 'f2AmplitudeNoiseRequirement.dat'],'w');
else
    fid = fopen([resultDir, 'OscillatorNoise/', 'f2AmplitudeNoiseRequirement.dat'],'w');
end
fprintf(fid,'#f, FreeRunLaser\n');
fclose(fid);

M = [f(:), (Xreq(actMap('Mod2.amp'),:))'];
if p.useMZI
    dlmwrite([resultDir, 'MZI/', 'f2AmplitudeNoiseRequirement.dat'],M, '-append');
else
    dlmwrite([resultDir, 'OscillatorNoise/', 'f2AmplitudeNoiseRequirement.dat'],M, '-append');
end

%}}}

%{{{ f3 Amplitude Noise
if and(~p.useMZI,~p.RAM)
    h1 = figure(1);
    loglog(f, Xreq(actMap('Mod3.amp'),:));
    
    legend('Intensity noise');
    title(['f3 Amplitude Noise Requirement (Safety Factor ',num2str(p.DispNoiseSafetyFactor),')'],'FontSize',16);
    xlabel('Frequency [Hz]','FontSize',16);
    h=ylabel('Relative Amplitude Noise [1/sqrt(Hz)]','FontSize',16);
    grid on
    print(h1, '-dpdf', [resultDir, 'OscillatorNoise/', 'f3AmplitudeNoiseRequirement.pdf']);
    saveas(h1,  [resultDir, 'OscillatorNoise/', 'f3AmplitudeNoiseRequirement.fig']);
    close(h1);
    
    fid = fopen([resultDir, 'OscillatorNoise/', 'f3AmplitudeNoiseRequirement.dat'],'w');
    fprintf(fid,'#f, FreeRunLaser\n');
    fclose(fid);
    
    M = [f(:), (Xreq(actMap('Mod3.amp'),:))'];
    
    dlmwrite([resultDir, 'OscillatorNoise/', 'f3AmplitudeNoiseRequirement.dat'],M, '-append');
end

%}}}

% f1&f3 typeA amplitude noise corresponding to difference in conversion rate b/w EOMs
if p.useMZI
    %f1 typeA
    h1 = figure(1);
    loglog(f, Xreq(actMap('Mod1A.amp'),:));
    
    legend('Intensity noise');
    title(['f1 typeA Amplitude Noise Requirement (Safety Factor ',num2str(p.DispNoiseSafetyFactor),')'],'FontSize',16);
    xlabel('Frequency [Hz]','FontSize',16);
    h=ylabel('Relative Amplitude Noise [1/sqrt(Hz)]','FontSize',16);
    grid on
    print(h1, '-dpdf', [resultDir, 'MZI/', 'f1A_AmplitudeNoiseRequirement.pdf']);
    saveas(h1,  [resultDir, 'MZI/', 'f1A_AmplitudeNoiseRequirement.fig']);
    close(h1);
    
    fid = fopen([resultDir, 'MZI/', 'f1A_AmplitudeNoiseRequirement.dat'],'w');
    fprintf(fid,'#f, FreeRunLaser\n');
    fclose(fid);
    
    M = [f(:), (Xreq(actMap('Mod1A.amp'),:))'];
    
    dlmwrite([resultDir, 'MZI/', 'f1A_AmplitudeNoiseRequirement.dat'],M, '-append');
    
    if p.f3EOM
        %f3 typeA
        h1 = figure(1);
        loglog(f, Xreq(actMap('Mod3A.amp'),:));
        
        legend('Intensity noise');
        title(['f3 typeA Amplitude Noise Requirement (Safety Factor ',num2str(p.DispNoiseSafetyFactor),')'],'FontSize',16);
        xlabel('Frequency [Hz]','FontSize',16);
        h=ylabel('Relative Amplitude Noise [1/sqrt(Hz)]','FontSize',16);
        grid on
        print(h1, '-dpdf', [resultDir, 'MZI/', 'f3A_AmplitudeNoiseRequirement.pdf']);
        saveas(h1,  [resultDir, 'MZI/', 'f3A_AmplitudeNoiseRequirement.fig']);
        close(h1);
        
        fid = fopen([resultDir, 'MZI/', 'f3A_AmplitudeNoiseRequirement.dat'],'w');
        fprintf(fid,'#f, FreeRunLaser\n');
        fclose(fid);
        
        M = [f(:), (Xreq(actMap('Mod3A.amp'),:))'];
        
        dlmwrite([resultDir, 'MZI/', 'f3A_AmplitudeNoiseRequirement.dat'],M, '-append');
    end
end
    

%}}}

%}}}

% f1%f2 for RAM amplitude noise
if p.RAM
    %f1
    h1 = figure(1);
    loglog(f, Xreq(actMap('Mod1AM1.amp'),:));
    
    legend('Intensity noise');
    title(['f1_RAM Amplitude Noise Requirement (Safety Factor ',num2str(p.DispNoiseSafetyFactor),')'],'FontSize',16);
    xlabel('Frequency [Hz]','FontSize',16);
    h=ylabel('Relative Amplitude Noise [1/sqrt(Hz)]','FontSize',16);
    grid on
    print(h1, '-dpdf', [resultDir, 'OscillatorNoise/', 'f1RAMAmplitudeNoiseRequirement.pdf']);
    saveas(h1,  [resultDir, 'OscillatorNoise/', 'f1RAMAmplitudeNoiseRequirement.fig']);
    close(h1);
    
    fid = fopen([resultDir, 'OscillatorNoise/', 'f1RAMAmplitudeNoiseRequirement.dat'],'w');
    fprintf(fid,'#f, FreeRunLaser\n');
    fclose(fid);
    
    M = [f(:), (Xreq(actMap('Mod1AM1.amp'),:))'];
    
    dlmwrite([resultDir, 'OscillatorNoise/', 'f1RAMAmplitudeNoiseRequirement.dat'],M, '-append');
    
    %f2
    h1 = figure(1);
    loglog(f, Xreq(actMap('Mod2AM1.amp'),:));
    
    legend('Intensity noise');
    title(['f2_RAM Amplitude Noise Requirement (Safety Factor ',num2str(p.DispNoiseSafetyFactor),')'],'FontSize',16);
    xlabel('Frequency [Hz]','FontSize',16);
    h=ylabel('Relative Amplitude Noise [1/sqrt(Hz)]','FontSize',16);
    grid on
    print(h1, '-dpdf', [resultDir, 'OscillatorNoise/', 'f2RAMAmplitudeNoiseRequirement.pdf']);
    saveas(h1,  [resultDir, 'OscillatorNoise/', 'f2RAMAmplitudeNoiseRequirement.fig']);
    close(h1);
    
    fid = fopen([resultDir, 'OscillatorNoise/', 'f2RAMAmplitudeNoiseRequirement.dat'],'w');
    fprintf(fid,'#f, FreeRunLaser\n');
    fclose(fid);
    
    M = [f(:), (Xreq(actMap('Mod2AM1.amp'),:))'];
    
    dlmwrite([resultDir, 'OscillatorNoise/', 'f2RAMAmplitudeNoiseRequirement.dat'],M, '-append');
    
end


%}}}

%{{{ === Scattered Light Noise === %this part is skipped for MZI
if p.IncludeSCL

f=logspace(-1,5,1000);
    
%Enable SCL injection
p.do_SCL = true;
    
mkdir([resultDir,'ScatteredLightNoise']);
    
%{{{ SCL amplitude

%Scattered light amplitudes
p.EsETMX = p.IncludeSCL*sqrt(1e-7); 
p.EsPR2 = p.IncludeSCL*sqrt(1e-7);
p.EsPRMAR = p.IncludeSCL*sqrt(1e-7);
p.EsSR2 = p.IncludeSCL*sqrt(1e-7); 
p.EsSRMAR = p.IncludeSCL*sqrt(1e-7);
p.EsITMtoBS = p.IncludeSCL*sqrt(1e-7); 
p.EsITMAR = p.IncludeSCL*sqrt(1e-7); 
p.EsREFL = p.IncludeSCL*sqrt(1e-7); 

%}}}
    
%{{{ Set steering mirror position offset to zero

p.SCL_ETMX_OFS = 0;
p.SCL_PR2_OFS = 0;
p.SCL_PRMAR_OFS = 0;
p.SCL_SR2_OFS = 0;
p.SCL_SRMAR_OFS = 0;
p.SCL_ITMtoBS_OFS = 0;
p.SCL_ITMAR_OFS = 0;
p.SCL_REFL_OFS = 0;

%}}}

%{{{ Reduce the SB order

% Number of higher oreder sidebands to be computed 
p.sbOrder=1;

% Generate RF vector
p.vMod = generateRFFrequencyVector([p.fmod1,p.fmod2,p.fmod3],p.sbOrder,1);

%}}}

%Loop for Carrier and RF SBs
FieldNames = {'Carrier', 'f1', 'f2'};
for jj = [1:3]

%{{{ RF vector for SCL injection
switch jj
  case 1
    %Carrier
    p.SCL_vRF = (p.vMod == 0);
  case 2
    %f1
    p.SCL_vRF = (p.vMod == p.fmod1 | p.vMod == - p.fmod1)./sqrt(2);
  case 3
    %f2
    p.SCL_vRF = (p.vMod == p.fmod2 | p.vMod == - p.fmod2)./sqrt(2);
end

%}}}

%{{{ Reconstruct the Optickle model

%% Reconstruct the Optickle model
% % Construct the model with the optimized demodulation phase.
[opts1,ns1,ls1,prs1,drs1] = modelLCGT(p);

%}}}

%{{{ tickle 1

%% Tickle

display('Start Tickling for SCL, 1st round ...')

tic
[fDCs1, sigDCs1, sigACs1, mMechs1, noiseACs1, noiseMechs1] = ptickle(opts1, [], f);
toc
display('Finished.')

%}}}

%{{{ Set steering mirror position offset to lambda/8

p.SCL_ETMX_OFS = opt.lambda/8;
p.SCL_PR2_OFS = opt.lambda/8;
p.SCL_PRMAR_OFS = opt.lambda/8;
p.SCL_SR2_OFS = opt.lambda/8;
p.SCL_SRMAR_OFS = opt.lambda/8;
p.SCL_ITMtoBS_OFS = opt.lambda/8;
p.SCL_ITMAR_OFS = opt.lambda/8;
p.SCL_REFL_OFS = opt.lambda/8;

%}}}

%{{{ Reconstruct the Optickle model

%% Reconstruct the Optickle model
% % Construct the model with the optimized demodulation phase.
[opts2,ns2,ls2,prs2,drs2] = modelLCGT(p);

%}}}

%{{{ tickle 2

%% Tickle

display('Start Tickling for SCL, 2nd round ...')

tic
[fDCs2, sigDCs2, sigACs2, mMechs2, noiseACs2, noiseMechs2] = ptickle(opts2, [], f);
toc
display('Finished.')

%}}}

%{{{ Save the tickle results

%% Save the tickle results
save([resultDir,'TickleResultsSCL-',FieldNames{jj},'.mat'],'fDCs1','sigDCs1','sigACs1','mMechs1','noiseACs1','noiseMechs1','fDCs2','sigDCs2','sigACs2','mMechs2','noiseACs2','noiseMechs2');

%}}}

%{{{ Prepare

%Angular freq
w = 2*pi*f;

%Drive name cell array

drvNames = {'ETMX', 'ETMY', 'ITMX', 'ITMY', 'BS', 'PRM', 'SRM', 'PM', ...
                    'AM', 'PM_SCL_ETMX', 'PM_SCL_PR2', 'PM_SCL_PRMAR', ...
            'PM_SCL_REFL', 'PM_SCL_SR2', 'PM_SCL_SRMAR', 'PM_SCL_ITMtoBS', ...
            'PM_SCL_ITMAR', 'Mod1.amp', 'Mod1.phase', 'Mod2.amp', ...
            'Mod2.phase', 'Mod3.amp', 'Mod3.phase'};


%Map object to return a drive index given a drive name
actMap = containers.Map(drvNames, [1:length(drvNames)]);

%Drive indices array
iDrv = getDriveNumbers(opts1, drvNames);

% DOF to Actuator conversion matrix
Mconv = zeros(length(iDrv),5);
%DARM
Mconv(1,1)=1/2; Mconv(2,1)=-1/2;
%CARM
Mconv(1,2)=0; Mconv(2,2)=0;
Mconv(8,2)=1; 
%MICH
if p.MICHBS
    Mconv(5,3)=1; Mconv(6,3)=-1/sqrt(2);Mconv(7,3)=1/sqrt(2);
else
    Mconv(1,3)=1; Mconv(2,3)=-1;Mconv(3,3)=-1; Mconv(4,3)=1;    
end
%PRCL
Mconv(6,4)=1;
%SRCL
Mconv(7,5)=1;

%Actuator vector for individual mirror
vActETMX = zeros(length(iDrv),1);
vActETMX(actMap('ETMX')) = 1;
vActETMY = zeros(length(iDrv),1);
vActETMY(actMap('ETMY')) = 1;
vActITMX = zeros(length(iDrv),1);
vActITMX(actMap('ITMX')) = 1;
vActITMY = zeros(length(iDrv),1);
vActITMY(actMap('ITMY')) = 1;
vActBS = zeros(length(iDrv),1);
vActBS(actMap('BS')) = 1;
vActPRM = zeros(length(iDrv),1);
vActPRM(actMap('PRM')) = 1;
vActSRM = zeros(length(iDrv),1);
vActSRM(actMap('SRM')) = 1;
vActPM = zeros(length(iDrv),1);
vActPM(actMap('PM')) = 1;
vActAM = zeros(length(iDrv),1);
vActAM(actMap('AM')) = 1;
vActSCLETMX = zeros(length(iDrv),1);
vActSCLETMX(actMap('PM_SCL_ETMX')) = 1;
vActSCLPR2 = zeros(length(iDrv),1);
vActSCLPR2(actMap('PM_SCL_PR2')) = 1;
vActSCLPRMAR = zeros(length(iDrv),1);
vActSCLPRMAR(actMap('PM_SCL_PRMAR')) = 1;
vActSCLREFL = zeros(length(iDrv),1);
vActSCLREFL(actMap('PM_SCL_REFL')) = 1;
vActSCLSR2 = zeros(length(iDrv),1);
vActSCLSR2(actMap('PM_SCL_SR2')) = 1;
vActSCLSRMAR = zeros(length(iDrv),1);
vActSCLSRMAR(actMap('PM_SCL_SRMAR')) = 1;
vActSCLITMtoBS = zeros(length(iDrv),1);
vActSCLITMtoBS(actMap('PM_SCL_ITMtoBS')) = 1;
vActSCLITMAR = zeros(length(iDrv),1);
vActSCLITMAR(actMap('PM_SCL_ITMAR')) = 1;
vActf1AM = zeros(length(iDrv),1);
vActf1AM(actMap('Mod1.amp')) = 1;
vActf1PM = zeros(length(iDrv),1);
vActf1PM(actMap('Mod1.phase')) = 1;
vActf2AM = zeros(length(iDrv),1);
vActf2AM(actMap('Mod2.amp')) = 1;
vActf2PM = zeros(length(iDrv),1);
vActf2PM(actMap('Mod2.phase')) = 1;
vActf3AM = zeros(length(iDrv),1);
vActf3AM(actMap('Mod3.amp')) = 1;
vActf3PM = zeros(length(iDrv),1);
vActf3PM(actMap('Mod3.phase')) = 1;

%Probe indices
iPrb = [getProbeNum(opts1,p.signalPorts{1}), getProbeNum(opts1,p.signalPorts{2}),...
        getProbeNum(opts1,p.signalPorts{3}), getProbeNum(opts1,p.signalPorts{4}),...
        getProbeNum(opts1,p.signalPorts{5})];


%}}}

%{{{ Detector Matrix D

%% Detector Matrix D
D1=sigACs1(iPrb, iDrv, :);
D2=sigACs2(iPrb, iDrv, :);

%}}}

%{{{ Actuation Matrix (DOF to Mirrors) A 

%% Actuation Matrix (DOF to Mirrors) A 

% Mechanical TF matrix
Mtf = repmat(eye(length(iDrv), length(iDrv)),[1,1,length(w)]);
Mtf(1,1,:) = freqresp(p.tfTM,w);
Mtf(2,2,:) = freqresp(p.tfTM,w);
Mtf(3,3,:) = freqresp(p.tfTM,w);
Mtf(4,4,:) = freqresp(p.tfTM,w);
Mtf(5,5,:) = freqresp(p.tfBS,w);
Mtf(6,6,:) = freqresp(p.tfPRM,w);
Mtf(7,7,:) = freqresp(p.tfSRM,w);
% Mtf(8,8,:) is the transfer function from laser frequency feedback
% signal to the phase modulation.
% It is 1/(2*pi*f) because  f = d phi/dt
Mtf(8,8,:) = 1./(i*w);
% Mtf(9,9,:) is the transfer function from the laser power actuation
% to the power variation of the laser. It is 1/2 because this actuator
% is actually an amplitude modulator. Note that dP/P = 2*dE/E.
Mtf(9,9,:) = 1/2;

A = zeros(length(iDrv),5,length(w));
%Loop for the frequency points
for ii=[1:length(w)]
    %Mmod = diag(diag(mMech(iDrv, iDrv,ii)));
    %Mmod = mMech(iDrv, iDrv,ii);
    Mmod=eye(length(iDrv), length(iDrv));
    A(:,:,ii) = Mmod*Mtf(:,:,ii)*Mconv;
end

%}}}

%{{{ Sensing Matrix S

%% Sensing Matrix S
S = eye(5);

%}}}

%{{{ Raw loop gain

G1 = zeros(5,5,length(w));
G2 = zeros(5,5,length(w));
for ii=[1:length(w)]
    G1(:,:,ii) = S*D1(:,:,ii)*A(:,:,ii);
    G2(:,:,ii) = S*D2(:,:,ii)*A(:,:,ii);    
end

%}}}

%{{{ Feedback Matrix F 

%{{{ DARM

%% DARM

%Fdarm = zpk([-2*pi*20,-2*pi*100, -2*pi*100],[-2*pi*1,-2*pi*800, -2*pi*800, -2*pi*1500],1);
z0 = -2*pi*20;
z1 = -2*pi*p.DARMUGF/2;
p0 = -2*pi*1;
p1 = -2*pi*4*p.DARMUGF;
p2 = -2*pi*8*p.DARMUGF;
Fdarm = zpk([z0,z1,z1],[p0,p1,p1,p2],1);
%  h = bodeplot(Fdarm);
%  p1 = getoptions(h);
%  p1.FreqUnits = 'Hz';
%  p1.Grid = 'on';
%  setoptions(h,p1);

% Set dc gain
ii = find(f >= p.DARMUGF, 1);
G0 = -sign(angle(G1(1,1,ii)))/abs(G1(1,1,ii)*freqresp(Fdarm,w(ii)));

set(Fdarm, 'k', G0);

%}}}

%{{{ CARM

%% CARM
%Fcarm = zpk([-2*pi*2e3, -2*pi*100],[-2*pi*150e5, -2*pi*200e3, -2*pi*1], 1);
Fcarm = zpk([-2*pi*2e3],[-2*pi*500e3, -2*pi*200e3, ...
                    -2*pi*0.1, -2*pi*0.1],1);

% Set dc gain
UGF = 10000;
ii = find(f >= UGF, 1);
G0 = -sign(angle(G1(2,2,ii)))/abs(G1(2,2,ii)*freqresp(Fcarm,w(ii)));
set(Fcarm, 'k', G0);

%}}}

%{{{ MICH

%% MICH
z1 = -2*pi*p.MICHUGF/2;
p1 = -2*pi*4*p.MICHUGF;
p2 = -2*pi*8*p.MICHUGF;

if p.DRSE
    Fmich = zpk([z1],[p1,p2],-1);
else
    Fmich = zpk([z1,z1],[p1,p1,p2],-1);
end


% Set dc gain
ii = find(f >= p.MICHUGF, 1);
G0 = -sign(angle(G1(3,3,ii)))/abs(G1(3,3,ii)*freqresp(Fmich,w(ii)));
set(Fmich, 'k', G0);


%}}}

%{{{ PRCL

%% PRCL
z1 = -2*pi*p.PRCLUGF/2;
p1 = -2*pi*4*p.PRCLUGF;
p2 = -2*pi*8*p.PRCLUGF;
Fprc = zpk([z1,z1],[p1,p1,p2],1);

% Set dc gain
ii = find(f >= p.PRCLUGF, 1);
G0 = -sign(angle(G1(4,4,ii)))/abs(G1(4,4,ii)*freqresp(Fprc,w(ii)));
set(Fprc, 'k', G0);


%}}}

%{{{ SRCL

%% SRCL
z1 = -2*pi*p.SRCLUGF/2;
p1 = -2*pi*4*p.SRCLUGF;
p2 = -2*pi*8*p.SRCLUGF;
Fsrc = zpk([z1,z1],[p1,p1,p2],-1);

% Set dc gain
ii = find(f >= p.SRCLUGF, 1);
G0 = -sign(angle(G1(5,5,ii)))/abs(G1(5,5,ii)*freqresp(Fsrc,w(ii)));
set(Fsrc, 'k', G0);


%}}}

%{{{ F matrix

%% F matrix

F = zeros(5,5,length(w));
F(1,1,:) = squeeze(freqresp(Fdarm, w));
F(2,2,:) = squeeze(freqresp(Fcarm, w));
F(3,3,:) = squeeze(freqresp(Fmich, w));
F(4,4,:) = squeeze(freqresp(Fprc, w));
F(5,5,:) = squeeze(freqresp(Fsrc, w));

%}}}

%}}}

%{{{ Recalculate Feed forward TF

F2s = zeros(5,5,length(w));

%Measure the TFs
H = zeros(5,5,length(w));

for ii = [1:length(w)]
    G = S*D1(:,:,ii)*A(:,:,ii)*F(:,:,ii);
    H(:,:,ii) = inv(eye(5)+G);
    
    %TF from MICH error to DARM error
    Vtmp = G*[0;0;1;0;0];
    TFmich = Vtmp(1);
    Vtmp = G*[0;0;0;1;0];
    TFprc = Vtmp(1);
    Vtmp = G*[0;0;0;0;1];
    TFsrc = Vtmp(1);
    
    %TF from DARM fb to DARM error
    Vtmp = S*D1(:,:,ii)*A(:,:,ii)*[1;0;0;0;0];
    TFdarm = Vtmp(1);
    
    %Feed forward matrix
    
    FF = zeros(5,5);
    FF(1,3) = -TFmich/TFdarm;
    FF(1,4) = -TFprc/TFdarm;
    FF(1,5) = -TFsrc/TFdarm;
    
    % Update the filter matrix F
    a=p.FeedForwardError;%*(2*rand-1);
    b=0*p.FeedForwardError;%*(2*rand-1);
    F2s(:,:,ii) = F(:,:,ii) + FF*(1+a+i*b);
end

%}}}

%{{{ Couplings without FF
k = 2*pi/opt.lambda;

Getm = zeros(length(w),1);
Gpr2 = zeros(length(w),1);
Gprmar = zeros(length(w),1);
Grefl = zeros(length(w),1);
Gsr2 = zeros(length(w),1);
Gsrmar = zeros(length(w),1);
Gitmtobs = zeros(length(w),1);
Gitmar = zeros(length(w),1);

for ii = [1:length(w)]
    H1(:,:,ii) = inv(eye(5)+1*S*D1(:,:,ii)*A(:,:,ii)*F(:,:,ii));
    H2(:,:,ii) = inv(eye(5)+1*S*D2(:,:,ii)*A(:,:,ii)*F(:,:,ii));    
    Gdarm = [1,0,0,0,0]*H1(:,:,ii)*S*D1(:,:,ii)*Mconv*[1;0;0;0;0]/sqrt(p.Pin);

    Getm(ii) = abs(sqrt(abs([1,0,0,0,0]*H1(:,:,ii)*S*D1(:,:,ii)*vActSCLETMX)^2+ ...
                    abs([1,0,0,0,0]*H2(:,:,ii)*S*D2(:,:,ii)*vActSCLETMX)^2)/p.EsETMX/Gdarm);
    
    Gpr2(ii) = abs(sqrt(abs([1,0,0,0,0]*H1(:,:,ii)*S*D1(:,:,ii)*vActSCLPR2)^2+ ...
                    abs([1,0,0,0,0]*H2(:,:,ii)*S*D2(:,:,ii)*vActSCLPR2)^2)/p.EsPR2/Gdarm);

    Gprmar(ii) = abs(sqrt(abs([1,0,0,0,0]*H1(:,:,ii)*S*D1(:,:,ii)*vActSCLPRMAR)^2+ ...
                    abs([1,0,0,0,0]*H2(:,:,ii)*S*D2(:,:,ii)*vActSCLPRMAR)^2)/p.EsPRMAR/Gdarm);

    Grefl(ii) = abs(sqrt(abs([1,0,0,0,0]*H1(:,:,ii)*S*D1(:,:,ii)*vActSCLREFL)^2+ ...
                    abs([1,0,0,0,0]*H2(:,:,ii)*S*D2(:,:,ii)*vActSCLREFL)^2)/p.EsREFL/Gdarm);

    Gsr2(ii) =  abs(sqrt(abs([1,0,0,0,0]*H1(:,:,ii)*S*D1(:,:,ii)*vActSCLSR2)^2+ ...
                    abs([1,0,0,0,0]*H2(:,:,ii)*S*D2(:,:,ii)*vActSCLSR2)^2)/p.EsSR2/Gdarm);

    Gsrmar(ii) = abs(sqrt(abs([1,0,0,0,0]*H1(:,:,ii)*S*D1(:,:,ii)*vActSCLSRMAR)^2+ ...
                    abs([1,0,0,0,0]*H2(:,:,ii)*S*D2(:,:,ii)*vActSCLSRMAR)^2)/p.EsSRMAR/Gdarm);

    Gitmtobs(ii) = abs(sqrt(abs([1,0,0,0,0]*H1(:,:,ii)*S*D1(:,:,ii)*vActSCLITMtoBS)^2+ ...
                    abs([1,0,0,0,0]*H2(:,:,ii)*S*D2(:,:,ii)*vActSCLITMtoBS)^2)/p.EsITMtoBS/Gdarm);

    Gitmar(ii) = abs(sqrt(abs([1,0,0,0,0]*H1(:,:,ii)*S*D1(:,:,ii)*vActSCLITMAR)^2+ ...
                    abs([1,0,0,0,0]*H2(:,:,ii)*S*D2(:,:,ii)*vActSCLITMAR)^2)/p.EsITMAR/Gdarm);
end
%
h1 = figure(1);
loglog(f, Getm, f, Gpr2, f, Gprmar, f, Grefl, f, Gsr2, f, Gsrmar, f, ...
       Gitmtobs, f, Gitmar);

legend('ETMX HR','PRMtoBS','PRM AR', 'REFL', 'SRMtoBS','SRM AR', 'ITMXtoBS', 'BStoITMX');
title('Scattered Light Noise Coupling','FontSize',16);
xlabel('Hz', 'FontSize', 16);
h=ylabel('|G| [m/rad]','FontSize',16);
grid on

baseFileName = [resultDir, 'ScatteredLightNoise/', ...
                'ScatteredLightNoiseCouplingNoFF-',FieldNames{jj}];

print(h1, '-dpdf', [baseFileName, '.pdf']);
saveas(h1,  [baseFileName, '.fig']);
close(h1);

fid = fopen([baseFileName,'.dat'],'w');
fprintf(fid,['#Scattering light noise coupling G: G is in the unit of ' ...
             'm/rad. This is the ratio of SNXXX over DARM in the LIGO ' ...
             'report T060073. \n']);
fprintf(fid, ['#In order to get the strain equivalent DARM noise from the ' ...
              'scattered light, calculate G*Es*dphi/(L*sqrt(Pin)).\n']);
fprintf(fid, ['#Es is the field amplitude of scattered light coupled back ' ...
              'to the interferometer. dphi is the phase noise of this ' ...
              'field. L is the arm length, Pin is the input laser power.\n']);
fprintf(fid,['#freq, ETMX HR, ETMX AR, PRMtoBS , PRM AR, SRMtoBS, ' ...
             'SRM AR, ITMtoBS, BStoITM\n']);
fclose(fid);

M = [f(:), Getm, Getm*sqrt(p.ETMXThr), Gpr2, Gprmar, Grefl, ... 
     Gsr2, Gsrmar, Gitmtobs, Gitmar];

dlmwrite([baseFileName, '.dat'], M, '-append');

%}}}

%{{{ Couplings without FB
k = 2*pi/opt.lambda;

Getm = zeros(length(w),1);
Gpr2 = zeros(length(w),1);
Gprmar = zeros(length(w),1);
Grefl = zeros(length(w),1);
Gsr2 = zeros(length(w),1);
Gsrmar = zeros(length(w),1);
Gitmtobs = zeros(length(w),1);
Gitmar = zeros(length(w),1);

for ii = [1:length(w)]
    H1(:,:,ii) = inv(eye(5)+0*S*D1(:,:,ii)*A(:,:,ii)*F(:,:,ii));
    H2(:,:,ii) = inv(eye(5)+0*S*D2(:,:,ii)*A(:,:,ii)*F(:,:,ii));    
    Gdarm = [1,0,0,0,0]*H1(:,:,ii)*S*D1(:,:,ii)*Mconv*[1;0;0;0;0]/sqrt(p.Pin);

    Getm(ii) = abs(sqrt(abs([1,0,0,0,0]*H1(:,:,ii)*S*D1(:,:,ii)*vActSCLETMX)^2+ ...
                    abs([1,0,0,0,0]*H2(:,:,ii)*S*D2(:,:,ii)*vActSCLETMX)^2)/p.EsETMX/Gdarm);
    
    Gpr2(ii) = abs(sqrt(abs([1,0,0,0,0]*H1(:,:,ii)*S*D1(:,:,ii)*vActSCLPR2)^2+ ...
                    abs([1,0,0,0,0]*H2(:,:,ii)*S*D2(:,:,ii)*vActSCLPR2)^2)/p.EsPR2/Gdarm);

    Gprmar(ii) = abs(sqrt(abs([1,0,0,0,0]*H1(:,:,ii)*S*D1(:,:,ii)*vActSCLPRMAR)^2+ ...
                    abs([1,0,0,0,0]*H2(:,:,ii)*S*D2(:,:,ii)*vActSCLPRMAR)^2)/p.EsPRMAR/Gdarm);

    Grefl(ii) = abs(sqrt(abs([1,0,0,0,0]*H1(:,:,ii)*S*D1(:,:,ii)*vActSCLREFL)^2+ ...
                    abs([1,0,0,0,0]*H2(:,:,ii)*S*D2(:,:,ii)*vActSCLREFL)^2)/p.EsREFL/Gdarm);
    
    Gsr2(ii) =  abs(sqrt(abs([1,0,0,0,0]*H1(:,:,ii)*S*D1(:,:,ii)*vActSCLSR2)^2+ ...
                    abs([1,0,0,0,0]*H2(:,:,ii)*S*D2(:,:,ii)*vActSCLSR2)^2)/p.EsSR2/Gdarm);

    Gsrmar(ii) = abs(sqrt(abs([1,0,0,0,0]*H1(:,:,ii)*S*D1(:,:,ii)*vActSCLSRMAR)^2+ ...
                    abs([1,0,0,0,0]*H2(:,:,ii)*S*D2(:,:,ii)*vActSCLSRMAR)^2)/p.EsSRMAR/Gdarm);

    Gitmtobs(ii) = abs(sqrt(abs([1,0,0,0,0]*H1(:,:,ii)*S*D1(:,:,ii)*vActSCLITMtoBS)^2+ ...
                    abs([1,0,0,0,0]*H2(:,:,ii)*S*D2(:,:,ii)*vActSCLITMtoBS)^2)/p.EsITMtoBS/Gdarm);

    Gitmar(ii) = abs(sqrt(abs([1,0,0,0,0]*H1(:,:,ii)*S*D1(:,:,ii)*vActSCLITMAR)^2+ ...
                    abs([1,0,0,0,0]*H2(:,:,ii)*S*D2(:,:,ii)*vActSCLITMAR)^2)/p.EsITMAR/Gdarm);
end
%
h1 = figure(1);

loglog(f, Getm, f, Gpr2, f, Gprmar, f, Grefl, f, Gsr2, f, Gsrmar, f, ...
       Gitmtobs, f, Gitmar);

legend('ETMX HR','PRMtoBS','PRM AR', 'REFL', 'SRMtoBS','SRM AR', 'ITMXtoBS', 'BStoITMX');
title('Scattered Light Noise Coupling','FontSize',16);
xlabel('Hz', 'FontSize', 16);
h=ylabel('|G| [m/rad]','FontSize',16);
grid on

baseFileName = [resultDir, 'ScatteredLightNoise/', ...
                'ScatteredLightNoiseCouplingNoFB-',FieldNames{jj}];

print(h1, '-dpdf', [baseFileName, '.pdf']);
saveas(h1,  [baseFileName, '.fig']);
close(h1);

fid = fopen([baseFileName,'.dat'],'w');
fprintf(fid,['#Scattering light noise coupling G: G is in the unit of ' ...
             'm/rad. This is the ratio of SNXXX over DARM in the LIGO ' ...
             'report T060073. \n']);
fprintf(fid, ['#In order to get the strain equivalent DARM noise from the ' ...
              'scattered light, calculate G*Es*dphi/(L*sqrt(Pin)).\n']);
fprintf(fid, ['#Es is the field amplitude of scattered light coupled back ' ...
              'to the interferometer. dphi is the phase noise of this ' ...
              'field. L is the arm length, Pin is the input laser power.\n']);
fprintf(fid,['#freq, ETMX HR, ETMX AR, PRMtoBS , PRM AR, SRMtoBS, ' ...
             'SRM AR, ITMtoBS, BStoITM\n']);
fclose(fid);

M = [f(:), Getm, Getm*sqrt(p.ETMXThr), Gpr2, Gprmar, Grefl, ... 
     Gsr2, Gsrmar, Gitmtobs, Gitmar];

dlmwrite([baseFileName, '.dat'], M, '-append');

%}}}

%{{{ Couplings
k = 2*pi/opt.lambda;

Getm = zeros(length(w),1);
Gpr2 = zeros(length(w),1);
Gprmar = zeros(length(w),1);
Grefl = zeros(length(w),1);
Gsr2 = zeros(length(w),1);
Gsrmar = zeros(length(w),1);
Gitmtobs = zeros(length(w),1);
Gitmar = zeros(length(w),1);

for ii = [1:length(w)]
    H1(:,:,ii) = inv(eye(5)+1*S*D1(:,:,ii)*A(:,:,ii)*F2s(:,:,ii));
    H2(:,:,ii) = inv(eye(5)+1*S*D2(:,:,ii)*A(:,:,ii)*F2s(:,:,ii));    
    Gdarm = [1,0,0,0,0]*H1(:,:,ii)*S*D1(:,:,ii)*Mconv*[1;0;0;0;0]/sqrt(p.Pin);

    Getm(ii) = abs(sqrt(abs([1,0,0,0,0]*H1(:,:,ii)*S*D1(:,:,ii)*vActSCLETMX)^2+ ...
                    abs([1,0,0,0,0]*H2(:,:,ii)*S*D2(:,:,ii)*vActSCLETMX)^2)/p.EsETMX/Gdarm);
    
    Gpr2(ii) = abs(sqrt(abs([1,0,0,0,0]*H1(:,:,ii)*S*D1(:,:,ii)*vActSCLPR2)^2+ ...
                    abs([1,0,0,0,0]*H2(:,:,ii)*S*D2(:,:,ii)*vActSCLPR2)^2)/p.EsPR2/Gdarm);

    Gprmar(ii) = abs(sqrt(abs([1,0,0,0,0]*H1(:,:,ii)*S*D1(:,:,ii)*vActSCLPRMAR)^2+ ...
                    abs([1,0,0,0,0]*H2(:,:,ii)*S*D2(:,:,ii)*vActSCLPRMAR)^2)/p.EsPRMAR/Gdarm);

    Grefl(ii) = abs(sqrt(abs([1,0,0,0,0]*H1(:,:,ii)*S*D1(:,:,ii)*vActSCLREFL)^2+ ...
                    abs([1,0,0,0,0]*H2(:,:,ii)*S*D2(:,:,ii)*vActSCLREFL)^2)/p.EsREFL/Gdarm);
    
    Gsr2(ii) =  abs(sqrt(abs([1,0,0,0,0]*H1(:,:,ii)*S*D1(:,:,ii)*vActSCLSR2)^2+ ...
                    abs([1,0,0,0,0]*H2(:,:,ii)*S*D2(:,:,ii)*vActSCLSR2)^2)/p.EsSR2/Gdarm);

    Gsrmar(ii) = abs(sqrt(abs([1,0,0,0,0]*H1(:,:,ii)*S*D1(:,:,ii)*vActSCLSRMAR)^2+ ...
                    abs([1,0,0,0,0]*H2(:,:,ii)*S*D2(:,:,ii)*vActSCLSRMAR)^2)/p.EsSRMAR/Gdarm);

    Gitmtobs(ii) = abs(sqrt(abs([1,0,0,0,0]*H1(:,:,ii)*S*D1(:,:,ii)*vActSCLITMtoBS)^2+ ...
                    abs([1,0,0,0,0]*H2(:,:,ii)*S*D2(:,:,ii)*vActSCLITMtoBS)^2)/p.EsITMtoBS/Gdarm);

    Gitmar(ii) = abs(sqrt(abs([1,0,0,0,0]*H1(:,:,ii)*S*D1(:,:,ii)*vActSCLITMAR)^2+ ...
                    abs([1,0,0,0,0]*H2(:,:,ii)*S*D2(:,:,ii)*vActSCLITMAR)^2)/p.EsITMAR/Gdarm);
end
%
h1 = figure(1);
loglog(f, Getm, f, Gpr2, f, Gprmar, f, Grefl, f, Gsr2, f, Gsrmar, f, Gitmtobs, f, Gitmar);

legend('ETMX HR','PRMtoBS','PRM AR', 'REFL', 'SRMtoBS','SRM AR', 'ITMXtoBS', 'BStoITMX');
title('Scattered Light Noise Coupling','FontSize',16);
xlabel('Hz', 'FontSize', 16);
h=ylabel('|G| [m/rad]','FontSize',16);
grid on

baseFileName = [resultDir, 'ScatteredLightNoise/', 'ScatteredLightNoiseCouplingFF-',FieldNames{jj}];

print(h1, '-dpdf', [baseFileName, '.pdf']);
saveas(h1,  [baseFileName, '.fig']);
close(h1);

fid = fopen([baseFileName,'.dat'],'w');
fprintf(fid,['#Scattering light noise coupling G: G is in the unit of ' ...
             'm/rad. This is the ratio of SNXXX over DARM in the LIGO ' ...
             'report T060073. \n']);
fprintf(fid, ['#In order to get the strain equivalent DARM noise from the ' ...
              'scattered light, calculate G*Es*dphi/(L*sqrt(Pin)).\n']);
fprintf(fid, ['#Es is the field amplitude of scattered light coupled back ' ...
              'to the interferometer. dphi is the phase noise of this ' ...
              'field. L is the arm length, Pin is the input laser power.\n']);
fprintf(fid,['#freq, ETMX HR, ETMX AR, PRMtoBS , PRM AR, REFL, SRMtoBS, ' ...
             'SRM AR, ITMtoBS, BStoITM\n']);
fclose(fid);

M = [f(:), Getm, Getm*sqrt(p.ETMXThr), Gpr2, Gprmar, Grefl, ... 
     Gsr2, Gsrmar, Gitmtobs, Gitmar];

dlmwrite([baseFileName, '.dat'], M, '-append');

%}}}

%{{{ Noise Requirements

%{{{ SCL Requirement for ETM
% The phase noise induced by the reflection on a vibrating object is
% dphi = 2*k*a, where a is the vibration amplitude of the scattering
% object. k is the wavenumber.
% The requirement is also scaled lineary by the amplitude Es
% of the scattered light

k = 2*pi/opt.lambda;

aEsReq = abs(3000*TotalNoise*sqrt(p.Pin)./(2*k*Getm)/p.DispNoiseSafetyFactor);
h1 = figure(1);
loglog(f, aEsReq);

%legend('Intensity noise');
title(['Scattered Light Requirement in ArmCavity (Safety Factor',num2str(p.DispNoiseSafetyFactor),')'],'FontSize',16);
xlabel('Frequency [Hz]','FontSize',16);
h=ylabel('Vibration x Field Amplitude [m*sqrt(W)/sqrt(Hz)]','FontSize',16);
grid on

baseFileName = [resultDir, 'ScatteredLightNoise/', 'ETMXSCLRequirement-', FieldNames{jj}];
                
print(h1, '-dpdf', [baseFileName, '.pdf']);
saveas(h1,  [baseFileName, '.fig']);
close(h1);

fid = fopen([baseFileName,'.dat'],'w');
fprintf(fid,'#f, a*Es\n');
fclose(fid);

M = [f(:), aEsReq];

dlmwrite([baseFileName, '.dat'], M, '-append');

%}}}

%{{{ SCL Requirement for PRC

% dphi = 2*k*a, where a is the vibration amplitude of the scattering
% object.
% The requirement is also scaled lineary by the amplitude Es
% of the scattered light

k = 2*pi/opt.lambda;

aEsReq = abs(3000*TotalNoise*sqrt(p.Pin)./(2*k*Gpr2)/p.DispNoiseSafetyFactor);
h1 = figure(1);
loglog(f, aEsReq);

%legend('Intensity noise');
title(['Scattered Light Requirement in PRC (Safety Factor',num2str(p.DispNoiseSafetyFactor),')'],'FontSize',16);
xlabel('Frequency [Hz]','FontSize',16);
h=ylabel('Vibration x Field Amplitude [m*sqrt(W)/sqrt(Hz)]','FontSize',16);
grid on

baseFileName = [resultDir, 'ScatteredLightNoise/', 'PR2SCLRequirement-', FieldNames{jj}];

print(h1, '-dpdf', [baseFileName, '.pdf']);
saveas(h1,  [baseFileName, '.fig']);
close(h1);

fid = fopen([baseFileName,'.dat'],'w');
fprintf(fid,'#f, a*Es\n');
fclose(fid);

M = [f(:), aEsReq];

dlmwrite([baseFileName, '.dat'], M, '-append');

%}}}

%{{{ SCL Requirement for PRM AR


k = 2*pi/opt.lambda;

aEsReq = abs(3000*TotalNoise*sqrt(p.Pin)./(2*k*Gprmar)/p.DispNoiseSafetyFactor);

h1 = figure(1);
loglog(f, aEsReq);

%legend('Intensity noise');
title(['Scattered Light Requirement PRM AR (Safety Factor',num2str(p.DispNoiseSafetyFactor),')'],'FontSize',16);
xlabel('Frequency [Hz]','FontSize',16);
h=ylabel('Vibration x Field Amplitude [m*sqrt(W)/sqrt(Hz)]','FontSize',16);
grid on

baseFileName = [resultDir, 'ScatteredLightNoise/', 'PRMARSCLRequirement-', FieldNames{jj}];

print(h1, '-dpdf', [baseFileName, '.pdf']);
saveas(h1,  [baseFileName, '.fig']);
close(h1);

fid = fopen([baseFileName,'.dat'],'w');
fprintf(fid,'#f, a*Es\n');
fclose(fid);

M = [f(:), aEsReq];

dlmwrite([baseFileName, '.dat'], M, '-append');


%}}}

%{{{ SCL Requirement for REFL

k = 2*pi/opt.lambda;

aEsReq = abs(3000*TotalNoise*sqrt(p.Pin)./(2*k*Grefl)/p.DispNoiseSafetyFactor);

h1 = figure(1);
loglog(f, aEsReq);

%legend('Intensity noise');
title(['Scattered Light Requirement REFL (Safety Factor',num2str(p.DispNoiseSafetyFactor),')'],'FontSize',16);
xlabel('Frequency [Hz]','FontSize',16);
h=ylabel('Vibration x Field Amplitude [m*sqrt(W)/sqrt(Hz)]','FontSize',16);
grid on

baseFileName = [resultDir, 'ScatteredLightNoise/', 'REFLSCLRequirement-', FieldNames{jj}];

print(h1, '-dpdf', [baseFileName, '.pdf']);
saveas(h1,  [baseFileName, '.fig']);
close(h1);

fid = fopen([baseFileName,'.dat'],'w');
fprintf(fid,'#f, a*Es\n');
fclose(fid);

M = [f(:), aEsReq];

dlmwrite([baseFileName, '.dat'], M, '-append');

%}}}

%{{{ SCL Requirement for SRC

%The requirement Xreq(12,:) is for the phase dphi.
% dphi = 2*k*a, where a is the vibration amplitude of the scattering
% object.
% The requirement is also scaled lineary by the amplitude Es
% of the scattered light

k = 2*pi/opt.lambda;

aEsReq = abs(3000*TotalNoise*sqrt(p.Pin)./(2*k*Gsr2)/p.DispNoiseSafetyFactor);
h1 = figure(1);
loglog(f, aEsReq);

%legend('Intensity noise');
title(['Scattered Light Requirement in SRC (Safety Factor',num2str(p.DispNoiseSafetyFactor),')'],'FontSize',16);
xlabel('Frequency [Hz]','FontSize',16);
h=ylabel('Vibration x Field Amplitude [m*sqrt(W)/sqrt(Hz)]','FontSize',16);
grid on

baseFileName = [resultDir, 'ScatteredLightNoise/', 'SR2SCLRequirement-', FieldNames{jj}];
print(h1, '-dpdf', [baseFileName, '.pdf']);
saveas(h1,  [baseFileName, '.fig']);
close(h1);

fid = fopen([baseFileName,'.dat'],'w');
fprintf(fid,'#f, a*Es\n');
fclose(fid);

M = [f(:), aEsReq];

dlmwrite([baseFileName, '.dat'], M, '-append');

%}}}

%{{{ SCL Requirement for SRM AR

k = 2*pi/opt.lambda;

aEsReq = abs(3000*TotalNoise*sqrt(p.Pin)./(2*k*Gsrmar)/p.DispNoiseSafetyFactor);
h1 = figure(1);
loglog(f, aEsReq);

%legend('Intensity noise');
title(['Scattered Light Requirement SRM AR (Safety Factor',num2str(p.DispNoiseSafetyFactor),')'],'FontSize',16);
xlabel('Frequency [Hz]','FontSize',16);
h=ylabel('Vibration x Field Amplitude [m*sqrt(W)/sqrt(Hz)]','FontSize',16);
grid on

baseFileName = [resultDir, 'ScatteredLightNoise/', 'SRMARSCLRequirement-', FieldNames{jj}];
                
print(h1, '-dpdf', [baseFileName, '.pdf']);
saveas(h1,  [baseFileName, '.fig']);
close(h1);

fid = fopen([baseFileName,'.dat'],'w');
fprintf(fid,'#f, a*Es\n');
fclose(fid);

M = [f(:), aEsReq];

dlmwrite([baseFileName, '.dat'], M, '-append');

%}}}

%{{{ SCL Requirement for ITMtoBS

% dphi = 2*k*a, where a is the vibration amplitude of the scattering
% object.
% The requirement is also scaled lineary by the amplitude Es
% of the scattered light

k = 2*pi/opt.lambda;

aEsReq = abs(3000*TotalNoise*sqrt(p.Pin)./(2*k*Gitmtobs)/p.DispNoiseSafetyFactor);
h1 = figure(1);
loglog(f, aEsReq);

%legend('Intensity noise');
title(['Scattered Light Requirement in ITMtoBS (Safety Factor',num2str(p.DispNoiseSafetyFactor),')'],'FontSize',16);
xlabel('Frequency [Hz]','FontSize',16);
h=ylabel('Vibration x Field Amplitude [m*sqrt(W)/sqrt(Hz)]','FontSize',16);
grid on

baseFileName = [resultDir, 'ScatteredLightNoise/', 'ITMtoBSSCLRequirement-', FieldNames{jj}];
                
print(h1, '-dpdf', [baseFileName, '.pdf']);
saveas(h1,  [baseFileName, '.fig']);
close(h1);

fid = fopen([baseFileName,'.dat'],'w');
fprintf(fid,'#f, a*Es\n');
fclose(fid);

M = [f(:), aEsReq];

dlmwrite([baseFileName, '.dat'], M, '-append');

%}}}

%{{{ SCL Requirement for ITMAR


k = 2*pi/opt.lambda;

aEsReq = abs(3000*TotalNoise*sqrt(p.Pin)./(2*k*Gitmar)/p.DispNoiseSafetyFactor);
h1 = figure(1);
loglog(f, aEsReq);

%legend('Intensity noise');
title(['Scattered Light Requirement in ITM AR (Safety Factor',num2str(p.DispNoiseSafetyFactor),')'],'FontSize',16);
xlabel('Frequency [Hz]','FontSize',16);
h=ylabel('Vibration x Field Amplitude [m*sqrt(W)/sqrt(Hz)]','FontSize',16);
grid on

baseFileName = [resultDir, 'ScatteredLightNoise/', 'ITMARSCLRequirement-', FieldNames{jj}];

print(h1, '-dpdf', [baseFileName, '.pdf']);
saveas(h1,  [baseFileName, '.fig']);
close(h1);

fid = fopen([baseFileName,'.dat'],'w');
fprintf(fid,'#f, a*Es\n');
fclose(fid);

M = [f(:), aEsReq];

dlmwrite([baseFileName, '.dat'], M, '-append');

%}}}

%}}}

    
    end % for ii = 1:3
end %if p.IncludeSCL

%}}}

end %if p.LoopNoise

end %if p.doTickle

%{{{ ****** Save Notes   ***********

%% ********* Notes ***********
% Open file
NoteFile = fopen([resultDir, 'Notes.txt'],'w');
%Header
fprintf(NoteFile, '#### Optickle Simulation for LCGT LSC ####\n');

fprintf(NoteFile, '----- IFO Configuration -----\n');
if p.DRSE
    fprintf(NoteFile, 'IFO Configuration: DRSE\n');
    fprintf(NoteFile, 'Detune phase = %g [deg]\n', p.detunePhase);
else
    fprintf(NoteFile, 'IFO Configuration: BRSE\n');    
end
if p.useMZ
    fprintf(NoteFile, 'Use Mach Zehnder for RF Modulation: Yes\n');    
else
    fprintf(NoteFile, 'Use Mach Zehnder for RF Modulation: No\n');  
end
if p.useMZI
    fprintf(NoteFile, 'Use delay-line Mach Zehnder for RF Modulation: Yes\n');    
else
    fprintf(NoteFile, 'Use delay-line Mach Zehnder for RF Modulation: No\n');  
end
if p.fixedOptics
    fprintf(NoteFile, 'Fixed Optics: Yes\n');    
else
    fprintf(NoteFile, 'Fixed Optics: No\n');  
end

fprintf(NoteFile, '\n---------- DC Readout ----------\n\n');
if p.DCReadout
    fprintf(NoteFile, 'DC Readout: Yes\n');
    fprintf(NoteFile, 'Arm Offset for DC Readout: +/- %g [pm]\n', p.armOffset/pm); 
    fprintf(NoteFile, 'Pmismatch=%.3g [mW]\nPdc=%.3g [mW]\n',Pmismatch/1e-3, Pdc/1e-3);
    fprintf(NoteFile, 'Homodyne angle = %.3g [deg]\n', HDphase);
else
    fprintf(NoteFile, 'DC Readout: No\n');  
end

fprintf(NoteFile, '\n---------- Scattered Light Injection ----------\n\n');
fprintf(NoteFile, 'ETMX Es = %g\n', p.EsETMX);
fprintf(NoteFile, 'PR2 Es = %g\n', p.EsPR2);
fprintf(NoteFile, 'PRM AR Es = %g\n', p.EsPRMAR);
fprintf(NoteFile, 'SR2 Es = %g\n', p.EsSR2);
fprintf(NoteFile, 'SRM AR Es = %g\n', p.EsSRMAR);
fprintf(NoteFile, 'ITMXtoBS Es = %g\n', p.EsITMtoBS);
fprintf(NoteFile, 'ITM AR Es = %g\n', p.EsITMAR);
fprintf(NoteFile, 'MICHX PO Reflectivity = %g [ppm]\n', (1- ...
                                                  p.MICHX_POThr)/ppm);
fprintf(NoteFile, 'REFL PO Reflectivity = %g [ppm]\n', (1- ...
                                                  p.REFL_POThr)/ppm);


fprintf(NoteFile, '\n---------- Asymmetry ----------\n\n');

if p.notperfect
    fprintf(NoteFile, 'Arm Asymmetry: Yes\n');
    fprintf(NoteFile, 'Mirror reflectivity mismatch = %g%%\n', p.armAsym*100);
    fprintf(NoteFile, 'Arm loss mismatch = %g [ppm]\n', p.ArmLossAsym/ppm);
else
    fprintf(NoteFile, 'Arm Asymmetry: No\n');  
end

fprintf(NoteFile, '\n---------- Input Optics ----------\n\n');
fprintf(NoteFile, 'Input Power before modulators = %g [W]\n',p.Pin);
fprintf(NoteFile, 'Modulation frequencies: f1 = %g, f2 = %g, f3 = %g [MHz]\n',p.fmod1/MHz,...
        p.fmod2/MHz, p.fmod3/MHz);
fprintf(NoteFile, 'Modulation index: f1 = %g, f2 = %g, f3 = %g\n',abs(p.g1),...
        abs(p.g2), abs(p.g3));
fprintf(NoteFile, 'Sideband order = %g\n',p.sbOrder);

fprintf(NoteFile, '\n-------- Power Recycling -------\n\n');
fprintf(NoteFile, 'PRG=%g\nRarm=%g\n',PRG,Rarm);

fprintf(NoteFile, '\n-------- Power in various parts -------\n\n');
fprintf(NoteFile, 'Carrier Power in XARM=%g, YARM=%g\n',XARMCarrierPower,YARMCarrierPower);
fprintf(NoteFile, 'f1 Power in XARM=%g, YARM=%g\n',XARMf1Power,YARMf1Power);
fprintf(NoteFile, 'f2 Power in XARM=%g, YARM=%g\n',XARMf2Power,YARMf2Power);
fprintf(NoteFile, '\n');

fprintf(NoteFile, 'Carrier Power in PRC=%g, SRC=%g\n', ...
        PRCCarrierPower, SRCCarrierPower);
fprintf(NoteFile, 'f1 Power in PRC=%g, SRC=%g\n', ...
        PRCf1Power, SRCf1Power);
fprintf(NoteFile, 'f2 Power in PRC=%g, SRC=%g\n', ...
        PRCf2Power, SRCf2Power);
fprintf(NoteFile, '\n');

fprintf(NoteFile, 'Carrier Power at REFL=%g,  AS=%g\n', ...
        REFLCarrierPower,AS_CarrierPower);
fprintf(NoteFile, 'f1 Power at REFL=%g,  AS=%g\n', ...
        REFL_f1Power, AS_f1Power);
fprintf(NoteFile, 'f2 Power at REFL=%g,  AS=%g\n', ...
        REFL_f2Power, AS_f2Power);

fprintf(NoteFile, '\n-------- Signal Extraction -------\n\n');

fprintf(NoteFile, 'Tickling frequency = %g Hz\n\n',p.ftickle);

fprintf(NoteFile, 'DARM: %s\n',p.signalPorts{1});
fprintf(NoteFile, 'DARM UGF: %g [Hz]\n',p.DARMUGF);
fprintf(NoteFile, 'CARM: %s\n',p.signalPorts{2});
fprintf(NoteFile, 'CARM UGF: %g [Hz]\n',p.CARMUGF);
fprintf(NoteFile, 'MICH: %s\n',p.signalPorts{3});
fprintf(NoteFile, 'MICH UGF: %g [Hz]\n',p.MICHUGF);
fprintf(NoteFile, 'PRCL: %s\n',p.signalPorts{4});
fprintf(NoteFile, 'PRCL UGF: %g [Hz]\n',p.PRCLUGF);
fprintf(NoteFile, 'SRCL: %s\n',p.signalPorts{5});
fprintf(NoteFile, 'SRCL UGF: %g [Hz]\n',p.SRCLUGF);

fprintf(NoteFile, '\n-------- Sensing Matrix -------\n\n');
fprintf(NoteFile, '====== Signal Matrix [W/m] ======\n');
fprintf(NoteFile, '         DARM,     CARM,     MICH,     PRCL,   SRCL\n');
fprintf(NoteFile, '%s: %.3g, %.3g, %.3g, %.3g, %.3g\n',p.signalPorts{1},...
    abs(mSens(1,1)),abs(mSens(1,2)),abs(mSens(1,3)),abs(mSens(1,4)),abs(mSens(1,5)));
fprintf(NoteFile, '%s: %.3g, %.3g, %.3g, %.3g, %.3g\n',p.signalPorts{2},...
    abs(mSens(2,1)),abs(mSens(2,2)),abs(mSens(2,3)),abs(mSens(2,4)),abs(mSens(2,5)));
fprintf(NoteFile, '%s: %.3g, %.3g, %.3g, %.3g, %.3g\n',p.signalPorts{3},...
    abs(mSens(3,1)),abs(mSens(3,2)),abs(mSens(3,3)),abs(mSens(3,4)),abs(mSens(3,5)));
fprintf(NoteFile, '%s: %.3g, %.3g, %.3g, %.3g, %.3g\n',p.signalPorts{4},...
    abs(mSens(4,1)),abs(mSens(4,2)),abs(mSens(4,3)),abs(mSens(4,4)),abs(mSens(4,5)));

fprintf(NoteFile, '%s: %.3g, %.3g, %.3g, %.3g, %.3g\n',p.signalPorts{5},...
    abs(mSens(5,1)),abs(mSens(5,2)),abs(mSens(5,3)),abs(mSens(5,4)),abs(mSens(5,5)));

fprintf(NoteFile, '\n====== Noise Normalized Signal Matrix [1/m/sqrt(Hz)] ======\n');
fprintf(NoteFile, '         DARM,     CARM,     MICH,     PRCL,   SRCL\n');
fprintf(NoteFile, '%s: %.3g, %.3g, %.3g, %.3g, %.3g\n',p.signalPorts{1},...
    abs(mSensNorm(1,1)),abs(mSensNorm(1,2)),abs(mSensNorm(1,3)),abs(mSensNorm(1,4)),abs(mSensNorm(1,5)));
fprintf(NoteFile, '%s: %.3g, %.3g, %.3g, %.3g, %.3g\n',p.signalPorts{2},...
    abs(mSensNorm(2,1)),abs(mSensNorm(2,2)),abs(mSensNorm(2,3)),abs(mSensNorm(2,4)),abs(mSensNorm(2,5)));
fprintf(NoteFile, '%s: %.3g, %.3g, %.3g, %.3g, %.3g\n',p.signalPorts{3},...
    abs(mSensNorm(3,1)),abs(mSensNorm(3,2)),abs(mSensNorm(3,3)),abs(mSensNorm(3,4)),abs(mSensNorm(3,5)));
fprintf(NoteFile, '%s: %.3g, %.3g, %.3g, %.3g, %.3g\n',p.signalPorts{4},...
    abs(mSensNorm(4,1)),abs(mSensNorm(4,2)),abs(mSensNorm(4,3)),abs(mSensNorm(4,4)),abs(mSensNorm(4,5)));

fprintf(NoteFile, '%s: %.3g, %.3g, %.3g, %.3g, %.3g\n',p.signalPorts{5},...
    abs(mSensNorm(5,1)),abs(mSensNorm(5,2)),abs(mSensNorm(5,3)),abs(mSensNorm(5,4)),abs(mSensNorm(5,5)));

fprintf(NoteFile, '\n====== Offset Matrix [W] ======\n');
fprintf(NoteFile, '%s: %.3g\n',p.signalPorts{1}, abs(mOfs(1)));
fprintf(NoteFile, '%s: %.3g\n',p.signalPorts{2}, abs(mOfs(2)));
fprintf(NoteFile, '%s: %.3g\n',p.signalPorts{3}, abs(mOfs(3)));
fprintf(NoteFile, '%s: %.3g\n',p.signalPorts{4}, abs(mOfs(4)));
fprintf(NoteFile, '%s: %.3g\n',p.signalPorts{5}, abs(mOfs(5)));

fprintf(NoteFile, '\n====== Offset Matrix in meter [m] ======\n');
fprintf(NoteFile, '         DARM,     CARM,     MICH,     PRCL,   SRCL\n');
fprintf(NoteFile, '%s: %.3g, %.3g, %.3g, %.3g, %.3g\n',p.signalPorts{1},...
    abs(mOfsMeter(1,1)),abs(mOfsMeter(1,2)),abs(mOfsMeter(1,3)),abs(mOfsMeter(1,4)),abs(mOfsMeter(1,5)));
fprintf(NoteFile, '%s: %.3g, %.3g, %.3g, %.3g, %.3g\n',p.signalPorts{2},...
    abs(mOfsMeter(2,1)),abs(mOfsMeter(2,2)),abs(mOfsMeter(2,3)),abs(mOfsMeter(2,4)),abs(mOfsMeter(2,5)));
fprintf(NoteFile, '%s: %.3g, %.3g, %.3g, %.3g, %.3g\n',p.signalPorts{3},...
    abs(mOfsMeter(3,1)),abs(mOfsMeter(3,2)),abs(mOfsMeter(3,3)),abs(mOfsMeter(3,4)),abs(mOfsMeter(3,5)));
fprintf(NoteFile, '%s: %.3g, %.3g, %.3g, %.3g, %.3g\n',p.signalPorts{4},...
    abs(mOfsMeter(4,1)),abs(mOfsMeter(4,2)),abs(mOfsMeter(4,3)),abs(mOfsMeter(4,4)),abs(mOfsMeter(4,5)));
fprintf(NoteFile, '%s: %.3g, %.3g, %.3g, %.3g, %.3g\n',p.signalPorts{5},...
    abs(mOfsMeter(5,1)),abs(mOfsMeter(5,2)),abs(mOfsMeter(5,3)),abs(mOfsMeter(5,4)),abs(mOfsMeter(5,5)));

fprintf(NoteFile, '\n====== RF Power at each PD [W] ======\n');
fprintf(NoteFile, 'REFL1: %.3g\n', abs(REFL1_RFPower));
fprintf(NoteFile, 'REFL2: %.3g\n',abs(REFL2_RFPower));
fprintf(NoteFile, 'POP1: %.3g\n',abs(POP1_RFPower));
fprintf(NoteFile, 'POP2: %.3g\n',abs(POP2_RFPower));
fprintf(NoteFile, 'REFL_1Dm: %.3g\n',abs(REFL1Dm_RFPower));
fprintf(NoteFile, 'REFL_1Dp: %.3g\n',abs(REFL1Dp_RFPower));
fprintf(NoteFile, 'REFL_2Dm: %.3g\n',abs(REFL2Dm_RFPower));
fprintf(NoteFile, 'REFL_2Dp: %.3g\n',abs(REFL2Dp_RFPower));

fprintf(NoteFile, '\n-------- Feed Forward -------\n\n');
fprintf(NoteFile, 'Feed forward error: %.2g%%\n',p.FeedForwardError*100);

fprintf(NoteFile, '\n-------- DC Power at detection ports -------\n\n');
fprintf(NoteFile, 'REFL1: %.3g [mW]\n',P_REFL1*1e3);
fprintf(NoteFile, 'REFL2: %.3g [mW]\n',P_REFL2*1e3);
fprintf(NoteFile, 'REFL3: %.3g [mW]\n',P_REFL3*1e3);
fprintf(NoteFile, 'REFL4: %.3g [mW]\n',P_REFL4*1e3);

fprintf(NoteFile, 'POP1: %.3g [mW]\n',P_POP1*1e3);
fprintf(NoteFile, 'POP2: %.3g [mW]\n',P_POP2*1e3);

fprintf(NoteFile, 'AS_DC: %.3g [mW]\n',P_AS*1e3);
fprintf(NoteFile, 'AS_PO: %.3g [mW]\n',P_ASPO*1e3);
fprintf(NoteFile, 'OMC_REFL: %g [mW]\n',P_OMCREFL*1e3);

fprintf(NoteFile, 'POX: %.3g [mW]\n',P_POX*1e3);
fprintf(NoteFile, 'POY: %.3g [mW]\n',P_POY*1e3);

% Close file
fclose(NoteFile);

%}}}