%Tune_PI_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;

Gain=-0.16;
taui=600;FTC=1;taud=0;
K=(1-OPz)*Gain*tf([taui 1],[taui 0])*tf([taud 1],[0.1*taud 1])*tf(1,[FTC 1]);
L=G*K;
Wu=inv(tf([0.000004 1],[40 1]));
N=0.1;
S=minreal(inv(1+L));
T=minreal(1-S);
KS=minreal(K*S);
SG=minreal(S*G);
WuKSGd=minreal(Wu*tf(KS)*tf(Gd));
WuKSN=minreal(Wu*tf(KS)*N);
% figure(1)
% subplot(211)
% bodemag(S,T,{1e-3,1e3});legend('S','T')
% subplot(212)
% bodemag(KS,SG,{1e-3,1e3});legend('KS','SG')
% figure(2)
% subplot(211)
% bodemag(WuKSGd(1),WuKSGd(2),WuKSGd(3),{1e-3,1e3})
% subplot(212)
% bodemag(WuKSN,{1e-3,1e3})
if max(pole(S))<0
    disp('NS')
else
    disp('Not NS')
end
Gmu=pck(Gh.a,Gh.b,Gh.c,Gh.d);
[sysnlcf,sig,sysnrcf] = sncfbal(Gmu);
[Al,Bl,Cl,Dl]=unpck(sysnlcf);
nlcf=ss(Al,Bl,Cl,Dl);
Nl=nlcf(1);
Ml=nlcf(2);
Kh=ss(K)/(1-OPz);
M=minreal(-[Kh;1]*inv(1+Gh*Kh)*inv(Ml));
disp('Peaks in S, T, WuKSGd, WuKSN, SG and M')
disp([norm(S,'inf') norm(T,'inf') norm(WuKSGd,'inf') norm(WuKSN,'inf') norm(SG,'inf'), norm(M,'inf')])
