% Noise budget script for iKAGRA IMC
%
% Author: Masayuki Nakano

%% Add paths
clear all
close all
cd /kagra/kagranoisebudget/iKAGRA

findNbSVNroot;  % find the root of Simulink NB
addpath(genpath([NbSVNroot 'Common/Utils']));
addpath(genpath([NbSVNroot 'Dev/Utils/']));
addpath(genpath([NbSVNroot 'FSS_old/IMC_MASTER']))
%load common mode servo filters
figdir = './results/';  % directory to save figures
datadir = './Common_Mode_Servo/';
cd /users/nakano/20160415IMCcaracterization
main
close all
cd /kagra/kagranoisebudget/iKAGRA

%% Define some parameters, filters, and noises
% see the Simulink file (noiseModel) for each parameter
freq = logspace(-1,5,3200);
noiseModel = 'IMC';
IFO = 'K1';
site = 'kamioka';

% get online data (see http://klog.icrr.u-tokyo.ac.jp/osl/index.php?r=1357)
OnlineData = GWData;
GPStime = 1147490800;    % time to get online data 
duration = 55;  % data get time duration
freqsamp = 16384;   % sampling frequency
freqmin = 0.1;  % minimum frequency (= frequency reslution)
Ndata = ceil(freqsamp/freqmin);
ndata = 2^nextpow2(Ndata);
duration2 = 505;  % data get time duration
freqsamp2 = 2048;   % sampling frequency
freqmin2 = 0.1;  % minimum frequency (= frequency reslution)
Ndata2 = ceil(freqsamp2/freqmin2);
ndata2 = 2^nextpow2(Ndata2);


% get LiveParts parameters
liveParts(noiseModel,GPStime,duration,freq);
% Plot actual openloop transfer function using calibration line
% meas=load('./TransferFunctions/20160409/MICHOLTF-UGFservo.txt');    % measured OLTF data
% meas(:,3)=angle(-exp(i*meas(:,3)/180*pi))/pi*180;


% Constants
c = 299792458.; % speed of light in vacuum (m/s)
hP = 6.62606957e-34;	% Planck's Constant [J*s]
lamb = 1064e-9; % laser wavelength (m)
nu = c/lamb;    % laser frequency (Hz)
FSR = 5.624e6;  %FSR

% 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
PdResp = containers.Map;    % PD rensponse [A/W]
TransImp = containers.Map;  % PD trans impedance [V/A]
Elec = containers.Map;  % electronics gain [V/V]
Coil = containers.Map;  % coil gain [N/V]
PZT = containers.Map; %PZT responce [Hz/V]
Opt = containers.Map; %Optical gain[V/Hz]
CMS_params;
CMS_filters;
% ADC and DAC
ADC_V2C = 2^16/20;  % ADC factor [cnts/V] 
DAC_C2V = 20/2^16;  % DAC factor [V/cnts]


% PD signal chain and noises
for pd = {'REFL'}
    pd = char(pd);
    PdResp([pd,'_RF']) = 1; % Not measured
    TransImp([pd,'_RF']) = 1; % Not measured
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %noise = load('./Noises/20160407/LSC-REFL_PDA1_RF17_Q_dark.txt');  % should be measured
    %Noise([pd,'_RF_PD']) = interp1(noise(:,1), noise(:,2), freq, 'linear');  % RF PD dark noise in [counts/rtHz];
end
DeModGain = 1; % temporary
noise = load('./Noises/adc_noise.txt');
%[data,time] = OnlineData.fetch(GPStime,duration,'K1:LSC-ADCNOISE_GND_OUT_DQ'); % use online data 
%[noisep,noisef] = pwelch(data,hanning(ndata),[],ndata,freqsamp); % calculate power spectral density
%noise = [noisef sqrt(noisep)]; 
Noise('ADC') = interp1(noise(:,1), noise(:,2), freq, 'linear');   % ADC noise [cnts/rtHz]
Noise('ADC') = 0; % since ADC noise is included in PD dark noise now
% Servo Noises
noise = [ff_servo_noise_fast servo_noise_fast];
% Noise('CMS_SLOWOUT') = interp1(noise(:,1), noise(:,2), freq, 'linear');  
Noise('CMS_SLOWOUT') = 0;
noise = [ff_servo_noise_slow servo_noise_slow];
Noise('CMS_SLOWMON') = interp1(noise(:,1), noise(:,2), freq, 'linear'); 
% Noise('CMS_SLOWMON') = 0;
% Senser Noises
noise = [ff_senser_noise senser_noise];
Noise('Demod') = interp1(noise(:,1), noise(:,2), freq, 'linear');  
for ii = 1:length(freq)
    noise = Noise('Demod');
    if isnan(noise(ii))
        noise(ii) = 0;
    end
    Noise('Demod') = noise;
end
        
    
% IMC Optical responses and noises
Noise('LaserFreq') = 10000./freq; %tipical noise
%MICHOpticalGain = -2*pi*nu/c*P0;  % W/m (theoretical)

%%%%%%%%Should be measured
Opt('IMC') = zpk([],-[cavpole]*2*pi,2*pi*cavpole*IMCOpticalGain); %IMCopticalGain and fcp
Noise('REFL_RF_SHOT') = 0; %sqrt(2*hP*nu*Ppd); % W/rtHz  THIS IS A FAKE (power at PD should be measured / Shot noise should be calculated in a case of dark fringe) 


% Suspension chain and noises
for mir = {'MCI','MCO','MCE','MCL'}
    mir = char(mir);
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%Should be measured
    Coil([mir,'_TM']) = 1;   % N/V see email from K. Yamamoto on Mar 22 (to be confirmed)
    Noise([mir,'_TM_Coil']) = ones(size(freq))/3600; % high power coil driver noise [V/rtHz]; See Figure 3 of https://dcc.ligo.org/LIGO-T080014
    
    
    Elec([mir,'_TM_Whitening']) = 1; % no whitening for iKAGRA
    Elec([mir,'_TM_DeWhitening']) = 1; % no dewhitening for iKAGRA
end

% PZT response
PZT('NPRO') = PZT_act;

% AA and 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_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);


