%% ABOUT THIS FILE
% ----------------------------------------------------------------------
% SUSPENSION CONTROL MODEL
% ----------------------------------------------------------------------
% 3D rigid body model
% Type-B1 prototype for KAGRA
% Coded by T. Sekiguchi on 2015/05/31
% ----------------------------------------------------------------------

%% PRELIMINARY

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

%% IMPORT MODEL
% FILE NAME
matfile='typeB1susmdl';
mdlfile='typeB1simctrl_150531';

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

% REDUCE DAMPING ON RIM
sys1.a(43,43)=sys1.a(43,43)/30;
sys1.a(49,49)=sys1.a(49,49)/30;
sys1.a(55,55)=sys1.a(55,55)/30;

% INCREASE DAMPING ON YF0, YRM,YTM
sys1.a(15,15)=sys1.a(15,15)*3000;
sys1.a(51,51)=sys1.a(51,51)*30;
sys1.a(57,57)=sys1.a(57,57)*300;



%% BLENDING FILTER F0
% Blending at 0.3 Hz
blend_LP = myzpk([0.075+1i*0.0581;0.075-1i*0.0581],[0.3;0.3;0.3;0.3;0.3],66.97);
blend_HP = myzpk([0.75+1i*0.581;0.75-1i*0.581;0;0;0],[0.3;0.3;0.3;0.3;0.3],1);
georesp  = zpk([-2.13+1i*5.19;-2.13-1i*5.19],[0;0],1);
vel2disp = zpk([],0,1);

blend_LVDT = blend_LP;
blend_GEO = minreal(blend_HP*georesp*vel2disp);


%% SERVO FILTER F0
dcservoflt = myzpk([3e-2;3e-2],[1e-4;10],330)...
            *myzpk([],[10;10],3948);
dampflt = myzpk(0,[100,100],1e4*pp);

servo_LF0 = dcservoflt;
servo_TF0 = dcservoflt;
servo_YF0 = dampflt;

gain_LF0 = -214.7;
gain_TF0 = -214.7;
gain_YF0 = -114.9;

%% SERVO FILTER IM
dampflt = myzpk(0,[100,100],1e4*pp);

servo_LIM = dampflt;
servo_TIM = dampflt;
servo_VIM = myzpk(0,[300,300],9e4*pp);
servo_RIM = dampflt;
servo_PIM = dampflt;
servo_YIM = dampflt;

gain_LIM = -155.0;
gain_TIM = -155.0;
gain_VIM = -384.5;
gain_RIM = -2.412;
gain_PIM = -2.399;
gain_YIM = -0.387;


%% SERVO FILTER TM
dampflt = myzpk(0,[100,100],1e4*pp);

servo_LTM = dampflt;
servo_PTM = dampflt;
servo_YTM = dampflt;

gain_LTM = -100.0;
gain_PTM = -1.133;
gain_YTM = -1.773;

%% SERVO FILTER GAS
dampflt = myzpk(0,[100,100],1e4*pp);

servo_VF0 = dampflt;
servo_VF1 = dampflt;
servo_VF2 = dampflt;

gain_VF0 = -1193.;
gain_VF1 = -940.7;
gain_VF2 = -383.3;

%% SERVO FILTER OL
servo_YOL_F0 = myzpk([],[1e-6;1e-2;1e-2],pp*1e-6*(pp*1e-2)^2*1000);
gain_YOL_F0 = 1;

%% CONTROL MODEL

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);


%% ZERO CONTROL
gain_LF0 = 0; gain_TF0 = 0; gain_YF0 = 0;
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;
gain_YOL_F0 = 0;
st=linmod(mdlfile);
sysc0=ss(st.a,st.b,st.c,st.d,'inputname',invl,'outputname',outvl);

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

%% POLE DAMPING
poledampingcmp(sysc0,sysc);
export_fig('figure/typeB1proto_damping_decaytime.pdf')

% 
% %% Noise Spectrum
% fRLF = 0.05; % 30 mHz
% filter_LFreject = myzpk([0;0],fRLF*[1;1],1);
% [magHP,~]       = mybode(filter_LFreject,freq);
% 
% nLVDT0=importdata('noise/LVDTNoiseDisp.txt');
% nGEO0 =importdata('noise/GEONoiseVelocity.txt');
% ngnd0 =importdata('noise/kamiokaQuiet.txt');
% nLVDT =interp1(nLVDT0(:,1),nLVDT0(:,2),freq')';
% nGEO  =interp1(nGEO0(:,1),nGEO0(:,2),freq')'./freq/2/pi;
% ngnd  =interp1(ngnd0(:,1),ngnd0(:,2),freq')'.*magHP;
% 
% % mypsdplot({nLVDT,nGEO,ngnd},freq);
% 
% pp=2*pi;fbF0=0.1;
% tstbG=myzpk([0,0,0,3.921,1.5395+1i*2.8561,1.5395-1i*2.8561]*fbF0,...
%     [1,1,1,1,1,1,1]*fbF0,1);
% tstbGa=myzpk(0,[],1);
% tstbG=tstbG*tstbGa;
% tstbL=myzpk([0.255,0.17245+1i*0.2868,0.17245-1i*0.2868]*fbF0,...
%     [1,1,1,1,1,1,1]*fbF0,35*(pp*fbF0)^4);
% %tstbG=tf([1,7,21,35,0,0,0,0],[1,7,21,35,35,21,7,1]);
% %tstbL=tf([35,21,7,1],[1,7,21,35,35,21,7,1]);
% [magG,~]=mybode(tstbG,freq);
% [magL,~]=mybode(tstbL,freq);
% % mybodeplot({tstbG,tstbL,tstbG+tstbL},freq);
% 
% nGc=nGEO.*magG;
% nLc=sqrt(nLVDT.^2+ngnd.^2).*magL;
% nSc=sqrt(nGc.^2+nLc.^2);
% 
% mypsdplot({ngnd,nGEO,nLVDT,nSc},freq,...
%     {'seismic','geophone','LVDT','sensor noise total'},'Noise Spectrum Plot',...
%     '[m/rtHz]',{'r-','b-','m-','k-'});
% 
% %% Servo
% % tstsF0=myzpk([0.05,0.05],[0.0001,20,20],1*pp*4000);
% % tstsF0b=myzpk(myfQ(0.42,2),myfQ(0.42,8),1);
% % tstsF0=tstsF0*tstsF0b;
% 
% tstsF0=myzpk(0,[20,20],10*pp*1e4);
% 
% %% Transfer Function
% systfL=takesysname(sysc0,'actLF0','LVDTLF0');
% systfG=takesysname(sysc0,'actLF0','GEOLF0');
% 
% 
% % mybodeuwplot({systfL*tstsF0},freq,{'OL'});
%