function varargout = lscov(A,b,V,alg)
%MATLAB Code Generation Library Function

%   Limitations:
%   1. Does not support sparse inputs.
%   2. Variable-sizing is required when A is rank-deficient or V is a
%      matrix and V is rank-deficient.

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

narginchk(2,4);
nargoutchk(0,4);
ONE = coder.internal.indexInt(1);
nobs = coder.internal.indexInt(size(A,1)); % Number of observations.
nrhs = coder.internal.indexInt(size(b,2)); % num right - hand sides
% Error checking.
coder.internal.assert(size(b,1) == nobs,'MATLAB:lscov:InputSizeMismatch');
% The following guarantees that S is not computed when nrhs > 1.
coder.internal.errorIf(nargout > 3 && nrhs > 1,'MATLAB:lscov:CantReturnCov');
coder.internal.errorIf(issparse(A) || issparse(b) || (nargin > 2 && issparse(V)), ...
    'Coder:toolbox:FunctionDoesNotSupportSparse','lscov');
if nargin < 4 || (coder.internal.isConst(isempty(alg)) && isempty(alg))
    ALG = AUTO;
else
    coder.internal.assert(coder.internal.isTextRow(alg),...
        'MATLAB:lscov:InvalidAlgArg');
    len = max(ONE,coder.internal.indexInt(strlength(alg)));
    if strncmpi(alg,'orth',len)
        ALG = ORTH;
    else
        coder.internal.assert(strncmpi(alg,'chol',len),...
            'MATLAB:lscov:InvalidAlgArg');
        ALG = CHOL;
    end
end
if coder.const(nargin < 3 || ...
        (coder.internal.isConst(isempty(V)) && isempty(V)))
    % V not given,assume the identity.
    VTYPE = UNSUPPLIED;
    [varargout{1:nargout}] = lscovChol(A,b,VTYPE);
else
    ISWEIGHTVECTOR = isWeightVector(V,nobs);
    ISVMATRIX = ismatrix(V) && size(V,1) == nobs && size(V,2) == nobs;
    coder.internal.assert(ISWEIGHTVECTOR || ISVMATRIX, ...
        'MATLAB:lscov:InvalidCovMat',nobs,nobs,nobs)
    xZERO = coder.internal.scalarEg(A,b,V);
    if ISWEIGHTVECTOR
        % V given as a weight vector.
        VTYPE = VECTOR;
        [varargout{1:nargout}] = lscovChol( ...
            cast(A,'like',xZERO), ...
            cast(b,'like',xZERO), ...
            VTYPE,cast(V,'like',xZERO));
    else
        % V given as a matrix.
        % Factor V as T*T'.
        VTYPE = MATRIX;
        [T,p] = factorcov(V,ALG ~= ORTH);
        if ALG == ORTH || p > 0
            if ALG == CHOL && p > 0
                % We will be using 'orth' instead of 'chol'. Issue a warning.
                coder.internal.warning('MATLAB:lscov:RankDefCovMat');
            end
            [varargout{1:nargout}] = lscovOrth( ...
                cast(A,'like',xZERO), ...
                cast(b,'like',xZERO), ...
                cast(V,'like',xZERO), ...
                cast(T,'like',xZERO));
        else
            coder.internal.assert(p == 0,'MATLAB:lscov:InvalidCovMatPosV');
            [varargout{1:nargout}] = lscovChol( ...
                cast(A,'like',xZERO), ...
                cast(b,'like',xZERO), ...
                VTYPE,cast(V,'like',xZERO), ...
                cast(T,'like',xZERO));
        end
    end
end

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

function varargout = lscovChol(A,b,VTYPE,V,T)
coder.internal.prefer_const(VTYPE);
[nobs,nvar] = size(A);
if nargin < 4
    xZERO = coder.internal.scalarEg(A,b);
    V = zeros(0,'like',xZERO);
else
    xZERO = coder.internal.scalarEg(A,b,V);
