% MPC for cooler
% v0: not working yet. Tamal, Cristina and Adriana worked on it 15.Oct.17
% Need to check: 
% 1. Mismatch between "vectorized model" and original model because the steady
% state is not the same.
% 2. Implementation of complementary constraint. 

clc; clear; close all;
addpath('C:\Documentos\Casadi\casadi-matlabR2014b-v3.2.3')
import casadi.*

F_c_max = 10;        % maximum cold fluid mass flow, kg/s
F_h_max = 3;         % maximum hot fluid mass flow, kg/s 
u10=0.5;             % initial opening for cold valve, [-]
u20= 1;              % initial opening for hot valve, [-]
rho_c = 980 ;        % cold fluid density, kg/m3
rho_h = 890;         % hot fluid density,  kg/m3
U = 150*1e-3;        % heat transfer coefficient,   kW/m2/K
A = 90/10;              % heat transfer area, m2
cp_c = 1.5;          % specific heat for cold fluid, kJ/kg/K
cp_h = 1.2;          % specific heat for hot fluid,  kJ/kg/K
Tinh = 70+273.15; % parameter
Tin_c0 = 20 + 273.15; % cold fluid inlet temperature
b = 0.5;      % volume fraction,taken as equal for both shell and tube sides
Aspec = 100;   % specific area to volume, taken from mathisen.phd
V_c = A *b/Aspec; % volume cold side, from mathisen.phd
V_h = A *b/Aspec; % volume hot side, from mathisen.phd

%% casadi definitions
xc = MX.sym('xc',10,1); % x(1)
xh = MX.sym('xh',10,1);
x = [xc;xh]; % Temperatures
u = MX.sym('u',2,1); % control input;  valve opening
F_c = u(1)*F_c_max; % cold side mass flow
F_h = u(2)*F_h_max; % hot side mass flow
Tinc = MX.sym('Tinc',1,1); % disturbance
d = Tinc;
xc_bar = [xc(2:10);Tinc]; %inlet to each control volume
xh_bar = [Tinh;xh(1:9)];

Cc1=F_c_max/(rho_c*V_c);
Cc2=U*A/(rho_c*V_c*cp_c);
Ch1=F_h_max/(rho_h*V_h);
Ch2=U*A/(rho_h*V_h*cp_h);

%% model derivativesU*A/rho_h/V_h/cp_h
%xc_dot = F_c/rho_c/V_c*(xc_bar-xc) + (xh-xc)*U*A/rho_c/V_c/cp_c; % cold side
%xh_dot = F_h/rho_h/V_h*(xh_bar-xh) - (xh-xc)*; % hot side

xc_dot = Cc1*u(1)*(xc_bar-xc) + Cc2*(xh-xc); % cold side
xh_dot = Ch1*u(2)*(xh_bar-xh) - Ch2*(xh-xc); % hot side

%% defining casadi function for the model
f = Function('f', {x,u,d}, {[xc_dot;xh_dot]});
% f(x,u) you can call the function like this
f2= Function('f2', {x,u,d}, {HX_ODE(x,u,d)});

dae= struct('x',x,'p',[u;d],'ode', f(x,u,d));
op = struct('t0',0,'tf',10);
F = integrator('F','idas',dae,op);

dae2= struct('x',x,'p',[u;d],'ode', f2(x,u,d));
F2 = integrator('F2','cvodes',dae2,op);

%% Initial values

Tc0 = [314.1411  310.3426  307.0824  304.2841  301.8823  299.8208  298.0514  296.5326  295.2291  294.1102]';
Th0 = [335.2388  328.4495  322.6229  317.6224  313.3307  309.6473  306.4859  303.7724  301.4434  299.4442]';
x0 = [Tc0;Th0]; 
u0 = [u10;u20;Tin_c0]; %u, d
u02=[u10;u20];
% two alternatives for finding steady state
% x01 = fsolve(@(x) HX_ODE(x,u0(1:2),u0(3)),x0);
% 
 r=rootfinder('r', 'newton', f);
 x02 = full(r(x0, u0(1:2),u0(3)));

%Using HX_ODE
r2=rootfinder('r2', 'newton', f2);
x03 = full(r2(x0, u02, Tin_c0)); 

%% Steady state
Xk = x0;  Uk = u0;
t_final = 599;
x_int = zeros(20,t_final+1);
x_int(:,1) = Xk;
for i =2:t_final+1
Fk = F2('x0',Xk,'p',Uk);
Xk = full(Fk.xf);
x_int(:,i) = Xk;
   if i == 30
      Uk = [0.5;1;295.15];
   end
end

%% Plot
figure;
  plot(0:t_final,x_int(:,:))

figure;
plot(0:t_final,x_int(1,:),0:t_final,x_int(10,:))
legend('Tc,out','Tc,in')
figure;
plot(0:t_final,x_int(11,:),0:t_final,x_int(20,:))
legend('Th,out','Th,in')
%
figure;
plot(0:t_final,x_int(1,:),0:t_final,x_int(11,:))
legend('Tc,1','Th,1')
figure;
plot(0:t_final,x_int(10,:),0:t_final,x_int(20,:))
legend('Tc,10','Th,10')



