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

%% 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='typeBp160620mdl';
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

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

%% IMPORT SIMULINK MODEL
mdlfile='typeBpsimctrl_160620'; % typeB1 ver.160108v2
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);


%% IMPORT SERVO FILTERS
addpath('servofilter');    % Add path to servo
typeBp160620_servo_lockaq;   % Damping phase
rmpath ('servofilter');    % Remove path to servo



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

%% PLOT SERVO FILTER

%% TRANSFER FUNCTION F0
%bodesusplotopt(sysc0,'actLF0','LVDT_LF0',freq,...
%    'ylim',[1e-4,1e1],'calibration',gain_act_LF0,'unit1','1',...
%    'position',[0 50 800 650]);
%export_fig('figure/typeBp_wIP160110v2/typeBp_wIP160110v2_TF_LF0_160116.pdf')


freq1=logspace(-2,3,1001);
mybodeplot({servo_VF1,servo_VF2},freq1,...
    {'VF1','VF2'},'F2 servo filter');
export_fig('figure/servofilter/typeBp160620_GASsevoflt_lockaq.pdf')

mybodeplot({servo_LF2,servo_TF2,servo_YF2},freq1,...
    {'L','T','Y'},'F2 servo filter');
export_fig('figure/servofilter/typeBp160620_F2sevoflt_lockaq.pdf')

mybodeplot({servo_LIM,servo_TIM,servo_VIM},freq1,...
    {'L','T','V'},'IM servo filter');
export_fig('figure/servofilter/typeBp160620_IMsevoflt1_lockaq.pdf')

mybodeplot({servo_RIM,servo_PIM,servo_YIM},freq1,...
    {'R','P','Y'},'IM servo filter');
export_fig('figure/servofilter/typeBp160620_IMsevoflt2_lockaq.pdf')

mybodeplot({servo_global_LTM,servo_oplev_PTM,servo_oplev_YTM},freq1,...
    {'LTM OpLev','PTM OpLev','YTM OpLev'},'TM servo filter');
export_fig('figure/servofilter/typeBp160620_TMsevofltOpLev_lockaq.pdf')


%%PLOT OLTF
bodesusplotcmpoptOLTF(sysc0,...
    {'actLF2','OSEM_LF2';'actTF2','OSEM_TF2';'actYF2','OSEM_YF2'},freq1,...
    'ylim',[1e-3,1e3],'calibration',...
    {gain_act_LF2*servo_LF2,gain_act_TF2*servo_TF2,gain_act_YF2*servo_YF2},...
    'unit1','1','title','Open Loop Gain',...
    'legend',{'LF2','TF2','YF2'});
export_fig('figure/OLTF/typeBp160620_OLTF_F2_lockaq.pdf')

%% TRANSFER FUNCTION GAS
bodesusplotcmpoptOLTF(sysc0,...
    {'actVF1','LVDT_VF1';'actVF2','LVDT_VF2'},freq2,...
    'ylim',[1e-5,1e2],'calibration',...
    {gain_act_VF1*servo_VF1,gain_act_VF2*servo_VF2},...
    'unit1','1','title','Open Loop Gain',...
    'legend',{'VF1','VF2'});
export_fig('figure/OLTF/typeBp160620_OLTF_GAS_lockaq.pdf')


%% 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'});
 export_fig('figure/OLTF/typeBp160620_OLTF_IM1_lockaq.pdf')
 
 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'});
 export_fig('figure/OLTF/typeBp160620_OLTF_IM2_lockaq.pdf')
 
 
 
 %% TRANSFER FUNCTION TM
 bodesusplotcmpoptOLTF(sysc0,...
     {'actLTM','IFO_LTM';'actPTM','OpLev_PTM';'actYTM','OpLev_YTM'},freq1,...
     'ylim',[1e-5,1e4],'calibration',...
     {gain_act_LTM*servo_global_LTM,gain_act_PTM*servo_oplev_PTM,gain_act_YTM*servo_oplev_YTM},...
     'unit1','1','title','Open Loop Gain',...
     'legend',{'LTM','PTM','YTM'});
 export_fig('figure/OLTF/typeBp160620_OLTF_TMoplev_lockaq.pdf')