%CompBounds.m
%clear all
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));
poles=pole(G);
I_RHPP=find(real(poles)>0)
if isempty(I_RHPP)
    disp('No unstable poles, smallest pole:')
    max(real(poles));
else
    disp('Unstable poles:')
    poles(I_RHPP)
end
meas=[1 8 13 57 58 59];
MaxDev=[1 1 0.5 50 1 0.002];
dW=zpk([-2*pi/180 -2*pi/160],[-2*pi/90 -2*pi/30 -2*pi/30],0.2/0.4476);
Results=[];
for i=1:length(meas)
    Gi=1/MaxDev(i)*G(meas(i))*(1-OPz);
    Gdi=1/MaxDev(i)*Gd(meas(i),:)*[dW*8.64,0,0;0,dW*0.36,0;0,0,1e5];
    poles=pole(Gi);
    I_RHPP=find(real(poles)>0);
    zeros=zero(Gi);
    I_RHPZ=find(real(zeros)>0);
%     figure(i)
%     plot(real(poles(I_RHPP)),imag(poles(I_RHPP)),'x',real(zeros(I_RHPZ)),imag(zeros(I_RHPZ)),'o')
    MinDist(i)=100;Qz=[];Qp=[];Qpz=[];
    for j=1:length(I_RHPP)
        for k=1:length(I_RHPZ)
            Dist=abs(zeros(I_RHPZ(k))-poles(I_RHPP(j)))/abs(zeros(I_RHPZ(k))+conj(poles(I_RHPP(j))));
            if Dist<MinDist(i)
                MinDist(i)=Dist;
            end
            Qpz(j,k)=1/(zeros(I_RHPZ(k))-poles(I_RHPP(j)));
        end
    end
    for j=1:length(I_RHPP)
        for k=1:length(I_RHPP)
            Qp(j,k)=1/(conj(poles(I_RHPP(j)))+poles(I_RHPP(k)));
        end
    end
    for j=1:length(I_RHPZ)
        for k=1:length(I_RHPZ)
            Qz(j,k)=1/(zeros(I_RHPZ(j))+conj(zeros(I_RHPZ(k))));
        end
    end
    if isempty(Qp)|isempty(Qz)|isempty(Qpz)
        MTmin(i)=1;
    else
        MTmin(i)=sqrt(1+(max(svd((Qp^(-0.5)*Qpz*Qz^(-0.5))))^2));
    end
    mTSmin(i)=compMSTmin(Gi);
    Bz=zpk([zeros(I_RHPZ)'],[-zeros(I_RHPZ)'],1);Bp=zpk([-poles(I_RHPP)'],[poles(I_RHPP)'],1);
    Gis=minreal(zpk(Gi)*inv(Bp));
    KSminSimp=abs(max(freqresp(inv(Gis),poles(I_RHPP))));
    Gims=minreal(Gis*inv(Bz));
    for j=1:3
        Gdij=minreal(zpk(Gdi(j)));
        pGd=pole(Gdij);I_RHPPGd=find(real(pGd)>0);
        zGd=zero(Gdij);I_RHPZGd=find(real(zGd)>0);
        Bdz=zpk([zGd(I_RHPZGd)'],[-zGd(I_RHPZGd)'],1);Bdp=zpk([-pGd(I_RHPPGd)'],[pGd(I_RHPPGd)'],1);
        Gdis=minreal(zpk(Gdij)*inv(Bdp));
        Gdims(j)=minreal(Gdis*inv(Bdz));
    end
    for j=1:length(I_RHPP)
    KSGdminj(j)=abs(max(freqresp(inv(Gis),poles(I_RHPP(j)))))*abs(max(max(freqresp(Gdims,poles((I_RHPP(j)))))));
    end
    KSGdmin=max(KSGdminj);
    MSmin=[];
    for j=1:length(I_RHPZ)
        for k=1:length(I_RHPP)
            if length(MSmin)<j
                MSmin(j)=abs(zeros(I_RHPZ(j))+conj(poles(I_RHPP(k))))/abs(zeros(I_RHPZ(j))-poles(I_RHPP(k)));
            else
                MSmin(j)=MSmin(j)*abs(zeros(I_RHPZ(j))+conj(poles(I_RHPP(k))))/abs(zeros(I_RHPZ(j))-poles(I_RHPP(k)));
            end
        end
        SGminj(j)=abs(freqresp(Gims,zeros(I_RHPZ(j))))*MSmin(j);
        for k=1:3
        SGdminj(j,k)=abs(freqresp(Gdims(k),zeros(I_RHPZ(j))))*MSmin(j);
        end
    end

    if isempty(MSmin)
        MSminSimp=1;
        SGmin=0;
        SGdmin=0;
    else
        MSminSimp=max(MSmin);
        SGmin=max(SGminj);
        SGdmin=max(max(SGdminj));
    end
    if MinDist(i)==100
        MinDist(i)=inf;
    end
    sys = ss(Gi); a=sys.a; b=sys.b; c=sys.c; d=sys.d;
    [al,bl,cl,dl,ar,br,cr,dr,msta] = stabproj(a,b,c,d);
    pr = gram(-ar,-br);
    qru = gram(-ar',cr');
    KSmin(i) = 1/min(sqrt(eig(pr*qru)));
    if isempty(min(zeros(I_RHPZ)))
        minRHPZ=inf;
    else
        minRHPZ=min(zeros(I_RHPZ));
    end
    
    %Results=[Results;MinDist(i) MTmin(i)  MSminSimp KSmin(i) KSminSimp SGmin SGdmin KSGdmin];
    Results=[Results; minRHPZ evalfr(Gi,0) mTSmin(i) KSmin(i) SGmin KSGdmin SGdmin];
end
Results
