% Noise budget script for KAGRA FSS modeling
%
% Author: Yuta Michimura

%% Add paths
clear all
close all
findNbSVNroot;  % find the root of Simulink NB
addpath(genpath([NbSVNroot 'Common/Utils']));
addpath(genpath([NbSVNroot 'Dev/Utils/']));

figdir = './figures/';  % directory to save figures

%% Define some parameters, filters, and noises
% see the Simulink file (noiseModel) for each parameter
freq = logspace(-1,7,1000);
noiseModel = 'KAGRA_FSS';
ib = 'bKAGRA';

% Constants
c = 299792458.; % speed of light in vacuum (m/s)
lamb = 1064e-9; % laser wavelength (m)
nu = c/lamb;    % laser frequency (Hz)

% Dictionaries
len = containers.Map;   % round-trip length (m)
finesse = containers.Map;   % finesse
fc = containers.Map;    % cavity pole (Hz)
CavHPF = containers.Map;  % cavity HPF (Hz/Hz)
CavLPF = containers.Map;  % cavity LPF (Hz/Hz)
Filt = containers.Map;  % filter transfer functions (V/arbV)
Act = containers.Map;   % actuator transfer functions (Hz/V)
nn = containers.Map;   % noises (Hz/rtHz)
actrange = containers.Map;  % actuation range (Hz)

% KAGRA cavity parameters
% FRC stands for fiber ring cavity in iKAGRA case, frequency reference cavity in bKAGRA case
if ib == 'iKAGRA';
    len('PMC') = 0.4;   % Miyoki PMC
    len('FRC') = 1.46*5.8;  % fiber ring cavity
    finesse('PMC') = 200;
    finesse('FRC') = 451;
elseif ib == 'bKAGRA';
    len('PMC') = 1.95;  % Ohmae PMC
    len('FRC') = 2*0.1; % ULE
    finesse('PMC') = 155;
    finesse('FRC') = 30000;
end
len('IMC') = 2*26.65;
len('CARM') = 2*3000;
finesse('IMC') = 500;
finesse('CARM') = 1530;
for kk=keys(len)
    cav = char(kk);
    fc(cav) = c/(2*len(cav)*finesse(cav));
end

% Transfer functions
for kk=keys(len)
    cav = char(kk);
    CavHPF(cav) = myzpk('zpk',[0],[fc(cav)],1/fc(cav));
    CavLPF(cav) = myzpk('zpk',[],[fc(cav)],1);
end
Filt('Temp') = myzpk('zpk',[],[10e3;30e3;10;100],1)*myzpk('zpk',[10;10;10],[0.1;0.1;0.1],1);  % Temp filter in TTFSS
Filt('TempDg') = myzpk('zpk',[10;10;10],[0.1;0.1;0.1],1);   % Digital system filter
Filt('PZT') = myzpk('zpk',[],[10e3;30e3;10],1); % PZT filter in TTFSS. Note that 200kHz-300kHz notch is ignored
Filt('EOM') = myzpk('zpk',[0;0;0;10e3;200e3],[100;100;100;1e3;1e3;4e6;4e6],1);   % EOM filter in TTFSS
TF_Common = myzpk('zpk',[4e3;20e3;20e3;4.5e3],[40;1e3;1e3;300],1);
Filt('MCe') = TF_Common*myzpk('zpk',[400],[4;4;100e3],1);    % same as in LIGO-D1002416
Filt('MCeDg') = myzpk('zpk',[],[0.01],1);   % Digital system filter
Filt('AOM') = TF_Common*myzpk('zpk',[0;0],[5;5],1);   % same as in LIGO-D1002416
if ib == 'iKAGRA';
    Filt('MCe2') = 0;   % No CARM loop for iKAGRA
    Filt('AO') = 0;
elseif ib == 'bKAGRA';
    Filt('MCe2') = TF_Common*myzpk('zpk',[400],[4;4;100e3],1);
    Filt('AO') = TF_Common*myzpk('zpk',[0;0],[5;5],1);
end
Act('Temp') = 3e9;
Act('PZT') = 1e6;
Act('EOM') = myzpk('zpk',[0],[1e9],15e-6);  % Newport 4004
Act('MCe') = myzpk('zpk',[],[1 5],280e6);
Act('AOM') = 5.3e6;

% Noises in Hz/rtHz
nn('NPRO') = 10e3./freq;

