% Simulation of the new 4d model
clc
clear all
% **** System parameters ****
par = v4_new_4d_parameters();
% **** System Inputs ****
z0 = 0.15;
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('#########################');

%dt = 0.005;    % Simulation step time (sec)
t_f = 2;        % Final simulation time (hour)

%% **********************
disp('Simulation of nonlinear model');
dx = [-1;0;0;0];
u=[z0;wG_in0;wL_in0];
M=eye(4);
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:2 4]);
end

disp('Simulation completed.');
disp('#########################');
%% ******** Linearization *************
% options = [0,1e-5,1e-5,1e-12,0,0,0,0,0,0,0,0,0,1e5];
% [x0,u0,y,dx,options] = trim('v2_New_4D_mdl',x0,u0,y0,[],[],[],[],[],options);
para = [1e-5 0 0];
[A,B,C,D]= v4_new_4d_linmod_knut(x0,y0,u0,par);
disp(['Linearized model at the operating point Z=' num2str(z0)])
sys = ss(A,B,C,D);
set(sys,'inputname',{'Z','wG_in','wL_in'},'outputname',{'P1','P2','Q','W','Pb','DPr','Rho','AlphaL','Psqrt'},'statename',{'m_G1','m_L1','m_G2','m_L2'});
present(sys)

%% ********************* 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) Mass flow rate form choke valve (\itW_{out}\rm\bf)','FontSize',10,'FontWeight','bold','FontName','Times');
ylabel('\itW_{out}\rm [kg/s]','FontSize',10,'FontName','Times')
xlabel('time [min]','FontSize',10,'FontName','Times')
xlim([0 t_f*60])
%ylim([5 35])
