%% Path setup

addpath('/home/aso/exp/LCGT/ISC/CoreModel/Optickle/LCGT-aso/Optickle');
addpath(genpath('/home/aso/exp/LCGT/ISC/CoreModel/Optickle/LCGT-aso/Optickle'));
addpath(genpath('/home/aso/exp/LCGT/ISC/CoreModel/Optickle/LCGT-aso/tools'));
cd('/home/aso/exp/LCGT/ISC/CoreModel/Optickle/LCGT-aso');


%% Results Dir

resultsDir = ['/home/aso/exp/LCGT/ISC/CoreModel/Optickle/LCGT-aso/results/',...
              'DRSE_withMZR1_25979/'];
cd(resultsDir)

%% Load saved parameters
load('Parameters.mat');

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

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


%% Sensing Matrix of 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);


%% DARM Sweep

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

%% CARM Sweep

    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');
    
%% MICH Sweep

    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']);
  
%% PRCL Sweep

    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');
    
%% SRCL Sweep

    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');
%% *********** Check DC fields ***************
pos=zeros(Ndrive,1);
[fDC, sigDC]=sweep(opt,pos);
%% Power at detection ports

P_REFL = abs(fDC(l.REFL,:)*conj(fDC(l.REFL,:))');
P_POX = abs(fDC(l.POX,:)*conj(fDC(l.POX,:))');
P_POY = abs(fDC(l.POY,:)*conj(fDC(l.POY,:))');
P_POP = abs(fDC(l.POP,:)*conj(fDC(l.POP,:))');
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,:))');
%% DC Readout 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);

Pmismatch = abs(fDC(l.OMCTR, find(p.vMod==0))).^2;

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

Pdc = abs(fDC(l.OMCTR, find(p.vMod==0))).^2;
%% 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


    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);
    
%% OSA plot in the SRC

    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);
    
%% OSA plot in the X arm

    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);
    
%% OSA plot in the Y arm

    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);
    
%% OSA plot at REFL

    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,1,1,'r');


    title(h(1), 'Fields at REFL', 'FontSize', 16);
    
%% OSA plot at OMC_TR


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

%% Load the tickle results
load('TickleResults.mat');
%% 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*0.9)/3000));
set(gca,'FontSize',16);
grid on
title('Pure Quantum Strain Sensitivity','FontSize',16);
xlabel('Hz','FontSize',16);
h=ylabel('1/sqrt(Hz)','FontSize',16);

%% ****** 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);
%% Raw loop gain G

G = zeros(5,5,length(w));
for ii=[1:length(w)]
    G(:,:,ii) = S*D(:,:,ii)*A(:,:,ii);
end
%% DARM Raw loop gain
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
%% CARM Raw loop gain
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
%% MICH Raw loop gain
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
%% PRCL Raw loop gain
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
%% SRCL Raw loop gain
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
%% ====== Feedback Matrix F ========
%% DARM OPLG

%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
%% CARM OPLG
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
%% MICH OPLG
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
%% PRCL OPLG
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
%% SRCL OPLG
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
%% 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 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
%% 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
%% 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
%% ===== Displacement Noise Coupling =====
%
% Seismic noise model = 1.5*10^-14/(f^6) m/sqrt(Hz) for each TM
% after the SAS attenuation.
%


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

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

%% ======= Laser Noise Couplings =======

%% Frequency noise requirement