%% 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_150616'; % 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.5,1,1001);
freq =logspace(-2.5,1.99,1001);

%% IP SERVO
ipservoflt = myzpk([0.05;0.08],[1e-4;1e1;1e1],3*(1e1*pp)^2);
freq2 =logspace(-4,1,1001);
bodesusplotopt(sysc0,'actLF0','LVDT_LF0',freq2,...
    'calibration',ipservoflt*gain_act_LF0,'title','Open loop gain',...
    'unit1','1');
export_fig('figure/typeB1proto_IPdamp_DCservo_OL_150629.pdf')


%% NOISE
% NOISE
 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]
 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]
 data_seism = importdata('../../noise/KamiokaSeismicHighNoise.dat'); % m/rtHz
noise_seism = interp1(data_seism(:,1),data_seism(:,2),freq')';% m//rtHz

%% MODEL
addpath('servofilter');         % Add path to servo
typeB1proto_servo_lock_150629;  % LOCK ACQUISITION MODE
rmpath ('servofilter');         % Remove path to servo
servo_LF0=ipservoflt;
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
[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;
magsT=sumpsd({magsL,magsG,magsS});
rmsT=makerms(freq,magsT);
rmsTv=makerms(freq,magsT.*freq*pp);
rms0S=makerms(freq,mag0S);
rms0Sv=makerms(freq,mag0S.*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({magsG,magsL,magsS,magsT,rmsT,rmsTv},freq,...
    'title','Blending at 0.05 Hz',...
    'legend',{'GEO','LVDT','Seismic','total','RMS',...
    'RMS velocity'},...
    'color',{'b-','g-','k-','r-','r--','m--'},...
    'ylim',[1e-12,1e-5],'ylabel','Magnitude [m/rtHz] or [m] or [m/sec]')
export_fig('figure/typeB1proto_IPdamp_DCservoON_150629.pdf')

%% COMPARE
mypsdplotopt({magsG,magsL,magsS,magsT,mag0S,reqBS},freq,...
    'title','Noise coupling',...
    'legend',{'geophone','LVDT','seismic','Controlled total'...
    'No controlled','BS requirement'},...
    'color',{'g-','b-','k-','r-','m--','k--'},...
    'ylim',[1e-20,1e-5],'ylabel','Magnitude [m/rtHz]')
export_fig('figure/typeB1proto_IPdamp_highfreq_noise_150630.pdf')

%% TILT

freq3=logspace(-2.5,1,1001);

[magt0,phst0]=bodesus(sysc0,'accPGND','IFO_LTM',freq3);
[magt1,phst1]=bodesus(sysc,'accPGND','IFO_LTM',freq3);
magt0=magt0.*freq3.*freq3*pp*pp;
magt1=magt1.*freq3.*freq3*pp*pp;

mypsdplotopt({magt1,magt0},freq3,...
    'title','Sensitivity to the tilt',...
    'ylim',[1e-3,1e4],'ylabel','Magnitude [m/rad]',...
    'color',{'r-','b-'},'legend',{'Control ON','Control OFF'})

export_fig('figure/typeB1proto_IPdamp_DCservoON_tilt_150629.pdf')