% Noise budget script for suspensions (checking actuation noises and saturations)
%
% Author: Yuta Michimura ( Original version )
% editer: Takahiro Miyamoto ( Add Type-A )

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

set(0,'defaultAxesFontSize',12);
set(0,'defaultTextFontSize',12);
set(0,'defaultAxesFontName','helvetica');
set(0,'defaultTextFontName','helvetica');
set(0,'DefaultLineMarkerSize',15);
set(0,'DefaultLineLineWidth',2);
co = [0 0 1;
      0 0.5 0;
      1 0 0;
      0 0.75 0.75;
      0.75 0 0.75;
      0.75 0.75 0;
      0.25 0.25 0.25];
set(groot,'defaultAxesColorOrder',co)

%% Define some parameters, filters, and noises
% see the Simulink file (noiseModel) for each parameter
freq = logspace(-1,4,1000);
noiseModel = 'SAS_TypeA';
IFO = 'K1';
MirName = 'ETM';

%% Displacement noise requirement and suspention type
if strcmp(MirName,'ITM')
    MirIndex = 3;
    SUSType = 'Type-A';
elseif strcmp(MirName,'ETM')
    MirIndex = 5;
    SUSType = 'Type-A';
elseif strcmp(MirName,'BS')
    MirIndex = 7;
    SUSType = 'Type-B';
elseif strcmp(MirName,'PRM')
    MirIndex = 8;
    SUSType = 'Type-Bp';
elseif strcmp(MirName,'SRM')
    MirIndex = 9;
    SUSType = 'Type-B';
elseif strcmp(MirName,'IMC')
    SUSType = 'Type-C';
end
try
    % for Type-A, Type-B, and Type-Bp suspensions
    fid = fopen('requirements/BRSE/DisplacementNoiseRequirement.dat');
    data = textscan(fid, '%f,%f,%f,%f,%f,%f,%f,%f,%f', 'CommentStyle','#', 'CollectOutput',true);
    data = cell2mat(data);
    fclose(fid);
    TMDispReq = interp1(data(:,1), data(:,MirIndex), freq, 'linear');
catch
    % for other suspensions (Type-C)
    fid = fopen('requirements/BRSE/MCDisplacementNoiseRequirement.dat');
    data = textscan(fid, '%f,%f', 'CommentStyle','#', 'CollectOutput',true);
    data = cell2mat(data);
    fclose(fid);
    TMDispReq = interp1(data(:,1), data(:,2), freq, 'linear');    
end
    
%% Dictionaries
Filt = containers.Map;  % digital filter transfer functions [cnts/cnts]
Susp = containers.Map;  % suspension actuation transfer functions [m/N]
Noise = containers.Map; % all sorts of noises
Elec = containers.Map;  % electronics gain [V/V]
CoilDriver = containers.Map; % coil driver V-I conversion [Ohm]
Coil = containers.Map; % coil gain [N/V]

%% DAC and its noise
DAC_C2V = 20/2^16;  % DAC factor [V/cnts]
DAC_Range = 2^16; % DAC range [cnts]
noise = load('Noises/adc_noise.txt');
Noise('DAC') = interp1(noise(:,1), noise(:,2), freq, 'linear')*DAC_C2V;   % DAC noise [V/rtHz]; see http://gwwiki.icrr.u-tokyo.ac.jp/JGWwiki/CLIO/Tasks/DigitalControl/Caltech_setup?action=AttachFile&do=get&target=analog_system_investigation.pdf

%% Whitening and Dewhitening filter
Whitening = myzpk('zpk',[1;1;1],[10;10;10],1);
DeWhitening = myzpk('zpk',[10;10;10],[1;1;1],1);

%% Coil Driver config
% Coil Driver Transfar function
  % HIGH Power Coil Driver
  CoDHi_TF = 40*2 ;
  % LOW Power Coil Driver
 % R1 = 750; R2 = 3900; C = 0.68e-6;
   R1 = 750; R2 = 4000; C = 0.68e-6;
   CoDLo_TF = myzpk('zpk',(1/(2*pi*C*R1)),(1/(2*pi*C*(R1+R2))),R2*2);  %Ohm; See https://dcc.ligo.org/LIGO-D070481, https://dcc.ligo.org/LIGO-T0900233

   R2 = 700;
   CoDLo_TF_High = myzpk('zpk',(1/(2*pi*C*R1)),(1/(2*pi*C*(R1+R2))),R2*2);  %Ohm; See https://dcc.ligo.org/LIGO-D070481, https://dcc.ligo.org/LIGO-T0900233
% Coil Driver Noise
  % LOW Power Coil Driver
  CoDLo_Noise = 170e-9./freq+6.4e-9; % coil driver noise [V/rtHz]; See https://dcc.ligo.org/LIGO-T0900233 (1/f noise at low frequency is assumed)
  % HIGH Power Coil Driver
  CoDHi_Noise = 57e-9./freq+2.1e-9; % coil driver noise [V/rtHz]; See Figure 3 of https://dcc.ligo.org/LIGO-T080014
  
