function [x,info] = cfp_solver_bo2(Pts,b,ColorPartition,options)
%
% BARANY-ONN algorithm 2 to solve colorful feasibility problem.
%
% **********
% * Syntax *
% **********
% [x,info] = cfp_solver_bo2(Pts)
% [x,info] = cfp_solver_bo2(Pts, b)
% [x,info] = cfp_solver_bo2(Pts, b, ColorPartition)
% [x,info] = cfp_solver_bo2(Pts, b, ColorPartition, options)
%
% ***************
% * Description *
% ***************
% [x,info] = cfp_solver_bo2(Pts, b, ColorPartition)
% solves for x satisfying the constraints:
%        _
%       |  Pts*x = b
%       |  if x(i) and x(j) have the same color, then x(i)*x(j)=0
%       |  sum(x)=1
%       |_ x>=0
%
% *******************
% * Input Arguments *
% *******************
% Pts is a matrix storing the coordinates of points. Each column of Pts
% stores the coordinate of one point. The number of rows is d, which is the
% number of dimensions of the Euclidean space. The points in each color of
% Pts must contain b in their convex hulls.
%   b is a column vector representing a point in the d-Euclidean space.
% This argument is optional. In default it will be the origin.
%   ColorPartition is a row vector of length (d+1). Each element is an
% integer, specifying the number of points in a color. For example [3 4 3]
% tells that the first 3 points in Pts are in the first color, the
% following 4 points are in the second color, and so on. This argument is
% optional. In default it assumes (d+1) points in each of the (d+1) colors.
% If the problem is not in the default case, user must provide this
% argument.
%   options is a struct that holds the addition parameters. This struct is
% for the purpose of extensibility. The field options.initT indicates the
% initial colourful simplex of the algorithm. For example, 
% options.initT=[1 5 7] corresponds to the colourful simplices generated by
% the points in the first, the fifth, and the seventh columns of Pts.
%
% ********************
% * Output Arguments *
% ********************
% x is the solution of the problem.
% info is a struct that tells the exiting state, following are its members.
% info.iter:   the number of iterations to solve the problem.
% info.time:   the number of seconds spent by the solver.
% info.feasible: 1 if feasible, -1 if exceeding maximum loop limit, -2 if
%                numerical error.
%
% *************
% * Algorithm *
% *************
% More details about the algorithm can be referred to the publication
% "Colourful Linear Programming and its Relatives" by IMRE BARANY and
% SHMUEL ONN (Math. Oper. Res. 22, 550-557, 1997).
%   The algorithm updates a colorful set of points T and a point xk on the
% surface of conv(T) at each iteration. In each iteration, The algorithm
% finds a linear combination transforming T into xk, such that some points
% in T has zero as coefficients; then replaces one of the zero-coefficient
% points with a point t of the same color, minimizing the inner product of
% t and xk; then find p, which is the projection of b in the line segment
% between t and xk; then shrink p to the surface of conv(T) and let xk=p.
%   There are some notes for this algorithm:
% 1) This algorithm has zigzagging performance in some cases.
% 2) The accumlation of numerical error on xk cannot be avoided, because
%    the update of xk depends on its previous value.
%

%%%%%%%%%%%%%%%%%%%%%%%%% Internal Comments %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Written by Sui Huang, Advanced Optimization Lab, McMaster University,
% Hamilton, Ontario, Canada.
%
% ************************
% * Modification History *
% ************************
% Jul 2005: First version.
% Aug 2005: Added comments.
% Sep 2005: Update the algorithm to shrink p.
% May 2006: Fixed a bug in generating data structure ColorMap.
% April  2007: Renamed to cfp_solver_bo2().
%
%
% ************************
% * Some Local Variables *
% ************************
% T  : A row vector containing the column indices of Pts. It indicates a
%      colorful set of points. This set is updated in each iteration until
%      it contains b in its convex hull.
% xk : A point in the colorful convex hull, whose distance to b is reduced
%      after each iteration.
% p  : A vector presenting a point belong to the convex hull of the
%      colorful set, notated conv(T).
% a  : A scalar between 0 and 1 for shrinking p.
%

% Initialize the time counter.
initTime = clock;
% Initialized: initTime

