%% ABOUT THIS FILE
% ----------------------------------------------------------------------
% Control servo filters for LOCK ACQUISITION MODE
% Type-B1 prototype for KAGRA
% Coded by T. Sekiguchi on 2015/06/29
% ----------------------------------------------------------------------
%% PRELIMINARY
clear all;  % Clear workspace
close all;  % Close plot windows
addpath('../../utility');  % Add path to utilities

%% CONSTANTS
pp=2*pi;

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

% F0 (Normalized with LVDT signals)
gain_act_LF0 = 214.7;
gain_act_TF0 = 214.7;
gain_act_YF0 = 158.9;

% IM (Normalized with OSEM signals)
gain_act_LIM = 183.6;
gain_act_TIM = 183.6;
gain_act_VIM = 384.5;
gain_act_RIM = 1.766;
gain_act_PIM = 1.746;
gain_act_YIM = 0.387;

% TM (Normalized with OSEM signals)
gain_act_LTM = 100.0;
gain_act_PTM = 1.133;
gain_act_YTM = 1.773;

% GAS (Normalzed with LVDT signals)
gain_act_VF0 = 1193.;
gain_act_VF1 = 940.7;
gain_act_VF2 = 383.3;



%% 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: 50 mHz
f_blend = 0.05;
w_blend = f_blend*pp;

% COEFFICIENTS LIST OF POLYNOMINAL EXPRESSION OF (s+w0)^n
n_blend = 7; % 7th 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);

% PLOT 1
% freq1=logspace(-3,2,1001);
% mybodeplot({blend_LP,blend_HP,blend_LP+blend_HP},freq1,...
%     {'Low-Pass','High-Pass','Sum'},'Blending filter');
% export_fig('figure/typeB1proto_blending_150618.pdf')

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

%% SERVO FILTER
% GENERAL SERVO
% damping servo with 300 Hz cutoff
dampflt = myzpk(0,myfQv(3e2,1),(3e2*pp)^2/pp);
% dc+damping servo for GAS
dcdampflt = 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 servo for GAS
dcflt = myzpk([],[1e-4;1e-1;myfQv(0.3,1)],...
    200*(1e-1*pp)*(1e-4*pp)*(0.3*pp)^2);
% dc+damping servo for IP
ipservoflt = myzpk(myfQv(0.05,1),[1e-4;myfQv(2,1);myfQv(50,1)],...
    1e3*(50*pp)^2*(2/0.05)^2*(1e-4*pp));

% SERVO FILTER F0
servo_LF0 = ipservoflt;
servo_TF0 = ipservoflt;
servo_YF0 = ipservoflt;

% PLOT 1
 freq1=logspace(-3,2,1001);
 mybodeplot({servo_LF0,servo_TF0,servo_YF0},freq1);
 export_fig('figure/typeB1proto_servoF0_151129.pdf',...
 	'xlim',[1e-1,1e1])

% SERVO FILTER IM
servo_LIM = 0.3*dampflt;
servo_TIM = 0.3*dampflt;
servo_VIM = 0.5*dampflt;
servo_RIM = 0.5*dampflt;
servo_PIM = 0.5*dampflt;
servo_YIM = 1.0*dampflt;

% PLOT 2
 freq1=logspace(-3,3,1001);
 mybodeplot({servo_LIM,servo_TIM,servo_VIM,servo_LIM,servo_RIM,servo_PIM,servo_YIM},freq1);
 export_fig('figure/typeB1proto_servoIMOSEM_151129.pdf')

% SERVO FILTER TM
servo_LTM = 0.5*dampflt;
servo_PTM = 0.5*dampflt;
servo_YTM = 0.5*dampflt;

% PLOT 3
 freq1=logspace(-3,3,1001);
 mybodeplot({servo_LTM,servo_PTM,servo_YTM},freq1);
 export_fig('figure/typeB1proto_servoTMOSEM_151129.pdf')

% SERVO FILTER GAS
servo_VF0 = dcdampflt;
servo_VF1 = dcflt;
servo_VF2 = dcflt;

% PLOT 4
 freq1=logspace(-3,2,1001);
 mybodeplot({servo_VF0,servo_VF1,servo_VF2},freq1);
 export_fig('figure/typeB1proto_servoGAS_151129.pdf')


%% OL SERVO
cmpYIM = myzpk(myfQv(1.2,50),myfQv(20,1),(50/1.2)^2*1);
srvtst_OLY = myzpk([0.2;myfQv(0.15,3)],...
    [1e-4;1e-2;myfQv(5,1);myfQv(5,1)],0.5*(5*pp)^2*(5*pp)^2)*...
    myresonantfilter(10,10,2);

cmpPIM = myzpk(myfQv(0.7,50),myfQv(50,1),(50/0.7)^2*1);
srvtst_OLP = myzpk([0.1;myfQv(0.3,5)],...
    [1e-4;1e-2;myfQv(15,1);myfQv(15,1)],1*(15*pp)^2*(15*pp)^2)*...
    myresonantfilter(10,10,2);

% SERVO FILTER OpLev
servo_oplev_PIM = srvtst_OLP*cmpPIM/2;
servo_oplev_YIM = srvtst_OLY*cmpYIM/2;
servo_oplev_PTM = 0;
servo_oplev_YTM = 0;

% PLOT 5
 freq1=logspace(-3,2,1001);
 mybodeplot({servo_oplev_PIM},freq1);
 export_fig('figure/typeB1proto_servoOLPIM_161129.pdf')

 % PLOT 6
 freq1=logspace(-3,2,1001);
 mybodeplot({servo_oplev_YIM},freq1);
 export_fig('figure/typeB1proto_servoOLYIM_161129.pdf')


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


%% GAIN
gain_LF0 = -1; gain_TF0 = -1; gain_YF0 = -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_VF0 = -1; gain_VF1 = -1; gain_VF2 = -1;
gain_oplev_PIM = -1; gain_oplev_YIM = -1;
gain_oplev_PTM = 0; gain_oplev_YTM = 0;
gain_global_LF0 = 0; gain_global_LIM = 0; gain_global_LTM = 0;