%% Suspension chain transfer functions and noises
if strcmp(MirName,'ETM')
    
    % COIL DRIVERS SELECT 'high' or 'low'
    MN_Coil_Driver = 'low' ;
    IM_Coil_Driver = 'low'  ;
    TM_Coil_Driver = 'low'  ;
    % Actuator coil Resistance config [ohm]
    Rcoil_MN = 50 ; % resistance of coil
    Rcoil_IM = 50 ; % resistance of coil
    Rcoil_TM = 50 ; % resistance of coil
    % CoilMagnet Actuator efficiency [N/A]
    CoEff_MN = 0.015 ;
    CoEff_IM = 0.015 ;
    CoEff_TM = 0.0015 ;
                    
    if strcmp(MN_Coil_Driver,'low')
        CoilDriver('MN') = CoDLo_TF_High ;
        Noise('MN_Coil') = CoDLo_Noise ;
    elseif strcmp(MN_Coil_Driver,'high')
        CoilDriver('MN') = CoDHi_TF ;
        Noise('MN_Coil') = CoDHi_Noise ;
    end
    if strcmp(IM_Coil_Driver,'low')
        CoilDriver('IM') = CoDLo_TF_High ;
        Noise('IM_Coil') = CoDLo_Noise ;
    elseif strcmp(IM_Coil_Driver,'high')
        CoilDriver('IM') = CoDHi_TF ;
        Noise('IM_Coil') = CoDHi_Noise ;
    end
    if strcmp(TM_Coil_Driver,'low')
        CoilDriver('TM') = CoDLo_TF ;
        Noise('TM_Coil') = CoDLo_Noise ;
    elseif strcmp(TM_Coil_Driver,'high')
        CoilDriver('TM') = CoDHi_TF ;
        Noise('TM_Coil') = CoDHi_Noise ;
    end
    
    Coil('MN') = 2*CoEff_MN/(CoilDriver('MN')+Rcoil_MN);   % N/V
    Coil('IM') = 2*CoEff_IM/(CoilDriver('IM')+Rcoil_IM);   % N/V
    Coil('TM') = 4*CoEff_TM/(CoilDriver('TM')+Rcoil_TM);   % N/V
    Elec('MN_Whitening') = Whitening ;
    Elec('IM_Whitening') = Whitening ;
    Elec('TM_Whitening') = Whitening ;
    Elec('MN_DeWhitening') = DeWhitening ;
    Elec('IM_DeWhitening') = DeWhitening ;
    Elec('TM_DeWhitening') = DeWhitening ;
    noise = load('Noises/kamiokaNoisy.txt');
    Noise('Seismic') = interp1(noise(:,1), noise(:,2), freq, 'linear');
    Filt('MN') = myzpk('zpk',[1;2],[0;0],-1000);  % cnts/cnts
    Filt('IM') = myzpk('zpk',[1;2],[0;0],-1000);  % cnts/cnts
    Filt('TM') = myzpk('zpk',(0),(10),10);  % cnts/cnts
    data = load('20161007TF/TypeA_TM2MN.txt');
    gg = interp1(data(:,1), data(:,2), freq, 'linear');
    ph = interp1(data(:,1), data(:,3), freq, 'linear');
    Susp('MN_TM') = frd(gg.*exp(1i*ph/180*pi),freq,'FrequencyUnit','Hz');  % MN act to TM [m/N]
    Susp('MN_TM_fit') = myzpk('zpk',[0.4487 20;0.6982 100;1.067 100;1.6 10],[0.4246 20;0.5495 100;0.7244 100;1.009 100;1.282 100;1.629 100],0.000744); % fitted Tf used just for OLTF plot (we need tfest fuction from System Identification Toolbox to avoid this)
    data = load('20161007TF/TypeA_TM2IM.txt');
    gg = interp1(data(:,1), data(:,2), freq, 'linear');
    ph = interp1(data(:,1), data(:,3), freq, 'linear');
    Susp('IM_TM') = frd(gg.*exp(1i*ph/180*pi),freq,'FrequencyUnit','Hz');  % IM act to TM [m/N]
    Susp('IM_TM_fit') = myzpk('zpk',[0.4487 20;0.6982 100;1.067 100;1.6 100],[0.4246 20;0.5495 100;0.7244 100;1.009 100;1.282 100;1.629 100],0.000744); % fitted Tf used just for OLTF plot (we need tfest fuction from System Identification Toolbox to avoid this)
    data = load('20161007TF/TypeA_TM2TM.txt');
    gg = interp1(data(:,1), data(:,2), freq, 'linear');
    ph = interp1(data(:,1), data(:,3), freq, 'linear');
    Susp('TM_TM') = frd(gg.*exp(1i*ph/180*pi),freq,'FrequencyUnit','Hz');  % TM act to TM [m/N]
    Susp('TM_TM_fit') = myzpk('zpk',[],[1 1],0.1);   % fitted TF
    dataH = load('TypeA_seismic_TF.txt');
    dataV = load('TypeA_seismic_vertical.txt');
    gg = interp1(dataH(:,1), dataH(:,2), freq, 'linear');
    ph = interp1(dataH(:,1), dataH(:,3), freq, 'linear');
    Susp('Seism_TM') = frd(gg.*exp(1i*ph/180*pi),freq,'FrequencyUnit','Hz');  % Seismic noise to TM [m/m]
    gg = interp1(dataV(:,1), dataV(:,2), freq, 'linear');
    ph = interp1(dataV(:,1), dataV(:,3), freq, 'linear');
    Susp('Seism_Vert') = frd(gg.*exp(1i*ph/180*pi),freq,'FrequencyUnit','Hz');  % Seismic noise to TM from vertical coupling [m/m]
    VertCoup = 0.01;    % vertical coupling
