function [F,f] = TEScasadi
% mass balance for the hot layer
% network balance for pressure
% integral flow control for demand control

import casadi.*
%% States
Vh      = MX.sym('Vh');                                  % hot layer volume
p       = MX.sym('p');                                       % network mass
intJ    = MX.sym('intJ');                                 % integral of qEP
x       = [Vh;p;intJ];                                % differential states
    
%% Disturbances
qS  =  MX.sym('qS');                 % hot water flowrate from swing boiler
q   =  MX.sym('q');                                                % demand 
d   = [qS;q];                                         % disturbances vector

%% Inputs fro MPC. zq is 
qSP =  MX.sym('qSP');                       % hot water direct to consumers
qD  =  MX.sym('qD');                                     % hot water dumped
qTP =  MX.sym('qTP');                        % hot water from tank to plant
qEP =  MX.sym('qEP');                      % hot water from electric boiler
qST = qS - qD - qSP;         % flowrate charging the tank from mass balance
u   = [qSP;qD;qTP;qEP];                                  % vector of inputs
%% Parameters
V    = 5000;                                             % [m3] tank volume
rho  = 1000;                                              % [kg/m3] density
p0   = 5;                                        % [bar] reference pressure
m0   = 15*1e6;                                % [kg] reference network mass
eps  = 0.1;                                   % [-] compressibility coeff

%% Hot layer tank mass balance 
dVhdt = qS - qD - qSP - qTP;                                 % mass balance

%% Pipeline mass balance
dpdt = p0/m0/eps*rho*(qSP+qTP+qEP-q); 

%% Form system of differential equations
diff = [dVhdt; dpdt; qEP];
ode  = struct('x',x,'p',[u;d],'ode', diff);
op   = struct('t0',0,'tf',1/60);

%% Objecive function, L
%L = 1e5*(p-5)^2 + 1e2*qD^2 + ...
 %   10*(qEP-0)^2 + 1e3*(qSP-qS)^2 - (qTP-0)^2;
%L = -5e2*qS - 1e2*qTP - 1e3*qST + qEP + 1e2*qD + 1e5*(p-5)^2;
%L = 0.1*(qSP/1000)^2 + (qTP/1000)^2 + 1e1*(qD/1000)^2 + 1e2*(qEP/1000)^2 + 1e4*(p-5)^2;
%L = 0.1*qS + qTP + 1e2*qD + 1e3*qEP + 1e5*(p-5)^2;
%L = (qTP/1000)^2 + 1e1*(qD/1000)^2 + 1e3*(qEP/1000)^2 + 1e4*(p-5)^2;%THIS
%L = 1e1*(qD/1000)^2 + 1e3*(qEP/1000)^2 + 1e4*(p-5)^2;
L =  1e3*(qEP/1000)^2 + 1e4*(p-5)^2;
%L  = qEP;
%% Casadi functions. 
f = Function('f', {x,[u;d]}, {diff,L},{'x','p'},{'xdot','qj'});
F = integrator('F','cvodes',ode,op);
