%% ABOUT THIS FILE
% ----------------------------------------------------------------------
% Parmeter setting for Damping CONTROL MODE
% Type-B SAS for KAGRA
% Coded by Y. Fujii on July 21th, 2018.
% ----------------------------------------------------------------------

%% CONSTANTS
pp=2*pi;

%% ACTUATOR NORMALIZATION
% Normalize the actuators using local sensors at 0 Hz

% F0 (Normalized with LVDT signals)
gain_act_LIP = 14.0873018646;
gain_act_TIP = 14.0873018646;
gain_act_YIP = 10.5199067183;

% IM (Normalized with OSEM signals)
gain_act_LIM = 13.5706673475;
gain_act_TIM = 13.572424637;
gain_act_VIM = 16.5498508245;
gain_act_RIM = 1.3506116279;
gain_act_PIM = 1.3442511683;
gain_act_YIM = 0.62257280636;

% TM (Normalized with OSEM signals)
gain_act_LTM = 13.0833579555;
gain_act_PTM = 1.0913493272;
gain_act_YTM = 1.63714586482;

% GAS (Normalzed with LVDT signals)
gain_act_GASF0 = 29.6956935458;
gain_act_GASF1 = 26.0113434191;
gain_act_GASBF = 16.5976532577;

%% OpLev Coupling

Sens_Mat_OL = [...
    1,0,0,0,0,0; ...
    0,1,0,0,0,0; ...
    0,0,1,0,0,0; ...
    0,0,0,1,0,0; ...
    0,0,0,0,1,0; ...
    0,0,0,0,0,1];

%% ACTUATOR COUPLING MATRIX
%(L, T, V, R, P, Y) 

% BF actuator position shift:
%dv_BF = 0.022;
%dv_BR = -0.103;
%dv_BF = -0.005;
%dv_BR = -0.0;
%dv_BF = 0.0;
%dv_BR = 0.0;

%Act_Mat_BR = [...
%    1,0,0,0,0,0; ...
%    0,1,0,0,0,0; ...
%    0,0,1,0,0,0; ...
%    0,0,0,1,0,0; ...
%    dv_BR,0,0,0,1,0; ...
%    0,0,0,0,0,1];
%Act_Mat_BF = [...
%    1,0,0,0,0,0; ...
%    0,1,0,0,0,0; ...
%    0,0,1,0,0,0; ...
%    0,0,0,1,0,0; ...
%    dv_BF,0,0,0,1,0; ...
%    0,0,0,0,0,1];
Act_Mat_IR = [...
    1,0,0,0,0,0; ...
    0,1,0,0,0,0; ...
    0,0,1,0,0,0; ...
    0,0,0,1,0,0; ...
    0,0,0,0,1,0; ...
    0,0,0,0,0,1];
Act_Mat_IM = [...
    1,0,0,0,0,0; ...
    0,1,0,0,0,0; ...
    0,0,1,0,0,0; ...
    0,0,0,1,0,0; ...
    0,0,0,0,1,0; ...
    0,0,0,0,0,1];
Act_Mat_RM = [...
    1,0,0,0,0,0; ...
    0,1,0,0,0,0; ...
    0,0,1,0,0,0; ...
    0,0,0,1,0,0; ...
    0,0,0,0,1,0; ...
    0,0,0,0,0,1];
Act_Mat_TM = [...
    1,0,0,0,0,0; ...
    0,1,0,0,0,0; ...
    0,0,1,0,0,0; ...
    0,0,0,1,0,0; ...
    0,0,0,0,1,0; ...
    0,0,0,0,0,1];


%% BLENDING FILTERS
% This part constructs filters for blending LVDT and geophone signals.
% Blending filters are constructed from polynominal expression of a Laplace
% transformed equation (s+w0)^n, where w0 is the blending frequency and n
% is an arbitrary (odd) integer.
% 
% % BLENDING FREQUENCY: 0.3 Hz
% f_blend = 0.5;
% w_blend = f_blend*pp;
% 
% % COEFFICIENTS LIST OF POLYNOMINAL EXPRESSION OF (s+w0)^n
% n_blend = 7; % 5th order blending
% nbd = (n_blend+1)/2;
% cf_poly = zeros(1,n_blend+1);
% for n=0:n_blend; cf_poly(n+1)=nchoosek(n_blend,n)*w_blend^(n); end
% 
% % BLENDING FILTERS
% blend_HP = tf([cf_poly(1:nbd),zeros(1,nbd)],cf_poly);
% blend_LP = tf(cf_poly(nbd+1:n_blend+1),cf_poly);
% 
% % BLENDING FILTERS (ZPK EXPRESSION)
% % blend_LP = myzpk([0.075+1i*0.0581;0.075-1i*0.0581],[0.3;0.3;0.3;0.3;0.3],66.97);
% % blend_HP = myzpk([0.75+1i*0.581;0.75-1i*0.581;0;0;0],[0.3;0.3;0.3;0.3;0.3],1);
% 
% % GEOPHONE RESPONSES
% georesp  = zpk([-2.13+1i*5.19;-2.13-1i*5.19],[0;0],1);
% vel2disp = zpk([],0,1);
% 
% % BLENDING FILTERS WITH GEOPHONE RESPONSES
% % blend_LVDT = blend_LP;
% % blend_GEO  = minreal(blend_HP*georesp*vel2disp);
blend_LVDT = 1;
blend_GEO  = 0;