elseif strcmp(MirName,'PRM')
    % HIGH POWER COIL DRIVERS FOR BOTH IM AND TM
    Rcoil = 13; % resistance of coil
    CoilDriver('IM') = 40*2;    % Ohm;  See https://dcc.ligo.org/LIGO-T0900232, https://dcc.ligo.org/LIGO-D0902747
    CoilDriver('TM') = 40*2;    % Ohm;  See https://dcc.ligo.org/LIGO-T0900232, https://dcc.ligo.org/LIGO-D0902747
    Coil('IM') = 1.12/(CoilDriver('IM')+Rcoil);   % N/V; See http://gwdoc.icrr.u-tokyo.ac.jp/cgi-bin/DocDB/ShowDocument?docid=3239
    Coil('TM') = 4*0.129/(CoilDriver('TM')+Rcoil);   % N/V; See http://gwdoc.icrr.u-tokyo.ac.jp/cgi-bin/DocDB/ShowDocument?docid=3239
    Noise('IM_Coil') = 57e-9./freq+2.1e-9; % coil driver noise [V/rtHz]; See Figure 3 of https://dcc.ligo.org/LIGO-T080014
    Noise('TM_Coil') = 57e-9./freq+2.1e-9; % coil driver noise [V/rtHz]; See Figure 3 of https://dcc.ligo.org/LIGO-T080014
    Elec('IM_Whitening') = myzpk('zpk',[1;1;1],[10;10;10],1);
    Elec('TM_Whitening') = myzpk('zpk',[1;1;1],[10;10;10],1);
    Elec('IM_DeWhitening') = myzpk('zpk',[10;10;10],[1;1;1],1);
    Elec('TM_DeWhitening') = myzpk('zpk',[10;10;10],[1;1;1],1);
    noise = load('Noises/kamiokaNoisy.txt');
    Noise('Seismic') = interp1(noise(:,1), noise(:,2), freq, 'linear');
    Filt('IM') = myzpk('zpk',[1;2],[0;0],-2);  % cnts/cnts
    Filt('TM') = myzpk('zpk',(0),(10),0.02);  % cnts/cnts
    % Type-B parameters http://gwdoc.icrr.u-tokyo.ac.jp/cgi-bin/DocDB/ShowDocument?docid=571
    mIM = 15.6; % mass of IM [kg]
    mTM = 10.7;  % mass of TM [kg]
    lIM = 0.5;  % wire length from BF to IM [m]
    lTM = 0.5;  % wire length from IM to TM [m]
    g = 9.806;
    f1 = 1/(2*pi)*sqrt(g/lTM);
    f2 = 1/(2*pi)*sqrt(g/lIM);
    Susp('IM_TM') = myzpk('zpk',[],[f1 100;f2*sqrt(1+mTM/mIM) 100],1/(mTM+mIM)/(2*pi*f2)^2);
    Susp('IM_TM_fit') = Susp('IM_TM');
    Susp('TM_TM') = myzpk('zpk',[],[f1 100],1/mTM/(2*pi*f1)^2);
    Susp('TM_TM_fit') = Susp('TM_TM');
    dataH = load('./suspensionTFs/typeBp_TF_length.dat');
    dataV = load('./suspensionTFs/typeBp_TF_vertical.dat');
    gg = interp1(dataH(:,1), dataH(:,2), freq, 'linear');
    ph = interp1(dataH(:,1), dataH(:,3), freq, 'linear');
    Susp('Seism_TM') = frd(gg.*exp(1i*ph/180*pi),freq,'FrequencyUnit','Hz');  % Seismic noise to TM [m/m]
    gg = interp1(dataV(:,1), dataV(:,2), freq, 'linear');
    ph = interp1(dataV(:,1), dataV(:,3), freq, 'linear');
    Susp('Seism_Vert') = frd(gg.*exp(1i*ph/180*pi),freq,'FrequencyUnit','Hz');  % Seismic noise to TM from vertical coupling [m/m]
    VertCoup = 0.01;    % vertical coupling
