clc
clear all
warning('off')
%This file are used to linearize the New modell of L-riser by Esmaeil. 
%The linearization is done by three different methods. After the
%linearization a controllability analysis is executed. 

%% **** System Inputs ****
par = v4_new_4d_parameters(); %All parameters defined in one file
z0 = 0.3; % 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_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,Q)
%d disturbance(wG_in, wL_in)
%u input (Z)
De=(diag([1 1 1])); %see Sigurds book at page 6
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 ****

G3 =Gu('W','Z');
G30 = abs(freqresp(G3,0));
%Have to have the disturbance matrix Gd for each output, they are all going
%to be analysed separately
Gd31=minreal(Gd('W','wG_in'));  Gd32=(Gd('W','wL_in'));  

%Both G1, G2 and G3 have the same poles.
disp('poles:')
p3=pole(G3);
disp(p3)
disp('zeros of Q:')
z3=zero(G3);
disp(z3)

%% *** Split G into stable, minimum and stable minimum parts***
ztot=zero(G3); ptot=pole(G3);
z=ztot(find(ztot>0)); p=ptot(find(ptot>0));
nz=length(z); np=length(p);

[Gs3 Gm3 Gms3]=gms2(G3);
[Gds31 Gdm31 Gdms31]=gmsd31(Gd31);
[Gds32 Gdm32 Gdms32]=gmsd32(Gd32);


%% CONTROLLABILITY ANALYSIS FOR Q**
%Finding Mpzi and Mzpi
if nz==0 | np==0
    Mpzi=1;
    Mzpi=1;
else    
   for i=1:nz
       s=zeros(np,1);
       for j=1:np
       s(j)=abs((z(i)+p(j)))/abs((z(i)-p(j)));
       end
       Mpzi(i)=prod(s);      %min for hver zero
   end
   Mpzi=max(Mpzi);
   
   for i=1:np
       t=zeros(nz,1);
       for j=1:nz
       t(j)=abs((z(j)+p(i)))/abs((z(j)-p(i)));
       end
       Mzpi(i)=prod(t);    %min for hver pole
   end
   Mzpi=max(Mzpi);
end   

%bounds
STmin3=max([Mzpi Mpzi]);
SG3=zeros(1,nz);
for i=1:nz
    SG3(i)=abs(evalfr(Gm3,(z(i))));
end 
if isempty(SG3)==1
    SGmin3=0;
else    
    SGmin3=max(SG3);
end

KS3=zeros(1,np);
for i=1:np
    KS3(i)=abs(evalfr(inv(Gs3),(p(i))));
end 

if isempty(KS3)==1
    KSmin3=0;
else    
    KSmin3=max(KS3);
end    

%First for disturbance |

SGd31=zeros(1,nz);
for i=1:nz
    SGd31(i)=Mpzi*abs(evalfr(Gdms31,(z(i))));
end 
if isempty(SGd31)==1
    SGdmin31=0;
else    
    SGdmin31=max(SGd31);
end    


KSGd31=zeros(1,np);
for i=1:np
    KSGd31(i)=abs((evalfr(inv(Gs3),(p(i))))*evalfr(Gdms31,p(i)));
end 
if isempty(KSGd31)==1
    KSGdmin31=0;
else    
    KSGdmin31=max(KSGd31);
end 

%For disturbance 2
SGd32=zeros(1,nz);
for i=1:nz
    SGd32(i)=Mpzi*abs(evalfr(Gdms32,(z(i))));
end 
if isempty(SGd32)==1
    SGdmin32=0;
else    
    SGdmin32=max(SGd32);
end    


KSGd32=zeros(1,np);
for i=1:np
    KSGd32(i)=abs((evalfr(inv(Gs3),(p(i))))*evalfr(Gdms32,p(i)));
end 
if isempty(KSGd32)==1
    KSGdmin32=0;
else    
    KSGdmin32=max(KSGd32);
end 

disp(['The lowest achivable peak for Msmin3 is: ', num2str(STmin3)]);
disp(['The bound for KSmin3 is: ', num2str(KSmin3)]);
disp(['The bound for KSd31 is: ', num2str(KSGdmin31)]);
disp(['The bound for KSd32 is: ', num2str(KSGdmin32)]);
disp(['The bound for SGmin3 is: ', num2str(SGmin3)]);
disp(['The bound for SGdmin31 is: ', num2str(SGdmin31)]);
disp(['The bound for SGdmin32 is: ', num2str(SGdmin32)]);





