function [x,info]=cfp_solver_multiupdate(Pts,b,ColorPartition,options)
%
% The Multi-update BARANY-ONN algorithm 1 to solve colorful linear
% programming problem.
%
% **********
% * Syntax *
% **********
% [x,info] = cfp_solver_multiupdate(Pts)
% [x,info] = cfp_solver_multiupdate(Pts, b)
% [x,info] = cfp_solver_multiupdate(Pts, b, ColorPartition)
% [x,info] = cfp_solver_multiupdate(Pts, b, ColorPartition, options)
%
% ***************
% * Description *
% ***************
% [x,info]=cfp_solver_multiupdate(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 *
% *************
% This algorithm is a variance of algorithm 1 in 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. The algorithm finds a linear
% combination transforming T into xk, such that some points in T has zero
% as coefficients; then replaces all of the zero-coefficient points in the
% following way:
%   Step 1: p = xk;
%   Step 2: for all zero-coefficient points in T
%           i)  replace it with a point of the same color minimizing the
%               inner product with p, say t.
%           ii) let p become the projection of the origin on the line
%               segment between t and p.
% After replacing the points, xk is updated to the point on the surface of
% conv(T), such that its norm is minimized.
%
%   There are some notes for this algorithm:
% 1) The accumlation of numerical error on xk cannot be avoided, because
%    the update of xk depends on its previous value.
% 2) The complexity of finding the minimum-norm-point of a simplex depends
%    on the complexity of convex quadratic programing subroutine.
%

%%%%%%%%%%%%%%%%%%%%%%%%% Internal Comments %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Written by Sui Huang, Advanced Optimization Lab, McMaster University,
% Hamilton, Ontario, Canada.
%
% ************************
% * Modification History *
% ************************
% Dec   2005: First rough version for quick test.
%             Named clp_solver_gravity().
% Feb   2006: Refined version.
%             * Handle degenerate case
%             * More numerically accurate way instead of simple using sum(),
%               norm() or dot() functions in some critical part.
% May   2006: Fix a bug in generating data structure ColorMap.
% April 2007: Renamed to cfp_solver_multiupdate().
% July  2007: Updated comments.
%
% ****************
% * Dependencies *
% ****************
% Following functions are not in basic MATLAB library, and this routine
% depends on them:
%   linprog()                optimization toolbox
%   quadprog()               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 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 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 = 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)/accnorm(Pts(:,cnt));
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;
color_out_list = 2:(d+1);
xk = Pts(:,T(1));          % Initialize xk to the first point in T.
LoopCnt = 0;               % Counter of iterations.
% Initialized: T xk LoopCnt color_out_list


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


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

    % 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

end
% Changed: T


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

return