elseif strcmp(MirName,'BS')
    % LOW POWER COIL DRIVERS ARE USED FOR BOTH IM AND TM
    Rcoil = 13; % resistance of coil
    R1 = 750; R2 = 3900; C = 0.68e-6;
    CoilDriver('IM') = myzpk('zpk',(1/(2*pi*C*R1)),(1/(2*pi*C*(R1+R2))),R2*2);  %Ohm; See https://dcc.ligo.org/LIGO-D070481, https://dcc.ligo.org/LIGO-T0900233
    R1 = 750; R2 = 3900; C = 0.68e-6;
    CoilDriver('TM') = myzpk('zpk',(1/(2*pi*C*R1)),(1/(2*pi*C*(R1+R2))),R2*2);  %Ohm; See https://dcc.ligo.org/LIGO-D070481, https://dcc.ligo.org/LIGO-T0900233
    Coil('IM') = 1.12/(CoilDriver('IM')+Rcoil);   % N/V; See http://gwdoc.icrr.u-tokyo.ac.jp/cgi-bin/DocDB/ShowDocument?docid=3239
    Coil('TM') = 4*0.0138/(CoilDriver('TM')+Rcoil);   % N/V; See http://gwdoc.icrr.u-tokyo.ac.jp/cgi-bin/DocDB/ShowDocument?docid=3239
    Noise('IM_Coil') = 170e-9./freq+6.4e-9; % coil driver noise [V/rtHz]; See https://dcc.ligo.org/LIGO-T0900233 (1/f noise at low frequency is assumed)
    Noise('TM_Coil') = 170e-9./freq+6.4e-9; % coil driver noise [V/rtHz]; See https://dcc.ligo.org/LIGO-T0900233 (1/f noise at low frequency is assumed)
    Elec('IM_Whitening') = myzpk('zpk',[1;1;1],[10;10;10],1);
    Elec('TM_Whitening') = myzpk('zpk',[1;1;1],[10;10;10],1);
    Elec('IM_DeWhitening') = myzpk('zpk',[10;10;10],[1;1;1],1);
    Elec('TM_DeWhitening') = myzpk('zpk',[10;10;10],[1;1;1],1);
    noise = load('Noises/kamiokaNoisy.txt');
    Noise('Seismic') = interp1(noise(:,1), noise(:,2), freq, 'linear');
    Filt('IM') = myzpk('zpk',[1;2],[0;0],-100);  % cnts/cnts
    Filt('TM') = myzpk('zpk',(0),(10),10);  % cnts/cnts
    data = load('./suspensionTFs/BS_IM2TM.dat');
    gg = interp1(data(:,1), data(:,2), freq, 'linear');
    ph = interp1(data(:,1), data(:,3), freq, 'linear');
    Susp('IM_TM') = frd(gg.*exp(1i*ph/180*pi),freq,'FrequencyUnit','Hz');  % IM act to TM [m/N]
    Susp('IM_TM_fit') = myzpk('zpk',[0.4487 20;0.6982 100;1.067 100;1.6 100],[0.4246 20;0.5495 100;0.7244 100;1.009 100;1.282 100;1.629 100],0.000744); % fitted Tf used just for OLTF plot (we need tfest fuction from System Identification Toolbox to avoid this)
    data = load('./suspensionTFs/BS_TM2TM.dat');
    gg = interp1(data(:,1), data(:,2), freq, 'linear');
    ph = interp1(data(:,1), data(:,3), freq, 'linear');
    Susp('TM_TM') = frd(gg.*exp(1i*ph/180*pi),freq,'FrequencyUnit','Hz');  % TM act to TM [m/N]
    Susp('TM_TM_fit') = myzpk('zpk',[],[0.6918 1000],0.0028);   % fitted TF
    dataH = load('./suspensionTFs/BS_seismic_TF.dat');
    dataV = load('./suspensionTFs/BS_seism_vertical.dat');
    gg = interp1(dataH(:,1), dataH(:,2), freq, 'linear');
    ph = interp1(dataH(:,1), dataH(:,3), freq, 'linear');
    Susp('Seism_TM') = frd(gg.*exp(1i*ph/180*pi),freq,'FrequencyUnit','Hz');  % Seismic noise to TM [m/m]
    gg = interp1(dataV(:,1), dataV(:,2), freq, 'linear');
    ph = interp1(dataV(:,1), dataV(:,3), freq, 'linear');
    Susp('Seism_Vert') = frd(gg.*exp(1i*ph/180*pi),freq,'FrequencyUnit','Hz');  % Seismic noise to TM from vertical coupling [m/m]
    VertCoup = 0.01;    % vertical coupling