% Assign some global control numbers and apply the default arguments.
[d NumPts]=size(Pts);                        % Space dimension and number
% of points.
NumColor = d+1;                              % Number of colors.
TOLERANCE = 10^(-12);                        % Allowed numerical error.
LoopLimit = 1000000;                         % Allowed iterations.
if (nargin==3)||((nargin==4)&&(~isfield(options,'initT')))
    options.initT = zeros(1,NumColor);
    base = 1;
    for i=1:NumColor
        options.initT(i) = base;
        base = base + ColorPartition(i);
    end
end
if (nargin<3)
    ColorPartition = (d+1)*ones(1,NumColor); % Default is (d+1) points for
                                             % each color.
    options.initT = zeros(1,NumColor);
    base = 1;
    for i=1:NumColor
        options.initT(i) = base;
        base = base + ColorPartition(i);
    end
end
if (nargin<2)
    b=zeros(d,1);                            % Default is origin.
end
% Initialized: NumPts NumColor d TOLERANCE LoopLimit ColorPartition b


% Preprocess the inputs:
%   _
%  |  b is translated to the origin
%  |  The coordinates after translation are kept in OriginPts
%  |_ The coordinates after translation and normalization are kept in Pts
OriginPts = Pts;
for cnt = 1:NumPts
    OriginPts(:,cnt) = OriginPts(:,cnt) - b;
    Pts(:,cnt) = Pts(:,cnt) - b;
    Pts(:,cnt) = Pts(:,cnt)/norm(Pts(:,cnt),2);
end
b = zeros(d,1);
% Initialized: OriginPts
% Changed: Pts b


% construct ColorMap. ColorMap is an alternative data structure to present
% which point is in which color to ease the programming.
ColorMap(NumColor).list = [];
base = 0;
for cnt=1:NumColor
    ColorMap(cnt).list = (base+1) : (base+ColorPartition(cnt));
    base = base + ColorPartition(cnt);
end
% Keep: ColorMap


% Initialize some variables for iterations.
T = options.initT;
xk = Pts(:,T(1));          % Initialize xk to the first point in T.
iFree = 2;                 % A color index to be replaced.
LoopCnt = 0;               % Counter of iterations.
% Initialized: T xk LoopCnt

% Test if the initial colorful set is feasible by trying to express b as a
% convex combination of T. Gaussian elimination is used by the function
% linsolve() to find the coefficients of mapping T to b.
[tmp, recp_cond] = linsolve([OriginPts(:,T); ones(1, NumColor)], [b;1]);
if ((recp_cond < TOLERANCE) && any(tmp < -TOLERANCE))
    % When the simplex is degenerate, use linear programming to check if b
    % is in the simplex.
    [tmp,LPfval,FeasibleFlag]...
        =linprog(sparse(zeros(d+1,1)),[],[],...
        [OriginPts(:,T); ones(1, NumColor)],[b;1],...
        zeros(d+1,1),[],[],...
        optimset('Display','off', 'LargeScale', 'off'));
else
    FeasibleFlag = all(tmp >= -TOLERANCE);
end
if (FeasibleFlag > 0)
    % feasible solution is found.
    info.iter = LoopCnt;
    info.time = etime(clock,initTime);
    info.feasible = 1;
    x = zeros(NumPts, 1);
    x(T) = tmp;
    return;
end