% Whitening and Dewhitening filters
Elec('REFL_DC_Whitening') = 1; % no whitening for iKAGRA
Elec('REFL_DC_DeWhitening') = 1; % no dewhitening for iKAGRA
Elec('REFL_RF_Whitening') = 1; % no whitening for iKAGRA
Elec('REFL_RF_DeWhitening') = 1; % no dewhitening for iKAGRA


% TM act to TM [m/N] see http://klog.icrr.u-tokyo.ac.jp/osl/?r=1340
Susp('MCI_TM_TM') = act_sus/Coil('MCL_TM')/Elec('MCL_TM_DeWhitening')/(DAC_C2V/2)/Elec('MCL_TM_Whitening');%Fake
Susp('MCE_TM_TM') = act_sus/Coil('MCL_TM')/Elec('MCL_TM_DeWhitening')/(DAC_C2V/2)/Elec('MCL_TM_Whitening');%Fake
Susp('MCO_TM_TM') = act_sus/Coil('MCL_TM')/Elec('MCL_TM_DeWhitening')/(DAC_C2V/2)/Elec('MCL_TM_Whitening');%Fake
Susp('MCL_TM_TM') = act_sus/Coil('MCL_TM')/Elec('MCL_TM_DeWhitening')/(DAC_C2V/2)/Elec('MCL_TM_Whitening');

