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.
%

%% Path setup

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

%% Read parameters
p=paramLCGT();

%% Create result directory

if p.DRSE
    resultDir = 'DRSE';
else
    resultDir = 'BRSE';
end
resultDir = [baseDir,'/results/',resultDir,'_',p.Name,'_',num2str(round(rem(now*86400,100000))),'/'];

mkdir(resultDir);

%% Flags

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

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

%% 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;

%% *********** Check DC fields ***************
pos=zeros(Ndrive,1);
[fDC, sigDC]=sweep(opt,pos);

%% 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);

%% Adjust Homodyne Phase

% 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;

% Add offset
testArmOffset = 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;

% 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.DCReadout * sqrt(PdcTarget/(Pdc/(testArmOffset^2)));

%% 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

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

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

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

%% 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

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

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

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

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

%% PRG

PRG=abs(fDC(l.PRMtoPR2, find(p.vMod==0))).^2/(abs(fDC(l.PRMbk, find(p.vMod==0))).^2);
Rarm=abs(fDC(l.ITMXtoBS, find(p.vMod==0))).^2/(abs(fDC(l.BStoITMX, find(p.vMod==0))).^2);
%% 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']);
    close(h1)
end
%% 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']);
    close(h1)
end
%% 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']);
    close(h1)
end
%% 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']);
    close(h1)
end
%% 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']);
    close(h1)
end
%% 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']);
    close(h1)
end
%% close figures
close all

%% === Optimize the demodulation phase ===
mkdir([resultDir,'VectorSensingMatrices']);
determineDemodPhase(p.vectorSensM, [resultDir,'VectorSensingMatrices/'], p);

%% Load the saved demodulation phases 
demodPhaseLCGT;

%% 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;
    
%% Save simulation parameters
save([resultDir,'Parameters.mat'],'p','opt','l','n','pr','dr');

%% === Sensing Matrix ===
[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

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']);
print(h2, '-dpdf', [resultDir, 'SensingMatrices/','SensingMatrix.pdf']);
print(h3, '-dpdf', [resultDir, 'SensingMatrices/','OffsetMatrix.pdf']);
print(h4, '-dpdf', [resultDir, 'SensingMatrices/','OffsetMatrixInMeter.pdf']);
%% Close figures
close all;

%% 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

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

%% ********* Mirror Sweeps *********
mkdir([resultDir,'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']);

    %close(h1);
end
%% 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']);

    %close(h1);
end
%% MICH Sweep

if sweepplots
    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_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_1Q', 'POP_1Q');
    print(h1, '-dpdf', [resultDir, 'Sweep/','SweepMICH.pdf']);

    close(h1);
end
%% 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_2I,:)));
        
    xlabel('Displacement [m]','FontSize',16);
    ylabel('Signal [W]','FontSize',16);
    title('PRCL Sweep','FontSize',16);
    grid on
    legend('REFL_2DmI','REFL_2I');
    print(h1, '-dpdf', [resultDir, 'Sweep/','SweepPRCL.pdf']);

    close(h1);
end
%% SRCL Sweep

if sweepplots
    Nstep = 100;
    range=1*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_1I,:)));
        
    xlabel('Displacement [m]','FontSize',16);
    ylabel('Signal [W]','FontSize',16);
    title('SRCL Sweep','FontSize',16);
    grid on
    legend('REFL_1DmI','REFL_1I');
    print(h1, '-dpdf', [resultDir,'Sweep/', 'SweepSRCL.pdf']);

    close(h1);
end
%% Close figures
close all;

if ~p.fixedOptics
%% ******** Tickle ************

f=logspace(1,4,100);
tic
[fDC, sigDC, sigAC, mMech, noiseAC, noiseMech] = ptickle(opt, [], f);
toc
%% Save the tickle results
save([resultDir,'TickleResults.mat'],'fDC','sigDC','sigAC','mMech','noiseAC','noiseMech','f');

%% Plot Pure Quantum Sensitivity
sigDARM = [0.5,-0.5]*squeeze(sigAC(pr.AS_DC, getDriveNumbers(opt, {'ETMX','ETMY'}),:));
sigDARM = sigDARM(:);

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

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']);
close(h1);
%% ****** Loop Noise Calculation **********
%******************************************
%
% Matirces for Loop Noise
%
% DOF Vector: (DARM, CARM, MICH, PRC, SRC)
%
% Mirror Vector: (ETMX, ETMY, ITMX, ITMY, BS, PRM, SRM)
%
% Probe Vector: Probes for [DARM, CARM, MICH, PRCL, SRCL]
%
%

%Angular freq
w = 2*pi*f;