end
xONE = xZERO + 1;
switch VTYPE
    case VECTOR
        % Weights given,scale rows of design matrix and response.
        % MATLAB uses implicit expansion here.
        %   D = sqrt(full(V(:)));
        %   A = D.*A;
        %   b = D.*b;
        V = sqrt(V);
        A = bsxfun(@times,V(:),A);
        b = bsxfun(@times,V(:),b);
    case MATRIX
        % Positive definite covariance matrix,incorporate its inverse into
        % the design matrix and response vector.
        % T is lower triangular.
        % A(:,:) = T\A;
        if coder.isRowMajor
            for colA = 1:nvar
                for j = 1:nobs
                    A(j,colA) = A(j,colA)/T(j,j);
                    for i = j+1:nobs
                        A(i,colA) = A(i,colA) - A(j,colA)*T(i,j);
                    end
                end
            end
        else
            A = coder.internal.blas.xtrsm('L','L','N','N',nobs,nvar, ...
                xONE,T,1,nobs,A,1,nobs);
        end
        % b(:,:) = T\b;
        nrhs = size(b,2);
        if coder.isRowMajor
            for colb = 1:nrhs
                for j = 1:nobs
                    b(j,colb) = b(j,colb)/T(j,j);
                    for i = j+1:nobs
                        b(i,colb) = b(i,colb) - b(j,colb)*T(i,j);
                    end
                end
            end
        else
            b = coder.internal.blas.xtrsm('L','L','N','N',nobs,nrhs, ...
                xONE,T,1,nobs,b,1,nobs);
        end
    otherwise % UNSUPPLIED
        % No covariance given,assume proportional to identity.
        % Do nothing
end
% Factor the design matrix,incorporate covariances or weights into the
% system of equations,and transform the response vector.
[Q,R,perm] = qr(A,0);
z = Q'*b;
% Use the rank-revealing QR to remove dependent columns of A.
if isempty(R)
    absR1 = ones('like',real(R));
else
    absR1 = abs(R(1));
end
tau = absR1*max(nobs,nvar)*eps(class(R));
% keepCols = (abs(diag(R)) > tau);
% rankA = sum(keepCols);
rankA = coder.internal.indexInt(0);
mn = eml_min(nobs,nvar);
keepCols = false(mn,1);
for k = 1:mn
    if abs(R(k,k)) > tau
        rankA = rankA + 1;
        keepCols(k) = true;
    end
end      
if eml_option('VariableSizing') && rankA ~= nvar
    coder.internal.warning('MATLAB:lscov:RankDefDesignMat');
    Q1 = Q(:,keepCols);
    R1 = R(keepCols,keepCols);
    z1 = z(keepCols,:);
    perm1 = perm(keepCols);
    [varargout{1:nargout}] = lscovCholCalc(A,b,Q1,R1,z1,perm1);
else
    coder.internal.assert(rankA == nvar, ...
        'Coder:toolbox:RankDefVarSizeRequired');
    % Vis-a-vis ignoring keepCols here, there is no column removal in the
    % full-rank case. Since z = Q'*b, z does not need any rows removed. We
    % don't need to remove extra rows from R because R will only be
    % referenced by LINSOLVE with the upper-triangular option, which
    % ignores any extra rows. If that were not the case, we would need to
    % use R(1:nvar,:), adding a data copy.
    [varargout{1:nargout}] = lscovCholCalc(A,b,Q,R,z,perm);
end

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

