function [L, BadH] = mpc_checkhessian(H)
% [L, BadH] = CheckHessian(H) (no hard error from this method)
% 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.
%
% If (original H is positive definite) && (min(diag(L)) > smallhessian)
%   return BadH = 0
% Else
%   If norm(H, inf) >= largehessian
%     return BadH = 2
%   Else
%     attempt to make H non-singular by augmenting diagonal terms
%     If successful
%       return BadH = 1;
%     Else
%       return BadH = 3;
%     End
%   End
% End
%
% compute Linv and Hinv as follows:
%   Linv = linsolve(L,eye(degrees),struct('LT',true));
%   Hinv = Linv'*Linv;

%   Author:  N L Ricker
%   Copyright 1986-2014 The MathWorks, Inc.

%#codegen

%% initialization
ONE = ones('like', H);
ZERO = zeros('like', H);
degrees = size(H,1);
smallhessian = ONE*10*sqrt(eps);
largehessian = ONE*1e10;
BadH = ZERO;
%% compute original L
if isempty(coder.target)
    Successful = ZERO;
    [L, info] = chol(H, 'lower');
else
    Successful = zeros(coder.internal.indexIntClass);
    emlN = cast(size(H,2),coder.internal.indexIntClass);
    emlONE = ones(coder.internal.indexIntClass);
    info = zeros('like',coder.internal.lapack.info_t);
    [L, info] = coder.internal.lapack.xpotrf('L',emlN,H,emlONE,emlN);
end
if info==Successful && min(diag(L)) > smallhessian
    % chol successful and L is good (it is possible that H is positive
    % definite but calculation of Linv by linsolve would still throw a
    % warning. Check for likelihood of this and augment H if necessary.
    return
else
    normH = norm(H, Inf);
    if normH >= largehessian
        BadH = ONE*2;
    else
        Tries = ZERO;
        while Tries <= 4
            H = H + power(10*ONE,Tries)*smallhessian*eye(degrees,'like',H);
            if isempty(coder.target)
                [L, info] = chol(H, 'lower');
            else
                [L, info] = coder.internal.lapack.xpotrf('L',emlN,H,emlONE,emlN);
            end
            if info==Successful && min(diag(L)) > smallhessian
                BadH = ONE;
                return
            else
                BadH = ONE*3;
                Tries = Tries + ONE;
            end
        end
    end
end

