% ----------------------------------------------------------------------
% Type-B1 prototype for KAGRA
% Coded by T Sekiguchi on 2015/07/30
% ----------------------------------------------------------------------
% Investigate servo design for optical levers and interferometer.
% ----------------------------------------------------------------------


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

%% MODEL IMPORT 0
load('typeB1susmdl_150729.mat');  % Import ss model
typeB1proto_no_control_150729;    % NO CONTROL MODEL
mdlfile='typeB1simctrl_150729';   % typeB1 ver.150729
st     =linmod(mdlfile);          % Linearize simulink model
invl   =strrep(st.InputName, [mdlfile,'/'],'');
outvl  =strrep(st.OutputName,[mdlfile,'/'],'');
sysc0  =ss(st.a,st.b,st.c,st.d,'inputname',invl,'outputname',outvl);

%% MODEL IMPORT
typeB1proto_servo_lock_150729;    % SERVO FOR LOCK ACQUISITION
mdlfile='typeB1simctrl_150729';   % typeB1 ver.150729
st     =linmod(mdlfile);          % Linearize simulink model
invl   =strrep(st.InputName, [mdlfile,'/'],'');
outvl  =strrep(st.OutputName,[mdlfile,'/'],'');
sysc   =ss(st.a,st.b,st.c,st.d,'inputname',invl,'outputname',outvl);

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





%% TRANSFER FUNCTION OL
tf_oplev_YIM_0=takesusonly(sysc0,'actYIM','OpLev_YTM');
tf_oplev_YTM_0=takesusonly(sysc0,'actYTM','OpLev_YTM');
mybodeuwplot({tf_oplev_YIM_0,tf_oplev_YTM_0},freq,{'YIM','YTM'})

%% COMPENSATE
cmpYIM = myzpk(myfQv(1.2,50),myfQv(20,1),(50/1.2)^2*25.3);
mybodeplot({tf_oplev_YTM_0,tf_oplev_YIM_0*cmpYIM},freq)



%% Blending
fb=5;
blendHP = myzpk([0;0;0;myfQv(3.16*fb,0.79)],[fb;fb;fb;fb;fb],1);
blendLP = myzpk(myfQv(0.316*fb,0.79),[fb;fb;fb;fb;fb],10*(fb*pp)^3);
% mybodeplot({blendLP,blendHP,blendLP+blendHP},freq)

% mybodeuwplot({tf_oplev_YTM_0*blendHP,tf_oplev_YIM_0*cmpYIM*blendLP,...
%    tf_oplev_YTM_0*blendHP+tf_oplev_YIM_0*cmpYIM*blendLP},freq)

tf_oplev_YB=tf_oplev_YTM_0*blendHP+tf_oplev_YIM_0*cmpYIM*blendLP;

mybodeuwplot({tf_oplev_YB},freq)

%% filter Yaw
srvtst_OLY = myzpk([0.2;myfQv(0.15,3)],...
    [1e-4;1e-2;myfQv(5,1);myfQv(5,1)],0.02*(5*pp)^2*(5*pp)^2)*...
    myresonantfilter(10,10,2);
% mybodeuwplot({tf_oplev_YB*srvtst_OLY},freq1);
mybodeuwplot({tf_oplev_YIM_0*cmpYIM*srvtst_OLY},freq1);





%% TRANSFER FUNCTION OL
tf_oplev_PIM_0=takesusonly(sysc0,'actPIM','OpLev_PTM');
tf_oplev_PTM_0=takesusonly(sysc0,'actPTM','OpLev_PTM');
mybodeuwplot({tf_oplev_PIM_0,tf_oplev_PTM_0},freq,{'PIM','PTM'})

%% COMPENSATE
cmpPIM = myzpk(myfQv(0.7,50),myfQv(50,1),(50/0.7)^2*1);
mybodeplot({tf_oplev_PTM_0,tf_oplev_PIM_0*cmpPIM},freq)

%% COMP
mybodeuwplot({cmpPIM,tf_oplev_PIM_0*cmpPIM},freq,{'CMP FLT','TF after CMP'})

%% filter TM
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);
mybodeuwplot({tf_oplev_PIM_0*cmpPIM*srvtst_OLP},freq1);

%% filter PITCH NO ROBUST
srvtst_OLPnr = myzpk([0.1;myfQv(0.3,5)],...
    [1e-4;1e-2;myfQv(4,1);myfQv(4,1)],1*(4*pp)^2*(4*pp)^2)*...
    myresonantfilter(10,10,2)*myresonantfilter(4.97,1000,1);
mybodeuwplot({tf_oplev_PIM_0*cmpPIM*srvtst_OLPnr},freq1);






%% Feedback filter
mybodeuwplot({srvtst_OLY},freq1);
mybodeuwplot({srvtst_OLP},freq1);

%% NR
mybodeuwplot({srvtst_OLPnr},freq1);

%% Total servo
mybodeuwplot({srvtst_OLY*cmpYIM},freq1);
mybodeuwplot({srvtst_OLP*cmpPIM},freq1);

%% COMP
mybodeuwplot({cmpYIM,tf_oplev_YIM_0*cmpYIM},freq,{'CMP FLT','TF after CMP'})
mybodeuwplot({cmpPIM,tf_oplev_PIM_0*cmpPIM},freq,{'CMP FLT','TF after CMP'})