function [x,info] = cfp_solver_enumstretch(Pts,b,ColorPartition)
%
% Use enumeration algorithm with heuristic stretching the distance between
% points to solve colorful feasibility problems.
%
% **********
% * Syntax *
% **********
% [x,info]=cfp_solver_enumstretch(Pts)
% [x,info]=cfp_solver_enumstretch(Pts,b)
% [x,info]=cfp_solver_enumstretch(Pts,b,ColorPartition)
%
% ***************
% * Description *
% ***************
% [x,info]=clp_solver_enummaxv(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
%       |  for all valid i, x(i)>=0
%       |_ sum(x) = 1
%
% *******************
% * 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.
%   b is a column vector representing a point in the d-Euclidean space.
% This argument is optional. In default it is 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.
%
% ********************
% * 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 inner iterations to solve the problem, which
%                indicates the number of colorful simplices tested in this
%                algorithm.
% info.numset:   the number of outer iterations to solve the problem, which
%                indicates the number of subset of points selected in this
%                algorithm.
% info.time:     the number of seconds spent by the solver.
% info.feasible: returns 1 if the problem is feasible, 0 if the problem is
%                not feasible.
%
% *************
% * Algorithm *
% *************
% We first choose a set of points and check all the colorful simplices in
% it. If no feasible solution is found, we find another set, check all the
% simplices in the new set, and then check simplices which contain points
% from both of the two sets, at last combine the two sets into the checked
% set. We go on with finding another set from unchecked set and continue
% the algorithm. The algorithm terminates when either a solution found or
% all the simplices were gone through. 
%   The method we use to choose a new set is like this: first find 1
% unchecked ponit from each color (omit if all points in that color are
% checked) using heuristic idea selecting the points far away from each
% other. We select up to one more point form each color if there are some
% more unchecked points to increase the total volume covered by the subset.
%   The complexity of testing each simplex is O(n^3).

%%       
%%%%%%%%%%%%%%%%%%%%%%%%% Internal Comments %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Written by Hanxing Zhang, Advanced Optimization Lab, McMaster University,
% Hamilton, Ontario, Canada.
% 
% ************************
% * Modification History *
% ************************
% August 2006: first edition and add comments by Hanxing Zhang
% Oct    2006: interface, variable scope, and comments are updated by Sui
%              Huang
% Mar    2007: modified the comment of nextEnum sub-routine
% April  2007: Renamed to cfp_solver_enumstretch().
%              Updated the interface by Sui Huang.
%
% ******************************************************************
% * Some Data Structures intended to be Shared by nested functions *
% ******************************************************************
% M:       stores the normalized coordinates of points
% Unchk:   the set of points generating colourful simplices that haven't
%          been tested
% Chk:     the set of points generating colourful simplices that have been
%          tested
% S:       the set of points moving from Unchk to Chk
%
% ****************
% * Coding Issue *
% ****************
% The sub-functions are nested into the main solver function, so all local
% variables defined in main solver function can be read/modified in the
% sub-functions but not in outer scopes, this way of sharing data avoids a
% danger: main solver function modifies data outside its scope by
% unfortunately using the same global variable name with outer scope.
% However, there is a drawback, some data which is not intended to be
% shared between main solver function and sub-functions, such as some
% counters, are forced to be shared if they are in the same variable name.
% To overcome this drawback, differences in variable names are made between
% main solver function and sub-functions.
%%

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

% Preprocessing data
[d NumPts]=size(Pts);                        % Space dimension and the
                                             % number of points.
if (nargin<3)||isempty(ColorPartition)
    ColorPartition = (d+1)*ones(1,d+1);      % Default is (d+1) points for
                                             % each color.
end
if (nargin<2)||isempty(b)
    b=zeros(d,1);                            % Default is origin.
end
NumColor = length(ColorPartition);  % The number of colors
M = Pts;                            % Buffer the normalized coordinate data
for cnt = 1:NumPts                  % Normalize all the points
    M(:,cnt) = M(:,cnt) - b;
    M(:,cnt) = M(:,cnt)/norm(M(:,cnt),2);
end

% Initialize the auxiliary indixing data structures:
% A colourful set of points is represented by a structure holding subsets
% of different colours, and each of these subset is represented by a vector
% holding the indices of points. For each colourful set of points, an extra
% vector may be used to track how many points of each colour do it has.
base = 0;
for cnt=1:NumColor              % Tells which points each color has.
    ColorMap(cnt).list = (base+1) : (base+ColorPartition(cnt));
    base = base + ColorPartition(cnt);
end
base = 0;
for cnt=1:(NumColor)            % Tells which points are not checked
    Unchk(cnt).list = (base+1) : (base+ColorPartition(cnt));
    nsUnchk(cnt) = ColorPartition(cnt);
    base = base + ColorPartition(cnt);
end
for cnt=1:(NumColor)              % An empty set of points
    EmptySet(cnt).list = [];
    nsEmptySet(cnt) = 0;
end
Chk = EmptySet;                 % Initialize the set of checked points
nsChk = nsEmptySet;
S = EmptySet;                   % Initialize the set S
nsS = zeros(1,NumColor);

info.numset = 0;              % Number of colourful subsets
info.iter   = 0;              % Number of simplices tested
while (1)

    % Increment the subset count.
    info.numset = info.numset+1;
   
    % Update the set S.
    flag = newS(ColorPartition,b);
    if (flag==2)
        % All points are checked, which means there exist no colourful
        % simplex covering the origin.
        x = zeros(NumPts,1);
        info.feasible = 0;
        info.time = etime(clock,initTime);
        return
    end

    % Check the simplices inside S. This section will matter if and only
    % if all the colors in the set have at least one point. Because S is a
    % sequence of vectors, and each vector is a sequence of indices of
    % points, it provides an order of colours and an order of points in
    % each colours, for the colourful set it is representing. These
    % ordering allow us to enumerate all the colourful simplices generated
    % by the set S. A vector T is used to enumerate the selections of
    % vertices for colourful simplices in the set:
    %       for example, T=[1 2 2] means choosing the first point
    %       in the first colour, the second point in the second
    %       colour, and the second point in the three, and use
    %       them as the vertices of a colourful simplex.
    if all(nsS)
        % Check the colourful simplices of S. The simplices are divided
        % into subsets: all the vertices are the first points of their
        % colour, one vertex is not the first, two vertices are not the
        % first, etc. These subsets are enumerated by the 0-1 vector U.
        repColor = find(nsS==2);  % repColor holds the indices of colours
                                  % that have two points in S
        lenRepColor = length(repColor);
        for len = 0:length(repColor)
            % Check the combinations of choosing len colours from the
            % colours that have two alternative points. For the chosen
            % colour, the second point will be used to generate a colourful
            % simplex with the points of other colours.
            U = zeros(1,length(repColor));  % U is a 0-1 vector identifying
            U(1:len) = 1;                   % a combination
            while (1)
                % Check the transversal T under the current combination U.
                % In this section each combination has only one transversal
                % identified by T. The reason why not directly enumerate T,
                % but enumerate U is that we try the colourful simplices
                % most probable to cover the origin first.
                T=ones(1,NumColor);  T(repColor)=T(repColor)+U;
                info.iter = info.iter+1;                     
                % Check the colourful simplex represented by T
                for cnt=1:(d+1)
                    L(cnt) = S(cnt).list(T(cnt));
                end
                tmp = linsolve([M(:,L); ones(1,d+1)], [zeros(d,1); 1]);
                if all(tmp>=(-TOLERANCE))
                    x = zeros(NumPts,1);
                    x(L) = tmp;
                    info.feasible = 1;
                    info.time = etime(clock,initTime);
                    return
                end
                % Increment the current combination
                [U,endflag] = nextComb(U,lenRepColor,len);   
                if endflag
                    break
                end
            end
        end
    end

    % If this is the first set of points, then this iteration is finished.
    if info.numset==1
        % add S to Chk
        for iColor=1:(d+1)
            Chk(iColor).list = [Chk(iColor).list S(iColor).list];
            nsChk(iColor) = nsChk(iColor) + nsS(iColor);
        end
        % end the current iteration
        continue
    end

    % Check the colourful simplices generated by Union(S,Chk), and with
    % vertices from both S and Chk. The enumeration skips the colourful
    % simplices with vertices from purely S or Chk, which have already been
    % tested. The method is to enumerate a group of subsets of
    % Union(S,Chk), such that all colourful simplices generated by them are
    % not tested; then enumerate and test colourful simplices for each
    % individual subset. A subset is represented by a 0-1 vector U. For
    % example, U=[0 1 0] represents the subset containing all the points of
    % colour 2 from S, and the points of other colours from Chk.
    repColor = find(nsS>=1);            % repColor lists what colours S has
    for len=1:min(d,length(repColor))   % len is the number of colors from
                                        % S points are selected to be
                                        % vertices of colourful simplices
        U=zeros(1,length(repColor));  U(1:len)=1;                              
        while (1)
            % Build a temporary set TmpSet with U
            TmpSet = Chk;  nsTmpSet = nsChk;
            for cnt=(U.*repColor)
                if cnt>0
                    TmpSet(cnt)=S(cnt);
                    nsTmpSet(cnt) = nsS(cnt);
                end
            end
            % Test all the colourful simplices generated by TmpSet
            T = ones(1,NumColor);
            while (1)
                % Increment the counter
                info.iter = info.iter + 1;
                % Check the colourful simplex represented by T
                for cnt=1:(d+1)
                    L(cnt) = TmpSet(cnt).list(T(cnt));
                end
                tmp = linsolve([M(:,L); ones(1,d+1)], [zeros(d,1); 1]);
                if all(tmp>=(-TOLERANCE))
                    x = zeros(NumPts,1);
                    x(L) = tmp;
                    info.feasible = 1;
                    info.time = etime(clock,initTime);
                    return
                end
                % Increment T
                T = nextEnum(T,nsTmpSet);
                if T(1)<0
                    break
                end
            end
            % Increment the combination U for current len.
            [U,endflag] = nextComb(U,length(repColor),len);
            if (endflag==1)
                break
            end
        end
    end
    % add S to Chk
    for iColor=1:(d+1)
        Chk(iColor).list = [Chk(iColor).list S(iColor).list];
        nsChk(iColor) = nsChk(iColor) + nsS(iColor);
    end
end % While main loop

%%
function Next = nextEnum(Current, UpBnd)
% Obtain the next number in the enumeration represented by the input vector
% Current, the first element of Current is the least significant digit, and
% the vector UpBnd holds the upper bound of all the digits. For the i-th
% digit, the minimum value is 1, and the maximum is UpBnd(i). Following are
% some examples:
%      Current       UpBnd         Next
%      2 1 1         3 2 3         3 1 1
%      3 1 1         3 2 3         1 2 1
l = length(UpBnd);
Next = Current;
for i=1:l;
    if Current(i)<UpBnd(i)
        Next(i)=Current(i)+1;
        return
    else
        Next(i)=1;
    end
end
Next = -ones(size(Next));
end
%%
function [Next,endflag] = nextComb(Current, TotalSize, ChooseSize)
% Obtain the next enumerated combination of selecting ChooseSize elements
% from TotalSize elements. Current is a 0-1 vector representing a
% combination.
% For example if Current=[0 1 0 1 0], then Next=[0 0 1 1 0] and endflag=0.
% If Current is already the last combination, such as Current=[ 0 0 1 1 1],
% then endflag=1.
if (ChooseSize==0)
    Next = -ones(1,TotalSize);
    endflag = 1;
    return
end
Position = find(Current);
for i=1:(ChooseSize-1)
    if ((Position(i+1)-Position(i))>1)
        % Increment the combination
        Position(i) = Position(i)+1;
        Position(1:(i-1))=1:(i-1);
        Next = zeros(1,TotalSize);
        Next(Position) = 1;
        endflag = 0;
        return
    end
end
if Position(ChooseSize)<TotalSize
    % Increment the combination
    Position(ChooseSize) = Position(ChooseSize)+1;
    Position(1:(ChooseSize-1)) = 1:(ChooseSize-1);
    Next = zeros(1,TotalSize);
    Next(Position) = 1;
    endflag = 0;
    return
end
endflag = 1;
Next = -ones(1,TotalSize);
end
%%
function flag = newS(ColorPartition,b)
% Select a set S of points, which are all chosen from set Unchk.
% Here is the meaning of flag:
%      0: found a new S with at least one point in each colour
%      1: found a new S, but some colours do not have point
%      2: no more unchecked points
% Side Effects: S, nsS

% Initialization
TOLERANCE=10^(-10);
S = EmptySet;
nsS = nsEmptySet;
if all(~nsUnchk)
    % If there is no more point in Unchk,return.
    flag = 2;
    return
else
    % If there are still points in Unchk,collect related data of
    % Unchk for further processing.
    emptyColor    = find(nsUnchk==0);
    singularColor = find(nsUnchk==1);
    multipleColor = find(nsUnchk>1);
    T = zeros(1,d+1);
end
% Find the first part of the points and insert them to S. If there are some
% colours that have only one point, then they will all be inserted to S; if
% no, then a point in a colour that have multiple unchecked points will be
% inserted to S.
if ~isempty(singularColor)
    % Initialize the reference point for max volume heuristic
    p = zeros(d,1);
    % Insert points to S
    for i=singularColor
        T(i) = Unchk(i).list;
        sel_j=Unchk(i).list;   p=p+M(:,sel_j);
        % add the point to S
        S(i).list=[S(i).list sel_j];  nsS(i)=nsS(i)+1;
        % remove the point from Unchk
        Unchk(i).list=setdiff(Unchk(i).list, sel_j);
        nsUnchk(i)=nsUnchk(i)-1;
    end
    % Compute the reference point for max volume heuristic
    p = p/length(singularColor);
else
    % Select the colour
    i = multipleColor(1);
    % Insert the first point of colour i to S
    sel_j = Unchk(i).list(1);
    T(i) = sel_j;
    % add the point to S
    S(i).list=[S(i).list sel_j];  nsS(i)=nsS(i)+1;
    % remove the point from Unchk
    Unchk(i).list=setdiff(Unchk(i).list, sel_j);
    nsUnchk(i)=nsUnchk(i)-1;
    % Compute the reference point for max volume heuristic
    p = M(:,sel_j);
end
% Find the second part of points and insert them to S with max volume
% heuristic, from the colors that have multiple unchecked points. If there
% is no colour in Unchk that has only one point, then the first point of
% the first colour in multipleColor would had been selected, and that
% colour no longer need to be search that colour.
iBegin = 1 + isempty(singularColor);
for i = multipleColor(iBegin:length(multipleColor))
    % Because it is assume that all points are normalized vectors, 2 is an
    % uppder bound of the minProd, which is the dot product of two vectors
    minProd = 2;
    % Go through all points of colour i, to find a point that minimize its
    % dot product with the reference point p
    for j = Unchk(i).list
        Prod = dot(p,M(:,j));
        if Prod<minProd
            minProd = Prod;
            sel_j = j;
        end
    end
    % Insert a point to S
    T(i) = sel_j;
    % add the point to S
    S(i).list=[S(i).list sel_j];  nsS(i)=nsS(i)+1;
    % remove the point from Unchk
    Unchk(i).list=setdiff(Unchk(i).list, sel_j);
    nsUnchk(i)=nsUnchk(i)-1;
    % Update reference point
    t = M(:,sel_j);
    p = (dot(t-p,t)*p + dot(p-t,p)*t) / dot(t-p,t-p);
end
% Return if S still has some colour without any point
if ~isempty(emptyColor)
    flag = 1;
    return
end
% Try to find some other points base on the existing points in S. The
% existing point in S generates a colourful Simplex. Let H(i) be the
% hyperplane generated by the vertices of this Simplex except the vertex in
% colour i. Find all the i, such that the vertex in colour i is seperated
% by H(i) from the origin. Then for each of these found colour i, if there
% is any point not seperated by H(i) from the origin, select the one that
% has the maximum distance from H(i) and insert it to S.
A = [M(:,T); ones(1,d+1)];
if (rank(A) < (d+1))
    % This routine does not continue for affinely dependent colorful set.
    flag = 0;
    return
end
Y = inv(A);       % Y(i,1:d) is a normal vector of H(i)
LUM = Y * [b;1];  % Negative element in LUM indicates seperated vertex
NegColorList = find(LUM<(-TOLERANCE))';
NumNegColor = length(NegColorList);
if NumNegColor==0
    % Return if there is no seperated vertex, which means the points in S
    % are already generating a colourful feasible simplex covering the
    % origin.
    flag = 0;
    return
end
MM = zeros(1, NumPts);
for i = NegColorList
    % If the points j and k are in the same colour i, MM(j) and MM(k) are
    % their relative distances to H(i)
    MM(ColorMap(i).list) = ...
        Y(i,:) * [M(:,ColorMap(i).list);ones(1,ColorPartition(i))];
end
MM(T) = 1;
for i = NegColorList
    % Find the point of i-th colour that is 1) not seperated by H(i) from
    % origin, 2) far away from H(i).
    [dum, tmp] = min(MM(Unchk(i).list));
    if (dum<=0)
        % Let h(i) be the hyper plane that parallel to H(i) and cross the
        % origin. Only the points that not between h(i) and H(i) are
        % possible to be inserted to S. This is to make sure that the
        % inserted point be far away from H(i).
        j = Unchk(i).list(tmp);
        % add the point to S
        S(i).list=[S(i).list j];  nsS(i)=nsS(i)+1;
        % remove the point from Unchk
        Unchk(i).list=setdiff(Unchk(i).list, j);
        nsUnchk(i)=nsUnchk(i)-1;
    end
end
flag = 0;
end
%%
end  %Main function return

