function [R, P, I] = cholp(A, piv)
%CHOLP  Cholesky factorization with pivoting of a pos. semidefinite matrix.
%       [R, P] = CHOLP(A) returns R and a permutation matrix P such that
%       R'*R = P'*A*P.  Only the upper triangular part of A is used.
%       If A is not positive definite, an error message is printed.
%
%       [R, P, I] = CHOLP(A) never produces an error message.
%       If A is positive definite then I = 0 and R is the Cholesky factor.
%       If A is not positive definite then I is positive and
%       R is (I-1)-by-N with P'*A*P - R'*R zeros in columns 1:I-1 and
%       rows 1:I-1.
%       [R, I] = CHOLP(A, 0) forces P = EYE(SIZE(A)), and therefore behaves
%       like [R, I] = CHOL(A).

%       This routine is based on the LINPACK routine CCHDC.  It works
%       for both real and complex matrices.
%
%       Reference:
%       G.H. Golub and C.F. Van Loan, Matrix Computations, Second
%       Edition, Johns Hopkins University Press, Baltimore, Maryland,
%       1989, sec. 4.2.9.

if nargin == 1, piv = 1; end

n = length(A);
pp = 1:n;
I = 0;

for k = 1:n

    if piv
       d = diag(A);
       [big, m] = max( d(k:n) );
       m = m+k-1;
    else
       big = A(k,k);  m = k;
    end
    if big <= 0, I = k; break, end

%   Symmetric row/column permutations.
    if m ~= k
       A(:, [k m]) = A(:, [m k]);
       A([k m], :) = A([m k], :);
       pp( [k m] ) = pp( [m k] );
    end

    A(k,k) = sqrt( A(k,k) );
    if k == n, break, end
    A(k, k+1:n) = A(k, k+1:n) / A(k,k);

%   For simplicity update the whole of the remaining submatrix (rather
%   than just the upper triangle).

    j = k+1:n;
    A(j,j) = A(j,j) - A(k,j)'*A(k,j);

end

R = triu(A);
if I > 0
    if nargout < 3, error('Matrix must be positive definite.'), end
    R = R(1:I-1,:);
end

if piv == 0
   P = I;
else
   P = eye(n); P = P(:,pp);
end