% Noise budget script for KAGRA FSS modeling
%
% Author: Masayuki Nakano
clear all
%% Add path
findNbSVNroot  % find the root of Simulink NB
addpath(genpath([NbSVNroot 'Common/Utils']));
addpath(genpath([NbSVNroot 'Dev/Utils/']));
addpath(genpath([NbSVNroot 'FSS/TTFSS_MASTER']));
addpath(genpath([NbSVNroot 'FSS/IMC_MASTER']));
addpath(genpath([NbSVNroot 'FSS/bKAGRA_phase1_IMC_FSS/data']));
addpath(genpath([NbSVNroot 'FSS/bKAGRA_phase1_IMC_FSS/functions']));
addpath(genpath([NbSVNroot 'FSS/mnutils']))
cd([NbSVNroot 'FSS/bKAGRA_phase1_IMC_FSS'])
figdir = [NbSVNroot 'FSS/bKAGRA_phase1_IMC_FSS/figures/'];
import_fittingdata
%% Define some parameters, filters, and noises
% see the Simulink file (noiseModel) for each parameter
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
Noise = containers.Map;   % noises (Hz/rtHz)
actrange = containers.Map;  % actuation range (Hz)
Elec = containers.Map;
Live_C = containers.Map;
TTFSS_Elec = TTFSS_filters();
CMS_Elec = CMS_filters();

% ADC and DAC
ADC_V2C = 2^16/40; %ADC factor [cnts/V]
DAC_C2V = 20/2^16; %DAC factor [V/cnts]

% AA and AI filters
%Analog AA/AI: 3rd order Butterworth at 10k  Hz, notch at 65535Hz
%(LIGO-T070038, LIGO-D070081)
%Digital AA/AI: /opt/rtcds/rtcore/release/src/fe/controller.c
[z,p,k] = butter(3,2*pi*1e4,'low','s');
Elec('Digital_AA') = zpk(z,p,k);
Elec('Analog_AA') = zpk(z,p,k);
Elec('Digital_AI') = zpk(z,p,k);
Elec('Analog_AI') = zpk(z,p,k);
[Act,CavLPF,Fitting,TTFSS_Elec('mixer')] = FSS_TFs(ADC_V2C,DAC_C2V,Elec); % estimate actuator and cavity transfer functions.
CavLPF('PMC') = 1;% PMC has not installed yet

%%
% livetime block

livetime = false;
if livetime
else
    Live_C('in1gain') = -14;
    Live_C('in2gain') = -1000;
    Live_C('in1pol') = 0;
    Live_C('in2pol') = 1;
    Live_C('common_compensation') = 1 ;
    Live_C('common_boost') = 0;
    Live_C('common_option') = 0;
    Live_C('common_generic') = 0;
    Live_C('fast_switch') = 0;
    Live_C('fastpol') = 1;
    Live_C('fast_option') = 0;
    Live_C('fast_gain') = 0;
    Live_C('slowpol') = 0;
    Live_C('slow_boost') = 1;
    Live_C('slow_compensation') = 1;
    Live_C('slow_generic') = 0;
    Live_C('slow_bypass') = 0;
    Live_C('slow_option') = 0;
    Live_C('TTFSS_common_gain') = 30;
    Live_C('TTFSS_fast_gain') = 15.4;
    LvFilt_MCL_SUS = zpk(-1*2*pi,-10*2*pi,10) * zpk(-0.5*2*pi,-0.05*2*pi,1) * zpk(-[100 100]*2*pi,-[10 10]*2*pi,1/10) * -0.2;
    LvFilt_TTFSS_TEMP = 0;
end
[Freq_ERRFB,ERR,FB] = import_err_fb(TTFSS_Elec);
freq = Freq_ERRFB('FSS_ERR');
% Noises in Hz/rtHz
seism = 1e-9./freq.^2;  % Kamioka seismic noise (m/rtHz)
Noise('NPRO') = 10e3./freq;
[z,p,k] = zpkdata(Act('Type-C'));
tf_typec = zpk(z{1},p{1},prod(p{1})/prod(z{1}));
c_typec = mnbode_list(tf_typec,freq,'c');
Noise('IMC') = seism .* abs(c_typec);
Noise('RefCav') = zeros(size(freq));
[Noise('TTFSS'),Noise('CMS')] = import_noise(TTFSS_Elec,CMS_Elec,Fitting,freq);
Noise('ADC') = zeros(size(freq));
Noise('DAC') = zeros(size(freq));
Noise('Driver') = zeros(size(freq));
Noise('RefCav_accorstic') = zeros(size(freq));
% Noise('RefCav_accorstic') = import_RefCavnoise(freq);
% Noise('KOACH')
% Noise('AirConditioner')



% % 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 = {'RefCav','IMC'};
loopNames = {'Temp','PZT','EOM','FSS total','MCe','AOM','IMC total'};
SW = ones(1,length(loopNames));

% SR560
SR560 = 5;

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

%% Servo
close all
TTFSS_CG = -4;
TTFSS_FG = -4;
SW = [0 0 0 0 0 0 0];
[A,B,C,D]=linmod(noiseModel);
systm=(ss(A,B,C,D));
Servo_TF = systm(8,8,:);
pzt_servo_c = mnbode_list(Servo_TF,freq,'c');
mnbode(freq,abs(pzt_servo_c),angle(pzt_servo_c)*180/pi,...
    Freq('TTFSS_PZT'),Abs('TTFSS_PZT'),Phase('TTFSS_PZT'))
