clc
clear all
warning ('off')

%% **** System Inputs ****
par = v4_new_4d_parameters(); %All parameters defined in one file
z0 =0.10; % NB!!! CHOKE OPENING
wG_in0 = 0.36;
wL_in0 = 8.64;
global u0;
u0 = [z0;wG_in0;wL_in0];
%% ******** Initializing *************
disp('Initializing...')
tic
[x0,y0,par] =  v4_new_4d_initialize(u0,par);
toc
disp('Initialization completed.');
disp('#########################');
%% ******** Linearization *************
%With this file it is possible to do the linearization with three different
%methods. Analytical, numerical and by using linmod in matlab.

%options = [0,1e-5,1e-5,1e-12,0,0,0,0,0,0,0,0,0,1e4];
%[x,u,y,dx,options] = trim('v4_New_4D_mdl',x0,u0,y0,[],[],[],[],[],options);
%para = [1e-5 0 0];
%[A,B,C,D]=linmod('v4_New_4D_mdl',x0,u0,para); 
[A,B,C,D]=linmod_knutfunk(x0,y0,u0,par); %analytical 
%[A,B,C,D] = v4_new_4d_LinModel_num(x0,y0,u0,par); %numerical
sys =(ss(A,B,C,D)); 
set(sys,'inputname',{'Z','wG_in','wL_in'},'outputname',{'P1','P2','W'},'statename',{'m_G1','m_L1','m_G2','m_L2'});
Gu = tf(sys({'P1','P2','W'},'Z'));
Gd = tf(sys({'P1','P2','W'},{'wG_in','wL_in'}));
%% ****Scaling****
%y output( P1,P2,W)
%d disturbance(wG_in, wL_in)
%u input (Z)
De=(diag([1 1 1])); 
Dd=(diag([0.04 1]));
Du=min(z0,1-z0);

Gu=(De^-1)*Gu*Du;
Gd=(De^-1)*Gd*Dd;
set(Gu,'inputname','Z','outputname',{'P1','P2','W'});
set(Gd,'inputname',{'wG_in','wL_in'},'outputname',{'P1','P2','W'});

%% ****Tranfer function and steady state values ****
G1 =(Gu('P1','Z')); G2 =(Gu('P2','Z')); G3 =(Gu('W','Z'));
G10 = abs(freqresp(G1,0)); G20 = abs(freqresp(G2,0)); G30 = abs(freqresp(G3,0)); % steady state value

%Have to have the disturbance matrix Gd for each output, they the gas and liquid flow rate into the riser are going
%to be analysed as two separately disturbances.
Gd11=(Gd('P1','wG_in')); Gd12=(Gd('P1','wL_in'));
Gd21=(Gd('P2','wG_in')); Gd22=(Gd('P2','wL_in')); 
Gd31=(Gd('W','wG_in'));  Gd32=(Gd('W','wL_in'));  

%Both G1, G2 and G3 have the same poles. Last pole set to zero. 
disp('poles:')
p=pole(G1);
disp(p)
disp('zeros of P1:')
z1=zero(G1);
disp(z1)
disp('zeros of P2:')
z2=zero(G2);
disp(z2)
disp('zeros of W:')
z3=zero(G3);
disp(z3)

z3(end)=0;

%Split G into stable and anti stable part, by using a matlab
%function. 
[GSu1, GNSu1]=stabsep(G1,'abstol',1e-5,'reltol',1e-15,'Offset',.01);
[GSu2, GNSu2]=stabsep(G2,'abstol',1e-5,'reltol',1e-15,'Offset',.01);
[GSu3, GNSu3]=stabsep(G3,'abstol',1e-5,'reltol',1e-15,'Offset',.01);

% Stable phase, minimum phase and minimum stable phase of all TF, Functions
% are made to performed this. 
[Gs1 Gm1 Gms1]=gms1(G1);
[Gs2 Gm2 Gms2]=gms2(G2);
[Gs3 Gm3 Gms3]=gms3(G3);
[Gds11 Gdm11 Gdms11]=gmsd11(Gd11);
[Gds12 Gdm12 Gdms12]=gmsd12(Gd12);
[Gds21 Gdm21 Gdms21]=gmsd21(Gd21);
[Gds22 Gdm22 Gdms22]=gmsd22(Gd22);
[Gds31 Gdm31 Gdms31]=gmsd31(Gd31);
[Gds32 Gdm32 Gdms32]=gmsd32(Gd32);

