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.30 % 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 ****
s=tf('s');
G2 = Gu('P2','Z');
G20 = abs(freqresp(G2,0));
%Have to have the disturbance matrix Gd for each output, they are all going
%to be analysed separately
Gd21=(Gd('P2','wG_in')); Gd22=(Gd('P2','wL_in')); 

%Both G1, G2 and G3 have the same poles.
disp('poles:')
p2=pole(G2);
disp(p2)
disp('zeros of P2:')
z2=zero(G2);
disp(z2)

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

[Gs2 Gm2 Gms2]=gms2(G2);
[Gds21 Gdm21 Gdms21]=gmsd21(Gd21);
[Gds22 Gdm22 Gdms22]=gmsd22(Gd22);

%% CONTROLLABILITY ANALYSIS FOR P2**
%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
STmin2=max([Mzpi Mpzi]);
SG2=zeros(1,nz);
for i=1:nz
    SG2(i)=abs(evalfr(Gm2,(z(i))));
end 
if isempty(SG2)==1
    SGmin2=0;
else    
    SGmin2=max(SG2);
end

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

if isempty(KS2)==1
    KSmin2=0;
else    
    KSmin2=max(KS2);
end    

%First for disturbance |

SGd21=zeros(1,nz);
for i=1:nz
    SGd21(i)=Mpzi*abs(evalfr(Gdms21,(z(i))));
end 
if isempty(SGd21)==1
    SGdmin21=0;
else    
    SGdmin21=max(SGd21);
end    


KSGd21=zeros(1,np);
for i=1:np
    KSGd21(i)=abs((evalfr(inv(Gs2),(p(i))))*evalfr(Gdms21,p(i)));
end 
if isempty(KSGd21)==1
    KSGdmin21=0;
else    
    KSGdmin21=max(KSGd21);
end 

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


KSGd22=zeros(1,np);
for i=1:np
    KSGd22(i)=abs((evalfr(inv(Gs2),(p(i))))*evalfr(Gdms22,p(i)));
end 
if isempty(KSGd22)==1
    KSGdmin22=0;
else    
    KSGdmin22=max(KSGd22);
end 

disp(['The lowest achivable peak for Msmin2 is: ', num2str(STmin2)]);
disp(['The bound for KSmin2 is: ', num2str(KSmin2)]);
disp(['The bound for KSd21 is: ', num2str(KSGdmin21)]);
disp(['The bound for KSd22 is: ', num2str(KSGdmin22)]);
disp(['The bound for SGmin2 is: ', num2str(SGmin2)]);
disp(['The bound for SGdmin21 is: ', num2str(SGdmin21)]);
disp(['The bound for SGdmin22 is: ', num2str(SGdmin22)]);
