%% ABOUT THIS FILE
% ----------------------------------------------------------------------
% Type-B1 prototype for KAGRA
% Coded by Y. Fujii on 2016/11/16
% ----------------------------------------------------------------------

%% 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='typeBp161115mdl';
load([matfile,'.mat']);

%% TUNING DAMPER
% This part compensates the failure in converting the structural damping to
% viscous damping.
% REDUCE DAMPING ON RIM
sys1.a(34,34)=sys1.a(34,34)/30; % RIM
sys1.a(40,40)=sys1.a(40,40)/30; % RRM
sys1.a(46,46)=sys1.a(46,46)/30; % RTM
% INCREASE DAMPING ON YF0
%sys1.a(15,15)=sys1.a(15,15)*3000; % YF1
%sys1.a(15,15)=sys1.a(15,15)*1000; % YF1
% INCREASE DAMPING ON LF1
%sys1.a(13,13)=sys1.a(13,13)*3000; % LF1
% INCREASE DAMPING ON TF1
%sys1.a(14,14)=sys1.a(14,14)*3000; % TF1
% INCREASE DAMPING ON YRM, YTM
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)*300;  % VTM


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

%% IMPORT SIMULINK MODEL
mdlfile='typeBp161115simctrl_LengthOL'; % typeBp_bKAGRA ver.161115
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);

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

%% TRANSFER FUNCTION BF
data_LBF=importdata('sumconplot/forceTF/typebp161115_LBF.dat');
data_LBF=[data_LBF(:,1),data_LBF(:,2)/0.0009128347724758092,data_LBF(:,3)];
data_TBF=importdata('sumconplot/forceTF/typebp161115_TBF.dat');
data_TBF=[data_TBF(:,1),data_TBF(:,2)/0.0009128347751106936,data_TBF(:,3)];
data_VBF=importdata('sumconplot/forceTF/typebp161115_VBF.dat');
data_VBF=[data_VBF(:,1),data_VBF(:,2)/0.0010885105636099731,data_VBF(:,3)];
data_RBF=importdata('sumconplot/forceTF/typebp161115_RBF.dat');
data_RBF=[data_RBF(:,1),data_RBF(:,2)/0.013142884471620242,data_RBF(:,3)];
data_PBF=importdata('sumconplot/forceTF/typebp161115_PBF.dat');
data_PBF=[data_PBF(:,1),data_PBF(:,2)/0.013142880382772719,data_PBF(:,3)];
data_YBF=importdata('sumconplot/forceTF/typebp161115_YBF.dat');
data_YBF=[data_YBF(:,1),data_YBF(:,2)/0.3463050128931313,data_YBF(:,3)];


bodesusplotSUMCONsimcmp(sysc0,'actVBF','LVDT_VBF',freq1,data_VBF,...
    'ylim',[1e-4,1e2],'calibration',gain_act_VBF,'unit1','1',...
    'title','Transfer function from actVBF to VBF');
export_fig('figures/TF/typebp161115_forceTFcomp_VBF.pdf')



%% TRANSFER FUNCTION IM
data_LIM=importdata('sumconplot/forceTF/typebp161115_LIM_OSEM.dat');
data_LIM=[data_LIM(:,1),data_LIM(:,2)/0.004469210075210013,data_LIM(:,3)];
data_TIM=importdata('sumconplot/forceTF/typebp161115_TIM_OSEM.dat');
data_TIM=[data_TIM(:,1),data_TIM(:,2)/0.00446920866908622,data_TIM(:,3)];
data_VIM=importdata('sumconplot/forceTF/typebp161115_VIM_OSEM.dat');
data_VIM=[data_VIM(:,1),data_VIM(:,2)/0.002655718793689245,data_VIM(:,3)];
data_RIM=importdata('sumconplot/forceTF/typebp161115_RIM_OSEM.dat');
data_RIM=[data_RIM(:,1),data_RIM(:,2)/0.3723329964821274,data_RIM(:,3)];
data_PIM=importdata('sumconplot/forceTF/typebp161115_PIM_OSEM.dat');
data_PIM=[data_PIM(:,1),data_PIM(:,2)/0.37499976840629107,data_PIM(:,3)];
data_YIM=importdata('sumconplot/forceTF/typebp161115_YIM_OSEM.dat');
data_YIM=[data_YIM(:,1),data_YIM(:,2)/2.443041324933432,data_YIM(:,3)];

data_LIM_Oplev=importdata('sumconplot/forceTF/typebp161115_LIM2LTMols.dat');
data_LIM_Oplev=[data_LIM_Oplev(:,1),data_LIM_Oplev(:,2)/0.000998271281155306,data_LIM_Oplev(:,3)];
data_PIM_Oplev=importdata('sumconplot/forceTF/typebp161115_PIM2PTMoplev.dat');
data_PIM_Oplev=[data_PIM_Oplev(:,1),data_PIM_Oplev(:,2)/0.33717661771175383,data_PIM_Oplev(:,3)];
data_YIM_Oplev=importdata('sumconplot/forceTF/typebp161115_YIM2YTMoplev.dat');
data_YIM_Oplev=[data_YIM_Oplev(:,1),data_YIM_Oplev(:,2)/2.3568716395982863,data_YIM_Oplev(:,3)];


bodesusplotSUMCONsimcmp(sysc0,'actVIM','OSEM_VIM',freq2,data_VIM,...
    'ylim',[1e-6,1e2],'calibration',gain_act_VIM,'unit1','1');
export_fig('figures/TF/typebp161115_forceTFcomp_VIM.pdf')



%% TRANSFER FUNCTION TM
data_LTM_Oplev=importdata('sumconplot/forceTF/typebp161115_LTM2LTMols.dat');
data_LTM_Oplev=[data_LTM_Oplev(:,1),data_LTM_Oplev(:,2)/0.0058144657401692735,data_LTM_Oplev(:,3)];
data_PTM_Oplev=importdata('sumconplot/forceTF/typebp161115_PTM2PTMoplev.dat');
data_PTM_Oplev=[data_PTM_Oplev(:,1),data_PTM_Oplev(:,2)/0.8453586747935663,data_PTM_Oplev(:,3)];
data_YTM_Oplev=importdata('sumconplot/forceTF/typebp161115_YTM2YTMoplev.dat');
data_YTM_Oplev=[data_YTM_Oplev(:,1),data_YTM_Oplev(:,2)/0.37216707035015684,data_YTM_Oplev(:,3)];


%% TRANSFER FUNCTION GAS
data_VF1=importdata('sumconplot/forceTF/typebp161115_VSF_GAS.dat');
data_VF1=[data_VF1(:,1),data_VF1(:,2)/0.0010884165498107523,data_VF1(:,3)];
data_VF2=importdata('sumconplot/forceTF/typebp161115_VBF_GAS.dat');
data_VF2=[data_VF2(:,1),data_VF2(:,2)/0.002655525675453235,data_VF2(:,3)];

bodesusplotSUMCONsimcmp(sysc0,'actVF2','LVDT_VF2',freq1,data_VF2,...
    'ylim',[1e-4,1e2],'calibration',gain_act_VF2,'unit1','1');
export_fig('figures/TF/typebp161115_forceTFcomp_VF2.pdf')