%% Calculate noise coupling
% Coded by A. Shoda 18/05/29

%% Add path
clear all;
close all;

addpath(genpath('D:\KAGRA SVN\VIS\SuspensionControlModel\utility'));
%freq = logspace(-2,2,1000);

%% Load model
load('BS_damped.mat');

%% Load noise model
data = dlmread('GASLVDTnoiseModel.txt');
freq = data(:,1);
nBF = data(:,2);
nF1 = data(:,3);
nF0 = data(:,4);

data = dlmread('IPLVDTnoiseModel.txt');
nIP = data(:,2);

data = dlmread('IMOSEMnoiseModel.txt');
nIM = data(:,2);

%% Load seismic
data = DttData('Seism.xml');
[fr, seisNS] = data.powerSpectrum('K1:PEM-IY0_SEIS_NS_SENSINF_OUT_DQ');
seism = interp1(fr,seisNS,freq);% m//rtHz
seism = seism./(2*pi*freq);

%% Plot noise model

mypsdplotopt({nIM,nBF,nF1,nF0,nIP,seism*1.e+6},freq,...
    'title','Noise Model',...
    'legend',{'OSEM (IM)','LVDT (BF)','LVDT (F1)','LVDT (F0)', 'LVDT (IP)','seismic'},...
    'color',{'r-','b-','c-','r-','g-','k-'},...
    'ylim',[1e-5,1],'ylabel','Magnitude [um/rtHz] ')
saveas(gcf,'noisemodel.pdf');

%% Project noise to pitch fluctuation
%IP
[IPL2n,~]=bodesus(sysc,'n_LVDT_LIP','OpLev_PTM',freq);
[IPT2n,~]=bodesus(sysc,'n_LVDT_TIP','OpLev_PTM',freq);
[IPY2n,~]=bodesus(sysc,'n_LVDT_YIP','OpLev_PTM',freq);
magIPL = IPL2n.*nIP;
magIPT = IPT2n.*nIP;
magIPY = IPY2n.*nIP;

magIP = sumpsd({magIPL,magIPT,magIPY});

%GAS
[BF2n,~]=bodesus(sysc,'n_LVDT_GASBF','OpLev_PTM',freq);
[F12n,~]=bodesus(sysc,'n_LVDT_GASF1','OpLev_PTM',freq);
[F02n,~]=bodesus(sysc,'n_LVDT_GASF0','OpLev_PTM',freq);
magBF = BF2n.*nBF;
magF1 = F12n.*nF1;
magF0 = F02n.*nF0;

magGAS = sumpsd({magBF,magF1,magF0});

%IM
[IML2n,~]=bodesus(sysc,'n_OSEM_LIM','OpLev_PTM',freq);
[IMT2n,~]=bodesus(sysc,'n_OSEM_TIM','OpLev_PTM',freq);
[IMV2n,~]=bodesus(sysc,'n_OSEM_LIM','OpLev_PTM',freq);
[IMR2n,~]=bodesus(sysc,'n_OSEM_RIM','OpLev_PTM',freq);
[IMP2n,~]=bodesus(sysc,'n_OSEM_PIM','OpLev_PTM',freq);
[IMY2n,~]=bodesus(sysc,'n_OSEM_YIM','OpLev_PTM',freq);
magIML = IML2n.*nIM;
magIMT = IMT2n.*nIM;
magIMV = IMV2n.*nIM;
magIMR = IMR2n.*nIM;
magIMP = IMP2n.*nIM;
magIMY = IMY2n.*nIM;

magIM = sumpsd({magIML,magIMT,magIMV,magIMR,magIMP,magIMY});

%% Seismic
[accL2n,~]=bodesus(sysc,'accLGND','Oplev_PTM',freq);
[accT2n,~]=bodesus(sysc,'accTGND','Oplev_PTM',freq);
[accY2n,~]=bodesus(sysc,'accYGND','Oplev_PTM',freq);
magLseism = accL2n.*seism.*(2*pi*freq).*(2*pi*freq);
magTseism = accT2n.*seism.*(2*pi*freq).*(2*pi*freq);
magYseism = accY2n.*seism.*(2*pi*freq).*(2*pi*freq);

% total
magtot = subpsd({magLseism,magTseism,magYseism});

%% Plot
mypsdplotopt({},freq,...
    'title','Noise Model',...
    'legend',{'OSEM (IM)','LVDT (BF)','LVDT (F1)','LVDT (F0)', 'LVDT (IP)','seismic'},...
    'color',{'r-','b-','c-','r-','g-','k-'},...
    'ylim',[1e-5,1],'ylabel','Magnitude [um/rtHz] ')
saveas(gcf,'BS_PitEst.pdf');