function [st,reA] = eNMPC_Setup(t,t_eNMPC,eNMPC,KS,KM,H,H_Ver)

% This function sets up the eNMPC scheme for the differential algebraic
% system used in the model. It is a stand alone code which requires CASADI
% to be loaded beforehand.
%
% The input is given as
%       t:          Time step for the integrator
%       t_eNMPC:    Time parameters for the eNMPC
%       eNMPC:      Tuning parameters for the eNMPC
%       KS:         Slave controller tuning parameters (not needed)
%       KM:         Master controller tuning parameters (not needed)
%       H:          Structure containing the different H matrices (not
%                   needed)
%       H_Ver:      Integer for the selected H matrix (not needed)
%
% The output is given as
%
%       st:         Structure containing the NLP for the integration of the
%                   economic NMPC
%       reA:        Structure containing the rearrangement values  



% Definition of the current path folder
FileName = mfilename('fullpath');
[directory,~,~] = fileparts(FileName);
[dirmain,~,~] = fileparts(directory);

% Import of CASADI
import casadi.*

% Load of the initial values and the model
load([dirmain '\Data\Init_Opt.mat'])
[~,dae,~,lb,ub,n] = AmmonReac_NewDist(1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Definition of the collocation points
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Change of the variables
st.ind_z = [1:30 61:67];
dae.ode  = substitute(dae.ode,dae.z(31:60),3*ones(30,1));
dae.ode  = substitute(dae.ode,dae.z(68:70),3*ones(n.u,1));
dae.alg  = dae.alg(st.ind_z);
dae.alg  = substitute(dae.alg,dae.z(31:60),3*ones(30,1));
dae.alg  = substitute(dae.alg,dae.z(68:70),3*ones(n.u,1));

dae.z = [dae.z(1:30); dae.z(61:63); dae.z(64); dae.z(65:67)];
n.z   = length(dae.z);
z0    = z0(st.ind_z);
lb.z  = lb.z(st.ind_z);         ub.z = ub.z(st.ind_z);

% Degree of interpolating polynomial
deg = 3;

% Get collocation points
tau_root = [0, collocation_points(deg, 'radau')];

% Coefficients of the collocation equation
C = zeros(deg+1,deg+1);

% Coefficients of the continuity equation
D = zeros(deg+1, 1);

% Coefficients of the quadrature function
B = zeros(deg+1, 1);

% Construct polynomial basis
for j=1:deg+1
    % Construct Lagrange polynomials to get the polynomial basis at the collocation point
    coeff = 1;
    for r=1:deg+1
        if r ~= j
            coeff = conv(coeff, [1, -tau_root(r)]);
            coeff = coeff/(tau_root(j)-tau_root(r));
        end
    end
    % Evaluate the polynomial at the final time to get the coefficients of the continuity equation
    D(j) = polyval(coeff, 1.0);
    
    % Evaluate the time derivative of the polynomial at all collocation points to get the coefficients of the continuity equation
    pder = polyder(coeff);
    for r=1:deg+1
        C(j,r) = polyval(pder, tau_root(r));
    end
    
    % Evaluate the integral of the polynomial to get the coefficients of the quadrature function
    pint = polyint(coeff);
    B(j) = polyval(pint, 1.0);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Definition of the DAE system
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Predefinition of the variables and the constraints
w = {};	st.lbw = [];   st.ubw = [];   w0 = [];    
g = {};	st.lbg = [];   st.ubg = [];    

% Definition of the initial conditions
x_0     = MX.sym('x_0',n.x);
z_0     = MX.sym('z_0',n.z);
w       = {w{:},x_0,z_0};
st.lbw  = [st.lbw; x0; z0];
st.ubw  = [st.ubw; x0; z0];
w0      = [w0;  x0; z0];

% Predefinition of the NLP variables
X_k = x_0;
Z_k = z_0;
X_kj = cell(t_eNMPC.N,deg);   % Due to MATLAB indexing, 0 corresponds to cell 1
Z_kj = cell(t_eNMPC.N,deg);   % Due to MATLAB indexing, 0 corresponds to cell 1
st.J = 0;
st.U_prev_k = cell(t_eNMPC.N,1);   % Due to MATLAB indexing, 0 corresponds to cell 1
st.U_prev_k{1} = MX.sym('U_prev_0',n.u);

% Definition of auxilliary variables
arrit = 1;

% Definition of the function for the DAE
fun_eNMPC = Function('fun_dae',{dae.x,dae.z,dae.p(1:n.u),dae.p(n.u+1:end)},...
                               {dae.ode,dae.alg,dae.quad},{'x','z','p','d'},{'xj','zj','qj'});

for k = 0:t_eNMPC.N-1
    
    % Definition of the previous and the current input step
    U_k = MX.sym(['U_' num2str(k)],n.u);

    % Addition of the input variable to the variabless
    w   = {w{:},U_k};
    st.lbw = [st.lbw; lb.u];
    st.ubw = [st.ubw; ub.u];
    w0  = [w0;  p0(1:n.u)];
    if k+1 ~= eNMPC.arr(arrit)
        g = {g{:}, U_k - st.U_prev_k{k+1}};
        st.lbg = [st.lbg; zeros(n.u,1)];
        st.ubg = [st.ubg; zeros(n.u,1)];
    elseif k+1 < eNMPC.arr(end)
        arrit = arrit+1;
    end
    
    % Definition of the states at each collocation point
    for j = 1:deg
        X_kj{k+1,j} = MX.sym(['x_' num2str(k) '_' num2str(j)],n.x);
        Z_kj{k+1,j} = MX.sym(['z_' num2str(k) '_' num2str(j)],n.z);
        w   = {w{:}, X_kj{k+1,j} ,Z_kj{k+1,j}};
        st.lbw = [st.lbw; lb.x; lb.z];
        st.ubw = [st.ubw; ub.x; ub.z];
        w0  = [w0;  x0;  z0];
    end
    
    
    % Definition of the loop over collocation points
    X_end_k = D(1)*X_k;
    
    for j = 1:deg
        
        % Expression for the state derivative at the collocation point        
        xp = C(1,j+1)*X_k;  % helper state
        for r = 1:deg
            xp = xp + C(r+1,j+1)*X_kj{k+1,r};
        end
        
        % Calculation of the function values
        [fj,zj,qj] =  fun_eNMPC(X_kj{k+1,j},Z_kj{k+1,j},U_k,dae.p(n.u+1:end));
        
        g = {g{:}, t_eNMPC.Varint(k+1)*fj-xp, zj};  % dynamics and algebraic constraints
        st.lbg = [st.lbg;zeros(n.x,1); zeros(n.z,1)];
        st.ubg = [st.ubg;zeros(n.x,1); zeros(n.z,1)];
        
        % Get an expression for the state at the end of the finite element
        X_end_k = X_end_k + D(j+1)*X_kj{k+1,j};
        
        % Zk =  D(j+1)*Zkj{j};  % Interpolation of z plays no role in collocation
                
        st.J = st.J + B(j+1)*qj*t_eNMPC.Varint(k+1)*eNMPC.z;
    end
        
    % Redefinition of the previous variable
    if k ~= t_eNMPC.N-1
        st.U_prev_k{k+2} = U_k;
    end
    
    st.J = st.J + (st.U_prev_k{k+1} - U_k)'*eNMPC.Du*(st.U_prev_k{k+1} - U_k);
    
    % New NLP variable for state at end of interval
    X_k = MX.sym(['x_' num2str(k+1)], n.x);
    
    w   = {w{:}, X_k};
    st.lbw = [st.lbw;  lb.x];
    st.ubw = [st.ubw;  ub.x];
    w0  = [w0;   x0];
    
    % Equality constraint at the end of the collocation sequence
    g   = {g{:}, X_end_k-X_k};
    st.lbg = [st.lbg;  zeros(n.x,1)];
    st.ubg = [st.ubg;  zeros(n.x,1)];
  
    % Inequality constraint for the input
    g   = {g{:}, sum(U_k)};
    st.lbg = [st.lbg;  0];
    st.ubg = [st.ubg;  1];
end

% Definition of the NLP problem and save of the parameters
st.dae = dae;
st.g_save = vertcat(g{:});
st.nlp = struct('x',vertcat(w{:}),'f',st.J,'g',st.g_save);
n.Dyn = length(vertcat(w{:}));

% Definition of the rows for the dimensions for the outputs (TO BE REDONE)
reA.xrow = [1         (n.x+n.z)*(deg+1)+n.u+1 ...
                    : n.x*(deg+1)+n.z*deg+n.u : n.Dyn];
reA.zrow = [n.x+1     n.x*(deg+1)+n.z*deg+n.u+1 ...
                    : n.x*(deg+1)+n.z*deg+n.u : n.Dyn];
reA.urow = [n.x+n.z+1 (n.x+n.z)*(deg+1)+n.u+n.x+1 ...
                    : n.x*(deg+1)+n.z*deg+n.u : n.Dyn];

% Definition of the outlet matrixes neglecting the values at the
% collocation points
reA.xM   = linspaceNDim(reA.xrow,reA.xrow+n.x-1,n.x);
reA.zM   = linspaceNDim(reA.zrow,reA.zrow+n.z-1,n.z);
reA.uM   = linspaceNDim(reA.urow,reA.urow+n.u-1,n.u);

% Additional points
st.l = 1;
st.it  = cell(t.N/t_eNMPC.mul,1);	st.it{1}.comb.x = w0;