elseif strcmp(MirName,'SRM')
    % LOW POWER COIL DRIVERS FOR BOTH IM AND TM
    Rcoil = 13; % resistance of coil
    R1 = 750; R2 = 3900; C = 0.68e-6;
    CoilDriver('IM') = myzpk('zpk',(1/(2*pi*C*R1)),(1/(2*pi*C*(R1+R2))),R2*2);  %Ohm See https://dcc.ligo.org/LIGO-D070481, https://dcc.ligo.org/LIGO-T0900233
    R1 = 750; R2 = 3900; C = 0.68e-6;
    CoilDriver('TM') = myzpk('zpk',(1/(2*pi*C*R1)),(1/(2*pi*C*(R1+R2))),R2*2);  %Ohm See https://dcc.ligo.org/LIGO-D070481, https://dcc.ligo.org/LIGO-T0900233
    Coil('IM') = 1.12/(CoilDriver('IM')+Rcoil);   % N/V; See http://gwdoc.icrr.u-tokyo.ac.jp/cgi-bin/DocDB/ShowDocument?docid=3239
    Coil('TM') = 4*0.0225/(CoilDriver('TM')+Rcoil);   % N/V; See http://gwdoc.icrr.u-tokyo.ac.jp/cgi-bin/DocDB/ShowDocument?docid=3239
    Noise('IM_Coil') = 170e-9./freq+6.4e-9; % coil driver noise [V/rtHz]; See https://dcc.ligo.org/LIGO-T0900233 (1/f noise at low frequency is assumed)
    Noise('TM_Coil') = 170e-9./freq+6.4e-9; % coil driver noise [V/rtHz]; See https://dcc.ligo.org/LIGO-T0900233 (1/f noise at low frequency is assumed)
    Elec('IM_Whitening') = myzpk('zpk',[1;1;1],[10;10;10],1);
    Elec('TM_Whitening') = myzpk('zpk',[1;1;1],[10;10;10],1);
    Elec('IM_DeWhitening') = myzpk('zpk',[10;10;10],[1;1;1],1);
    Elec('TM_DeWhitening') = myzpk('zpk',[10;10;10],[1;1;1],1);
    noise = load('./Noises/kamiokaNoisy.txt');
    Noise('Seismic') = interp1(noise(:,1), noise(:,2), freq, 'linear');
    Filt('IM') = myzpk('zpk',[1;2],[0;0],-50);  % cnts/cnts
    Filt('TM') = myzpk('zpk',(0),(10),5);  % cnts/cnts
    % Type-B parameters http://gwdoc.icrr.u-tokyo.ac.jp/cgi-bin/DocDB/ShowDocument?docid=571
    mIM = 15.6; % mass of IM [kg]
    mTM = 10.7;  % mass of TM [kg]
    lIM = 0.5;  % wire length from BF to IM [m]
    lTM = 0.5;  % wire length from IM to TM [m]
    g = 9.806;
    f1 = 1/(2*pi)*sqrt(g/lTM);
    f2 = 1/(2*pi)*sqrt(g/lIM);
    Susp('IM_TM') = myzpk('zpk',[],[f1 100;f2*sqrt(1+mTM/mIM) 100],1/(mTM+mIM)/(2*pi*f2)^2);
    Susp('IM_TM_fit') = Susp('IM_TM');
    Susp('TM_TM') = myzpk('zpk',[],[f1 100],1/mTM/(2*pi*f1)^2);
    Susp('TM_TM_fit') = Susp('TM_TM');
    dataH = load('./suspensionTFs/typeB_TF_length.dat');
    dataV = load('./suspensionTFs/typeB_TF_vertical.dat');
    gg = interp1(dataH(:,1), dataH(:,2), freq, 'linear');
    ph = interp1(dataH(:,1), dataH(:,3), freq, 'linear');
    Susp('Seism_TM') = frd(gg.*exp(1i*ph/180*pi),freq,'FrequencyUnit','Hz');  % Seismic noise to TM [m/m]
    gg = interp1(dataV(:,1), dataV(:,2), freq, 'linear');
    ph = interp1(dataV(:,1), dataV(:,3), freq, 'linear');
    Susp('Seism_Vert') = frd(gg.*exp(1i*ph/180*pi),freq,'FrequencyUnit','Hz');  % Seismic noise to TM from vertical coupling [m/m]
    VertCoup = 0.01;    % vertical coupling
elseif strcmp(MirName,'IMC')
    Filt('IM') = 0;  % no feedback on IM
    Filt('TM') = 0.4;  % cnts/cnts
    CoilDriver('IM') = 0;   % no feedback on IM
    CoilDriver('TM') = 50;  % See http://tamago.mtk.nao.ac.jp/tama/ifo/general_lib/circuits/000414_coil_driver/coildrv.pdf
    Coil('IM') = 0;   % no feedback on IM
    m = pi*(95.95e-3/2)^2*29.5e-3*2.2e3; % IMC mass [kg]
    Coil('TM') = 6.9e-6*m*(0.944*2*pi)^2;   % N/V; See http://gwclio.icrr.u-tokyo.ac.jp/lcgtsubgroup/inoutoptics/2015/06/mce625-26.html
    Susp('IM_TM') = 0;  % no feedback on IM
    Susp('IM_TM_fit') = 0; % no feedback on IM
    Susp('TM_TM') = myzpk('zpk',[],[0.944,4.03],1/m/(0.944*2*pi)^2);  % TM act to TM [m/N]; See http://gwclio.icrr.u-tokyo.ac.jp/lcgtsubgroup/inoutoptics/2015/06/mce625-26.html
    Susp('TM_TM_fit') = Susp('TM_TM');   % fitted TF
    Noise('IM_Coil') = 0; % no feedback on IM
    Noise('TM_Coil') = 0.92e-9+2e-8./freq+2e-11*freq.^0.5; % coil driver noise [V/rtHz]; See Figure 9 of http://tamago.mtk.nao.ac.jp/tama/ifo/general_lib/circuits/000414_coil_driver/coildrv.pdf
    Elec('IM_Whitening') = 1;
    Elec('TM_Whitening') = 1; % no whitening filters
    Elec('IM_DeWhitening') = 1;
    Elec('TM_DeWhitening') = 1;
    noise = load('./Noises/kamiokaNoisy.txt');
    Noise('Seismic') = interp1(noise(:,1), noise(:,2), freq, 'linear');
    dataH = load('./suspensionTFs/TypeC_seismic_TF.txt');
    gg = interp1(dataH(:,1), dataH(:,2), freq, 'linear');
    ph = interp1(dataH(:,1), dataH(:,3), freq, 'linear');
    Susp('Seism_TM') = frd(gg.*exp(1i*ph/180*pi),freq,'FrequencyUnit','Hz'); % Seismic noise to TM [m/m]; Generated with ./SuspensionTFs/LCGT_SAS_TypeC.m from Takahashi-san
    Susp('Seism_Vert') = myzpk('zpk',[],[6 20;32 20],1);  % Seismic noise to TM from vertical coupling [m/m]; Isolation ratio from stacks are not included, only from double-pendulum; From Arai-san's proto_iso_flex_damp.dat
    VertCoup = 0.01;    % vertical coupling
