function [x,resnorm,resid,exitflag,output,lambda] = lsqnonneg(C,d,options,varargin)
%MATLAB Code Generation Library Function

%   Limitations:
%   1. Requires variable sizing support.
%   2. Does not support X0 input at all. The X0 input is ignored in MATLAB
%      except that there is a warning notifying the user that it is
%      ignored. Since MATLAB no longer supports the X0 input, this
%      codegen limitation does not need to be documented in any way.

%   Copyright 1984-2016 The MathWorks, Inc.
%#codegen

coder.inline('always');
coder.extrinsic('getString','message');
narginchk(1,4);
nargoutchk(0,6);
if nargin == 1
    if nargout <= 1 && strcmpi(C,'defaults')
        % If just 'defaults' passed in, return the default options in X
        x = defaultopt;
        return
    end
    eml_invariant(isstruct(C) && ...
        isfield(C,'C') && isfield(C,'d') && isfield(C,'options'), ...
        'MATLAB:optimfun:lsqnonneg:InputArg');
    [x,resnorm,resid,exitflag_p,lambda,outeriter] = lsqnonneg_local(C.C,C.d,C.options);
    printtype = optimget(C.options,'Display',defaultopt,'fast');
else
    if nargin < 3
        % options = optimset('lsqnonneg');
        options = defaultopt;
    end
    [x,resnorm,resid,exitflag_p,lambda,outeriter] = lsqnonneg_local(C,d,options,varargin{:});
    printtype = optimget(options,'Display',defaultopt,'fast');
end
exitflag = double(exitflag_p);
VERBOSE = verbose(printtype);
if nargout >= 5 || VERBOSE
    % Because Coder doesn't support unicode strings, it turns out that
    % using the translated messages from the message catalog can lead to an
    % error. At the present time, we must hard-code the English strings.
    %
    % it_count_exceeded_msg = coder.const( ...
    %     getString(message('MATLAB:optimfun:lsqnonneg:IterationCountExceeded')));
    it_count_exceeded_msg = ['Exiting: Iteration count is exceeded, exiting LSQNONNEG.' ...
        char(10) 'Try raising the tolerance (OPTIONS.TolX).']; %#ok<CHARTEN>
    % terminated_msg = coder.const( ...
    %     getString(message('MATLAB:optimfun:lsqnonneg:OptimizationTerminated')));
    terminated_msg = 'Optimization terminated.';
    if VERBOSE
        if exitflag_p
            fprintf(1,'%s\n',terminated_msg);
        else
            fprintf(1,'%s\n',it_count_exceeded_msg);
        end
    end
    if nargout >= 5
        if exitflag_p
            msg = terminated_msg;
        else
            msg = it_count_exceeded_msg;
        end
        output = struct( ...
            'iterations',double(outeriter), ...
            'algorithm','active-set', ...
            'message',msg);
    end
end

%--------------------------------------------------------------------------

function v = verbose(printtype)
coder.internal.prefer_const(printtype);
v = ~( ...
    strcmp(printtype,'none') || ...
    strcmp(printtype,'off') || ...
    strcmp(printtype,'notify') || ...
    strcmp(printtype,'notify-detailed'));
eml_invariant(~v || ...
    strcmp(printtype,'final') || ...
    strcmp(printtype,'final-detailed') || ...
    strcmp(printtype,'iter') || ...
    strcmp(printtype,'iter-detailed'), ...
    'MATLAB:optimfun:lsqnonneg:InvalidOptParamDisplay');

%--------------------------------------------------------------------------

function [x,resnorm,resid,exitflag,lambda,outeriter] = ...
    lsqnonneg_local(C,d,options,varargin)
narginchk(3,inf);
eml_invariant(isreal(C) && isreal(d), ...
    'MATLAB:optimfun:lsqnonneg:ComplexCorD');
eml_invariant(isa(C,'double') && isa(d,'double'), ...
    'MATLAB:optimfun:lsqnonneg:NonDoubleInput');
tol = 10*eps*norm(C,1)*length(C);
toltmp = optimget(options,'TolX',defaultopt,'fast');
if ~strcmpi(toltmp,'10*eps*norm(c,1)*length(c)')
    eml_invariant(isscalar(toltmp) && isfloat(toltmp), ...
        'MATLAB:optimfun:lsqnonneg:OptTolXNotPosScalar');
    tol(1) = toltmp;