%% Servo EOM
close all
TTFSS_CG = -4;
TTFSS_FG = -4;
SW = [0 0 0 0 0 0 0];
[A,B,C,D]=linmod(noiseModel);
systm=(ss(A,B,C,D));
Servo_TF = systm(3,8,:);
pzt_servo_c = mnbode_list(Servo_TF,freq,'c');
mnbode(freq,abs(pzt_servo_c),angle(pzt_servo_c)*180/pi,...
    Freq('TTFSS_EOM'),Abs('TTFSS_EOM'),Phase('TTFSS_EOM'))

%% CMS AOM
% close all
SW = [0 0 0 0 0 0 0];
[A,B,C,D]=linmod(noiseModel);
systm=(ss(A,B,C,D));
Servo_TF = systm(6,9,:);
aom_servo_c = mnbode_list(Servo_TF,freq,'c');
mnbode(freq,abs(aom_servo_c),angle(aom_servo_c)*180/pi,...
    Freq('CMS_AOM_H'),Abs('CMS_AOM_H'),Phase('CMS_AOM_H'))

%% CMS SUS
close all
SW = [0 0 0 0 0 0 0];
[A,B,C,D]=linmod(noiseModel);
systm=(ss(A,B,C,D));
Servo_TF = systm(5,9,:);
sus_servo_c = mnbode_list(Servo_TF/DAC_C2V,freq,'c');
mnbode(freq,abs(sus_servo_c),angle(sus_servo_c)*180/pi,...
    Freq('CMS_SUS'),Abs('CMS_SUS'),Phase('CMS_SUS'))

%% OLTF FSS
TTFSS_CG = 30;
TTFSS_FG = 15.4;
figure()
% close all
% cav = 'RefCav';
% if strcmp(cav,'RefCav')
%     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;
% end
% % OLTF for each actuator loop
% SW = zeros(1,length(loopNames));
% SW(swact) = ones(1,length(swact));
% SW(swtot) = zeros(1,length(swtot));
SW = [0 1 0 0 0 0 0];
[A,B,C,D]=linmod(noiseModel);
systm=(ss(A,B,C,D));
EOM_OLTF = systm(4,4,:);
eom_olg_c = mnbode_list(EOM_OLTF,Freq('OLG_FSS(OUT2-OUT1)'),'c');
SW = [0 0 1 0 0 0 0];
[A,B,C,D]=linmod(noiseModel);
systm=(ss(A,B,C,D));
PZT_OLTF = systm(4,4,:);
pzt_olg_c = mnbode_list(PZT_OLTF,Freq('OLG_FSS(OUT2-OUT1)'),'c');
SW = [0 1 1 0 0 0 0];
[A,B,C,D]=linmod(noiseModel);
systm=(ss(A,B,C,D));
FSS_OLTF = systm(4,4,:);
fss_olg_c = mnbode_list(FSS_OLTF,Freq('OLG_FSS(OUT2-OUT1)'),'c');
% mnbode(freq,abs(fss_olg_c),angle(fss_olg_c)*180/pi)

%%
mnbode(Freq('OLG_FSS(OUT2-OUT1)'),Abs('OLG_FSS(OUT2-OUT1)')*(-TTFSS_Elec('common_gain1')),Phase('OLG_FSS(OUT2-OUT1)'),...
    Freq('OLG_FSS(OUT2-OUT1)'),abs(pzt_olg_c),angle(pzt_olg_c)*180/pi,...
    Freq('OLG_FSS(OUT2-OUT1)'),abs(eom_olg_c),angle(eom_olg_c)*180/pi,...
    Freq('OLG_FSS(OUT2-OUT1)'),abs(fss_olg_c),angle(fss_olg_c)*180/pi,...
    'xlim',[1e3 2e6],'ylim',[1e-2 1e3],'linestyle',{'*','--','--','-'},...
    'legend',{'measured','olg pzt','olg eom','olg all'},'title','FSS OLG')
%% OLTF FSS with IMC
TTFSS_CG = 30;
TTFSS_FG = 15.4;
close all
SW = [0 1 1 0 1 1 1];
[A,B,C,D]=linmod(noiseModel);
systm=(ss(A,B,C,D)); 
FSS_with_IMC_OLTF = systm(4,4,:);
fss_imc_olg_c = mnbode_list(FSS_with_IMC_OLTF,freq,'c');
mnbode(Freq('OLG_FSS_with_IMC'),Abs('OLG_FSS_with_IMC')*(-TTFSS_Elec('common_gain1')),Phase('OLG_FSS_with_IMC'),...
    freq,abs(fss_imc_olg_c),angle(fss_imc_olg_c)*180/pi)

%% OLTF IMC
close all
SW = [1 1 1 1 0 1 0];
[A,B,C,D]=linmod(noiseModel);
systm=(ss(A,B,C,D)); 
FSS_with_IMC_OLTF = systm(7,7,:);
fss_imc_olg_c = mnbode_list(FSS_with_IMC_OLTF,freq,'c');
mnbode(Freq('OLG_IMC'),Abs('OLG_IMC'),Phase('OLG_IMC'),...
    freq,abs(fss_imc_olg_c),angle(fss_imc_olg_c)*180/pi)
% %% OPENLOOP TRANSFER FUNCTIONS %%
% ii=1;
% for kk=servocavNames
%     cav = char(kk);
%     if strcmp(cav,'RefCav')
%         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;
%     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));

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

%%


%% OUT OF LOOP NOISE BUDGET %%
close all
dof = 'IMC_ERR';
SW = ones(1,length(loopNames));
% Compute noises
[noises, sys] = nbFromSimulink(noiseModel, freq, 'dof', dof);
saveFunctionCache();    % save cached outputs
nb = nbGroupNoises(noiseModel, noises, sys);
nb.sortModel();
matlabNoisePlot(nb);

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