function [H,Hinv,Linv,Ac,Kx,Kr,Ku1,Kv,Kut,Mlim,Mx,Mu1,Mv,BadH,Wy,Wdu,Jm,SuJm,Su1,Sx,Hv,Wu,I1,Mrows,Wecr,I2Jm,Mdis,Ndis,Vdis,numdis,maxdis] ...
    = mpc_buildmat(model,mvindex,mdindex,uwt,duwt,ywt,umin,umax,dumin,dumax,ymin,ymax,uoff,voff,yoff,...
    Vumin,Vumax,Vdumin,Vdumax,Vymin,Vymax,p,m,isQP,nx,ny,nmv,nv,Wecr,mixedconstraints,dset,dvalues,isHyb)
%MPC_BUILDMAT  Builds optimization matrices (constrained) / gain controllers (unconstrained)
%
%   Constrained: degrees = TotalFreeMoves + 1, Unconstrained: degrees = TotalFreeMoves
%
%   H       degrees-by-degrees
%   Hinv    degrees-by-degrees
%   Linv    degrees-by-degrees
%
%   Kx      nxQP-by-TotalFreeMoves
%   Kr      ny*p-by-TotalFreeMoves
%   Ku1     nmv-by-TotalFreeMoves
%   Kv      (nmd+1)*(p+1)-by-TotalFreeMoves
%   Kut     nmv*p-by-TotalFreeMoves
%
%   Ac      nCons-by-degrees / []
%
%   Mlim    nCons-by-1 / []
%   Mx      nCons-by-nxQP / []
%   Mu1     nCons-by-nmv / []
%   Mv      nCons-by-(nmd+1)*(p+1) / []
%
%   Jm      nmv*p-by-TotalFreeMoves
%   SuJm    nCons-by-nxQP
%
%   Mdis    TotalFreeMoves-by-TotalFreeMoves (if there are discrete variables)
%           or empty otherwise

%   Author: A. Bemporad
%   Copyright 1986-2020 The MathWorks, Inc.

verbose = mpccheckverbose;

% Build Jm (blocking moves matrix) and DUFree (free input moves)
Inu=eye(nmv);
nb=size(m,2); % Row vector
if nb==1
    moves=[ones(1,m-1) p-m+1];
else
    moves=m; % sum(m)=P has already been checked in chkm.m
end
nmoves=length(moves);
TotalFreeMoves=nmoves*nmv;

Jm=zeros(nmv*p,TotalFreeMoves); % The number of columns of Jm equals
% the number of optimization vars

%DUFree=zeros(TotalFreeMoves,1); % DUfree(i)=1 iff DU(i) is free
% length(find(index)) = number of optimization vars
%
j=0;
for i=1:nmoves
    ni=moves(i);
    %Jm(j*nu+1:(j+ni)*nu,1+(i-1)*nu:i*nu)=kron([1;zeros(ni-1,1)],Inu);
    %DUFree(j*nu+1:(j+ni)*nu)=kron([1;zeros(ni-1,1)],ones(nu,1));
    
    % ALTERNATIVE way of computing Jm
    Jm(j*nmv+1:(j+1)*nmv,1+(i-1)*nmv:i*nmv)=Inu;
    j=j+ni;
end
% ALTERNATIVE way of computing DUFree
%DUFree=sum(Jm,2);

% u = I2*du+I1*u(-1)
I1=kron(ones(p,1),Inu);

% Build Wu
if ~isa(uwt,'cell')
    if isrow(uwt)
        % uniform weights (row vector of nmv)
        Wu = (uwt(:).^2)';
    else
        % time-varying weights (row vector of p*nmv)
        uwtc=uwt';uwtc=uwtc(:);
        Wu=(uwtc.^2)';
    end
else
    % off-diagonal
    Wu=kron(eye(p),uwt{1});
end

% Build Wdu.
if ~isa(duwt,'cell')
    if isrow(duwt)
        % uniform weights (row vector of nmv)
        Wdu = (duwt(:).^2)';
    else
        % time-varying weights (row vector of p*nmv)
        duwtc=duwt';duwtc=duwtc(:);
        Wdu=(duwtc.^2)';
    end
