%% ABOUT THIS FILE
% ----------------------------------------------------------------------
% Type-A with Dummy Mass for KAGRA
% Coded by K. Okutomi on 2016.07.21
% ----------------------------------------------------------------------

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

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

%% TUNING DAMPER
% This part compensates the failure in converting the structural damping to
% viscous damping.
% sys1.a(15,15)=sys1.a(15,15)*280; % YF0
% sys1.a(16,16)=sys1.a(16,16)*100000; % velLMD
% sys1.a(21,21)=sys1.a(21,21)*300000; % velYMD
% sys1.a(27,27)=sys1.a(27,27)*300000; % velYSF1

%%
%%%%% NO-CONTROL MODEL %%%%%
%% IMPORT SERVO FILTERS
addpath('parameter');         % Add path to servo
TypeA_DP_paramNoCtrl160713;      % NO CONTROL MODE
rmpath ('parameter');         % Remove path to servo

%% BF COUPRING MATRIX
cMat_BF = [1 0 0 0 0 0;... % L
           0 1 0 0 0 0;... % T
           0 0 1 0 0 0;... % V
           0 0 0 1 0 0;... % R
           0 0 0 0 1 0;... % P
           0 0 0 0 0 1];   % Y

%% IMPORT SIMULINK MODEL Case.1
mdlfile='TypeA_DP_ctrl160721'; % typeA_DP ver.160713
st     =linmod(mdlfile);
invl   =strrep(st.InputName, [mdlfile,'/'],'');
outvl  =strrep(st.OutputName,[mdlfile,'/'],'');
sysc1  =ss(st.a,st.b,st.c,st.d,'inputname',invl,'outputname',outvl);
       
%% IMPORT SIMULINK MODEL Case.2
cMat_BF(5,1) = 0.005; % L -> P coupling
mdlfile='TypeA_DP_ctrl160721'; % typeA_DP ver.160713
st     =linmod(mdlfile);
invl   =strrep(st.InputName, [mdlfile,'/'],'');
outvl  =strrep(st.OutputName,[mdlfile,'/'],'');
sysc2  =ss(st.a,st.b,st.c,st.d,'inputname',invl,'outputname',outvl);

%% IMPORT SIMULINK MODEL Case.3
cMat_BF(5,1) = 0.0137; % L -> P coupling
mdlfile='TypeA_DP_ctrl160721'; % typeA_DP ver.160713
st     =linmod(mdlfile);
invl   =strrep(st.InputName, [mdlfile,'/'],'');
outvl  =strrep(st.OutputName,[mdlfile,'/'],'');
sysc3  =ss(st.a,st.b,st.c,st.d,'inputname',invl,'outputname',outvl);

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


%% PLOT TRANSFER FUNCTION OF THE SUSPENSION MECHANICAL RESPONSE

%% LBF -> PBF
close all
bodesusplotcmpopt4({sysc1,sysc2,sysc3},...
    'injLBF','LVDT_PBF',...
    freq,...
    'ylim',[1e-6,1e2],...
    'title','Transfer Function from actLBF to PBF',...
    'legend',{'0 mm: Center of mass','5 mm: Suspension point','13.7 mm: Bending point'},...
    'unit1','N','unit2','rad');
saveas(gca, 'fig/tfTypeA_DP_couplingPBFactLBF_160721t','epsc')