% PLOT 1
% freq1=logspace(-3,2,1001);
% mybodeplot({blend_LP,blend_HP,blend_LP+blend_HP},freq1);

% PLOT 2
% freq1=logspace(-3,2,1001);
% mybodeplot({blend_LVDT,blend_GEO},freq1);

%% ACTUATOR COUPLING MATRIX
%(L, T, V, R, P, Y) 

%% SERVO FILTER
% GENERAL SERVO
% damping servo with 300 Hz cutoff
dampflt = myzpk(0,[3e1,3e1],9e2*pp);
dampflt1 = myzpk(0,[1e1,1e1],9e2*pp);
dampflt2 = myzpk([0,0.5,0.5],[3e2,3e2,30,30],9e7*pp);
dampflt4 = myzpk(0,[3e2,3e2],9e4*pp);
dampflt5 = myzpk(0,[1.5e2,1.5e2],50e3*pp);

dampfltb = myzpk(0,myfQv(3e1,1),9e2*pp);
dampflt1b = myzpk(0,myfQv(1e1,1),9e2*pp);
dampflt4b = myzpk(0,myfQv(3e2,1),(3e2*pp)^2/pp);
dampflt5b = myzpk(0,myfQv(1.5e2,1),50e3*pp);
dampflt6b = myzpk(0,myfQv(5e0,1),9e2*pp);
dampflt7b = myzpk(0,myfQv(3e0,1),9e2*pp);
% DC + damping servo with G=400 at 0.1 mHz
dcdampflt = myzpk([1e-1;1e-1],[1e-4;1e1],10)...
          * myzpk([],[3;3],(3*pp)^2);
dcdampflt1 = myzpk(myfQv(0.1,1),[1e-4;myfQv(6,1);myfQv(6,1)],...
    200*(1e-4*pp)*(6*pp)^4/(0.1*pp)^2);
% DC filter
dcflt = myzpk([],[1e-4;1e1],100*1e-3*pp^2)...
          * myzpk([],[3e-1;3e-1],(3e-1*pp)^2);
dcflt1 = myzpk([],[1e-4;1e-1;myfQv(0.05,1)],...
    200*(1e-1*pp)*(1e-4*pp)*(0.3*pp)^2);
      
% SERVO FILTER F0
servo_LIP = dcdampflt*20;
servo_TIP = dcdampflt*20;
servo_YIP = dcdampflt*10;

% SERVO FILTER F2
% servo_LBF = 10.*dampflt6b/5/5/5/2;
% servo_TBF = 10.*dampflt6b/5/5/5/2;
% servo_VBF = dampflt4b;
% servo_RBF = dampflt4b/5*3*2*2*2;
% servo_PBF = dampflt4b/5*3*2*2*2;
% servo_YBF = 20.*dampflt6b/2/5/3/4*2;


% SERVO FILTER IM
servo_LIM = 5.5*dampflt1b/5/3;
servo_TIM = 5.5*dampflt1b/5/3;
servo_VIM = 8.0*dampflt4b/2;
servo_RIM = 2.0*dampflt5b/2;
servo_PIM = 1.5*dampflt5/2;
servo_YIM = 1.5*dampflt6b/20;

% SERVO FILTER TM
servo_LTM = 1.5*dampflt6b/5/5;
servo_PTM = 1.5*dampflt6b*1.5/5/5/3/5;
servo_YTM = 1.5*dampflt6b/5/8/5;

% SERVO FILTER GAS
servo_GASF0 = dcdampflt1*3;
servo_GASF1 = dcdampflt1*2;
servo_GASBF = dcflt*7;
%servo_GASBF = 0.01*dcdampflt1*100/2;

% SERVO FILTER OpLev
servo_oplev_PIM = 0;
servo_oplev_YIM = 0;
% servo_oplev_LTM = 0;
% servo_oplev_PTM = 0;
% servo_oplev_YTM = 0;

% SERVO FILTER IFO
% servo_global_LIP = 0;
servo_global_LIM = 0;
servo_global_LTM = 0;

%% GAIN
gain_LIP = -1; gain_TIP = -1; gain_YIP = -1;
gain_LIM = -1; gain_TIM = -1; gain_VIM = -1;
gain_RIM = -1; gain_PIM = -1; gain_YIM = -1;
gain_LTM = -1; gain_PTM = -1; gain_YTM = -1;
gain_GASF0 = -1; gain_GASF1 = -1; gain_GASBF = -1;
gain_oplev_PIM = 0; gain_oplev_YIM = 0;
gain_oplev_LTM = 0;
gain_global_LIP = 0; gain_global_LIM = 0; gain_global_LTM = 0;

% Saving point on July 21th, 2018.