% Simulation of the new 6d model
clc
clear all
% **** System parameters ****
par = v1_new_6d_parameters();
% **** System Inputs ****
z1 = 1; % Subsea choke valve
z2 = 0.1; % Topside choke valve
global u0;
u0 = [z1;z2;0;0];
% ******** Finding initial states *************
disp('Initializing...')
tic
[x0,y0,par] =  v1_new_6d_initialize(u0,par);
toc
disp('Initialization completed.');
disp('#########################');

t_f = 2;        % Final simulation time (hour)

%% **********************
disp('Simulation of nonlinear model');
dx = [0;0;-1;0;0;0];
u=[z1;z2];
M=eye(6);
options=odeset('Mass',M,'BDF','on','AbsTol',1e-12,'RelTol',1e-12,'MaxStep',1);
[t,x]=ode15s(@v1_new_6d_model,[0;3600*t_f],x0 + dx,options,u0,'derivatives',par);
% *** Calculating measurements ****
n = length(t);
y = zeros(6,n);
for k = 1:n
    yt = v1_new_6d_model(t(k),x(k,:),u0,'measurements',par);
    y(:,k) = yt([1 3 4 7 9 8]);
end

disp('Simulation completed.');
disp('#########################');
%% ******** Linearization *************
[A,B,C,D]=v1_new_6d_linmod(x0,y0,u0,par); %analytical
%[A,B,C,D] = v1_new_6d_linmod_num(x0,u0,par); %numerical
sys =(ss(A,B,C,D));
set(sys,'inputname',{'Z1','Z2','d1','d2'},'outputname',{'P_bh','P_wh','W_in','P_in','P_rb','DP_r','P_t','Q_out','W_out','Rho_t','Alpha_L'},'statename',{'m_Gw','m_Lw','m_Gp','m_Lp','m_Gr','m_Lr'});
%% ****Scaling****
%y output( P1,P2,W)
%d disturbance(wG_in, wL_in)
%u input (Z)
De=(diag([1 1 1 1 1 1 1 2 1 50 1]));
Dd=(diag([0.04 1]));
Du=diag([min(z1,1-z1) min(z2,1-z2)]);

Bs1 = B(:,1:2)*Du;
Bs2 = B(:,3:4)*Dd;
Bs = [Bs1 Bs2];

Cs = (De^-1)*C;

Ds1 = (De^-1)*D(:,1:2)*Du;
Ds2 = (De^-1)*D(:,3:4)*Dd;
Ds = [Ds1 Ds2];
sys =(ss(A,Bs,Cs,Ds));
set(sys,'inputname',{'Z1','Z2','d1','d2'},'outputname',{'P_bh','P_wh','W_in','P_in','P_rb','DP_r','P_t','Q_out','W_out','Rho_t','Alpha_L'},'statename',{'m_Gw','m_Lw','m_Gp','m_Lp','m_Gr','m_Lr'});
Gu = tf(sys({'P_bh','P_wh','W_in','P_in','P_rb','DP_r','P_t','Q_out','W_out','Rho_t','Alpha_L'},'Z2'));
present(sys)
%% ********************* Figure 1 **************************
figure(1)
clf
set(gcf,'Color',[1,1,1])
set(gca,'FontSize',8)

subplot(3,2,1)
set(gca,'FontSize',8,'OuterPosition',[0  2/3  1/2  1/3])
plot(t/60,y(1,:),'k');
title('a) Bottom-hole pressure (\itP\rm\bf_{bh})','FontSize',12,'FontWeight','bold','FontName','Times');
ylabel('\itP\rm_{bh} [bar]','FontSize',12,'FontName','Times')
xlabel('time [min]','FontSize',12,'FontName','Times')
xlim([0 t_f*60])
%ylim([68 80])

subplot(3,2,2)
set(gca,'FontSize',8,'OuterPosition',[1/2  2/3  1/2  1/3])
plot(t/60,y(2,:),'k');
title('b) Inlet mass flow rate (\itw\rm\bf_{in})','FontSize',12,'FontWeight','bold','FontName','Times');
ylabel('\itw\rm_{in} [kg/s]','FontSize',12,'FontName','Times')
xlabel('time [min]','FontSize',12,'FontName','Times')
xlim([0 t_f*60])
%ylim([5 35])

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

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

subplot(3,2,5)
set(gca,'FontSize',8,'OuterPosition',[0  0 1/2  1/3])
plot(t/60,y(5,:),'k');
title('e) Outlet mass flow rate (\itw\rm\bf_{out})','FontSize',12,'FontWeight','bold','FontName','Times');
ylabel('\itw\rm_{out} [kg/s]','FontSize',12,'FontName','Times')
xlabel('time [min]','FontSize',12,'FontName','Times')
xlim([0 t_f*60])
%ylim([5 35])


subplot(3,2,6)
set(gca,'FontSize',8,'OuterPosition',[1/2  0  1/2  1/3])
plot(t/60,y(6,:),'k');
title('f) Outlet volumetric flow rate (\itQ\rm\bf_{out})','FontSize',12,'FontWeight','bold','FontName','Times');
ylabel('\itQ\rm_{out} [litre/s]','FontSize',12,'FontName','Times')
xlabel('time [min]','FontSize',12,'FontName','Times')
xlim([0 t_f*60])
%ylim([5 35])
