clc
clear all
warning ('off')
%This file are used to linearize the New modell of L-riser made by Esmaeil. 
%The linearization can be performed in three different ways. After the
%linearization a controllability analysis is performed. 

%% **** 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. They give
%the same anwear

%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); %linmod function
%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,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'); 
G10 = abs(freqresp(G1,0));
%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'));
%Both G1, G2 and G3 have the same poles.
disp('poles:')
p=pole(G1);
disp(p)
disp('zeros of P1:')
z1=zero(G1);
disp(z1)

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

[Gs1 Gm1 Gms1]=gms1(G1);
[Gds11 Gdm11 Gdms11]=gmsd11(Gd11);
[Gds12 Gdm12 Gdms12]=gmsd12(Gd12);

%% CONTROLLABILITY ANALYSIS FOR P1**

%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);     
   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);  
   end
   Mzpi=max(Mzpi);
end   

%bounds
STmin1=max([Mzpi Mpzi]);
SG1=zeros(1,nz);
for i=1:nz
    SG1(i)=abs(evalfr(Gm1,(z(i))));
end 
if isempty(SG1)==1
    SGmin1=0;
else    
    SGmin1=max(SG1);
end

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

if isempty(KS1)==1
    KSmin1=0;
else    
    KSmin1=max(KS1);
end    

%First for disturbance 1

SGd11=zeros(1,nz);
for i=1:nz
    SGd11(i)=Mpzi*abs(evalfr(Gdms11,(z(i))));
end 
if isempty(SGd11)==1
    SGdmin11=0;
else    
    SGdmin11=max(SGd11);
end    


KSGd11=zeros(1,np);
for i=1:np
    KSGd11(i)=abs((evalfr(inv(Gs1),(p(i))))*evalfr(Gdms11,p(i)));
end 
if isempty(KSGd11)==1
    KSGdmin11=0;
else    
    KSGdmin11=max(KSGd11);
end 

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


KSGd12=zeros(1,np);
for i=1:np
    KSGd12(i)=abs((evalfr(inv(Gs1),(p(i))))*evalfr(Gdms12,p(i)));
end 
if isempty(KSGd12)==1
    KSGdmin12=0;
else    
    KSGdmin12=max(KSGd12);
end 

disp(['The lowest achivable peak for Msmin1 is: ', num2str(STmin1)]);
disp(['The bound for KSmin1 is: ', num2str(KSmin1)]);
disp(['The bound for KSd11 is: ', num2str(KSGdmin11)]);
disp(['The bound for KSd12 is: ', num2str(KSGdmin12)]);
disp(['The bound for SGmin1 is: ', num2str(SGmin1)]);
disp(['The bound for SGdmin11 is: ', num2str(SGdmin11)]);
disp(['The bound for SGdmin12 is: ', num2str(SGdmin12)]);