%seism = 1e-9./freq.^2;  % Kamioka seismic noise (m/rtHz)
%TF_IMCsus = myzpk(freq,[],[1 5;4 5],1);
%nn('IMC') = nu*seism/len('IMC').*abs(TF_IMCsus)';
%TF_CARMsus = myzpk(freq,[],[1 5;2 5;3 5;4 5;5 5;6 5;7 5],1); % not realistic!!!
%nn('CARM') = nu*seism/len('CARM').*abs(TF_CARMsus)';
TypeC = 8e-9*freq.^-2./(1+(2./freq).^-8);
TypeBp = 2.3e-10*freq.^-7;
TypeBpfixed = 2e-10*freq.^-5;
TypeA = sqrt((2e-17*freq.^-6.5).^2 + (2*10^-16*freq.^-8).^2)*6e6;
if ib == 'iKAGRA';
    nn('FRC') = 5e3./freq;  % http://gwdoc.icrr.u-tokyo.ac.jp/cgi-bin/private/DocDB/ShowDocument?docid=3256
    nn('CARM') = nu*TypeBp/len('CARM');  % Type-Bp fixed
elseif ib == 'bKAGRA';
    nn('FRC') = sqrt((0.1./sqrt(freq)).^2+(0.6./freq).^2);  % NOT REAL
    nn('IMC') = nu*TypeC/len('IMC');   % Type-C
    nn('CARM') = nu*TypeA/len('CARM');  % Type-A
end
nn('IMC') = nu*TypeC/len('IMC');   % Type-C
% bKAGRA frequency noise requirement after IMC without CARM loop
% (used as "Measured" amplitude spectral density of OOL NbNoiseSinks)
%freqNReq = 3e-5 + abs(myzpk(freq,[],[200;200;200],1))' + 1e5./freq.^3;
fid = fopen('./requirements/BRSE/MCFrequencyNoiseRequirement.dat');
data = textscan(fid, '%f,%f', 'CommentStyle','#', 'CollectOutput',true);
data = cell2mat(data);
freqNReq = interp1(data(:,1), data(:,2), freq, 'linear');

% Actuation ranges in Hz
% (used as "Measured" amplitude spectral density of FB NbNoiseSinks)
actrange('Temp') = 30e9;
actrange('PZT') = 100e6;
actrange('EOM') = 0.6e6;
actrange('AOM') = 40e6;
actrange('MCe') = 0;

% Switches
servocavNames = {'FRC','IMC','CARM'};
loopNames = {'Temp','PZT','EOM','FRC total','MCe','AOM','IMC total','AO','MCe2','CARM total'};
SW = ones(1,length(loopNames));


%% Print the Simulink file
try
    print(['-s',noiseModel],'-dpdf',[figdir,noiseModel,'.pdf']);
catch
    open(noiseModel);
    print(['-s',noiseModel],'-dpdf',[figdir,noiseModel,'.pdf']);
end

%% Plot filters
ii=1;
for kk=servocavNames
    cav = char(kk);
    if strcmp(cav,'FRC')
        filters = {'Temp','TempDg','PZT','EOM'};
        yls = [-14,6];
    elseif strcmp(cav,'IMC')
        filters = {'MCe','MCeDg','AOM'};
        yls = [-14,2];
    elseif strcmp(cav,'CARM')
        filters = {'MCe2','AO'};
        yls = [-14,2];
    end
    figure(10000*ii)
    for ff=filters
        ff = char(ff);
        plotbode(freq,Filt(ff))
    end
    subplot(2,1,1)
    ylim([10^yls(1),10^yls(2)]);
    xlim([1e-1,1e7]);
    set(gca,'YTick',logspace(yls(1),yls(2),yls(2)-yls(1)+1));
    set(gca,'XTick',logspace(-1,7,9));
    title([cav,' servo filters'])
    subplot(2,1,2)
    xlim([1e-1,1e7]);
    set(gca,'XTick',logspace(-1,7,9));
    legend(filters);
    saveas(gcf,[figdir,cav,'Filters.eps'], 'epsc')
    ii=ii+1;
end

%% Plot noises
figure(1111)
plotspectrum(freq,freqNReq);
hold all
for kk=keys(nn)
    kk=char(kk);
    plotspectrum(freq,nn(kk));
end
legend(['bKAGRA requirement',keys(nn)]);
ylabel('Frequency noise [Hz/rtHz]');
ylim([1e-6,1e6]);
xlim([1e-1,1e7]);
set(gca,'YTick',logspace(-6,6,13));
set(gca,'XTick',logspace(-1,7,9));
saveas(gcf,[figdir,'Noises.eps'], 'epsc')
saveas(gcf,[figdir,'Noises.png'])

