function [X, exitflag] = classic_caratheodory(Pts, b, tol)
%
% A solver of Caratheodory Problem using pivoting algorithm.
%
% **********
% * Syntax *
% **********
% [X, exitflag] = classic_caratheodory(Pts, b, tol)
%
% ***************
% * Description *
% ***************
% [X, exitflag] = classic_caratheodory(Pts, b, tol) finds a solution for
% the constraints Pts*X = b, sum(X) = 1 and X > =0.
%
% *******************
% * Input Arguments *
% *******************
% Pts are the coordinates of a set of points. Each column of Pts contains
% the coordinates of one point.
%   b is a column vector containing the coordinates of one point inside the
% convex hull of points represented by Pts.
%   tol is a small positive number used as tolerance to avoid numerical
% error such as dividing a number by a small number.
%
% ********************
% * Output Arguments *
% ********************
% X is the solution. Each element of X contains the coefficient of
% corresponding point in Pts. If the problem is solved, then exitflag > 0,
% otherwise exitflag <= 0.
%
% *************
% * Algorithm *
% *************
% Uses a matrix to represent the constraints Pts*X=b and sum(X)=1, then a
% pivoting algorithm is applied to the matrix for finding a solution that
% satisfies the constraint X>=0 besides the other two.
%


%%%%%%%%%%%%%%%%%%%%%%%%% Internal Comments %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Written by Sui Huang, Advanced Optimization Lab, McMaster University,
% Hamilton, Ontario, Canada.
% 
% ************************
% * Modification History *
% ************************
% May  2005  : First version.
% July 2005  : Added comments.
% August 2005: Fixed a bug of detecting infeasible case.
% 


% Initialize the matrices representing constraints, and some control
% numbers.
n = size(Pts, 2) + 1;                        % Width of M.
M = [b Pts; ones(1,n)];                      % Represents the constraints
                                             % Pts*X=b and sum(X)=1.
m = size(M, 1);                              % Height of M.
FlashPeriod = 25;                            % Period of refreshing M.
LoopLimit = 1000;                            % Allowed iterations.
% Initialized: n m M FlashPeriod LoopLimit


% Initialize the basis, apply it on M, and remove the redundant constraints
% from M. Also obtain the matrix OriginM for refreshing M periodically.
BASIS = zeros(1, m);                         % Allocate memory.
RowFlag = zeros(1, m);                       % Flags indicating redundancy.
for i = 1:m
    for j = 2:n
        if (abs(M(i, j)) > tol)
            % Add the point to the basis and flag the row.
            BASIS(i) = j;
            RowFlag(i) = 1;
            % Pivot on M(i,j).
            M(i,:)=M(i,:)/M(i,j);
            for row=[(1:(i-1)) ((i+1):m)]
                M(row,:) = M(row,:) - M(row,j)*M(i,:);
            end
            M(:,j) = zeros(1,m);
            M(i,j) = 1;
        end
    end
    if ((~RowFlag(i)) && (abs(M(i,1))>tol))
        % The problem is infeasible.
        X = zeros((n-1), 1);
        exitflag = -1;
        return;
    end
end
tmp = find(RowFlag);                         % Find the necessary rows.
m = length(tmp);                             % Number of necessary rows.
M = M(tmp,:);                                % Keep the necessary rows.
BASIS = BASIS(tmp);                          % Keep the necessary column
                                             % indices in basis.
OriginM = M;                                 % OriginM is for periodical
                                             % refreshment of M.
% Initialized: BASIS OriginM
% Changed: M m

% Solution search.
LoopCnt = 0;
FlashCnt = 0;
while (1)
    
    % Search for the next pivoting location. Algorithm scans through all
    % the rows in the order of least column index and find the first
    % row with negative value at the first column, then find the minimum
    % column number (except 1) on that row with negative element. The row
    % and column number indicates the pivoting location. If the row number
    % is not found, then a valid solution is reached. If row number is
    % found but column number is not found, then the Caratheodary problem
    % is infeasible.
    [dum, rowOrder] = sort(BASIS, 'ascend'); % Least column index order.
    msg = 'v';                               % 'v' stands for 'valid'.
    for i=rowOrder
        if (M(i,1)<=(-tol))
            msg = '';
            break;                           % The row index is found.
        end
    end
    if (msg == 'v')
        break;                               % Terminate, since valid 
                                             % solution is reached.
    end
    msg = 'f';                               % 'f' stands for 'infeasible'.
    for j=2:n
        if (M(i,j)<=(-tol))
            msg = '';
            break;                           % The column index is found.
        end
    end
    if (msg == 'f')
        break;                               % Terminate, since the problem
                                             % is infeasible.
    end
    % Initialized: i j
    
    
    % Update BASIS according to the next pivoting location.
    BASIS(i) = j;
    % Changed: BASIS
    
    
    % Count the number of iterations.
    if (LoopCnt>LoopLimit)
        % Return an error if the problem is not solved in too many
        % iterations.
        error('too many iterations');
    end
    LoopCnt = LoopCnt+1;
    FlashCnt = FlashCnt + 1;
    % Changed: LoopCnt FlashCnt
    
    
    % Update M
    if (FlashCnt ~= FlashPeriod)
        % Pivot on M(i,j).
        M(i,:)=M(i,:)/M(i,j);
        for row=[(1:(i-1)) ((i+1):m)]
            M(row,:) = M(row,:) - M(row,j)*M(i,:);
        end
        M(:,j) = zeros(n,1);
        M(i,j) = 1;
    else
        % Sort the basis to ascendent order before recalculating M from
        % OriginM. The purpose is to speed up the calculation of least
        % column index order of row numbers, since it should be found
        % faster if there is less disorder in BASIS.
        BASIS = sort(BASIS, 'ascend');
        % Recalculate M from OriginM.
        M(:,BASIS) = eye(size(M,1));
        NonB = setdiff(1:m, BASIS);
        M(:, NonB) = inv(OriginM(:,BASIS)) * OriginM(:,NonB);
        % Reset FlashCnt.
        FlashCnt = 0;
    end
    
end

% Prepare the outputs.
if (msg == 'v')                    % Valid solution is found.
    exitflag = 1;
    X = zeros((n-1), 1);
    X(BASIS-1) = inv(OriginM(:,BASIS)) * OriginM(:,1);
elseif (msg == 'f')                % The problem is infeasible.
    exitflag = -1;
    X = zeros((n-1), 1);
end

return