end
n = coder.internal.indexInt(size(C,2));
% Initialize vector of n zeros and Infs (to be used later)
nZeros = zeros(n,1);
% Initialize set of non-active columns to null
NAC = false(n,1);
% Initialize set of active columns to all and the initial point to zeros
x = nZeros;
w = nZeros;
z = nZeros;
[w,resid] = UpdateWAndResid(w,C,x,d);
% Set up iteration criterion
IZERO = coder.internal.indexInt(0);
outeriter = IZERO;
iter = IZERO;
itmax = 3*n;
exitflag = true;
% The following two variables are always defined before they are used,
% anyway, but the compiler may not realize that this is true.
inew = IZERO;
maxz = 0;
% Outer loop to put variables into set to hold positive coefficients
while AnyActiveAndGTTol(NAC,w,tol)
    outeriter = outeriter + 1;
    % Find variable with largest Lagrange multiplier
    k = coder.internal.indexInt(1);
    % Find the first basis column.
    while k <= n
        if ~NAC(k)
            inew = k;
            maxz = w(k);
            break
        end
        k = k + 1;
    end
    % Now find basis column with the largest value of w.
    while k <= n
        if ~NAC(k) && w(k) > maxz
            inew = k;
            maxz = w(k);
        end
        k = k + 1;
    end
    % Move variable t from zero set to positive set
    NAC(inew) = true;
    % Compute intermediate solution using only variables in positive set
    % z(NAC) = C(:,NAC)\d;
    z = LSQNACd(C,NAC,d,z);
    % inner loop to remove elements from the positive set which no longer belong
    while AnyzNACle0(z,NAC)
        iter = iter + 1;
        if iter > itmax
            exitflag = false;
            resnorm = dot(resid,resid);
            x = z;
            lambda = w;
            return
        end
        alpha = ComputeAlpha(z,NAC,x);
        % Find indices where intermediate solution z is approximately negative
        x = x + alpha*(z - x);
        % Reset NAC given intermediate values of x
        NAC = UpdateNAC(NAC,x,tol);
        z = LSQNACd(C,NAC,d,z); % Re-solve for z
    end
    x = z;
    [w,resid] = UpdateWAndResid(w,C,x,d);
end
lambda = w;
resnorm = dot(resid,resid);

%--------------------------------------------------------------------------

function NAC = UpdateNAC(NAC,x,tol)
% Update Non-Active-Column indicator vector.
coder.inline('always');
for k = 1:numel(NAC)
    NAC(k) = NAC(k) && ~(abs(x(k)) < tol);
end

%--------------------------------------------------------------------------

function p = AnyzNACle0(z,NAC)
% True iff z for any non-active column is non-positive.
% p = any(z(NAC) <= 0);
coder.inline('always');
p = false;
for k = 1:numel(z)
    if NAC(k) && (z(k) <= 0)
        p = true;
        break
    end
end

%--------------------------------------------------------------------------

function z = LSQNACd(C,NAC,d,z)
% Least-squares using only the Non-Active-Columns of C.
% z(NAC) = C(:,NAC)\d;
% The values in z are ignored on input. The input is to ensure reuse of the
% storage for z.
IZERO = coder.internal.indexInt(0);
m = coder.internal.indexInt(size(C,1));
n = coder.internal.indexInt(size(C,2));
nNAC = count(NAC);
assert(nNAC <= n); %<HINT>
Ctmp = coder.nullcopy(zeros(m,nNAC,'like',C));
col = IZERO;
for j = 1:n
    if NAC(j)
        col = col + 1;
        for i = 1:m
            Ctmp(i,col) = C(i,j);
        end
    end
end
ztmp = mldivide(Ctmp,d);
iztmp = IZERO;
for j = 1:n
    if NAC(j)
        iztmp = iztmp + 1;
        z(j) = ztmp(iztmp);
    else
        z(j) = 0;
    end
end

%--------------------------------------------------------------------------

function n = count(p)
% n = sum(p), where p is logical.
coder.inline('always');
n = coder.internal.indexInt(0);
for k = 1:numel(p)
    n = n + coder.internal.indexInt(p(k));
end

%--------------------------------------------------------------------------

function p = AnyActiveAndGTTol(NAC,w,tol)
% p = any(~NAC) && any(w(~NAC) > tol)
coder.inline('always');
p = false;
for k = 1:numel(NAC)
    if ~NAC(k) && w(k) > tol
        p = true;
        return
    end
end

%--------------------------------------------------------------------------

function [w,resid] = UpdateWAndResid(w,C,x,d)
% resid = d - C*x;
resid = coder.internal.blas.xgemv('N',size(C,1),size(C,2),-1,C,1,size(C,1),x,1,1,1,d,1,1);
% w = C'*resid;
w = coder.internal.blas.xgemv('T',size(C,1),size(C,2),1,C,1,size(C,1),resid,1,1,0,w,1,1);

%--------------------------------------------------------------------------

function alpha = ComputeAlpha(z,NAC,x)
% Find indices where intermediate solution z is approximately negative
% Q = (z <= 0) & NAC;
% Choose new x subject to keeping new x nonnegative
% alpha = min(x(Q)./(x(Q) - z(Q)));
coder.inline('always');
alpha = coder.internal.inf;
for k = 1:numel(z)
    if NAC(k) && z(k) <= 0
        tmp = x(k)/(x(k) - z(k));
        if tmp < alpha
            alpha = tmp;
        end
    end
end

%--------------------------------------------------------------------------

function options = defaultopt
options = struct('Display','notify','TolX','10*eps*norm(C,1)*length(C)');

%--------------------------------------------------------------------------