% The iterations updating T until b is in conv(T).
while any(abs(xk)>TOLERANCE)

    % Count the number of iterations.
    LoopCnt = LoopCnt + 1;
    if (LoopCnt>LoopLimit)&&(mod(LoopCnt,LoopLimit)==1)
        % Give a warning if the problem is not solved in too many
        % iterations.
        sbuf=sprintf('Warning: over loop limit in routine %s\n', ...
            'cfp_solver_bo2');
        disp(sbuf);
        sbuf=sprintf('Loop limit: %f   Current loop count: %f\n', ...
            LoopLimit, LoopCnt');
        disp(sbuf);
        if (floor(LoopCnt/LoopLimit)>10)
            info.iter = LoopCnt;
            info.time = etime(clock,initTime);
            info.feasible = -1;
            x = zeros(NumPts, 1);
            return;
        end
    end
    % Changed: LoopCnt

    % Replace one zero-coefficient points.
    p = xk;                          % Initialize the projection of origin
    minProd = TOLERANCE;             % Initialize the minimum inner
    % product.
    for j=ColorMap(iFree).list       % Search for the point of minimum
        % inner product p.
        Prod = dot(Pts(:,j), p);
        if (Prod < minProd)
            j_in = j;
            minProd = Prod;
        end
    end
    T(iFree) = j_in;                       % Replace one point.
    t = Pts(:,j_in);                       % t is the new point.
    p = (dot(t-p,t)*p + dot(p-t,p)*t) ...  % p is the projection of
        / dot(t-p,t-p);                    % origin on a line segment.
    % Changed: T


    % Shrink p to the surface of conv(T), and let xk=p. The algorithm goes
    % through all the separating facets of conv(T). A separating facet
    % generates a hyperplane separating origin from conv(T) in the
    % Euclidean space.
    Y = inv([Pts(:,T); ones(1, NumColor)]);  % Each row of Y contains the
    % normal vector of a facet;
    % the negative elements in the last column indicates
    % separating facets.
    if any(any(~isfinite(Y)))
        % When T is affinely dependent, use linprog() shrink p.
        [tmp,lambda,exitflag]=...
            linprog([zeros(d+1,1) 1], [], [], ...
            [Pts(:,T) -p; ones(1,d+1) 0], [zeros(d,1); 1], ...
            zeros(d+2,1), ones(d+2,1), ...
            [ones(d+1,1)/(d+1); 1], ...
            optimset('LargeScale', 'off', 'Display', 'off') );
        if (exitflag<=0)
            info.iter = LoopCnt;
            info.time = etime(clock,initTime);
            info.feasible = -2;
            x = zeros(NumPts,1);
            return
        end
        if (a<TOLERANCE)
            % terminate the algorithm
            break;
        end
        xk = a*p;
        [dum, iFree] = min(tmp(1:(d+1)));
        % Find the index of the point that can be replaced.
        if (dum>TOLERANCE)
            A = [Pts(:,T); ones(1, NumColor)];
            iFree = 0;
            for i=1:(d+1)
                [tmp,lambda,exitflag] = linprog(zeros(d,1),[],[],...
                    A(:,[1:(i-1) (i+1):(d+1)]),[xk;1],...
                    zeros(d,1), ones(d,1),...
                    ones(d,1)/d, ...
                    optimset('Large Scale', 'off', 'Display', 'off'));
                if (exitflag>=1)
                    iFree = i;
                    break;
                end
            end
            if (iFree==0)
                info.iter = LoopCnt;
                info.time = etime(clock,initTime);
                info.feasible = -2;
                x = zeros(NumPts,1);
                return
            end
        end
        continue;
    else
        % Solves the linear optimization problem:
        %            min a
        % such that: a*(Y(i,1:d)*p)<=Y(i,d+1) forall i;
        % where    :  _
        %            |  if Y(i,d+1)<0, the function Y(i,1:d)*x=Y(i,d+1) is
        %            |  the affine hyperplane generated by a seperating
        %            |_ facet.
        a = -Inf;
        for i = find(Y(:,(d+1))<(-TOLERANCE))'
            m = -(Y(i,1:d)*p);
            if ((abs(m) > TOLERANCE) && ((Y(i,d+1)/m) > a))
                a = Y(i,d+1)/m;
                iFree = i;
            end
        end
        % If a is between 0 and 1 after scanning all the separating facets,
        % b is not in the convex hull of T. Otherwise b is already in the
        % convex hull of T and the algorithm can terminate.
        if (a>0)
            % The condition a<=1 is satisfied by the context that only
            % separating facets are scanned.
            xk = a*p;
        else
            % Terminate the algorithm.
            break;
        end
    end
    % Changed: xk iFree

end
% Changed: T


% Prepare the results and terminate the algorithm.
[tmp, recp_cond] = linsolve([OriginPts(:,T); ones(1, NumColor)], [b;1]);
if ((recp_cond < TOLERANCE) && any(tmp < -TOLERANCE))
    % When the simplex is degenerate, use linear programming to check if b
    % is in the simplex.
    tmp = linprog(sparse(zeros(d+1,1)),[],[],...
        [OriginPts(:,T); ones(1, NumColor)],[b;1],...
        zeros(d+1,1),[],[],optimset('Display','off'));
end
info.iter = LoopCnt;
info.time = etime(clock,initTime);
info.feasible = 1;
x = zeros(NumPts,1);
x(T) = tmp;

return
