clc
clear all

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

load Data
z0 = 0.17;             %Initial valve position in step-test
Kc0= -0.025;           %gain used for the step test
dy_s = 7;  
t_init = 322.2;
dt = 0.6;

y = Data (:,3);
n = length(y);
t = 0:dt:(n-1)*dt;
r = [155*ones(1,t_init/dt) 162*ones(1,n-t_init/dt)];

%%
% %Low pass filter
n = length(y);
yf1 = zeros(1,n);
yf = zeros(1,n);
yff = zeros(1,n);
alpha = 0.25; % alpha=1 means no filtering
y_rev = zeros(1,n);
for k=1:n
    if (k==1)
        yf1(k) = y(k);
    elseif (k==2)
        yf1(:,k) = alpha*y(k) + (1-alpha)*yf1(:,k-1);
    else
        yf1(:,k) = alpha^2*y(k) + 2*(1-alpha)*yf1(:,k-1) - (1-alpha)^2*yf1(:,k-2);
    end
    y_rev(1,n-k+1) = yf1(:,k);
end


for k=1:n
    if (k==1)
        yf(k) = y_rev(k);
    elseif (k==2)
        yf(:,k) = alpha*y_rev(k) + (1-alpha)*yf(:,k-1);
    else
        yf(:,k) = alpha^2*y_rev(k) + 2*(1-alpha)*yf(:,k-1) - (1-alpha)^2*yf(:,k-2);
    end
    yff(1,n-k+1) = yf(:,k);
end

% End of low-pass filter

%%
i_init = round(mean(find(t==t_init)));
y_init = mean(yff(1:i_init));  

t_plant = t(i_init:end);
y_plant = yff(i_init:end);


yp = max(y_plant);
dy_p = abs(yp - y_init);

i_yp = find(y_plant==yp);
t_p1 = t_plant(i_yp);

yu = min(y_plant(i_yp:end));
dy_u = abs(yu - y_init);
i_yu = find(y_plant==yu);
t_u = t_plant(i_yu);

y_inf = mean(y_plant(round(2*length(y_plant)/3):end));  
dy_inf = abs(y_inf - y_init);

deltaT = t_u - t_p1;
tp = t_p1 - t_init;

%%
%%%%%%Simple Model%%%%%%

z = 0.17                             %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 = 3000;                         %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 = 6.7;                                                          %Length of riser
z_star= 0.16;                                                       %Bifurcation point
K_pc = 16e-04;                                                      %Valve constant (m2)
a = (1/rho)*((W/K_pc)^2);                                           %Constant parameter

p_star= (rho_l*g*L_r)+p_s+ p_vmin;            
delta_p_star = a/z_star^2;

p_fo = p_star-delta_p_star;                                        %inlet pressure at fully open position of the valve(at z=1) (pa)


K_z = -2*a/z^3;                                                    %static gain of the system (pa)
K_z0 = -2*a/z0^3/1000;

%%
%%%PI tuning parameters%%

T_osc= 90;                                     %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 Experiments AND FIND THE MAXIMUM STABILITY!')

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

%MODEL
z_t = 0.1:0.01:1;
n = length(z_t);
Pin = zeros(1,n);
K_z_t = zeros(1,n);


for i = 1:n
    Pin(i) =(a/z_t(i)^2 + p_fo)/1000;
    K_z_t(i) = (-2*a/z_t(i)^3)/1000;
    disp(['Z=' num2str(z_t(i))]);
    Kc=Betha*T_osc/K_z_t(i)*sqrt(z_t(i)/z_star);
    disp(['K_c=' num2str(1e4*Kc)]);
    tauI_z=3*T_osc*(z_t(i)/z_star);
    disp(['Tau_I=' num2str(tauI_z)]);
end

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

%Experiments

load steady_state
z_ss = steady_state(:,1);
p_ss = steady_state(:,2);

figure (2)
plot (z_ss, p_ss,'k','LineWidth',2.5);
hold on

load Data_open
z_open = Data_open(:,1);
p_min = Data_open(:,2);
p_max = Data_open(:,3);

figure (2)
plot (z_open,p_min,'k',z_open,p_max,'k','Linewidth',2.5);
legend('Simple static MATLAB model','Experimental',2);