noise = load('./Noises/adc_noise.txt');
Noise('DAC') = interp1(noise(:,1), noise(:,2), freq, 'linear')*DAC_C2V/2;   % DAC noise [V/rtHz]; noise level is OK, but fake (see http://gwwiki.icrr.u-tokyo.ac.jp/JGWwiki/CLIO/Tasks/DigitalControl/Caltech_setup?action=AttachFile&do=get&target=analog_system_investigation.pdf)

m2Hz = fsr/1064e-9;
noise = [ff_seise,seis*m2Hz];
Noise('MCL_SEIS') = interp1(noise(:,1), noise(:,2), freq, 'linear'); % temporaly

%Seis_TypeC = 8e-9*freq.^-2./(1+(2./freq).^-8);
%Seis_TypeB = 1.5e-11*freq.^-9;
%Seis_TypeBp=2.3e-10*freq.^-7;
%Seis_TypeBpfixed=2e-10*freq.^-5;

% seismic motion to mirror motion [m/m]
Susp('MCI_SEIS_TM') = zpk([],...
    2*pi*[1i/2*(f0/Q*1i+sqrt(4*f0^2-f0^2/Q^2)) 1i/2*(f0/Q*1i-sqrt(4*f0^2-f0^2/Q^2))],4*pi^2*f0^2);
Susp('MCL_SEIS_TM') = zpk([],...
    2*pi*[1i/2*(f0/Q*1i+sqrt(4*f0^2-f0^2/Q^2)) 1i/2*(f0/Q*1i-sqrt(4*f0^2-f0^2/Q^2))],4*pi^2*f0^2);
Susp('MCE_SEIS_TM') = zpk([],...
    2*pi*[1i/2*(f0/Q*1i+sqrt(4*f0^2-f0^2/Q^2)) 1i/2*(f0/Q*1i-sqrt(4*f0^2-f0^2/Q^2))],4*pi^2*f0^2);
Susp('MCO_SEIS_TM') = zpk([],...
    2*pi*[1i/2*(f0/Q*1i+sqrt(4*f0^2-f0^2/Q^2)) 1i/2*(f0/Q*1i-sqrt(4*f0^2-f0^2/Q^2))],4*pi^2*f0^2);

% Measured noise at fast control signal
noise = [ff_fast_fb fast_fb]; 
MeasuredNoise = interp1(noise(:,1), noise(:,2), freq, 'linear');


%% Compare Plot actual openloop transfer function using calibration line
freq = logspace(-1,5,3200);
IMC_SW = 1;    % turn off the loop to measure OLTF
IMC_SWslow = 0;
IMC_SWfast = 1;
freq = logspace(-1,5,3200);
[A,B,C,D]=linmod(noiseModel);
systm=ss(A,B,C,D);
OLTF=systm(3,3,:);
[OLTF_uncal,OLTF_ph]=bode(OLTF,2*pi*freq);    % theoretical openloop transfer function gain at fcal
loglog(freq,squeeze(OLTF_uncal),ff_slow_olg,abs_slow_olg)


IMC_SW = 0;    % turn off the loop to measure OLTF
IMC_SWslow = 0;
IMC_SWfast = 1;
freq = logspace(-1,5,3200);
[A,B,C,D]=linmod(noiseModel);
systm=ss(A,B,C,D);
OLTF=systm(1,1,:);
[OLTF_uncal,OLTF_ph]=bode(OLTF,2*pi*freq);    % theoretical openloop transfer function gain at fcal
figure()
loglog(freq,squeeze(OLTF_uncal),ff_fastonly_olg,abs_fastonly_olg)

IMC_SW = 0;    % turn off the loop to measure OLTF
IMC_SWslow = 1;
IMC_SWfast = 1;
freq = logspace(-1,5,3200);
[A,B,C,D]=linmod(noiseModel);
systm=ss(A,B,C,D);
OLTF=systm(1,1,:);
[OLTF_uncal,OLTF_ph]=bode(OLTF,2*pi*freq);    % theoretical openloop transfer function gain at fcal
figure()
loglog(freq,squeeze(OLTF_uncal),ff_olg,abs_olg)

%%
IMC_SW = 1;    % turn on the loop

%% Compute noises and save cache
% Compute noises
[noises, sys] = nbFromSimulink(noiseModel, freq, 'dof', 'MCLfastFB');

% save cached outputs
saveFunctionCache();

%% Plot noise budget
close all
set(0, 'DefaultAxesFontSize',10);
disp('Plotting noises')
nb = nbGroupNoises(noiseModel, noises, sys);
nb.sortModel();
matlabNoisePlot(nb);
% 
% figure(2)
% set(gca,'YTick',logspace(-17,-5,13));
% set(gca,'XTick',logspace(-1,4,6));
% % saveas(gcf,[figdir,'MICHNoiseBudgetActuator.png'])
% 
% figure(4)
% set(gca,'YTick',logspace(-17,-5,13));
% set(gca,'XTick',logspace(-1,4,6));
% % saveas(gcf,[figdir,'MICHNoiseBudgetSensor.png'])
% 
% figure(1)
% set(gca,'YTick',logspace(-17,-5,13));
% set(gca,'XTick',logspace(-1,4,6));
% legend('Location','southeast')
% saveas(gcf,[figdir,'MICHNoiseBudget.png'])