%% ABOUT THIS FILE
% ----------------------------------------------------------------------
% Type-B1 prototype for KAGRA
% Coded by T. Sekiguchi on 2015/06/16
% ----------------------------------------------------------------------

%% PRELIMINARY
clear all;  % Clear workspace
close all;  % Close plot windows
addpath('../../utility');  % Add path to utilities
g = 9.81;

%% IMPORT SUSPENSION MODEL
matfile='typeB1susmdl';
load([matfile,'.mat']);

%% TUNING DAMPER
% This part compensates the failure in converting the structural damping to
% viscous damping.
% REDUCE DAMPING ON RIM
sys1.a(43,43)=sys1.a(43,43)/30; % RIM
sys1.a(49,49)=sys1.a(49,49)/30; % RRM
sys1.a(55,55)=sys1.a(55,55)/30; % RTM
% INCREASE DAMPING ON YF0
sys1.a(15,15)=sys1.a(15,15)*3000; % YF0
% INCREASE DAMPING ON YRM, YTM
sys1.a(51,51)=sys1.a(51,51)*30;   % YRM
sys1.a(57,57)=sys1.a(57,57)*300;  % YTM

%% ZERO CONTROL CASE
addpath('servofilter');         % Add path to servo
typeB1proto_no_control_150618;  % NO CONTROL
rmpath ('servofilter');         % Remove path to servo
mdlfile='typeB1simctrl_150629'; % typeB1 ver.150616
% SETTING FILTERS
st     =linmod(mdlfile);
invl   =strrep(st.InputName, [mdlfile,'/'],'');
outvl  =strrep(st.OutputName,[mdlfile,'/'],'');
sysc0  =ss(st.a,st.b,st.c,st.d,'inputname',invl,'outputname',outvl);



%% FREQUENCY
freq =logspace(-2,1.99,1001);

%% NOISE
% NOISE
 data_OSEM = importdata('../../noise/OSEMnoiseworstproto_disp.dat'); % um/rtHz
