function [H, Ku1, Kut, Kx, Kv, Kr] = mpc_calculatehessian(...
    Wy, Wu, Wdu, SuJm, I2Jm, Jm, I1, Su1, Sx, Hv, nmv, ny)
% Calculate hessian and related terms for controller initialization,
% on-line tuning, and adaptive mode.

% Author:  N L Ricker, R Chen
%#codegen

% Wy/Wu/Wdu are weight matrices and their sizes indicate the following:
%   a row vector (1-by-n): uniform weight across horizon
%   a row vector (1-by-p*n): time-varying weight
%   a block-diagonal matrix (p*n-by-p*n): uniform off-diagonal weight across horizon

% Effect of change in weights
WySuJm = WtMult(Wy, SuJm, ny); 
WuI2Jm = WtMult(Wu, I2Jm, nmv);
WduJm  = WtMult(Wdu, Jm,  nmv);
% Compute cost function Hessian and linear terms
H   =  SuJm'*WySuJm + Jm'*WduJm + I2Jm'*WuI2Jm;
Ku1 =  Su1'*WySuJm + I1'*WuI2Jm;
Kut = -WuI2Jm;
Kx  =  Sx'*WySuJm;
Kv  =  Hv'*WySuJm;
Kr  = -WySuJm;

function WM = WtMult(W, M, nwt)
% Helper to compute W*M 
[wrow,wcol] = size(W);
if wrow == 1
    % Case when W is a row vector representing the diagonal of the full
    % weighting matrix.
    WM = zeros(size(M), 'like', M);
    nrow = size(WM,1);
    ONEINT16 = ones('int16');
    ixw = ONEINT16;
    for i = 1:nrow
        WM(i,:) = W(1,ixw)*M(i,:);
        ixw = ixw + ONEINT16;
        if ixw > wcol
            % This causes the last nwt values to be used repeatedly.
            ixw = int16(wcol - nwt) + ONEINT16;
        end
    end
else
    % Case when W is a full matrix.
    WM = W*M;
end