%% OPENLOOP TRANSFER FUNCTIONS %%
ii=1;
for kk=servocavNames
    cav = char(kk);
    if strcmp(cav,'FRC')
        swact = 1:3;    % switches for each actuator loop
        swtot = 4;  % switches for total loop
        swcl = [];  % switches need to be closed when measuring OLTF
    elseif strcmp(cav,'IMC')
        swact = 5:6;
        swtot = 7;
        swcl = 1:4;
    elseif strcmp(cav,'CARM')
        swact = 8:9;
        swtot = 10;
        swcl = [1:4,6:7];   % MCe loop in IMC should be opened
    end
    OLTF=[];
    % OLTF for each actuator loop
    SW = zeros(1,length(loopNames));
    SW(swcl) = ones(1,length(swcl));
    SW(swtot) = ones(1,length(swtot));
    [A,B,C,D]=linmod(noiseModel);
    systm=ss(A,B,C,D);
    for kk=swact
        OLTF=[OLTF,systm(kk,kk,:)];
    end
    % OLTF for total loop
    SW = zeros(1,length(loopNames));
    SW(swcl) = ones(1,length(swcl));
    SW(swact) = ones(1,length(swact));
    [A,B,C,D]=linmod(noiseModel);
    systm=ss(A,B,C,D);
    OLTF=[OLTF,systm(swtot,swtot,:)];
    % plot OLTFs
    figure(100*ii)
    plotbode(freq,OLTF);
    legend(loopNames{swact},loopNames{swtot});
    subplot(2,1,1)
    loglog([freq(1),freq(end)],[1,1],'k')
    ylim([1e-6,1e6]);
    xlim([1e-1,1e7]);
    set(gca,'YTick',logspace(-6,6,7));
    set(gca,'XTick',logspace(-1,7,9));
    title([cav,' servo OLTFs'])
    subplot(2,1,2)
    xlim([1e-1,1e7]);
    set(gca,'XTick',logspace(-1,7,9));
    saveas(gcf,[figdir,cav,'OLTF.eps'], 'epsc')
    ii=ii+1;
end

% turn on all the switches back
SW = ones(1,length(loopNames));


%% OUT OF LOOP NOISE BUDGET %%
for kk=servocavNames
    dof = [char(kk),'OOL'];
    SW = ones(1,length(loopNames)); % turn on all the switches
    if strcmp(char(kk),'FRC')
        SW(5:10) = 0;  % disable IMC/CARM loop
    elseif strcmp(char(kk),'IMC')
        SW(8:10) = 0;   % disable CARM loop
    elseif strcmp(char(kk),'CARM')
        SW(5) = 0;  % disable MCe loop in IMC loop
    end
    % Compute noises
    [noises, sys] = nbFromSimulink(noiseModel, freq, 'dof', dof);
    saveFunctionCache();    % save cached outputs
    % Make a quick NB plot
    disp('Plotting noises')
    nb = nbGroupNoises(noiseModel, noises, sys);
    nb.sortModel();
    matlabNoisePlot(nb);
    ylim([1e-6,1e6])
    xlim([1e-1,1e7]);
    set(gca,'YTick',logspace(-6,6,13));
    set(gca,'XTick',logspace(-1,7,9));
    [~,~,~,str] = legend(gca);
    legend({'Requirement',str{2:end}},'Interpreter','None');    % Replace 'Measured' with 'Requirement'
    saveas(gcf,[figdir,dof,'.eps'], 'epsc')
    saveas(gcf,[figdir,dof,'.png'])
end


%% FEEDBACK SIGNAL NOISE BUDGET %%
SW = ones(1,length(loopNames)); % turn on all the switches
for kk=keys(Act);
    dof = [char(kk),'FB'];
    % Compute noises
    [noises, sys] = nbFromSimulink(noiseModel, freq, 'dof', dof);
    saveFunctionCache();    % save cached outputs
    disp('Plotting noises')
    nb = nbGroupNoises(noiseModel, noises, sys);
    nb.sortModel();
    matlabNoisePlot(nb);
    ylim([1e-6,1e6])
    xlim([1e-1,1e7]);
    set(gca,'YTick',logspace(-6,6,13));
    set(gca,'XTick',logspace(-1,7,9));
    [~,~,~,str] = legend(gca);
    legend({'Limit',str{2:end}},'Interpreter','None');    % Replace 'Measured' with 'Limit'
    saveas(gcf,[figdir,dof,'.eps'], 'epsc')
end