end

%% AI filters
% Anaglog AA/AI: 3rd-order Butterworth at 10kHz, notch at 65535Hz (LIGO-T070038, LIGO-D070081)
% Digital AA/AI: /opt/rtcds/rtscore/release/src/fe/controller.c
[z,p,k]=butter(3,2*pi*1e4,'low','s');
Elec('Digital_AI') = zpk(z,p,k);
Elec('Analog_AI') = zpk(z,p,k);

%% LSC filter (including PD signal chain and such)
Filt('LSC') = myzpk('zpk',(20),(200),3e12);

%% Switches
loopNames = {'Overall','TM','IM','MN'};
SW = ones(1,length(loopNames));

%% Plot noises, transfer functions
% used for modeling report
pleaseplot = 1; % make it 0 if you want to skip plotting
if pleaseplot
   % print Simulink model
%    try
%        print(['-s',noiseModel],'-dpdf',[figdir,noiseModel,'.pdf']);
%    catch
%        open(noiseModel);
%        print(['-s',noiseModel],'-dpdf',[figdir,noiseModel,'.pdf']);
%    end
   % plot suspension transfer functions
   figure(10)
   if strcmp(MirName,'BS') || strcmp(MirName,'PRM') || strcmp(MirName,'SRM') 
     plotbode(freq,Susp('IM_TM'))
     plotbode(freq,Susp('TM_TM'))
     legend({'IM to TM','TM to TM'})
   elseif strcmp(MirName,'ETM') || strcmp(MirName,'ITM')
     plotbode(freq,Susp('MN_TM'))
     plotbode(freq,Susp('IM_TM'))
     plotbode(freq,Susp('TM_TM'))
     legend({'MN to TM','IM to TM','TM to TM'})
   elseif strcmp(MirName,'IMC')
     plotbode(freq,Susp('TM_TM'))
     legend({'TM to TM'})
   end
   subplot(2,1,1)
   ylabel('Gain [m/N]')
   ylim([1e-9,1e1])
   set(gca,'YTick',logspace(-9,1,11));
   xlim([freq(1),freq(end)]);
   set(gca,'XTick',logspace(log10(freq(1)),log10(freq(end)),log10(freq(end))-log10(freq(1))+1));
   saveas(gcf,[figdir,MirName,'Susp.eps'],'epsc')
   % plot coil driver I-V conversion factors
   figure(20)
   if strcmp(MirName,'BS') || strcmp(MirName,'SRM')  
       plotbode(freq,[CoilDriver('IM'), CoilDriver('TM')])
       legend({'low power (IM)','low power (TM)'})
   elseif strcmp(MirName,'ETM') || strcmp(MirName,'ITM')
       plotbode(freq,[CoilDriver('MN'),CoilDriver('IM'), CoilDriver('TM')])
       legend({'low power (MN)','low power (IM)','low power (TM)'})       
   elseif strcmp(MirName,'PRM')
       plotbode(freq,[CoilDriver('IM'), CoilDriver('TM')])
       legend({'high power (IM)','high power (TM)'})
   elseif strcmp(MirName,'IMC')
       plotbode(freq,CoilDriver('TM'))
       legend({'IMC coil driver'})
   end
   xlim([freq(1),freq(end)]);
   set(gca,'XTick',logspace(log10(freq(1)),log10(freq(end)),log10(freq(end))-log10(freq(1))+1));
   subplot(2,1,1)
   ylabel('Gain [Ohm]')
   ylim([1e1,1e4])
   set(gca,'YTick',logspace(1,4,4));
   xlim([freq(1),freq(end)]);
   set(gca,'XTick',logspace(log10(freq(1)),log10(freq(end)),log10(freq(end))-log10(freq(1))+1));
   saveas(gcf,[figdir,MirName,'CoilDriver.eps'],'epsc')   
   % plot coil driver noises
   figure(30)
   if strcmp(MirName,'BS') || strcmp(MirName,'SRM') 
       plotspectrum(freq,[Noise('IM_Coil')',Noise('TM_Coil')'])
       legend({'low power (IM)','low power (TM)'})
   elseif strcmp(MirName,'ETM') || strcmp(MirName,'ITM')
       plotspectrum(freq,[Noise('MN_Coil')',Noise('IM_Coil')',Noise('TM_Coil')'])
       legend({'low power (MN)','low power (IM)','low power (TM)'})
   elseif strcmp(MirName,'PRM')
       plotspectrum(freq,[Noise('IM_Coil')',Noise('TM_Coil')'])
       legend({'high power (IM)','high power (TM)'})
   elseif strcmp(MirName,'IMC')
       plotspectrum(freq,(Noise('TM_Coil')'))
       legend({'IMC coil driver'})
   end
   xlim([freq(1),freq(end)]);
   set(gca,'XTick',logspace(log10(freq(1)),log10(freq(end)),log10(freq(end))-log10(freq(1))+1));
   ylabel('Input equivalent noise [V/rtHz]')
   ylim([1e-9,1e-6])
   set(gca,'YTick',logspace(-9,-6,4));
   saveas(gcf,[figdir,MirName,'CoilDriverNoise.eps'],'epsc')
   % plot DAC noise
   figure(40)
   plotspectrum(freq,Noise('DAC'))
   xlim([freq(1),freq(end)]);
   set(gca,'XTick',logspace(log10(freq(1)),log10(freq(end)),log10(freq(end))-log10(freq(1))+1));
   ylabel('Noise [V/rtHz]')
   ylim([1e-6,1e-4])
   set(gca,'YTick',logspace(-6,-4,3));
   saveas(gcf,[figdir,'DACNoise.eps'],'epsc')
   % seismic attenuation ratio
   figure(50)
   plotbode(freq,Susp('Seism_TM'))
   plotbode(freq,Susp('Seism_Vert'))
   legend({'Seis to TM','Seis to TM vert'})
   subplot(2,1,1)
   ylabel('Gain [m/m]')
   ylim([1e-14,1e2])
   set(gca,'YTick',logspace(-14,2,17)) ;
   xlim([freq(1),freq(end)]) ;
   set(gca,'XTick',logspace(log10(freq(1)),log10(freq(end)),log10(freq(end))-log10(freq(1))+1));
   saveas(gcf,[figdir,MirName,'Seis.eps'],'epsc')
   % plot seismic noise
   figure(60)
   plotspectrum(freq,Noise('Seismic'))
   xlim([freq(1),freq(end)]);
   set(gca,'XTick',logspace(log10(freq(1)),log10(freq(end)),log10(freq(end))-log10(freq(1))+1));
   ylabel('Seismic Noise [m/rtHz]')
   ylim([1e-13,1e-5])
   set(gca,'YTick',logspace(-13,-5,9));
   saveas(gcf,[figdir,'SeismicNoise.eps'],'epsc')
   % plot whitening/dewhitening filters
   figure(70)
   plotbode(freq,[Elec('IM_Whitening'), Elec('TM_Whitening'),Elec('IM_DeWhitening'),Elec('TM_DeWhitening')])
   legend({'whitening (IM)','whitening (TM)','dewhitening (IM)','dewhitening (TM)'})
   xlim([freq(1),freq(end)]);
   set(gca,'XTick',logspace(log10(freq(1)),log10(freq(end)),log10(freq(end))-log10(freq(1))+1));
   subplot(2,1,1)
   ylabel('Gain [V/V, cnt/cnt]')
   ylim([1e-4,1e4])
   set(gca,'YTick',logspace(-4,4,9));
   xlim([freq(1),freq(end)]);
   set(gca,'XTick',logspace(log10(freq(1)),log10(freq(end)),log10(freq(end))-log10(freq(1))+1));
   saveas(gcf,[figdir,MirName,'Whitening.eps'],'epsc')
end

%% Plot openloop transfer function
swact = 2:3;  % switches for each actuator loop
swtot = 1;  % switches for the total loop
OLTF=[];
% OLTF for each actuator loop
SW = zeros(1,length(loopNames));
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(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)
plotbode(freq,OLTF);
%% plot decoration
legend(loopNames{swact},loopNames{swtot});
subplot(2,1,1)
loglog([freq(1),freq(end)],[1,1],'k')
ylim([1e-6,1e6]);
xlim([freq(1),freq(end)]);
set(gca,'YTick',logspace(-6,6,7));
set(gca,'XTick',logspace(log10(freq(1)),log10(freq(end)),log10(freq(end))-log10(freq(1))+1));
title([MirName,' OLTF'])
subplot(2,1,2)
xlim([freq(1),freq(end)]);
set(gca,'XTick',logspace(log10(freq(1)),log10(freq(end)),log10(freq(end))-log10(freq(1))+1));
saveas(gcf,[figdir,MirName,'OLTF.png'])
saveas(gcf,[figdir,MirName,'OLTF.eps'],'epsc')

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

%% Compute TM displacement noises and plot
% Compute noises
[noises, sys] = nbFromSimulink(noiseModel, freq, 'dof', 'TMDisp');

%% save cached outputs
saveFunctionCache();

%% Plot noise budget
set(0, 'DefaultAxesFontSize',14);
disp('Plotting noises')
nb = nbGroupNoises(noiseModel, noises, sys);
nb.sortModel();
matlabNoisePlot(nb);
%%
figure(1)
loglog(freq,TMDispReq,'b-');  % plot requirement
ylim([1e-21,1e-10]);
xlim([1e-1,1e2]);
set(gca,'YTick',logspace(-21,-10,12));
set(gca,'XTick',logspace(-1,3,5));
leg = legend(gca);
%legend({'Requirement',leg.String{3:end}},'Interpreter','None');    % Replace 'Measured' with 'Requirement'
legend({'Requirement','Sum','Actuator noise','Seismic nise'},'Interpreter','None');    % Replace 'Measured' with 'Requirement'
saveas(gcf,[figdir,MirName,'DispNoise.png'])
saveas(gcf,[figdir,MirName,'DispNoise.eps'],'epsc')
%%
figure(2)
loglog(freq,TMDispReq,'-','Color',[0 0.5 0]);  % plot requirement
ylim([1e-26,1e-13]);
xlim([1e1,1e2]);
set(gca,'YTick',logspace(-26,-13,14));
set(gca,'XTick',logspace(1,2,2));
leg = legend(gca);
%legend({leg.String{:},'Requirement'},'Interpreter','None');
legend({'Sum','SAS/MR CoilDriver','SAS/MR DAC','SAS/IM CoilDriver','SAS/IM DAC','SAS/TM CoilDriver','SAS/TM DAC','Requirement'});
saveas(gcf,[figdir,MirName,'ActuatorNoise.png'])
saveas(gcf,[figdir,MirName,'ActuatorNoise.eps'],'epsc')

%%
figure(77)
loglog(freq,TMDispReq,'b-') ;
hold on;
noise_act = nb.modelNoises{1,1}.asd ;
freq_act = nb.modelNoises{1,1}.f ;
loglog(freq_act,noise_act,'r-') ;
grid on ;
set(gca,'YTick',logspace(-22,-18,5)) ;
set(gca,'XTick',logspace(1,3,3)) ;
ylim([8e-22,1e-18]) ; 
xlim([1e1,1e3]) ;
legend({'Requirement','Act noise'}) ;
xlabel('Frequency [Hz]') ;
ylabel('TM displacement [m/rtHz]') ;
title('TM Displacement Noise from Act') ;
saveas(gcf,[figdir,MirName,'Act_Noise.png']) ;


%% Compute TM feedback signal noise budget (for saturation check)
close all
%% Compute noises
[noises, sys] = nbFromSimulink(noiseModel, freq, 'dof', 'TMFB');

%% save cached outputs
saveFunctionCache();

%% Plot noise budget
set(0, 'DefaultAxesFontSize',14);
disp('Plotting noises')
nb = nbGroupNoises(noiseModel, noises, sys);
nb.sortModel();
matlabNoisePlot(nb);
%%
figure(1)
ylim([1e-5,1e6]);
xlim([1e-1,1e3]);
set(gca,'YTick',logspace(-5,6,12));
set(gca,'XTick',logspace(-1,3,5));
leg = legend(gca);
%legend({'DAC limit',leg.String{2:end}},'Interpreter','None');    % Replace 'Measured' with 'DAC limit'
legend({'DAC limit','Sum','Actuator noise','Seismic noise'});    % Replace 'Measured' with 'DAC limit'
plotcumulativeRMS(nb.sumNoise.f,nb.sumNoise.asd,[0,0,1]);
saveas(gcf,[figdir,MirName,'TMFB.png'])
saveas(gcf,[figdir,MirName,'TMFB.eps'],'epsc')

%% Compute IM feedback signal noise budget (for saturation check)
close all
%% Compute noises
[noises, sys] = nbFromSimulink(noiseModel, freq, 'dof', 'IMFB');

%% save cached outputs
saveFunctionCache();

%% Plot noise budget
set(0, 'DefaultAxesFontSize',14);
disp('Plotting noises')
nb = nbGroupNoises(noiseModel, noises, sys);
nb.sortModel();
matlabNoisePlot(nb);
%%
figure(1)
ylim([1e-5,1e6]);
xlim([1e-1,1e3]);
set(gca,'YTick',logspace(-5,6,12));
set(gca,'XTick',logspace(-1,3,5));
leg = legend(gca);
%legend({'DAC limit',leg.String{2:end}},'Interpreter','None');    % Replace 'Measured' with 'DAC limit'
legend({'DAC limit','Sum','Actuator noise','Seismic noise'});    % Replace 'Measured' with 'DAC limit'
plotcumulativeRMS(nb.sumNoise.f,nb.sumNoise.asd,[0,0,1]);
saveas(gcf,[figdir,MirName,'IMFB.png'])
saveas(gcf,[figdir,MirName,'IMFB.eps'],'epsc')

%% Compute MN feedback signal noise budget (for saturation check)
close all
%% Compute noises
[noises, sys] = nbFromSimulink(noiseModel, freq, 'dof', 'MNFB');

%% save cached outputs
saveFunctionCache();

%% Plot noise budget
set(0, 'DefaultAxesFontSize',14);
disp('Plotting noises')
nb = nbGroupNoises(noiseModel, noises, sys);
nb.sortModel();
matlabNoisePlot(nb);
%%
figure(1)
ylim([1e-5,1e6]);
xlim([1e-1,1e3]);
set(gca,'YTick',logspace(-5,6,12));
set(gca,'XTick',logspace(-1,3,5));
leg = legend(gca);
%legend({'DAC limit',leg.String{2:end}},'Interpreter','None');    % Replace 'Measured' with 'DAC limit'
legend({'DAC limit','Sum','Actuator noise','Seismic noise'});    % Replace 'Measured' with 'DAC limit'
plotcumulativeRMS(nb.sumNoise.f,nb.sumNoise.asd,[0,0,1]);
saveas(gcf,[figdir,MirName,'MNFB.png'])
saveas(gcf,[figdir,MirName,'MNFB.eps'],'epsc')

close all


%%
fff = nb.modelNoises{1,1}.modelNoises{1,1}.f ;
noise = nb.modelNoises{1,1}.modelNoises{1,1}.asd ;
KKK = [fff;noise];
fileID = fopen('IMCoilDriver.txt','w');
fprintf(fileID,'%6s %12s\n', '# freq', '[m/rtHz]');
fprintf(fileID,'%6.2f %12.8f\n',KKK);
fclose(fileID);