noise_OSEM = interp1(data_OSEM(:,1),data_OSEM(:,2)*1e-6,freq')'; %m/rtHz
n_OSEM_LTM = noise_OSEM/sqrt(4);        % [m/rtHz]
n_OSEM_PTM = noise_OSEM/sqrt(2)/117e-3; % [rad/rtHz]
n_OSEM_YTM = noise_OSEM/sqrt(2)/117e-3; % [rad/rtHz]
n_OSEM_LIM = noise_OSEM/sqrt(1);        % [m/rtHz]
n_OSEM_TIM = noise_OSEM/sqrt(2);        % [m/rtHz]
n_OSEM_VIM = noise_OSEM/sqrt(3);        % [m/rtHz]
n_OSEM_RIM = noise_OSEM/sqrt(3/2)/96e-3;% [rad/rtHz]
n_OSEM_PIM = noise_OSEM/sqrt(3/2)/96e-3;% [rad/rtHz]
n_OSEM_YIM = noise_OSEM/sqrt(2)/87e-3;  % [rad/rtHz]

 data_LVDT = importdata('../../noise/LVDTnoiseADC_disp.dat'); % um/rtHz
noise_LVDT = interp1(data_LVDT(:,1),data_LVDT(:,2)*1e-6,freq')';     %  m/rtHz
n_LVDT_LF0 = noise_LVDT/sqrt(3/2);        % [m/rtHz]
n_LVDT_TF0 = noise_LVDT/sqrt(3/2);        % [m/rtHz]
n_LVDT_YF0 = noise_LVDT/sqrt(3)/600e-3;   % [rad/rtHz]
n_LVDT_VF0 = noise_LVDT*2;                % [m/rtHz]
n_LVDT_VF1 = noise_LVDT*2;                % [m/rtHz]
n_LVDT_VF2 = noise_LVDT*2;                % [m/rtHz]


 data_GEO = importdata('../../noise/GEOnoiseproto_vel.dat'); % um/sec/rtHz
noise_GEO = interp1(data_GEO(:,1),data_GEO(:,2)*1e-6,freq')';%  m/sec/rtHz
n_GEO_LF0 = noise_GEO/sqrt(3/2);        % [m/rtHz]
n_GEO_TF0 = noise_GEO/sqrt(3/2);        % [m/rtHz]
n_GEO_YF0 = noise_GEO/sqrt(3)/594e-3;   % [m/rtHz]

 data_seism = importdata('../../noise/KamiokaSeismicHighNoise.dat'); % m/rtHz
noise_seism = interp1(data_seism(:,1),data_seism(:,2),freq')';% m//rtHz

%% NOISE MODEL
mypsdplotopt({noise_OSEM,noise_seism},freq,...
    'title','Noise Model',...
    'legend',{'OSEM','seismic'},...
    'color',{'r-','k-'},...
    'ylim',[1e-13,1e-5],'ylabel','Magnitude [m/rtHz] ')


%% MODEL
addpath('servofilter');         % Add path to servo
typeB1proto_servo_obs_150706;   % SCIENECE MODE
rmpath ('servofilter');         % Remove path to servo
mdlfile='typeB1simctrl_150629'; % typeB1 ver.150616
st     =linmod(mdlfile);
invl   =strrep(st.InputName, [mdlfile,'/'],'');
outvl  =strrep(st.OutputName,[mdlfile,'/'],'');
sysc   =ss(st.a,st.b,st.c,st.d,'inputname',invl,'outputname',outvl);


%% CALC GAS
[magg0,~]=bodesus(sysc,'n_LVDT_VF0','IFO_LTM',freq);
[magg1,~]=bodesus(sysc,'n_LVDT_VF1','IFO_LTM',freq);
[magg2,~]=bodesus(sysc,'n_LVDT_VF2','IFO_LTM',freq);
[magVg,~]=bodesus(sysc,'accVGND','IFO_LTM',freq);
[mag0Vg,~]=bodesus(sysc0,'accVGND','IFO_LTM',freq);
mag0Vg=mag0Vg.*freq.*freq*pp*pp.*noise_seism;
magg0=magg0.*n_LVDT_VF0;
magg1=magg1.*n_LVDT_VF1;
magg2=magg2.*n_LVDT_VF2;
magVg=magVg.*freq.*freq*pp*pp.*noise_seism;
maggT=sumpsd({magg0,magg1,magg2});

%% CALC TOP TRANS
[magsL,~]=bodesus(sysc,'n_LVDT_LF0','IFO_LTM',freq);
[magsG,~]=bodesus(sysc,'n_GEO_LF0','IFO_LTM',freq);
[magsS,~]=bodesus(sysc,'accLGND','IFO_LTM',freq);
[mag0S,~]=bodesus(sysc0,'accLGND','IFO_LTM',freq);
magsL=magsL.*n_LVDT_LF0;
magsG=magsG.*n_GEO_LF0;
magsS=magsS.*freq.*freq*pp*pp.*noise_seism;
mag0S=mag0S.*freq.*freq*pp*pp.*noise_seism;
magsLG=sumpsd({magsL,magsG});
mag0T=sumpsd({mag0Vg,mag0S});
magsT=sumpsd({magsL,magsG,magsS,magVg});
rmsT=makerms(freq,magsT);
rmsTv=makerms(freq,magsT.*freq*pp);
rms0T=makerms(freq,mag0T);
rms0Tv=makerms(freq,mag0T.*freq*pp);

%% CALC TOP PITCH
[magpL,~]=bodesus(sysc,'n_LVDT_LF0','OpLev_PTM',freq);
[magpG,~]=bodesus(sysc,'n_GEO_LF0','OpLev_PTM',freq);
[magpS,~]=bodesus(sysc,'accLGND','OpLev_PTM',freq);
[mag0pS,~]=bodesus(sysc0,'accLGND','OpLev_PTM',freq);
[magpoL,~]=bodesus(sysc,'n_OSEM_LIM','OpLev_PTM',freq);
[magpoP,~]=bodesus(sysc,'n_OSEM_PIM','OpLev_PTM',freq);
magpoL=magpoL.*n_OSEM_LIM;
magpoP=magpoP.*n_OSEM_PIM;
magpL=magpL.*n_LVDT_LF0;
magpG=magpG.*n_GEO_LF0;
magpS=magpS.*freq.*freq*pp*pp.*noise_seism;
magpT=sumpsd({magpL,magpG,magpS,magpoL,magpoP});
mag0pS=mag0pS.*freq.*freq*pp*pp.*noise_seism;

rms0pT=makerms(freq,mag0pS);
rms0pTv=makerms(freq,mag0pS.*freq*pp);
rmspT=makerms(freq,magpT);
rmspTv=makerms(freq,magpT.*freq*pp);

%% CALC ABOUT OSEM
[magoL,~]=bodesus(sysc,'n_OSEM_LIM','IFO_LTM',freq);
[magoP,~]=bodesus(sysc,'n_OSEM_PIM','IFO_LTM',freq);
[magoY,~]=bodesus(sysc,'n_OSEM_YIM','IFO_LTM',freq);
[magoT,~]=bodesus(sysc,'n_OSEM_TIM','IFO_LTM',freq);
[magoR,~]=bodesus(sysc,'n_OSEM_RIM','IFO_LTM',freq);
[magoV,~]=bodesus(sysc,'n_OSEM_VIM','IFO_LTM',freq);
magoL=magoL.*n_OSEM_LIM;
magoT=magoT.*n_OSEM_TIM;
magoV=magoV.*n_OSEM_VIM;
magoR=magoR.*n_OSEM_RIM;
magoP=magoP.*n_OSEM_PIM;
magoY=magoY.*n_OSEM_YIM;

[magtL,~]=bodesus(sysc,'n_OSEM_LTM','IFO_LTM',freq);
[magtP,~]=bodesus(sysc,'n_OSEM_PTM','IFO_LTM',freq);
[magtY,~]=bodesus(sysc,'n_OSEM_YTM','IFO_LTM',freq);
magtL=magtL.*n_OSEM_LTM;
magtP=magtP.*n_OSEM_PTM;
magtY=magtY.*n_OSEM_YTM;

magoTot=sumpsd({magoL,magoP,magoY,magoT,magoR,magoV,magtL,magtP,magtY});
rmsoTot=makerms(freq,magoTot);
rmsoTotv=makerms(freq,magoTot.*freq*pp);

%% ALL
magaT=sumpsd({magsT,magoTot});
rmsaT=makerms(freq,magaT);
rmsaTv=makerms(freq,magaT.*freq*pp);

%% Requirement
reqBS=freq;
for i=1:length(freq)
   if freq(i)<8
    reqBS(i)=0;
   else
    reqBS(i)=1e-15*(freq(i)/10)^(-1.5);
   end
end





%% PLOT
mypsdplotopt({magoTot,maggT,magsLG,magaT,mag0T,reqBS},freq,...
    'title','Control noise coupling during observation phase',...
    'legend',{'OSEM noise','GAS control noise','top control noise'...
    'With control total','No control',...
    'BS requirement'},...
    'color',{'m-','c-','g-','r-','b-','k--'},...
    'ylim',[1e-18,1e-5],'ylabel','Magnitude [m/rtHz] or [m/sec]')
export_fig('figure/typeB1proto_damping_obs_150706.pdf')

%% PLOT PITCH
mypsdplotopt({mag0pS,magpS,magpT,rms0pT,rmspT},freq,...
    'title','Noise coupling from OSEMs',...
    'legend',{'no control',...
    'seismic with control','total with control','no control RMS',...
    'with control RMS'},...
    'color',{'b-','k-','r-','c--','m--'},...
    'ylim',[1e-12,1e-5],'ylabel','Magnitude [rad/rtHz] or [rad]')
% export_fig('figure/typeB1proto_damping_pitch_150703.pdf')