%% ******** Controllability analysis*************

%*** Sensitivity and complementery sensitivity peak***
%Sensitivity peak for G1, where MSmin=TSmin!! 
[p, z1]=pzmap(G1);
RHPp1=p(find(p>0));
RHPz1=z1(find(z1>0));

if length(RHPp1)>0 && length(RHPz1)>0
np1=length(RHPp1); nz1=length(RHPz1);
sys1=ss(G1); [V,E]=eig(sys1.A); C=sys1.C*V;
for i=1:np1
    Yp(:,i)=C(:,i)/norm(C(:,i)); %Pole direction
    end
for i=1:nz1
    [U,S,V]=svd(evalfr(sys1,RHPz1(i))); Yz(:,i)=U(:,end); %zero direction
end
Qp1=(Yp'*Yp).*(1./(diag(RHPp1')*ones(np1)+ones(np1)*diag(RHPp1)));
Qz1=(Yz'*Yz).*(1./(diag(RHPz1)*ones(nz1)+ones(nz1)*diag(RHPz1')));
Qzp1=(Yz'*Yp).*(1./(diag(RHPz1)*ones(nz1,np1)-ones(nz1,np1)*diag(RHPp1)));

Msmin1=sqrt(1+norm(sqrtm(inv(Qz1))*Qzp1*sqrtm(inv(Qp1)))^2);

else
    Msmin1=1;
end

% Sensitivity peak for G2
[p, z2]=pzmap(G2);
RHPp2=p(find(p>0));
RHPz2=z2(find(z2>0));

if length(RHPp2)>0 && length(RHPz2)>0
np2=length(RHPp2); nz2=length(RHPz2);
G2=ss(G2); [V,E]=eig(G2.A); C=G2.C*V;
for j=1:np2
    Yp(:,j)=C(:,j)/norm(C(:,j)); %Pole direction
    end
for i=1:nz2
    [U,S,V]=svd(evalfr(G2,RHPz2(i))); Yz(:,i)=U(:,end); %zero direction
end
Qp2=(Yp'*Yp).*(1./(diag(RHPp2')*ones(np2)+ones(np2)*diag(RHPp2)));
Qz2=(Yz'*Yz).*(1./(diag(RHPz2)*ones(nz2)+ones(nz2)*diag(RHPz2')));
Qzp2=(Yz'*Yp).*(1./(diag(RHPz2)*ones(nz2,np2)-ones(nz2,np2)*diag(RHPp2)));
Msmin2=sqrt(1+norm((Qz2^(-0.5))*Qzp2*(Qp2^(-0.5)))^2);

else
    Msmin2=1;
end

%Sensitivity peak for G3
[p, z3]=pzmap(G3);
z3(end)=0;
RHPp3=p(find(p>0));
RHPz3=z3(find(z3>0));

if length(RHPp3)>0 && length(RHPz3)>0

np3=length(RHPp3); nz3=length(RHPz3);
sys3=ss(G3); [V,E]=eig(sys3.A); C=sys3.C*V;
for i=1:np3
    Yp3(:,i)=C(:,i)/norm(C(:,i)); %Pole direction
    end
for i=1:nz3
    [U,S,V]=svd(evalfr(sys3,RHPz3(i))); Yz3(:,i)=U(:,end); %zero direction
end
Qp3=(Yp3'*Yp3).*(1./(diag(RHPp3')*ones(np3)+ones(np3)*diag(RHPp3)));
Qz3=(Yz3'*Yz3).*(1./(diag(RHPz3)*ones(nz3)+ones(nz3)*diag(RHPz3')));
Qzp3=(Yz3'*Yp3).*(1./(diag(RHPz3)*ones(nz3,np3)-ones(nz3,np3)*diag(RHPp3)));

Msmin3=sqrt(1+norm(sqrtm(inv(Qz3))*Qzp3*sqrtm(inv(Qp3)))^2);

else
    Msmin3=1;
end

%% ***KS ****
s=tf('s') ;
%To find the bound for KS the mirror image of the antistable part of the
%G`s are used. If a zero is very close to be negativ, this zero will be
%implemented in the antistable part.

%Lower bound on KS for the output P1
tf=(GNSu1)';
h1=hsvd(tf);
KSmin1=1/min(h1);

%Lower bound on KS for the output P2
tf2=(GNSu2)' ;
h2=hsvd(tf2);
KSmin2=1/min(h2);

%Lower bound on KS for the output W
tf3=(GNSu3)' ;
h3=hsvd(tf3);
KSmin3=1/min(h3);
 
%% *** KSGd **** 
%There are two disturbances for each output, and they will be considered
%separately. d1=wG_in and d2=wL_in

%Lower bound on KSGd1 for the output P1
solve1=((inv(Gdms11))*G1); 
[t11s t11ns]=stabsep(solve1);
tfKSd11=(t11ns)' ;
h4=hsvd(tfKSd11);
KSGd11=1/min(h4);

%Lower bound on KSGd2 for the output P1
solve2=((inv(Gdms12))*G1);
[t12s t12ns]=stabsep(solve2);
tfKSd12=(t12ns)';
h5=hsvd(tfKSd12);
KSGd12=1/min(h5);
% 
%Lower bound on KSGd1 for the output P2
solve3=((inv(Gdms21))*G2); %Put in one stable zero, so stabsep works
[t21s t21ns]=stabsep(solve3);
tfKSd21=(t21ns)' ;
h6=hsvd(tfKSd21);
KSGd21=1/min(h6);

%Lower bound on KSGd2 for the output P2
solve4=((inv(Gdms22))*G2);
[t22s t22ns]=stabsep(solve4);
tfKSd22=(t22ns)' ;
h7=hsvd(tfKSd22);
KSGd22=1/min(h7);

%Lower bound on KSGd1 for the output W
solve5=((inv((Gdms31)*(1.e-2*s+1)*(1.e-2*s+1)))*G3);
[t31s t31ns]=stabsep(solve5);
tfKSd31=(t31ns)';
h8=hsvd(tfKSd31);
KSGd31=1/min(h8);
%
%Lower bound on KSGd2 for the output W
solve6=((inv((Gdms32)*(1.e-2*s+1))*G3));
[t32s t32ns]=stabsep(solve6);
tfKSd32=(t32ns)';
h9=hsvd(tfKSd32);
KSGd32=1/min(h9);

%% SG *****
%Used the poles and zeros that made the highest bound, since this bound is
%only tight for one RHP-pole and one RHP-zero. So the pole and zero closest
%to each other will give the highest peak.
%SG for P1
[p, z1]=pzmap(G1);
RHPp1=p(find(p>0));
RHPz1=z1(find(z1>0));

if length(RHPp1)>0 && length(RHPz1)>0
np1=length(RHPp1); nz1=length(RHPz1);
sys1=ss(G1); [V,E]=eig(sys1.A); C=sys1.C*V;
for i=1:np1
    Yp(:,i)=C(:,i)/norm(C(:,i)); %Pole direction
end
    for i=1:nz1
    [U,S,V]=svd(evalfr(sys1,RHPz1(i))); Yz(:,i)=U(:,end); %zero direction
    end
    
num1=abs((Yz'*(evalfr(Gms1,RHPz1(1))))*((evalfr(Gms1,RHPp1(1)))^-1)*Yp);
den1=norm(Yz'*(evalfr(Gms1,RHPz2(1))),2)*norm(((evalfr(Gms1,RHPp1(1)))^-1)*Yp,2);
cos1=(num1/den1)^2;
sin1=1-cos1;

term1 = (abs(RHPz1(1)+conj(RHPp1(1))))^2/(abs(RHPz1(1)-RHPp1(1)))^2;
SGmin1=norm((Yz'*(evalfr(Gms1,RHPz1(1)))),2)*sqrt(sin1+(term1*cos1)) ; 


 else 
    SGmin1=0;
end
 
% SG for P2
%Have two RHP-poles and two RHP-zeros. Since these bound is valid for one
%of each, the poles and zeros that is closest to the origin is chosen,
%since these gives the highest bound on SG. 
[p, z2]=pzmap(G2);
RHPp2=p(find(p>0));
RHPz2=z2(find(z2>0));

if length(RHPp2)>0 && length(RHPz2)>0
np2=length(RHPp2); nz2=length(RHPz2);
G2=ss(G2); [V,E]=eig(G2.A); C=G2.C*V;
for j=1:np2
    Yp(:,j)=C(:,j)/norm(C(:,j)); %Pole direction
end
    
for i=1:nz2
    [U,S,V]=svd(evalfr(G2,RHPz2(i))); Yz(:,i)=U(:,end); %zero direction
end
%Chose the pole and zero that makes the worst bound, does not matterr which
%one is chosen to be used. 

num2=abs((Yz'*(evalfr(Gms2,RHPz2(2))))*((evalfr(Gms2,RHPp2(2)))^-1)*Yp);
den2=norm(Yz'*(evalfr(Gms2,RHPz2(2))),2)*norm(((evalfr(Gms2,RHPp2(2)))^-1)*Yp,2);
cos2=(num2/den2)^2;
sin2=1-cos2;

term1 = (abs(RHPz2(2)+conj(RHPp2(2))))^2/(abs(RHPz2(2)-RHPp2(2)))^2;
SGmin2=norm((Yz'*(evalfr(Gms2,RHPz2(2)))),2)*sqrt(sin2+(term1*cos2));
else
    SGmin2=0;
end

% SG for W
[p3, z3]=pzmap(G3);
z3(end)=0;
RHPp3=p(find(p>0));
RHPz3=z3(find(z3>0));

if length(RHPp3)>0 && length(RHPz3)>0
np3=length(RHPp3); nz3=length(RHPz3);
G3=ss(G3); [V,E]=eig(G3.A); C=G3.C*V;
for j=1:np3
    Yp(:,j)=C(:,j)/norm(C(:,j)); %Pole direction
end
    
for i=1:nz3
    [U,S,V]=svd(evalfr(G3,RHPz3(i))); Yz(:,i)=U(:,end); %zero direction
end
%Chose the pole and zero that makes the worst bound, does not matterr which
%one is chosen to be used. 

num3=abs((Yz'*(evalfr(Gms3,RHPz3(1))))*((evalfr(Gms3,RHPp3(1)))^-1)*Yp);
den3=norm(Yz'*(evalfr(Gms3,RHPz3(1))),2)*norm(((evalfr(Gms3,RHPp3(1)))^-1)*Yp,2);
cos3=(num3/den3)^2;
sin3=1-cos3;

term1 = (abs(RHPz3(1)+conj(RHPp3(1))))^2/(abs(RHPz3(1)-RHPp3(1)))^2;
SGmin3=norm((Yz'*(evalfr(Gms3,RHPz3(1)))),2)*sqrt(sin3+(term1*cos3));
else
    SGmin3=0;
end

%% ***** SGd ********
%SGd11
[p, z1]=pzmap(G1);
RHPp1=p(find(p>0));
RHPz1=z1(find(z1>0));

if length(RHPp1)>0 && length(RHPz1)>0
np1=length(RHPp1); nz1=length(RHPz1);
sys1=ss(G1); [V,E]=eig(sys1.A); C=sys1.C*V;
for i=1:np1
    Yp(:,i)=C(:,i)/norm(C(:,i)); %Pole direction
end
    for i=1:nz1
    [U,S,V]=svd(evalfr(sys1,RHPz1(i))); Yz(:,i)=U(:,end); %zero direction
    end
    
num1=abs((Yz'*(evalfr(Gdms11,RHPz1(2))))*((evalfr(Gdms11,RHPp1(2)))^-1)*Yp);
den1=norm(Yz'*(evalfr(Gdms11,RHPz1(2))),2)*norm(((evalfr(Gdms11,RHPp1(2)))^-1)*Yp,2);
cos1=(num1/den1)^2;
sin1=1-cos1;

term1 = (abs(RHPz1(2)+conj(RHPp1(2))))^2/(abs(RHPz1(2)-RHPp1(2)))^2;
SGdmin11=norm((Yz'*(evalfr(Gdms11,RHPz1(2)))),2)*sqrt(sin1+(term1*cos1)) ; 


 else 
    SGdmin11=0;
end

%SGdmin12
[p, z1]=pzmap(G1);
RHPp1=p(find(p>0));
RHPz1=z1(find(z1>0));

if length(RHPp1)>0 && length(RHPz1)>0
np1=length(RHPp1); nz1=length(RHPz1);
sys1=ss(G1); [V,E]=eig(sys1.A); C=sys1.C*V;
for i=1:np1
    Yp(:,i)=C(:,i)/norm(C(:,i)); %Pole direction
end
    for i=1:nz1
    [U,S,V]=svd(evalfr(sys1,RHPz1(i))); Yz(:,i)=U(:,end); %zero direction
    end
    
num1=abs((Yz'*(evalfr(Gdms12,RHPz1(2))))*((evalfr(Gdms12,RHPp1(2)))^-1)*Yp);
den1=norm(Yz'*(evalfr(Gdms12,RHPz1(2))),2)*norm(((evalfr(Gdms12,RHPp1(2)))^-1)*Yp,2);
cos1=(num1/den1)^2;
sin1=1-cos1;

term1 = (abs(RHPz1(2)+conj(RHPp1(2))))^2/(abs(RHPz1(2)-RHPp1(2)))^2;
SGdmin12=norm((Yz'*evalfr(Gdms12,RHPz1(2))),2)*sqrt(sin1+(term1*cos1)) ; 

 else 
    SGdmin12=0;
end

%SGdmin21
[p, z2]=pzmap(G2);
RHPp2=p(find(p>0));
RHPz2=z2(find(z2>0));

if length(RHPp2)>0 && length(RHPz2)>0
np2=length(RHPp2); nz2=length(RHPz2);
G2=ss(G2); [V,E]=eig(G2.A); C=G2.C*V;
for j=1:np2
    Yp(:,j)=C(:,j)/norm(C(:,j)); %Pole direction
end
    
for i=1:nz2
    [U,S,V]=svd(evalfr(G2,RHPz2(i))); Yz(:,i)=U(:,end); %zero direction
end
num2=abs((Yz'*(evalfr(Gdms21,RHPz2(2))))*((evalfr(Gdms21,RHPp2(2)))^-1)*Yp);
den2=norm(Yz'*(evalfr(Gdms21,RHPz2(2))),2)*norm(((evalfr(Gdms21,RHPp2(2)))^-1)*Yp,2);
cos2=(num2/den2)^2;
sin2=1-cos2;

term1 = (abs(RHPz2(2)+conj(RHPp2(2))))^2/(abs(RHPz2(2)-RHPp2(2)))^2;
SGdmin21=norm((Yz'*(evalfr(Gdms21,RHPz2(2)))),2)*sqrt(sin2+(term1*cos2));
else
    SGdmin21=0;
end

%SGdmin22
[p, z2]=pzmap(G2);
RHPp2=p(find(p>0));
RHPz2=z2(find(z2>0));

if length(RHPp2)>0 && length(RHPz2)>0
np2=length(RHPp2); nz2=length(RHPz2);
G2=ss(G2); [V,E]=eig(G2.A); C=G2.C*V;
for j=1:np2
    Yp(:,j)=C(:,j)/norm(C(:,j)); %Pole direction
end
    
for i=1:nz2
    [U,S,V]=svd(evalfr(G2,RHPz2(i))); Yz(:,i)=U(:,end); %zero direction
end
num2=abs((Yz'*(evalfr(Gdms22,RHPz2(2))))*((evalfr(Gdms22,RHPp2(2)))^-1)*Yp);
den2=norm(Yz'*(evalfr(Gdms22,RHPz2(2))),2)*norm(((evalfr(Gdms22,RHPp2(2)))^-1)*Yp,2);
cos2=(num2/den2)^2;
sin2=1-cos2;

term1 = (abs(RHPz2(2)+conj(RHPp2(2))))^2/(abs(RHPz2(2)-RHPp2(2)))^2;
SGdmin22=norm((Yz'*(evalfr(Gdms22,RHPz2(2)))),2)*sqrt(sin2+(term1*cos2));
else
    SGdmin22=0;
end

%SGdmin31
[p3, z3]=pzmap(G3);
z3(end)=0;
RHPp3=p(find(p>0));
RHPz3=z3(find(z3>0));

if length(RHPp3)>0 && length(RHPz3)>0
np3=length(RHPp3); nz3=length(RHPz3);
G3=ss(G3); [V,E]=eig(G3.A); C=G3.C*V;
for j=1:np3
    Yp(:,j)=C(:,j)/norm(C(:,j)); %Pole direction
end
    
for i=1:nz3
    [U,S,V]=svd(evalfr(G3,RHPz3(i))); Yz(:,i)=U(:,end); %zero direction
end

num3=abs((Yz'*(evalfr(Gdms31,RHPz3(2))))*((evalfr(Gdms31,RHPp3(2)))^-1)*Yp);
den3=norm(Yz'*(evalfr(Gdms31,RHPz3(2))),2)*norm(((evalfr(Gdms31,RHPp3(2)))^-1)*Yp,2);
cos3=(num3/den3)^2;
sin3=1-cos3;

term1 = (abs(RHPz3(2)+conj(RHPp3(2))))^2/(abs(RHPz3(2)-RHPp3(2)))^2;
SGdmin31=norm((Yz'*(evalfr(Gdms31,RHPz3(2)))),2)*sqrt(sin3+(term1*cos3));
else
    SGdmin31=0;
end

%SGdmin32
[p3, z3]=pzmap(G3);
z3(end)=0;
RHPp3=p(find(p>0));
RHPz3=z3(find(z3>0));

if length(RHPp3)>0 && length(RHPz3)>0
np3=length(RHPp3); nz3=length(RHPz3);
G3=ss(G3); [V,E]=eig(G3.A); C=G3.C*V;
for j=1:np3
    Yp(:,j)=C(:,j)/norm(C(:,j)); %Pole direction
end
    
for i=1:nz3
    [U,S,V]=svd(evalfr(G3,RHPz3(i))); Yz(:,i)=U(:,end); %zero direction
end


num3=abs((Yz'*(evalfr(Gdms32,RHPz3(2))))*((evalfr(Gdms32,RHPp3(2)))^-1)*Yp);
den3=norm(Yz'*(evalfr(Gdms32,RHPz3(2))),2)*norm(((evalfr(Gdms32,RHPp3(2)))^-1)*Yp,2);
cos3=(num3/den3)^2;
sin3=1-cos3;

term1 = (abs(RHPz3(2)+conj(RHPp3(2))))^2/(abs(RHPz3(2)-RHPp3(2)))^2;
SGdmin32=norm((Yz'*(evalfr(Gdms32,RHPz3(2)))),2)*sqrt(sin3+(term1*cos3));
else
    SGdmin32=0;
end

disp(['The lowest achivable peak for Msmin1 is: ', num2str(Msmin1)]);
disp(['The lowest achivable peak for Msmin2 is: ', num2str(Msmin2)]);
disp(['The lowest achivable peak for Msmin3 is: ', num2str(Msmin3)]);
disp(['The bound for KSmin1 is: ', num2str(KSmin1)]);
disp(['The bound for KSmin2 is: ', num2str(KSmin2)]);
disp(['The bound for KSmin3 is: ', num2str(KSmin3)]);
disp(['The bound for KSGd11 is: ', num2str(KSGd11)]);
disp(['The bound for KSGd12 is: ', num2str(KSGd12)]);
disp(['The bound for KSGd21 is: ', num2str(KSGd21)]);
disp(['The bound for KSGd22 is: ', num2str(KSGd22)]);
disp(['The bound for KSGd31 is: ', num2str(KSGd31)]);
disp(['The bound for KSGd32 is: ', num2str(KSGd32)]);
disp(['The bound for SGmin1 is: ', num2str(SGmin1)]);
disp(['The bound for SGmin2 is: ', num2str(SGmin2(1:1))]);
disp(['The bound for SGmin3 is: ', num2str(SGmin3)]);
disp(['The bound for SGdmin11 is: ', num2str(SGdmin11)]);
disp(['The bound for SGdmin12 is: ', num2str(SGdmin12)]);
disp(['The bound for SGdmin21 is: ', num2str(SGdmin21(1:1))]);
disp(['The bound for SGdmin22 is: ', num2str(SGdmin22(1:1))]);
disp(['The bound for SGdmin31 is: ', num2str(SGdmin31(1:1))]);
disp(['The bound for SGdmin32 is: ', num2str(SGdmin32(1:1))]);



















