function [x,info] = cfp_solver_bo1(Pts,b,ColorPartition,options)
%
% BARANY-ONN algorithm 1 to solve colorful linear programming problem.
%
% **********
% * Syntax *
% **********
% [x, info] = cfp_solver_bo1(Pts)
% [x, info] = cfp_solver_bo1(Pts, b)
% [x, info] = cfp_solver_bo1(Pts, b, ColorPartition)
% [x, info] = cfp_solver_bo1(Pts, b, ColorPartition, options)
%
% ***************
% * Description *
% ***************
% [x, info] = cfp_solver_bo1(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 found in the publication
% "Colourful Linear Programming and its Relatives" by IMRE BARANY and
% SHMUEL ONN (Math. Oper. Res. 22, 550-557, 1997).
%   In each iteration the algorithm finds the point xk in the convex hull
% of the current colorful set of points with minimum distance to b
% (considered as the origin). xk can be represented as a linear combination
% of the points of the current colorful set, and some points have zero as
% their coefficients. The algorithm replaces one of these points with a
% point of the same color, which minimizes the inner product with xk.
%

%%%%%%%%%%%%%%%%%%%%%%%%% 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: Use linsolve() function for speed up.
% May    2006: Fixed a bug in generating data structure ColorMap.
% April  2007: Renamed to cfp_solver_bo1().
%
% ****************
% * Dependencies *
% ****************
% Following functions are not in basic MATLAB library, and this routine
% depends on them:
%   linprog()                    optimization toolbox
%   quadprog()                   from MATLAB Optimization toolbox
%
% ************************
% * 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 minimum.
% 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 to shrink a vector.
%

% 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 = 100000;                          % 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 the iterations.
T = options.initT;
LoopCnt = 0;     % Counter of iterations.
% Initialized: T LoopCnt


% The iterations of updating the colorful set of points until the convex
% hull of the set contains b.
while (1)

    % 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'));
    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

    % 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_bo1');
        disp(sbuf);
        sbuf=sprintf('Loop limit: %f   Current loop count: %f\n', ...
            LoopLimit, LoopCnt');
        disp(sbuf);
        if (floor(LoopCnt/LoopLimit)>1000)
            info.iter = LoopCnt;
            info.time = etime(clock,initTime);
            info.feasible = -1;
            x = zeros(NumPts, 1);
            return;
        end
    end
    % Changed: LoopCnt


    % use quadratic optimization routine to obtain the minimum-norm-point
    % in conv(T). Following is the formulation of the problem in 2D, which
    % can be generalized to high dimensional space.
    %   Let the vertices of simplex be (x11,x12,x13), (x21,x22,x23),
    % (x31,x32,x33). Let c1,c2,c3>=0 and c1+c2+c3=1 be the coefficients of
    % the vertices to generate a point in simplex. The square of 2-norm of
    % a point in simplex is (the objective function to be minimized):
    %               c1*c1*(x11*x11+x12*x12+x13*x12)
    %             +             ...
    %             + ci*cj*(xi1*xj1+xi2*xj2+xi3*xj3)
    %             +             ...
    %             + c3*c3*(x31*x31+x32*x32+x33*x33)
    % The variables are c1,c2,c3. The constraints are c1,c2,c3>=0 and
    % c1+c2+c3=1.

    % Get the matrix for objective function.
    H = zeros(NumColor,NumColor);
    for i=1:NumColor
        for j=i:NumColor
            % Use a more numerically accurate way to calculate dot product,
            % since numerical stability is important.
            H(i,j) = accsum(Pts(:,T(i)).*Pts(:,T(j)));
            H(j,i) = H(i,j);
        end
    end
    % Get the optimal coefficients.
    [c,dummy,FeasibleFlag] ...
        = quadprog(H,[],[],[],ones(1,NumColor),...
        1,zeros(NumColor,1),[],...
        ones(1,NumColor)/NumColor,...
        optimset('Display','off','LargeScale','off'));
    if (FeasibleFlag<=0)
        info.iter = LoopCnt;
        info.time = etime(clock,initTime);
        info.feasible = -2;
        x = zeros(NumPts,1);
        return
    end
    % Get the minimum-norm-point in the simplex.
    xk = zeros(d,1);
    for i = 1:d
        xk(i) = accdot(Pts(i,T), c');
    end
    % Get the list of colors that the points can be replaced.
    color_out_list = find(abs(transpose(c))<TOLERANCE);
    if isempty(color_out_list)
        % T is degenerated and unfortunately the quadratic optimization
        % routine did not find any replacable point. Then we need to find
        % one by testing elements in T one by one.
        for i=1:NumColor
            tempT = T([1:(i-1) (i+1):NumColor]);
            [tmp,LPfval,FeasibleFlag]...
            =linprog(sparse(zeros(NumColor-1,1)),[],[],...
             [Pts(:,tempT); ones(1, NumColor-1)],[xk;1],...
             zeros(NumColor-1,1),[],[],optimset('Display','off'));
            if (FeasibleFlag==1)
                color_out_list = i;
                break;
            end
        end
    end

    % Select a point to be replaced.
    color_out_index = color_out_list(1);
    % Initialized: color_out_index

    % Update the colorful set of points.
    minProd = TOLERANCE;                   % Initialize the minimum inner
    % product.
    for j=ColorMap(color_out_index).list   % Search for the point of
        % minimum inner product with xk.
        Prod = dot(Pts(:,j), xk);
        if (Prod < minProd)
            j_in = j;
            minProd = Prod;
        end
    end
    T(color_out_index) = j_in;             % Replace the point in the set.
    % Changed: T

end

return
