%% Path setup

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

%% Load Saved Results
load('results/BRSE_17_45_SDM_BAE_R1_27891/Parameters.mat');
load('results/BRSE_17_45_SDM_BAE_R1_27891/TickleResults.mat');

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

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

% CARM
h1=figure(2);
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
h1=figure(3);
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
h1=figure(4);
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
h1=figure(5);
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

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

%% 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('Quantum Noise at each signal port','FontSize',16);
xlabel('Hz','FontSize',16);
h=ylabel('W/sqrt(Hz)','FontSize',16);
grid on


%% ===== 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)*Mconv*[1;0;0;0;0];
    XqnDARM(ii) = NSall(1,ii)./OptgDARM(ii);
    OptgCARM(ii) = D(2,:,ii)*Mconv*[0;1;0;0;0];
    XqnCARM(ii) = NSall(2,ii)./OptgCARM(ii);
    OptgMICH(ii) = D(3,:,ii)*Mconv*[0;0;1;0;0];
    XqnMICH(ii) = NSall(3,ii)./OptgMICH(ii);
    OptgPRCL(ii) = D(4,:,ii)*Mconv*[0;0;0;1;0];
    XqnPRCL(ii) = NSall(4,ii)./OptgPRCL(ii);
    OptgSRCL(ii) = D(5,:,ii)*Mconv*[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

%% 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));
set(gca,'FontSize',14);
legend('Total','DARM','CARM','MICH','PRC','SRC');
title('Shot Noise Coupling','FontSize',16);
xlabel('Frequency [Hz]','FontSize',16);
h=ylabel('Strain [1/sqrt(Hz)]','FontSize',16);
grid on


fid = fopen('ShotNoiseCoupling.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('ShotNoiseCoupling.dat',M, '-append');


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

% 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)*[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*[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

M = [f(:), (abs(Edarm(1,:))./abs(calib))', (abs(Eetmx)./abs(calib))',...
    (abs(Eetmy)./abs(calib))', (abs(Eitmx)./abs(calib))',...
    (abs(Eitmy)./abs(calib))',(abs(Ebs)./abs(calib))',...
    (abs(Eprm)./abs(calib))',(abs(Esrm)./abs(calib))'];


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