function [x,stdx,mse,S] = lscovCholCalc(A,b,Q,R,z,perm)
% Compute the LS coefficients,filling in zeros in elements corresponding
% to rows of R that were thrown out.
[nobs,nvar] = size(A);
nrhs = size(b,2); % num right - hand sides
rankA = size(R,1);
xZERO = coder.internal.scalarEg(A,b,z);
UPPERTRIANGULAR = coder.const(struct('UT',true));
xx = linsolve(R,z,UPPERTRIANGULAR); % xx = R\z;
x = zeros(nvar,nrhs,'like',xZERO);
x(perm,1:nrhs) = xx;
if nargout > 1
    % Compute the MSE, need it for the std. errs. and covs.
    dfe = nobs - rankA;
    if dfe > 0
        res = b - Q*z;
        mse = sumXTimesConjXDim1(res)/dfe;
    else % rankA == nobs, and so Ax == b exactly.
        mse = zeros(1,nrhs,'like',real(xZERO));
    end
    % Compute the covariance matrix of the LS estimates,or just their
    % SDs. Fill in zeros corresponding to exact zero coefficients.
    Rinv = linsolve(R,eye(rankA,'like',real(xZERO)),UPPERTRIANGULAR);
    if nargout > 3
        S = zeros(nvar,nvar,'like',xZERO);
        % mse is a scalar because nrhs == 1.
        S(perm,perm) = Rinv*Rinv'*mse;
        % S is guaranteed to be Hermitian
        stdx = sqrt(diag(S));
    else
        stdx = zeros(nvar,nrhs,'like',real(xZERO));
        % This is an outer product if nrhs > 1.
        stdx(perm,1:nrhs) = vecnorm(Rinv,2,2)*sqrt(mse);
    end
end

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

function [Q1,Q0,R,perm,rankA] = orthFactor(A)
[nobs,nvar] = size(A);
[Q,R1,perm1] = qr(A,'vector'); % returns the _full_ Q and R
% Use the rank - revealing QR to remove dependent columns of A.
tau = abs(R1(1))*max(nobs,nvar)*eps(class(R1));
maxRank = min(coder.internal.indexInt(nobs),coder.internal.indexInt(nvar));
rankA = countNonZeroDiagonalElements(R1,tau,nvar);
if eml_option('VariableSizing') && rankA < maxRank
    coder.internal.warning('MATLAB:lscov:RankDefDesignMat');
    R = R1(1:rankA,1:rankA);
    perm = perm1(1:rankA);
else
    coder.internal.assert(eml_option('VariableSizing') || rankA == maxRank, ...
        'Coder:toolbox:RankDefVarSizeRequired');
    R = R1(1:maxRank,1:maxRank);
    perm = perm1(1:maxRank);
end
% Separate out the columns of Q that are orthogonal to A,and get the
% orthogonal components of T and b. Q0'*T can be tall,square,or
% wide,and can be rank deficient.
[Q1,Q0] = splitSquareMatrix(Q,rankA);

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

function [x,stdx,mse,S] = lscovOrth(A,b,V,T)
% Use Paige's algorithm to avoid inverting V's Cholesky factor.
[nobs,nvar] = size(A);
nrhs = size(b,2); % num right-hand sides
xZERO = coder.internal.scalarEg(A,b,V);
rankV = size(T,2);
[Q1,Q0,R,perm,rankA] = orthFactor(A);
T0 = Q0'*T;
b0 = Q0'*b;
% Determine whether T is entirely in the column space of A. When A is
% full-rank square, it always is.
if rankA == nobs
    zeroE = true;
else
    % Otherwise,check if T0 is effectively zero (small compared to T). Q0
    % and T are factorizations of other matrices, so set tol liberally to
    % allow for the extra errors resulting from their computation.
    tau = max(nobs,rankV)*eps(class(T0));
    if norm(T0,1) < norm(T,1)*tau
        coder.internal.warning('MATLAB:lscov:TOrthogToNullSpace');
        zeroE = true;
    else
        zeroE = false;
    end
end
if zeroE
    % When T is entirely in span(A),then b = A*x + T*e can be satisfied
    % with e = 0,as long as b is in span(A) too.
    e = zeros(rankV,nrhs,'like',real(xZERO));
    if nargout > 1
        P = zeros(rankV,'like',T);
        mse = zeros(1,nrhs,'like',real(xZERO));
    end
    if rankA == nobs
        % If A is full-rank square,then b is always in span(A).
        feasible = true;
    else
        % If b0 is effectively zero,then b is in span(A).
        tau = norm(b,1)*nobs.^2*eps(class(b0));
        feasible = (norm(b0,1) < tau);
    end