else
    % off-diagonal
    Wdu=kron(eye(p),duwt{1});
end

% Build Wy
if ~isa(ywt,'cell')
    if isrow(ywt)
        % uniform weights (row vector of ny)
        Wy = (ywt(:).^2)';
    else
        % time-varying weights (row vector of p*ny)
        ywtc=ywt';ywtc=ywtc(:);
        Wy=(ywtc.^2)';
    end
else
    % off-diagonal
    Wy=kron(eye(p),ywt{1});
end

% Calculate prediction model coefficients and coefficients of custom
% constraints, if any.
if isempty(mixedconstraints)
    E = [];
    F = [];
    S = [];
    G = [];
    nCC = 0;
else
    E = mixedconstraints.E;
    F = mixedconstraints.F;
    S = mixedconstraints.S;
    G = mixedconstraints.G;
    nCC = size(E,1);
end
[SuJm, Sx, Su1, Hv, I2Jm] = mpc_constraintcoef(model.a, model.b(:,mvindex), model.b(:,mdindex), model.c, model.d(:,mdindex), Jm);
[MuCC, MvCC, Mu1CC, MxCC, MlimCC] = mpc_customconstraintcoef(SuJm, Sx, Su1, Hv, model.c, model.d(:,mdindex), Jm, nCC, E, F, S, G, uoff, voff, yoff);
% Cost function:
%
% J = (u-ut)'*Wu*(u-ut) + du'*Wdu*du + (y-r)'Wy*(y-r)
%
%   = z'*(I2'*Jm'*Wu*I2*Jm + Jm'*Wdu*Jm+Jm'*Su'*Wy*Su*Jm)*z            [quadratic term] 
%
%   + 2*(-ut'*Wu*I2*Jm + u(-1)'*(I1'*Wu*I2*Jm+Su1*Wy*Su*Jm) ...
%        + x(0)'*Sx'*Wy*Su*Jm + v'*Hv*Wy*Su*Jm -r'*Wy*Su*Jm)*z         [linear term]
%   
%   + (I1*u(-1)-ut)'*Wu*(I1*u(-1)-ut) + (Sx*x(0) + Su1*u(-1) + Hv*v - r)'*Wy ...
%      *(Sx*x(0) + Su1*u(-1) + Hv*v - r)                               [constant term]

% Build H,Ku1,Kx,Kv,Kd,Kut

[H, Ku1, Kut, Kx, Kv, Kr] = mpc_calculatehessian(...
    Wy, Wu, Wdu, SuJm, I2Jm, Jm, I1, Su1, Sx, Hv, nmv, ny);

if isHyb
    [Mdis,Ndis,Vdis,numdis,maxdis]=mpc_discreteconstraints(dset,dvalues,I2Jm,moves);
else
    Mdis=0; % Simulink does not accept empty params
    Ndis=int16(0);
    Vdis=0;
    numdis=int16(0);
    maxdis=int16(0);
end

if isHyb
    % Compute coordinate transformation matrix
    Mdisinv=linsolve(Mdis,eye(TotalFreeMoves),struct('LT',true)); % Mdis is unit lower triangular
end

if ~isQP
    %%% UNCONSTRAINED CASE.  
    
    if isHyb
        % Apply coordinate transformation
        H=Mdisinv'*H*Mdisinv;
    end
    [Linv, Hinv, BadH] = localInvertHessian(H);    

    % define dummy QP matrices
    Ac=[];
    Mlim=[];
    Mx=[];
    Mu1=[];
    Mv=[];
    Mrows = zeros(0,1);
else
    %%% CONSTRAINED CASE.  
    % Nomenclature:
    %
    % Ac*z + Mx*x + Mu1*u1 + Mv*v + Mlim >= 0
    %
    % z     QP decision variables (including slack)
    % x     Plant & disturbance states
    % u1    Last move state
    % v     Current and projected MDs
    % Ac, Mx, Mu1, Mv, Mlim arrays defined below.

    % Build Mu, Mx, Mu1, Mv, matrices
    I3  = eye(TotalFreeMoves);  % NOTE: for free du variables only
    Mu  = [SuJm; -SuJm; I2Jm; -I2Jm; I3; -I3];
    aux = 2*(nmv*p + TotalFreeMoves);
    Mx  = [-Sx; Sx; zeros(aux,nx)];
    Mu1 = [-Su1; Su1; -I1; I1; zeros(2*TotalFreeMoves,nmv)];
    Mv  = [-Hv; Hv; zeros(aux,(p+1)*nv)];
    % Mrows initially contains the indices of all possible bound contraints.
    Mrows =  (1:size(Mu,1))';

    % Build Mlim matrix

    % Delete from dumin,dumax entries relative to blocked moves, because they
    % are constraint of the form dumin <=0 <=dumax

    index=zeros(TotalFreeMoves,1); % index(i)=1 iff DU(i) free
    % length(index) = number of optimization vars

    j=0;
    for i=1:nmoves
        ni=moves(i);
        index(j*nmv+1:(j+ni)*nmv)=[ones(nmv,1);zeros((ni-1)*nmv,1)];
        j=j+ni;
    end

    index2=find(~index); % index2 contains indices of DUs that are 0

    % Transpose to put matrices in a single column below

    dumin=dumin';dumin=dumin(:);
    dumax=dumax';dumax=dumax(:);
    Vdumin=Vdumin';Vdumin=Vdumin(:);
    Vdumax=Vdumax';Vdumax=Vdumax(:);

    % Check if blocked moves DU=0 are infeasible
    if any(dumin(index2,:)>0)
        if verbose
            fprintf('-->%s',ctrlMsgUtils.message('MPC:computation:PosLowerBound'));
        end
    end
    if any(dumax(index2,:)<0)
        if verbose
            fprintf('-->%s',ctrlMsgUtils.message('MPC:computation:NegUpperBound'));
        end
    end
    % Delete the redundant constraints
    dumin(index2,:)=[];
    dumax(index2,:)=[];
    Vdumin(index2,:)=[];
    Vdumax(index2,:)=[];

    % now umin=[umin_1(0), umin_2(0), ..., umin_nu(0);
    %          [umin_1(1), umin_2(1), ..., umin_nu(1);
    %          [...         ...             ...
    %          [umin_1(p-1), umin_2(p-1), ..., umin_nu(p-1);
    % Remove constraints on U where DU=0, but take the
    % smallest UMIN (or UMAX) and the smallest VUmin (or VUmax) for the remaining constraint
    % over the free input move u

    k=0;
    for i=1:nmoves
        blocked=((k+1):k+moves(i))'; % indices of frozen moves for the i-th degree of freedom,
        % including the first free move and the remaining nonfree moves

        umin2=max(umin(blocked,:),[],1); % most stringent lower constraint (umin2=row vector)
        umax2=min(umax(blocked,:),[],1); % most stringent upper constraint (umax2=row vector)

        umin(k+1,:)=umin2; % tighten the constraint on the free move
        umax(k+1,:)=umax2; % tighten the constraint on the free move

        Vumin2=min(Vumin(blocked,:),[],1); % most stringent violation of lower constraint (umin2=row vector)
        Vumax2=min(Vumax(blocked,:),[],1); % most stringent violation of upper constraint (umax2=row vector)

        Vumin(k+1,:)=Vumin2; % tighten the violation multiplier on the free move
        Vumax(k+1,:)=Vumax2; % tighten the violation multiplier on the free move

        k=k+moves(i); % update for next step
    end


    % Now take out from umin,umax entries relative to nonfree blocked moves

    umin=umin';
    umin=umin(:);
    umax=umax';
    umax=umax(:);
    Vumin=Vumin';
    Vumin=Vumin(:);
    Vumax=Vumax';
    Vumax=Vumax(:);

    umin(index2,:)=[];
    umax(index2,:)=[];
    Vumin(index2,:)=[];
    Vumax(index2,:)=[];

    % Remove corresponding constraints from Mu,Mx,Mu1,Mv
    iU = 2*p*ny + [index2; index2 + p*nmv];
    Mu(iU,:)  = [];
    Mx(iU,:)  = [];
    Mu1(iU,:) = [];
    Mv(iU,:)  = [];
    % and delete corresponding Mrows indices
    Mrows(iU) = [];

    % Construct Mlim to contain bounds of the remaining constraints.
    ymin=ymin';
    ymax=ymax';
    Mlim=[ymax(:);-ymin(:);umax(:);-umin(:);dumax;-dumin];
    % Remove constraints that have Mlim(i)=Inf or = -Inf
    isinf = find(~isfinite(Mlim));
    Mlim(isinf,:) = [];
    Mu(isinf,:)   = [];
    Mx(isinf,:)   = [];
    Mu1(isinf,:)  = [];
    if ~isempty(Mv)
        Mv(isinf,:) = [];
    else
        Mv = zeros(size(Mlim));
    end

    % Also remove all infinite constraints from Mrows.
    Mrows(isinf) = [];

    % Define the matrices needed for the QP solver
    % For efficiency, only add slack var if there are output constraints (any ymin/ymax is finite)
    % or mixed I/O constraints

    % Add slack variable for soft output constraints
    % slack var should only be added in case there are output constraints,
    % mixed constraints, or customcost/customconstraints)
    Vymax=Vymax';
    Vymin=Vymin';
    Me=-[Vymax(:);Vymin(:);Vumax(:);Vumin(:);Vdumax(:);Vdumin(:)];
    Me(isinf,:)=[];
    nCC = size(G,1);
    if ~isempty(mixedconstraints)
        % Add mixed I/O constraint terms to the constraint matrices
        Mu   = [Mu; MuCC];
        Mv   = [Mv; MvCC];
        Mx   = [Mx; MxCC];
        Mu1  = [Mu1; Mu1CC];
        Mlim = [Mlim; MlimCC];
        Me   = [Me; -reshape(mixedconstraints.V(:,ones(1,p+1)), nCC*(p+1), 1)];
    end
    Mu = [Mu, Me];
    H = [H, zeros(TotalFreeMoves,1); zeros(1,TotalFreeMoves), Wecr];

    % Note:  custom constraints always retained in the QP so they are not
    % included in Mrows.

    Ac=-Mu;

    if isHyb
        % Apply coordinate transformation
        H(1:TotalFreeMoves,1:TotalFreeMoves)=Mdisinv'*H(1:TotalFreeMoves,1:TotalFreeMoves)*Mdisinv;
        Ac(:,1:TotalFreeMoves)=Ac(:,1:TotalFreeMoves)*Mdisinv;
    end
    
    % Check positive definiteness of H
    [Linv, Hinv, BadH] = localInvertHessian(H);
end

function [Linv, Hinv, BadH] = localInvertHessian(H)
% Invert hessian.  If not positive definite, make it so and set BadH flag
% BadH = 0  No problem detected
% BadH = 1  Initial chol(H) fails or H is too small. Fixed by augmenting diagonal terms.
% BadH = 2  chol(H) fails and norm(H) is very large. Plant model probably unstable.
% BadH = 3  chol(H) fails for any other reason. Usually means that H is singular or not positive definite.
[L, BadH] = mpc_checkhessian(H);
if BadH == 1
    ctrlMsgUtils.warning('MPC:computation:HessianSingular');
elseif BadH == 2
    normH = norm(H, Inf);
    ctrlMsgUtils.error('MPC:computation:HessianLarge',num2str(normH, '%10.2e'));
elseif BadH == 3
    ctrlMsgUtils.error('MPC:computation:HessianSingular');
end
Linv = linsolve(L,eye(size(H,1)),struct('LT',true));
Hinv = Linv'*Linv;
