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

%% PRELIMINARY ----------------------------------------------------------------------------
clear all;  % Clear workspace
close all;  % Close plot windows
addpath('../../utility');  % Add path to utilities
% addpath('/usr/local/matlab/altmany-export_fig-76bd7fa');
g = 9.81;

%% IMPORT SUSPENSION MODEL ----------------------------------------------------------------------------
matfile='SR3_180502';
load([matfile,'mdl.mat']);

%% TUNING DAMPER
% This part compensates the failure in converting the structural damping to
% viscous damping.

% REDUCE DAMPING ON IM
sys1.a(37,37)=sys1.a(37,37)/3.e3; % RIR
sys1.a(43,43)=sys1.a(43,43)/3.e3; % RIM
%sys1.a(40,40)=sys1.a(40,40)/30; % RRM

% INCREASE DAMPING ON F0
sys1.a(13,13)=sys1.a(13,13)/3.e3; % LF0
sys1.a(14,14)=sys1.a(14,14)/3.e3; % TF0
sys1.a(15,15)=sys1.a(15,15)*300; % YF0

% INCREASE DAMPING ON MD
%sys1.a(16,16)=sys1.a(16,16)/5000; % LMD
%sys1.a(17,17)=sys1.a(17,17)/5000; % TMD

% INCREASE DAMPING ON RM, TM
%sys1.a(42,42)=sys1.a(42,42)*30;   % YRM
%sys1.a(48,48)=sys1.a(48,48)*300;  % YTM

% INCREASE DAMPING ON VTM
%sys1.a(45,45)=sys1.a(45,45)*100;  % VTM

%% IMPORT SERVO FILTERS ----------------------------------------------------------------------------
addpath('servofilter');    % Add path to servo
DefaultParameters;         % NO CONTROL MODE
rmpath ('servofilter');    % Remove path to servo

%% IMPORT SIMULINK MODEL ----------------------------------------------------------------------------
mdlfile='typeBsimctrl'; % typeB_bKAGRA ver. 20180502
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);


%% MODEL ----------------------------------------------------------------------------
addpath('servofilter');         % Add path to servo
servo_damp2;        % Damping MODE
rmpath ('servofilter');         % Remove path to servo

%% IMPORT SIMULINK MODEL -------------------------------------------------------------
mdlfile='typeBsimctrl';
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);


%% FREQUENCY ----------------------------------------------------------------------
freq1=logspace(-2,3,1001);
freq2=logspace(-3,2,1001);
freq=logspace(-2,2,1001);

%%% --- Preperation for  PLOT --- %%%
output_dir = 'figures/servofilters';

%% PLOT SERVO FILTER ----------------------------------------------------------------------

%% TRANSFER FUNCTION F0
mybodeplot({servo_LIP, servo_TIP, servo_YIP}, freq1,...
    {'LIP','TIP','YIP'});
figname = sprintf('%s/servo_IP_damp.pdf', output_dir);
export_fig(figname)

mybodeplot({servo_GASF0, servo_GASF1, servo_GASBF},freq,...
    {'VF0','VF1','VBF'});
figname = sprintf('%s/servo_GAS_damp.pdf', output_dir);
export_fig(figname)

mybodeplot({servo_LIM, servo_TIM, servo_VIM},freq1,...
    {'LIM','TIM','VIM'});
figname = sprintf('%s/servo_IM-LTV_damp.pdf', output_dir);
export_fig(figname)

mybodeplot({servo_RIM,servo_PIM,servo_YIM},freq1,...
    {'RIM','PIM','YIM'});
figname = sprintf('%s/servo_IM-RPY_damp.pdf', output_dir);
export_fig(figname)

mybodeplot({servo_LTM,servo_PTM,servo_YTM},freq1,...
    {'LTM','PTM','YTM'});
figname = sprintf('%s/servo_TM-LPY_damp.pdf', output_dir);
export_fig(figname)

%%PLOT OLTF ----------------------------------------------------------------------
output_dir_OL = 'figures/OLTF';

%% OPEN TRANSFER FUNCTION GAS
bodesusplotcmpoptOLTF(sysc0,...
    {'actGASF0','LVDT_GASF0'; 'actGASF1','LVDT_GASF1'; 'actGASBF','LVDT_GASBF'},freq2,...
    'ylim',[1e-5,1e2],'calibration',...
    {gain_act_GASF0*servo_GASF0, gain_act_GASF1*servo_GASF1, gain_act_GASBF*servo_GASBF},...
    'unit1','1','title','Open Loop Gain',...
    'legend',{'VF0','VF1','VF2'});
figname = sprintf('%s/OLTF_GAS_damp.pdf', output_dir_OL);
export_fig(figname)

%% OPEN TRANSFER FUNCTION F0
bodesusplotcmpoptOLTF(sysc0,...
    {'actLIP','LVDT_LIP'; 'actTIP','LVDT_TIP'; 'actYIP','LVDT_YIP'},freq2,...
    'ylim',[1e-5,1e2],'calibration',...
    {gain_act_LIP*servo_LIP, gain_act_TIP*servo_TIP, gain_act_YIP*servo_YIP},...
    'unit1','1','title','Open Loop Gain',...
    'legend',{'LIP','TIP','YIP'});
figname = sprintf('%s/OLTF_IP_damp.pdf', output_dir_OL);
export_fig(figname)

%% OPEN TRANSFER FUNCTION IM
bodesusplotcmpoptOLTF(sysc0,...
     {'actLIM','OSEM_LIM'; 'actTIM','OSEM_TIM'; 'actVIM','OSEM_VIM'},freq1,...
     'ylim',[1e-5,1e4],'calibration',...
     {gain_act_LIM*servo_LIM, gain_act_TIM*servo_TIM, gain_act_VIM*servo_VIM},...
     'unit1','1','title','Open Loop Gain',...
     'legend',{'LIM','TIM','VIM'});
figname = sprintf('%s/OLTF_IM-LTV_damp.pdf', output_dir_OL);
export_fig(figname)

bodesusplotcmpoptOLTF(sysc0,...
     {'actRIM','OSEM_RIM'; 'actPIM','OSEM_PIM'; 'actYIM','OSEM_YIM'},freq1,...
     'ylim',[1e-4,1e3],'calibration',...
     {gain_act_RIM*servo_RIM, gain_act_PIM*servo_PIM, gain_act_YIM*servo_YIM},...
     'unit1','1','title','Open Loop Gain',...
     'legend',{'RIM','PIM','YIM'});
figname = sprintf('%s/OLTF_IM-RPY_damp.pdf', output_dir_OL);
export_fig(figname)
 
 
 %% TRANSFER FUNCTION TM
bodesusplotcmpoptOLTF(sysc0,...
     {'actLTM','OpLev_LTM'; 'actPTM','OpLev_PTM'; 'actYTM','OpLev_YTM'},freq,...
     'ylim',[1e-5,1e4],'calibration',...
     {gain_act_LTM*servo_LTM, gain_act_PTM*servo_PTM, gain_act_YTM*servo_YTM},...
     'unit1','1','title','Open Loop Gain',...
     'legend',{'LTM','PTM','YTM'});
figname = sprintf('%s/OLTF_TM-LPY_damp.pdf', output_dir_OL);
export_fig(figname)