%Tune_hinf_PI.m
load BifData
global data mode
data=gen_data;
mode='derivatives';
OPz=0.3;
I_OPz=find(abs(us(1,:)-OPz)==min(abs(us(1,:)-OPz)));
[A,B,C,D,E,F,ys]=Linearize(@two_fluid,[],xs(:,I_OPz),[],us(1,I_OPz),us(2:4,I_OPz));

G=minreal(ss(A,B,C,D));Gd=minreal(ss(A,E,C,F));Gh=G(1);
G=Gh*(1-OPz);Gd=Gd(1,:);
dW=zpk([-2*pi/180 -2*pi/160],[-2*pi/90 -2*pi/30 -2*pi/30],0.2/0.4476);
Gd(1)=Gd(1)*dW*8.64;
Gd(2)=Gd(2)*dW*0.36;
Gd(3)=Gd(3)*tf(1,[10 1])*1e5;
rate=1/60;

Wu=((inv(tf([1/(10000*rate) 1],[1/rate 1]))));
N=1e-1;
Gdi=minreal(Gd);
systemnames = 'G Gdi Wu N';
inputvar = '[d(3);n(1);u(1)]';
input_to_G = '[u]';
input_to_Gdi = '[d]';
input_to_N = '[n]';
input_to_Wu= '[u]';
outputvar = '[Wu;-G-Gdi+N]';
sysoutname = 'P1';
cleanupsysic='yes';
sysic;
P1=minreal(ss(P1));
[K,g,gfin,info]=hinfsyn(P1,1,1,'TOLGAM',1e-6);

K1=minreal(K);
L1=minreal(G*K1);
S1=minreal(inv(1+L1));
T1=minreal(1-S1);
KSGd1=minreal(K1*S1*Gdi);
figure(2)
bodemag(KSGd1(1),KSGd1(2),KSGd1(3),inv(Wu),{1e-4,1e4})

MS=norm(S1,'inf');MT=norm(T1,'inf');MWuKS=norm(minreal(Wu*K1*S1*N),'inf');
MSG=norm(minreal(S1*G),'inf');MWuKSGd=norm(minreal(Wu*KSGd1),'inf');
MSGd=norm(minreal(S1*Gd),'inf');
disp('S T WuKS SG WuKSGd SGd')
disp([MS MT MWuKS MSG MWuKSGd MSGd])
figure(3)
bodemag(S1,T1)

rate=0.0238;%3*rateH2;
%Wu=inv(tf([0.00001/rate 1],[1/rate 1]));
Wu=((inv(tf([1/(10000*rate) 1],[1/rate 1]))));
N=1e-1;wd1=-2;
Wp=tf([1/5 10^wd1],[1 0.1*10^wd1]);
R=zpk([],[-1/100 -1/100],0.01/100,'DisplayFormat','time-constant');
systemnames = 'G Gdi Wu N Wp R';
inputvar = '[r(1);d(3);n(1);u(1)]';
input_to_G = '[u]';
input_to_Gdi = '[d]';
input_to_N = '[n]';
input_to_Wu= '[u]';
input_to_R= '[r]';
input_to_Wp= '[R-G-Gdi]';
outputvar = '[Wu;Wp;R-G-Gdi+N]';
sysoutname = 'P2';
cleanupsysic='yes';
sysic;
%P2=[0 P2(1,2) P2(1,3) P2(1,4) P2(1,5) 0; ...
%    P2(2,1) 0 0 0 0 0; ...
%    P2(3,:)]
P2=minreal(ss(P2));
[K2,g,gfin,info]=hinfsyn(P2,1,1,'TOLGAM',1e-6);

K2=minreal(K2);
L2=minreal(G*K2);
S2=minreal(inv(1+L2));
T2=minreal(1-S2);
KSGd2=minreal(K2*S2*Gdi);
figure(4)

bodemag(KSGd2(1),KSGd2(2),KSGd2(3),inv(Wu),{1e-4,1e4})

MS=norm(minreal(Wp*S2*R),'inf');MT=norm(T2,'inf');MWuKS=norm(minreal(Wu*K2*S2*N),'inf');
MSG=norm(minreal(Wp*S2*G),'inf');MWuKSGd=norm(minreal(Wu*KSGd2),'inf');
MSGd=norm(minreal(Wp*S2*Gd),'inf');
disp('S T WuKS SG WuKSGd SGd')
disp([MS MT MWuKS MSG MWuKSGd MSGd])
disp('WpSR WpSGd WuKSN WuKSGd')
disp([MS MSGd MWuKS MWuKSGd])
figure(5)
bodemag(S2,inv(Wp),T2)

K=K2*(1-OPz);