clc
clear all

%%
%%%%%%%STEP TEST INFORMATION%%%%%%%%

% load z30_148_150
% z0 = 0.3;             %Initial valve position in step-test
% Kc0= -0.1;            %gain used for the step test
% dy_s = 2;  
% t_init = 200;
% dt = 0.1;
% 
% t = z30_148_150(:,1);
% y = z30_148_150(:,2);
% r = z30_148_150(:,3);
% u = z30_148_150(:,4);

load z40_141_142
z0 = 0.4;             %Initial valve position in step-test
Kc0= -0.15;           %gain used for the step test
dy_s = 1;  
t_init = 300;
dt = 0.1;

t = z40_141_142(:,1);
y = z40_141_142(:,2);
r = z40_141_142(:,3);
u = z40_141_142(:,4);


i_init = find(t==t_init);
y_plant = y(i_init:end);
t_plant = t(i_init:end);
u_plant = u(i_init:end);
y_init = y(i_init-10);    
u_init = u(i_init-10);
yp = max(y_plant);     %Step change is positive
dy_p = abs(yp - y_init);
i_yp = find(y_plant==yp);
t_p1 = mean(t_plant(i_yp));
yu = min(y_plant(i_yp:10*i_yp));
dy_u = abs(yu - y_init);
i_yu = find(y_plant==yu);
t_u = mean(t_plant(i_yu));
y_inf = y_plant(end);
dy_inf = abs(y_inf - y_init);
tp = t_p1 - t_init;

deltat = t_u - t_p1;

%%
%%%%%%MODEL%%%%%%

z = 0.3                             %The operating point
g = 9.81;                           %Gravity (m/s2)
Wg_in=0.0024;                       %Inlet mass flow rate of gas (Kg/sec)
Wl_in=0.39298;                      %Inlet mass flow rate of liquid (Kg/sec)
W = Wg_in+Wl_in;                    %Inlet mass flow rate (Kg/sec)
R = 8314;                           %Gas constant (J/(K.Kmol))
M_g = 29;                           %Molecular weight of Gas (kg/kmol)

p_s=101325;                         %Seperator pressure (pa)
p_vmin = 0;                         %minimum Pressure drop over the valve (Pa)
T=15+273.15;                        %Riser temprature (K)
par.r2 = 0.025;                     %Radius of riser (m)
par.A2 = pi*par.r2^2;               %Cross section area of riser (m2)
rho_g= (p_s+p_vmin)*M_g/(R*T)       %Average gas density at the outlet(Kg/m3)
rho_l=1000;                         %Liquid density (Kg/m3)

alpha_g = Wg_in/(Wg_in+Wl_in)                                      %Average gas mass fraction
alpha_l=(1-alpha_g)*rho_g/((1-alpha_g)*rho_g+alpha_g*rho_l)        %liquid volume fraction

rho = alpha_l*rho_l+(1-alpha_l)*rho_g        %Average mixture density
L_r = 5.30%5.15;                             %Length of riser
z_star= 0.26;                                %Bifurcation point
cd = 0.31;                                   %Discharge coefficient of valve
K_pc = sqrt(2)* par.A2                       %Valve constant (m2)
a = (1/rho)*((W/K_pc)^2)                     %Constant parameter
p_star= (rho_l*g*L_r)+p_s+ p_vmin            
fz_star = z_star*cd/sqrt(1-z_star^2*cd^2)                             
delta_p_star = a/fz_star^2
p_fo = p_star-delta_p_star                   %inlet pressure at fully open position of the valve(at z=1) (pa)

fz = z*cd/sqrt(1-z^2*cd^2)
K_z = -2*a/z^3*cd^2                          %static gain of the system (pa)
K_z0 = -2*a/z0^3*cd^2

%%
%%%PI tuning parameters%%

T_osc= 140                                     %period of slugging oscillations in sec in the open loop system 
Betha=-log((dy_inf-dy_u)/(dy_p-dy_inf))/(2*deltat)+ Kc0*K_z0 *((dy_p-dy_inf)/dy_inf)^2/(4*tp)
Kc=Betha*T_osc/K_z*sqrt(z/z_star)
tauI_z=3*T_osc*(z/z_star)
disp('FEED TO OLGA AND FIND THE MAXIMUM STABILITY!')

%%
%%%%%%%%%%Plot%%%%%%%

%MODEL
z_t = 0.2:0.001:1;
n = length(z_t);
Pin = zeros(1,n);
K_z_t = zeros(1,n);


for i = 1:n
    fz = z_t(i)*cd/sqrt(1-z_t(i)^2*cd^2);
    Pin(i) =(a/fz^2 + p_fo)/1000;
    K_z_t(i) = (-2*a/z_t(i)^3*cd^2)/1000; 
end


figure(1)
clf
plot(z_t,Pin,'r','LineWidth',2.5);
xlabel('Z');
ylabel('Inlet Pressure [kPa]');
hold on
grid on


%%
%%%%%%OLGA MODEL

load Openloop
z_olga = Openloop (:,1);
P_max = Openloop (:,2);
P_min = Openloop (:,3);
P_ss = Openloop (:,4);


figure (1)
plot (z_olga,P_ss,'b','LineWidth',2.5);
hold on
legend('Simple static MATLAB model','OLGA case',2)

plot (z_olga,P_max,'b','LineWidth',2.5);
hold on
plot (z_olga,P_min,'b','LineWidth',2.5);
grid on