%Drive indices
iDrv = [dr.ETMX, dr.ETMY, dr.ITMX, dr.ITMY, dr.BS, dr.PRM, dr.SRM];
% DOF to Mirror conversion matrix
Mconv = [1,-1,0,0,0,0,0;
        1,1,0,0,0,0,0;
        1,-1,-1,1,0,0,0;
        0,0,0,0,0,1,0;
        0,0,0,0,0,0,1]';
    
%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 

% Mechanical TF matrix
Mtf = zeros(7,7,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);

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

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

S = eye(5);
%% 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']);
close(h1);

% 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']);
close(h1);

% 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']);
close(h1);

% 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']);
close(h1);

% 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']);
close(h1);
%% ====== Feedback Matrix F ========
%% DARM
mkdir([resultDir,'OpenLoopGains']);

%Fdarm = zpk([-2*pi*100, -2*pi*100],[-2*pi*800, -2*pi*800, -2*pi*1500],1);
z1 = -2*pi*p.DARMUGF/2;
p1 = -2*pi*4*p.DARMUGF;
p2 = -2*pi*8*p.DARMUGF;
Fdarm = zpk([z1,z1],[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 = 1/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));

% 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']);
close(h1);
%% CARM
Fcarm = zpk([-2*pi*2e3],[-2*pi*150e5, -2*pi*200e3],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 = -1/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']);
close(h1);
%% 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 = -1/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']);
close(h1);
%% 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 = 1/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']);
close(h1);
%% 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 = -1/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']);
close(h1);
%% 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
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']);
close(h1);

%% 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));

for ii = [1:length(w)]
    H = inv(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/0.9*[1,0,0,0,0]*H*S*D(:,:,ii)*[0.5;-0.5;0;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']);
close(h1);
%% 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;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,5) = -TFsrc/TFdarm;
    
    % Update the filter matrix F
    a=p.FeedForwardError*(2*rand-1);
    b=p.FeedForwardError*(2*rand-1);
    F2(:,:,ii) = F(:,:,ii) + FF*(1+a+i*b);
end

% 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/0.9*[1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*[0.5;-0.5;0;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']);
close(h1);
%% ===== Displacement Noise Coupling =====
%
% Seismic noise model = 1.5*10^-14/(f^6) m/sqrt(Hz) for each TM
% after the SAS attenuation.
%

mkdir([resultDir,'DisplacementNoise']);

Eetmx = zeros(1,length(w));
Eetmy = zeros(1,length(w));
Eitmx = zeros(1,length(w));
Eitmy = zeros(1,length(w));
Ebs = zeros(1,length(w));
Eprm = zeros(1,length(w));
Esrm = zeros(1,length(w));
calib = zeros(1,length(w));
for ii = [1:length(w)]
    H(:,:,ii) = inv(eye(5)+S*D(:,:,ii)*A(:,:,ii)*F2(:,:,ii));
    xseis = 1.5e-14/(f(ii)^6);
    Eetmx(ii) = [1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*[1;0;0;0;0;0;0]*xseis;
    Eetmy(ii) = [1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*[0;1;0;0;0;0;0]*xseis;
    Eitmx(ii) = [1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*[0;0;1;0;0;0;0]*xseis;
    Eitmy(ii) = [1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*[0;0;0;1;0;0;0]*xseis;
    Ebs(ii) = [1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*[0;0;0;0;1;0;0]*xseis;
    Eprm(ii) = [1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*[0;0;0;0;0;1;0]*xseis;
    Esrm(ii) = [1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*[0;0;0;0;0;0;1]*xseis;
    calib(ii) = 3000/0.9*[1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii)*[0.5;-0.5;0;0;0;0;0];
end

%
h1 = figure(1);
loglog(f, abs(Edarm(1,:))./abs(calib),f, abs(Eetmx)./abs(calib),...
    f, abs(Eetmy)./abs(calib),f, abs(Eitmx)./abs(calib),...
    f, abs(Eitmy)./abs(calib),f, abs(Ebs)./abs(calib),...
    f, abs(Eprm)./abs(calib),f, abs(Esrm)./abs(calib));

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

Xreq = zeros(7,length(w));
for ii = [1:length(w)]
    H(:,:,ii) = inv(eye(5)+S*D(:,:,ii)*A(:,:,ii)*F2(:,:,ii));
    QN = [1,0,0,0,0]*H(:,:,ii)*S*NSdarm(:,ii);
    T = [1,0,0,0,0]*H(:,:,ii)*S*D(:,:,ii);
    Xreq(:,ii) = abs((QN./T/p.DispNoiseSafetyFactor))';
end

h1 = figure(1);
loglog(f, 1.5e-14./(f.^6), f, Xreq(1,:),f, Xreq(2,:),f, Xreq(3,:),f, Xreq(4,:),...
    f, Xreq(5,:),f, Xreq(6,:),f, Xreq(7,:));

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

%% Frequency noise requirement

%% ********* 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.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---------- 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-------- 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-------- 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);