% Simulation of the new 4d model
clc
clear all
% **** System parameters ****
par = v4_new_4d_parameters();
% **** System Inputs ****
z0 = 1; % NB! This is choke valve opening [0 1]
% System goes unstable at z=0.05
wG_in0 = 0.36;
wL_in0 = 8.64;
global u0;
u0 = [z0;wG_in0;wL_in0];
% ******** Finding initial states *************
disp('Initializing...')
tic
[x0,y0,par] =  v4_new_4d_initialize(u0,par);
toc
disp('Initialization completed.');
disp('#########################');

%% **********************
disp('Simulation of nonlinear model');
% This a deviation form the steady state to make model unstable
% in unstable operating point
dx = [-1;0;0;0];
u=[z0;wG_in0;wL_in0];
M=eye(4);
t_f = 1;        % Final simulation time (hour)
options=odeset('Mass',M,'BDF','on','AbsTol',1e-12,'RelTol',1e-12,'MaxStep',1);
[t,x]=ode15s(@v4_new_4d_model,[0;3600*t_f],x0 + dx,options,u0,'derivatives',par);
% *** Calculating measurements ****
n = length(t);
y = zeros(3,n);
for k = 1:n
    yt = v4_new_4d_model(t(k),x(k,:),u0,'measurements',par);
    y(:,k) = yt(1:3);
end

disp('Simulation completed.');
disp('#########################');
%% ******** Linearization *************
[A,B,C,D]=v4_new_4d_linmod_num(x0,u0,par); %analytical
sys =(ss(A,B,C,D));
set(sys,'inputname',{'Z','wG_in','wL_in'},'outputname',{'P1','P2','Q','W'},'statename',{'m_G1','m_L1','m_G2','m_L2'});
Gu = tf(sys({'P1','P2','Q','W'},'Z'));
Gd = tf(sys({'P1','P2','Q','W'},{'wG_in','wL_in'}));

G1 = tf(sys('P1','Z'));
G2 = tf(sys('P2','Z'));
G3 = tf(sys('Q','Z'));
G4 = tf(sys('W','Z'));
zpk(G1)
zpk(G2)
zpk(G3)
zpk(G4)

% Steady-state gains
G10 = abs(freqresp(G1,0));
G20 = abs(freqresp(G2,0));
G30 = abs(freqresp(G3,0));
G40 = abs(freqresp(G4,0));

disp('poles:')
p = pole(G1);
disp(p)
disp('Period of the linearized model [sec]')
disp(2*pi/abs(imag(p(3))))

disp('zeros of P1:')
disp(zero(G1))
disp('zeros of P2:')
disp(zero(G2))
disp('zeros of Q:')
disp(zero(G3))

disp('zeros of W:')
disp(zero(G4))

%% ********************* Figure 1 **************************
figure(1)
clf
set(gcf,'Color',[1,1,1])
set(gca,'FontSize',8)

subplot(3,1,1)
set(gca,'FontSize',8,'OuterPosition',[0  2/3  1  1/3])
plot(t/60,y(1,:),'k');
title('a) Pressure at inlet of pipeline (\itP\rm\bf_1)','FontSize',10,'FontWeight','bold','FontName','Times');
ylabel('\itP\rm_1 [bar]','FontSize',10,'FontName','Times')
xlabel('time [min]','FontSize',10,'FontName','Times')
xlim([0 t_f*60])
%ylim([68 80])


subplot(3,1,2)
set(gca,'FontSize',8,'OuterPosition',[0  1/3  1  1/3])
plot(t/60,y(2,:),'k');
title('b) Pressure at top of riser (\itP\rm\bf_2)','FontSize',10,'FontWeight','bold','FontName','Times');
ylabel('\itP\rm_2 [bar]','FontSize',10,'FontName','Times')
xlabel('time [min]','FontSize',10,'FontName','Times')
xlim([0 t_f*60])
%ylim([50 62])


subplot(3,1,3)
set(gca,'FontSize',8,'OuterPosition',[0  0  1  1/3])
plot(t/60,y(3,:),'k');
title('c) Volumetric flow form choke valve (\itQ_{out}\rm\bf)','FontSize',10,'FontWeight','bold','FontName','Times');
ylabel('\itQ_{out}\rm [L/s]','FontSize',10,'FontName','Times')
xlabel('time [min]','FontSize',10,'FontName','Times')
xlim([0 t_f*60])
%ylim([5 35])
