function [SuJm, Sx, Su1, Hv, I2Jmp] = mpc_constraintcoefLTV(A, Bu, Bv, C, Dv, Jm)
% MPC_CONSTRAINTCOEF compute output prediction model coefficients and
% constraint coefficients that depend on the prediction model. 
%
%   Inputs:
%       A, Bu, Bv, C, Dv: prediction model parameters, 3-D signals
%       Jm: constant matrix

% Author: A. Bemporad, N.L. Ricker, R. Chen
% Copyright 1986-2016 The MathWorks, Inc.

%#codegen

% Notation:
%   Prediction model:
%       y = Su*du + Hv*v + Sx*x0 + Su1*u1
%   where:
%       y     p*ny, future outputs at intervals k+1:k+p
%       du    p*nu, MV increments, k:k+p-1
%       v     (p+1)*nv, current and future MD values, k:k+p
%       x0    nx, current states
%       u1    nmv, MV values used over the previous sampling instant.
%       Su, Hv, Sx, Su1 are constant model coefficients.
%
%   Relationship between MV increments and optimization decisions:
%       du = Jm*z
%   where:
%       z     blocked MV increments (actual decision variables for QP)
%       Jm    constant matrix mapping dum to du.
%
%   Relationship between MV increments and MVs:
%       u = I2*du + I1*u1 = I2*Jm*z + I1*u1
%   where:
%       u     p*nu MV values, k:k+p-1
%       I1, I2 are constant matrices
%
%   Ultimately, we will solve (at each control interval)
%       Min f'*z + 0.5*z'*H*z
%           s.t. Ac*z >= b
%   where:
%       z1 = [z; e]  includes the slack variable (for constraint softening)
%       Vectors f and b depend on the controller and plant state at each interval (i.e., v, x0, u1)
%       Matrices H and Ac depend on the prediction model and are constant unless the model changes
%
%   Standard bound constraints:
%       -y + Vymax*e  >= -ymax
%        y + Vymin*e  >=  ymin
%       -u + Vumax*e  >= -umax
%        u + Vumin*e  >=  umin
%      -du + Vdumax*e >= -dumax
%       du + Vdumin*e >=  dumin
%
% Some or all constraints may be irrelevant, e.g., redundant or with bounds
% at -Inf.  A key assumption is that the QP constraints retain their
% identity but their coefficients may change if the prediction model is
% updated.

% Prediction model coefficients Su*Jm, Su1, Sx, Hv.  Special handling for
% the case in which v is constant over the prediction horizon.
ny = size(C,1);     % number of OV
nx = size(C,2);     % number of augmented states (nxp+nxdist+nxnoise)
nu = size(Bu,2);    % number of MV
nv = size(Bv,2);    % number of MD + 1 (last column is for DX offset)
p = size(Jm,1)/nu;  % prediction horizon
pny1 = (p-1)*ny;
pnu = p*nu;
% Initialization and pre-allocation
Hv = [C(:,:,2)*Bv(:,:,1), Dv(:,:,2), zeros(ny,(p-1)*nv,'like',A); zeros(pny1,(p+1)*nv,'like',A)];
Sx = [C(:,:,2)*A(:,:,1); zeros(pny1,nx,'like',A)];
Su1 = [C(:,:,2)*Bu(:,:,1); zeros(pny1,nu,'like',A)];
Su = [Su1(1:ny,1:nu), zeros(ny,(p-1)*nu,'like',A); zeros(pny1,pnu,'like',A)];
% AA stores products of matrices A(i). At a generic prediction step k>=1
% AA=[A(k)*A(k-1)*...*A(1), A(k)*A(k-1)*...*A(2),...,A(k)*A(k-1), A(k),0,...,0]
if p==1
    AA = A(:,:,2);
else
    AA = [A(:,:,2) zeros(nx,(p-2)*nx,'like',A)];
end
CA = zeros(1,1,'like',A); % dummy value
Sum = zeros(1,1,'like',A); % dummy value
% Loop through horizon
for i=2:p
    rows = (i-1)*ny+(1:ny);
    Su1(rows,:)=C(:,:,i+1)*Bu(:,:,i);
    Su(rows,(i-1)*nu+(1:nu))=Su1(rows,:);
    for j=i-1:-1:1
        Su1(rows,:)=Su1(rows,:)+C(:,:,i+1)*AA(:,(j-1)*nx+(1:nx))*Bu(:,:,j);
        Su(rows,(j-1)*nu+(1:nu))=Su1(rows,:);
    end
    Hv(rows,i*nv+(1:nv))=Dv(:,:,i+1);
    Hv(rows,(i-1)*nv+(1:nv))=C(:,:,i+1)*Bv(:,:,i);
    for j=1:i-1
        Hv(rows,(j-1)*nv+(1:nv))=C(:,:,i+1)*AA(:,(j-1)*nx+(1:nx))*Bv(:,:,j);
    end
    Sx(rows,:)=C(:,:,i+1)*AA(:,1:nx)*A(:,:,1);
    if i<p
        % Update AA
        %AA(:,1:(i-1)*nx)=A(:,:,i+1)*AA(:,1:(i-1)*nx);
        for ct=1:i-1
            AA(:,(ct-1)*nx+(1:nx)) = A(:,:,i+1) * AA(:,(ct-1)*nx+(1:nx));
        end
        AA(:,(i-1)*nx+(1:nx))=A(:,:,i+1);
    end
end
SuJm = Su*Jm;
I2Jmp = kron(tril(ones(p,'like',A)),eye(nu,'like',A))*Jm;