else
    if nargout > 1
        % The minimum-norm solution to (Q0'*T)*e = (Q0'*b) is the error vector
        % required to satisfy b = A*x + T*e.
        [e,err,P] = lsminnorm(T0,b0);
        % Compute the MSE,need it for the std. errs. and cov matrix.
        dfe = size(P,2);
        mse = sumXTimesConjXDim1(e)/dfe;
    else
        [e,err] = lsminnorm(T0,b0);
    end
    % e should satisfy (Q0'*T)*e = (Q0'*b) exactly. Set a zero
    % tolerance dependent on e's length. e might be several vectors.
    tau = vecnorm(e,2,1)*rankV*eps(class(err));
    % feasible = all(err < tau);
    feasible = true;
    for k = 1:size(e,2)
        feasible = feasible && (err(k) < tau(k));
    end
end
% If b is not achievable with A*x + T*e, there's no solution.
if nrhs == 1
    coder.internal.assert(feasible,'MATLAB:lscov:InfeasibleRHS');
else
    coder.internal.assert(feasible,'MATLAB:lscov:InfeasibleRHScol');
end
% T*e accounts exactly for the component of b that is orthogonal
% to A (i.e.,the part that can't be accounted for by A*x).
% Subtract T*e from b; everything that is left over is in the
% column space of A.
b = b - T*e;
% Find x so that A*x exactly equals what remains in b. Fill in zeros
% in elements corresponding to rows of R that were thrown out.
z = Q1'*b;
x = zeros(nvar,nrhs,'like',xZERO);
x(perm,1:nrhs) = R\z;
if nargout > 1
    % Compute the covariance matrix of the LS estimates, or just their
    % SDs. Fill in zeros corresponding to exact zero coefficients.
    % Compute IMPPT = eye(rankV) - P*P'.
    if zeroE
        % P = 0.
        TIMPPT = cast(T,'like',xZERO);
    else
        IMPPT = -P*P';
        for k = 1:rankV
            IMPPT(k,k) = IMPPT(k,k) + 1;
        end
        TIMPPT = cast(T*IMPPT,'like',xZERO);
    end
    C = R\(Q1'*TIMPPT);
    if nargout > 3
        % mse is a scalar because nrhs == 1.
        S = zeros(nvar,nvar,'like',xZERO);
        % mse is a scalar.
        S(perm,perm) = C*C'*mse;
        % S is guaranteed to be Hermitian;
        stdx = sqrt(diag(S));
    else
        stdx = zeros(nvar,nrhs,'like',real(xZERO));
        stdx(perm,1:nrhs) = vecnorm(C,2,2)*sqrt(mse);
    end
end

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

function [T,p] = factorcov(V,useChol)
%FACTORCOV Factor a positive semidefinite symmetric covariance matrix.
%   [T,p] = FACTORCOV(V) factors the covariance matrix V so that V = T*T'.
%   If V is positive definite, T is its lower triangular Cholesky factor,
%   and P is zero. If V is positive semidefinite, T is rectangular,and P
%   is the number of zero eigenvalues. Otherwise, T is empty and P is the
%   number of negative eigenvalues.
coder.internal.prefer_const(useChol);
ZERO = coder.internal.indexInt(0);
ONE = coder.internal.indexInt(1);
n = size(V,1);
if ~ishermitian(V)
    coder.internal.error('MATLAB:lscov:InvalidCovMatSymV');
    T = coder.internal.nan(size(V),'like',V);
    p = coder.internal.indexInt(n);
    return
end
if useChol
    % Use Cholesky to factor V as T*T', with T lower triangular, if V is
    % positive definite.
    % [T,dp] = chol(V);
    [T,info] = coder.internal.lapack.xpotrf('L',n,V,ONE,n);
    if info == ZERO
        % Zero entries above the main diagonal.
        p = ZERO;
        T = tril(T);
        return
    end
end
% Either useChol is false or Cholesky failed because V is not positive
% definite. Factor V as U*D*U'.
% [U,D] = eig(V,'vector');
% Use SCHUR since we already know that V is Hermitian.
[U,D] = schur(V);
d = zeros(n,1,'like',real(V));
maxabsD = zeros('like',d);
for k = 1:n
    d(k) = real(D(k,k));
    absDk = abs(d(k));
    if absDk > maxabsD
        maxabsD = absDk;
    end
end
% Set a zero tolerance for eigenvalues,dependent on the size of the
% problem. Eigenvalues less than -tol are considered negative, those
% between -tol and tol are considered zero, those greater than tol are
% considered positive.
tol = maxabsD*n*eps(class(d));
numnegeigs = ZERO;
numposeigs = ZERO;
numzeroeigs = ZERO;
col = ZERO;
for k = 1:n
    if d(k) < -tol
        numnegeigs = numnegeigs + 1;
    elseif d(k) > tol
        numposeigs = numposeigs + 1;
        col = col + 1;
        c = sqrt(d(k));
        for i = 1:n
            U(i,col) = U(i,k)*c;
        end
    else
        numzeroeigs = numzeroeigs + 1;
    end
end
% If V is semidefinite,we can factor it in the form V == T*T' using
% the eigenvalue decomposition, but T is no longer triangular.
if eml_option('VariableSizing')
    T = U(:,1:col);
else
    T = U;
    coder.internal.assert(numposeigs == n, ...
        'Coder:toolbox:variableSizingRequiredV'); % Variable-sizing is required for rank-deficient V.
end
if numnegeigs > 0
    % If V is indefinite,that's an error.
    p = -numnegeigs;
    T = coder.internal.nan(size(V),'like',V);
else
    p = numzeroeigs;
end

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

function [e,mse,P] = lsminnorm(T0,b0)
%LSMINNORM Minimum norm least squares solution.
%   E = LSMINNORM(T0,B0) computes the minimum norm least squares solution
%   to the system T0*E = B0. If T0 has full column rank,then E is unique.
%   Otherwise,E has the minimum Euclidean norm of all solutions that
%   minimize (T0*E - B0)'*(T0*E - B0).
%
%   [E,MSE] = LSMINNORM(T0,B0) returns the mean squared error of the
%   solution,i.e.,(T0*E - B0)'*(T0*E - B0)./(M - N).
%
%   [E,MSE,P] = LSMINNORM(T0,B0) returns the matrix P from the orthogonal
%   decomposition T0 = Q*L*P',where L is lower triangular.

%   References:
%      [1] Golub,G.H. and Van Loan,C.F. (1996) Matrix Computations,
%          3rd ed.,Johns Hopkins University Press,pp. 271 - 272.
%      [2] Goodall,C.R. (1993) "Computation using the QR Decomposition",
%          in Computational Statistics,Vol. 9 of Handbook of Statistics,
%          edited by C.R. Rao,North - Holland,pp. 477 - 478.
ZERO = coder.internal.indexInt(0);
[m,n] = size(T0);
if m > n
    minmn = n;
    maxmn = m;
else
    minmn = m;
    maxmn = n;
end
kb = size(b0,2);
% Use rank - revealing QR to transform T0*e = b0 into R*e = Q'*b0,a triangular
% system,and at the same time find linear dependencies among columns of
% T0. T0 came from Q0'*T,so set tol liberally to allow for the extra
% errors involved.
[Q,R,perm] = qr(T0,0);
tol = abs(R(1))*(maxmn.^2)*eps(class(R));
% rankT0 = sum(abs(diag(R)) > tol);
rankT0 = ZERO;
for k = 1:minmn
    if abs(R(k,k)) > tol
        rankT0 = rankT0 + 1;
    end
end
if rankT0 == n || ~eml_option('VariableSizing')
    % When T0 is full rank,R is square,and R*e = Q'*b0 has a unique solution.
    coder.internal.assert(rankT0 == n,'Coder:toolbox:RankDefVarSizeRequired');
    z = Q'*b0;
    etmp = R\z;
    e = coder.nullcopy(etmp);
    e(perm,1:kb) = etmp;
    if nargout > 2
        % P = fliplr(eye(n));
        P = zeros(n,'like',T0);
        for k = 1:n
            P(n - k + 1,k) = 1;
        end
    end
else
    % The usual case is that T0 is wide. Warn if it is tall and deficient.
    if rankT0 < minmn
        coder.internal.warning('MATLAB:lscov:NullSpaceNotSpanned');
    end
    [P,S] = qr(R(1:rankT0,:)',0); % T0 = Q*L*P',where L = S'
    z = Q(:,1:rankT0)'*b0;
    etmp = P*(S'\z);
    e = coder.nullcopy(etmp);
    e(perm,1:kb) = etmp;
    if nargout > 2
        % unperm(perm) = 1:n;
        unperm = coder.nullcopy(perm);
        for k = 1:n
            unperm(perm(k)) = k;
        end
        P = P(unperm,:); % unpermute P for the outside world
    end
end
if rankT0 < m
    % Might use (b0'*b0 - z'*z) instead,but because the mse in this context
    % is normally zero,that tends to be inaccurate.
    r = T0*e - b0;
    mse = sumXTimesConjXDim1(r)/(m - double(rankT0));
else
    mse = zeros(1,kb,class(e));
end

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

function y = CHOL
coder.inline('always');
y = 'c';

function y = ORTH
coder.inline('always');
y = 'o';

function y = UNSUPPLIED
coder.inline('always');
y = 'u';

function y = AUTO
coder.inline('always');
y = 'a';

function y = VECTOR
coder.inline('always');
y = 'v';

function y = MATRIX
coder.inline('always');
y = 'm';

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

function p = isWeightVector(V,nobs)
coder.internal.prefer_const(nobs);
p = false;
if isreal(V) && isvector(V)
    lenv = coder.internal.indexInt(length(V));
    if nobs == lenv
        p = true;
        for k = 1:lenv
            p = p && (V(k) >= 0);
        end
    end
end

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

function s = countNonZeroDiagonalElements(R,tau,nrows)
coder.internal.prefer_const(nrows);
m = min(coder.internal.indexInt(nrows),coder.internal.indexInt(size(R,1)));
n = coder.internal.indexInt(size(R,2));
minmn = min(m,n);
s = coder.internal.indexInt(0);
for k = 1:minmn
    if abs(R(k,k)) > tau
        s = s + 1;
    end
end

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

function y = sumXTimesConjXDim1(x)
y = vecnorm(x,2,1).^2;

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

function [Q1,Q2] = splitSquareMatrix(Q,lastColQ1)
% Q1 = Q(:,1:lastColQ1), Q2 = Q(:,lastColQ1+1:end)
m = coder.internal.indexInt(size(Q,1));
k = coder.internal.indexInt(lastColQ1);
if k < 0
    Q1 = zeros(m,0,'like',Q);
    Q2 = Q;
elseif k > m
    Q1 = Q;
    Q2 = zeros(m,0,'like',Q);
else
    Q1 = coder.nullcopy(zeros(m,k,'like',Q));
    for j = 1:k
        for i = 1:m
            Q1(i,j) = Q(i,j);
        end
    end
    nQ0 = m - k;
    Q2 = coder.nullcopy(zeros(m,nQ0,'like',Q));
    for j = 1:nQ0
        for i = 1:m
            Q2(i,j) = Q(i,j + k);
        end
    end
end

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