clc
clear all
warning ('off')
%% **** System Inputs ****
par = v4_new_4d_parameters(); %All parameters defined in one file
z0 =0.100; % 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 *************
[A,B,C,D]=v4_new_4d_linmod_knut(x0,y0,u0,par); %analytical 

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'));
Gtest=[G2;G3];
G10 = abs(freqresp(G1,0)); G20 = abs(freqresp(G2,0)); G30 = abs(freqresp(G3,0));
Gtest0 = abs(freqresp(Gtest,0));
% steady state value
%Have to have the disturbance matrix Gd for each output, they are all going
%to be analysed separately
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'));  

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
disp('zeros of test:')
zd21=zero(Gd21);
zd31=zero(Gd31);
zd32=zero(Gd32);
zd22=zero(Gd22);


ztest=[z2;z3]
zdtest=[zd21 ;zd31; zd22; zd32];

%Split G into stable and anti stable part, by using a matlab
%function. 
[GStestu, GNStestu]=stabsep(Gtest,'abstol',1e-15,'reltol',1e-15);

ptot1=pole(Gtest);
z1=ztest(find(ztest>0)); p1=ptot1(find(ptot1>0));
nz1=length(z1); np1=length(p1);

s=TF('s');
b=1;a=1;     
for i = 1:np1
    b=b*(s-p1(i))/(s+p1(i));
end;
for i = 1:nz1
    a=a*(s+z1(i))/(s-z1(i));
end    

Gs=minreal(tf(b*Gtest));Gm=minreal(tf(Gtest*a)); Gms=minreal(tf(b*Gtest*a));

[Gds21 Gdm21 Gdms21]=gmsd21(Gd21);
[Gds22 Gdm22 Gdms22]=gmsd22(Gd22);
[Gds31 Gdm31 Gdms31]=gmsd31(Gd31);
[Gds32 Gdm32 Gdms32]=gmsd32(Gd32);

Gdtest=[(Gd21) (Gd31);
          (Gd22) (Gd32)];

ptotd=pole(Gdtest);
zd=zdtest(find(zdtest>0)); pd=ptotd(find(ptotd>0));
nzd=length(zd); npd=length(pd);

s=tf('s');
b=1;a=1;     
for i = 1:npd
    b=b*(s-pd(i))/(s+pd(i));
end;
for i = 1:nzd
    a=a*(s+zd(i))/(s-zd(i));
end    

Gds=minreal(tf(b*Gdtest)); Gdm=minreal(tf(Gdtest*a)); Gdms=minreal(tf(b*Gdtest*a));


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

%*** Sensitivity and complementery sensitivity peak***
%Sensitivity peak for G1, where MSmin=TSmin!! 
RHPp1=p(find(p>0));
RHPztest=ztest(find(ztest>0));

if length(RHPp1)>0 && length(RHPztest)>0
np1=length(RHPp1); nz1=length(RHPztest);
sys1=ss(Gtest); [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,RHPztest(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(RHPztest)*ones(nz1)+ones(nz1)*diag(RHPztest')));
Qzp1=(Yz'*Yp).*(1./(diag(RHPztest)*ones(nz1,np1)-ones(nz1,np1)*diag(RHPp1)));

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

else
    Msmin=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=(GNStestu)';
h1=hsvd(tf);
KSmin=1/min(h1);

%% *** KSGd ***
%%%%%SOMETHING IS WRONG HERE, problems when using Hankel singular values.
%problem could be because this is not a square system. 
%Lower bound on KSGd 
solve1=((inv((Gdms)*(1.e-2*s+1)))*Gtest); 
[t11s t11ns]=stabsep(solve1);
tfKSd11=(t11ns)' ;
h4=hsvd(tfKSd11);
KSGd=1/min(h4);

%% 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. Have to chose one RHP-pole and one RHP-zero to put into the bound.

%SG
RHPp=p(find(p>0));
RHPz=ztest(find(ztest>0));

if length(RHPp)>0 && length(RHPz)>0
np=length(RHPp); nz=length(RHPz);
sys1=ss(Gtest); [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,RHPz(i))); Yz(:,i)=U(:,end); %zero direction
    end
    
num1=abs(((Yz(:,2))'*(evalfr(Gms,RHPz(2))))*(pinv((evalfr(Gms,RHPp(2)))))*Yp(:,2));
den1=norm((Yz(:,2))'*(evalfr(Gms,RHPz(2))),2)*norm((pinv(evalfr(Gms,RHPp(2))))*Yp(:,2),2);
cos1=(num1/den1)^2;
sin1=1-cos1;

term1 = (abs(RHPz(2)+conj(RHPp(2))))^2/(abs(RHPz(2)-RHPp(2)))^2;
SGmin=norm((Yz(:,2)'*(evalfr(Gms,RHPz(2)))),2)*sqrt(sin1+(term1*cos1)) ; 


 else 
    SGmin=0;
end



%% ** SGd ***
RHPp=p(find(p>0));
RHPz=ztest(find(ztest>0));

if length(RHPp)>0 && length(RHPz)>0
np=length(RHPp); nz=length(RHPz);
sys1=ss(Gtest); [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,RHPz(i))); Yz(:,i)=U(:,end); %zero direction
    end
    
num1=abs((Yz(:,2)'*(evalfr(Gdms,RHPz(2))))*(pinv(evalfr(Gdms,RHPp(2))))*Yp(:,2));
den1=norm(Yz(:,2)'*(evalfr(Gdms,RHPz(2))),2)*norm((pinv(evalfr(Gdms,RHPp(2))))*Yp(:,2),2);
cos1=(num1/den1)^2;
sin1=1-cos1;

term1 = (abs(RHPz(2)+conj(RHPp(2))))^2/(abs(RHPz(2)-RHPp(2)))^2;
SGdmin=norm((Yz(:,2)'*(evalfr(Gdms,RHPz(2)))),2)*sqrt(sin1+(term1*cos1)) ; 


 else 
    SGdmin=0;
end

disp(['The lowest achivable peak for Msmin is: ', num2str(Msmin)]);
disp(['The bound for KSmin is: ', num2str(KSmin)]);
disp(['The bound for SGmin is: ', num2str(SGmin)]);
disp(['The bound for KSGd is: ', num2str(KSGd)]);
disp(['The bound for SGd is: ', num2str(SGdmin)]);




