%% ABOUT THIS FILE
% ----------------------------------------------------------------------
% Type-B1 prototype for KAGRA
% Coded by T. Sekiguchi on 2015/06/16
% ----------------------------------------------------------------------

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

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

%% TUNING DAMPER
% This part compensates the failure in converting the structural damping to
% viscous damping.
% REDUCE DAMPING ON RIM
sys1.a(43,43)=sys1.a(43,43)/30; % RIM
sys1.a(49,49)=sys1.a(49,49)/30; % RRM
sys1.a(55,55)=sys1.a(55,55)/30; % RTM
% INCREASE DAMPING ON YF0
sys1.a(15,15)=sys1.a(15,15)*3000; % YF0
% INCREASE DAMPING ON YRM, YTM
sys1.a(51,51)=sys1.a(51,51)*30;   % YRM
sys1.a(57,57)=sys1.a(57,57)*300;  % YTM

%% ZERO CONTROL CASE
addpath('servofilter');         % Add path to servo
typeB1proto_no_control_150618;  % NO CONTROL
rmpath ('servofilter');         % Remove path to servo
mdlfile='typeB1simctrl_150616'; % typeB1 ver.150616
% SETTING FILTERS
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
freq =logspace(-2,2,1001);
freq2=logspace(-1,1,2001);
freq0=logspace(-2,3,2001);
freq3=logspace(-2,1,2001);

%% AA
mybodeplot({dampflt},freq0);

%% OL GAIN
clblstOL={...
    dampflt*gain_act_LIM,...
    dampflt*gain_act_TIM,...
    dampflt*gain_act_VIM,...
    dampflt*gain_act_RIM,...
    dampflt*gain_act_PIM,...
    dampflt*gain_act_YIM...
    };

doflst={...
     'actLIM','OSEM_LIM';
     'actTIM','OSEM_TIM';
     'actVIM','OSEM_VIM';
     'actRIM','OSEM_RIM';
     'actPIM','OSEM_PIM';
     'actYIM','OSEM_YIM'};

bodesusplotcmpopt(sysc0,...
    doflst(1:3,:),freq0,...
    'title',...
    'Open Loop Gain of IM-OSEM damping with gain=1',...
    'color',{'r-','b-','g-'},...
    'calibration',clblstOL(1:3));
export_fig('figure/typeB1proto_OSEMdamp_OLTF1_150703.pdf')

bodesusplotcmpopt(sysc0,...
    doflst(4:6,:),freq0,...
    'title',...
    'Open Loop Gain of IM-OSEM damping with gain=1',...
    'color',{'m-','c-','y-'},...
    'calibration',clblstOL(4:6));
export_fig('figure/typeB1proto_OSEMdamp_OLTF2_150703.pdf')


%% OL GAIN TM
clblstOL2={...
    dampflt*gain_act_LTM*0.5,...
    dampflt*gain_act_PTM*0.5,...
    dampflt*gain_act_YTM*0.5...
    };

doflst2={...
     'actLTM','OSEM_LTM';
     'actPTM','OSEM_PTM';
     'actYTM','OSEM_YTM'};

bodesusplotcmpopt(sysc0,...
    doflst2,freq0,...
    'title',...
    'Open Loop Gain of TM-OSEM damping with gain=0.5',...
    'color',{'r-','b-','g-'},...
    'calibration',clblstOL2);
%export_fig('figure/typeB1proto_OSEMdamp_OLTF3_150703.pdf')


%% SETTING
addpath('servofilter');         % Add path to servo
typeB1proto_servo_lock_150629;  % LOCK ACQUISITION MODE
rmpath ('servofilter');         % Remove path to servo
gain_LIM=0;gain_TIM=0;gain_VIM=0;
gain_RIM=0;gain_PIM=0;gain_YIM=0;
gain_LTM=0;gain_PTM=0;gain_YTM=0;
gain_VF0=0;gain_VF1=0;gain_VF2=0;
dampflt = myzpk(0,[3e2,3e2],9e4*pp);
servo_LIM = dampflt;
servo_TIM = dampflt;
servo_VIM = dampflt;
servo_RIM = dampflt;
servo_PIM = dampflt;
servo_YIM = dampflt;
servo_LTM = dampflt;
servo_PTM = dampflt;
servo_YTM = dampflt;

gainlist=[0,-0.1,-0.2,-0.5,-1];
sysd=cell(1,length(gainlist));
lgndlst=cell(1,length(gainlist));
clblst=cell(1,length(gainlist));







%% DAMPING ABOUT LTM
for i=1:length(gainlist)
gain_LTM=gainlist(i);
lgndlst{i}=['gain=',num2str(gainlist(i))];
clblst{i}=zpk([0,0],[],1);

mdlfile='typeB1simctrl_150616'; % typeB1 ver.150616
st     =linmod(mdlfile);
invl   =strrep(st.InputName, [mdlfile,'/'],'');
outvl  =strrep(st.OutputName,[mdlfile,'/'],'');
sysd{i}=ss(st.a,st.b,st.c,st.d,'inputname',invl,'outputname',outvl);
end
gain_LTM=gain_LTM*0;

%% PLOT ABOUT LTM
for i=1:length(sysd)
    disp(['gain=',num2str(gainlist(i))])
    [~,~]=findQ(sysd{i},0.657,0.01);
end

%% PLOT ABOUT LTM
bodesusplotcmpopt2(sysd,'accLGND','LTM',freq2,...
    'ylim',[1e-4,1e2],'legend',lgndlst,'title',...
    'Transfer function from LGND to LTM with TM-OSEM damping',...
    'ylabel','Magnitude [m/m]','calibration',clblst,...
    'xlim',[1e-1,4]);
% export_fig('figure/typeB1proto_OSEMdamp_LTM_150703.pdf')




%% DAMPING EFFECT
addpath('servofilter');         % Add path to servo
typeB1proto_servo_lock_150703;  % NO CONTROL
rmpath ('servofilter');         % Remove path to servo
mdlfile='typeB1simctrl_150616'; % typeB1 ver.150616
% SETTING FILTERS
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);

%% COMPARISON
poledampingcmp(sysc0,sysc);
export_fig('figure/typeB1proto_OSEMdamp_poledamp_150703.pdf')