%% 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
freq2 =logspace(-2,3,3001);

%% 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);

%% Act TF
bodesusplotcmpopt(sysc,{'actLTM','IFO_LTM';'actLIM','IFO_LTM'},freq2,...
    'calibration',{gain_act_LTM,gain_act_LIM})

%% COMPENSATION

cmpstflt_LTM = 1/0.558;
cmpstflt_LIM = 1/0.141*myzpk(myfQv(1,3),[2e2;2e2],(2e2/1)^2);
bodesusplotcmpopt(sysc,{'actLTM','IFO_LTM';'actLIM','IFO_LTM'},freq2,...
    'calibration',{gain_act_LTM*cmpstflt_LTM,gain_act_LIM*cmpstflt_LIM})

%% BLENDING

fb_Lc = 10;
wb_Lc = fb_Lc*pp;
nb_Lc = 3; % 3rd order blending
nbd_Lc = (nb_Lc+1)/2;
cf_poly_Lc = zeros(1,nb_Lc+1);
for n=0:nb_Lc; cf_poly_Lc(n+1)=nchoosek(nb_Lc,n)*wb_Lc^(n); end
% BLENDING FILTERS
blend_LTM = tf([cf_poly_Lc(1:nbd_Lc),zeros(1,nbd_Lc)],cf_poly_Lc);
blend_LIM = tf(cf_poly_Lc(nbd_Lc+1:nb_Lc+1),cf_poly_Lc);

% mybodeplot({blend_LTM,blend_LIM},freq);

tf_b_LTM=takesusonly(sysc,'actLTM','IFO_LTM')...
    *gain_act_LTM*cmpstflt_LTM*blend_LTM;
tf_b_LIM=takesusonly(sysc,'actLIM','IFO_LTM')...
    *gain_act_LIM*cmpstflt_LIM*blend_LIM;
tf_b_Lact=tf_b_LTM+tf_b_LIM;

mybodeplot({tf_b_LTM,tf_b_LIM,tf_b_Lact},freq2);

% bodesusplotcmpopt(sysc,{'actLTM','IFO_LTM';'actLIM','IFO_LTM'},freq,...
%     'calibration',...
%     {gain_act_LTM*cmpstflt_LTM*blend_LTM,gain_act_LIM*cmpstflt_LIM*blend_LIM})

%% servo filter

svt_global_Lact = myzpk([myfQv(0.6,1);2e1],[1e-2;1e-2;2e2;myfQv(5e2,1)],...
    10*600*(5e2*pp)^2*3);
% mybodeplot({tf_b_LTM*svt_global_Lact,...
%     tf_b_LIM*svt_global_Lact,...
%     tf_b_Lact*svt_global_Lact},freq2);

%% open loop no blending

oltf_global_LTM=takesusonly(sysc,'actLTM','IFO_LTM')...
    *gain_act_LTM*cmpstflt_LTM*svt_global_Lact;
mybodeplot({oltf_global_LTM},freq2);
mybodeplot({svt_global_Lact*cmpstflt_LTM},freq2);

% MAXIMUM IN PROTOTYPE;
% coupling = 130e-3 [N/A]
% max voltage = 10 [V]
% coil resistance = 30 [Ohm]
% noise level = 2e-6 [V/rtHz]
% max current = 330e-3 [A]
% max force per coil = 40e-3 [N]
% max force in TM length = 160e-3 [N]
% TM spring constant = 180 [N/m]
% max displacement controlled = 0.9e-3 [m]
% noise = 32e-9 [N/rtHz]
% noise disp = 8e-13 [m/rtHz]

%% MODEL
addpath('servofilter');         % Add path to servo
typeB1proto_servo_global_150706;   % GLOBAL CONTROL
rmpath ('servofilter');         % Remove path to servo
mdlfile='typeB1simctrl_150706'; % typeB1 ver.150616
st     =linmod(mdlfile);
invl   =strrep(st.InputName, [mdlfile,'/'],'');
outvl  =strrep(st.OutputName,[mdlfile,'/'],'');
sysg   =ss(st.a,st.b,st.c,st.d,'inputname',invl,'outputname',outvl);

%% FREQUENCY
freq =logspace(-2,1.9,2001);

%% 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

%% BODE
[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);
[magsV,~]=bodesus(sysc,'accVGND','IFO_LTM',freq);
magsV=magsV.*freq.*freq*pp*pp.*noise_seism;
magsL=magsL.*n_LVDT_LF0;
magsG=magsG.*n_GEO_LF0;
magsS=magsS.*freq.*freq*pp*pp.*noise_seism;
magsT=sumpsd({magsL,magsG,magsS,magsV});
rmssT=makerms(freq,magsT);
rmssTv=makerms(freq,magsT.*freq*pp);

%% BODE
[maggL,~]=bodesus(sysg,'n_LVDT_LF0','IFO_LTM',freq);
[maggG,~]=bodesus(sysg,'n_GEO_LF0','IFO_LTM',freq);
[maggS,~]=bodesus(sysg,'accLGND','IFO_LTM',freq);
[maggV,~]=bodesus(sysg,'accVGND','IFO_LTM',freq);
maggV=maggV.*freq.*freq*pp*pp.*noise_seism;
maggL=maggL.*n_LVDT_LF0;
maggG=maggG.*n_GEO_LF0;
maggS=maggS.*freq.*freq*pp*pp.*noise_seism;
maggT=sumpsd({maggL,maggG,maggS,maggV});
rmsgT=makerms(freq,maggT);
rmsgTv=makerms(freq,maggT.*freq*pp);

%% actuator
[magaS,~]=bodesus(sysg,'accLGND','actLTM_mon',freq);
magaS=magaS.*freq.*freq*pp*pp.*noise_seism;
rmsaS=makerms(freq,magaS);

%% 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({magsT,maggT,rmsgT,reqBS},freq,...
    'title','Control noise coupling during observation phase',...
    'legend',{'Unlocked','Locked','Locked RMS',...
    'BS requirement'},...
    'color',{'k-','r-','m--','k--'},...
    'ylim',[1e-18,1e-5],'ylabel','Magnitude [m/rtHz] or [m]')
% export_fig('figure/typeB1proto_damping_obs_150706.pdf')


%% ACTUATOR
mypsdplotopt({magaS,rmsaS},freq,...
    'title','Actuator power',...
    'legend',{'Locked','Locked RMS',...
    'BS requirement'},...
    'color',{'r-','m--'},...
    'ylim',[1e-16,1e-3],'ylabel','Magnitude [N/rtHz] or [N]')
% export_fig('figure/typeB1proto_damping_obs_150706.pdf')

%% ACT NOISE

bodesusplotopt(sysc,'actLTM','IFO_LTM',freq);