function [x,info]=cfp_solver_hybrid(Pts,b,ColorPartition,options)
%
% The hybrid of Multi-update algorithms and Barany-Onn-2 algorithm to solve
% colourful feasibility problem.
%
% **********
% * Syntax *
% **********
% [x, info] = cfp_solver_hybrid(Pts)
% [x, into] = cfp_solver_hybrid(Pts, b)
% [x, info] = cfp_solver_hybrid(Pts, b, ColorPartition)
% [x, info] = cfp_solver_hybrid(Pts, b, ColorPartition, options)
%
% ***************
% * Description *
% ***************
% [x, info] = cfp_solver_hybrid(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
%
% *******************
% * 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.light:  the number of light iterations (not need to break an
%              oscillation).
% info.heavy:  the number of heavy iterations (breaks an oscillation).
% 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 *
% *************
% The algorithm updates a colorful set of points T and a point xk on the
% surface of conv(T) at each iteration. Normally the algorithm works 
% similar to Barany-Onn-2 algorithm but attempts to update multiple points
% at each iteration. If oscillation is detected (the algorithm reaches a
% previously reached colorful set), then a Multi-update step is used to
% quit the oscillation.
%   There are some notes for this algorithm:
% 1) It does not detect all the oscillations because it only records a
% finite number of previous colorful sets.
% 2) Some cases of oscillation may only take short times, but the algorithm
%    will still use Multi-update step to quit it.
%

%%%%%%%%%%%%%%%%%%%%%%%%% Internal Comments %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Written by Sui Huang, Advanced Optimization Lab, McMaster University,
% Hamilton, Ontario, Canada.
% 
% ************************
% * Modification History *
% ************************
% Sep 2005: First version.
% Jly 2007: Unied with the other routines in the thesis:
%           "Colourful Feasibility: Algorithms, Bounds, and Implications".
% 
% ****************
% * Dependencies *
% ****************
% Following functions are not in basic MATLAB library, and this routine
% depends them:
%   linprog()
%   quadprog()
%
% ************************
% * 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 used to shrink vectors.
% TRec: A data structure to record the previous colorful sets. 
%

% 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.
TRecLen = 2;                                 % The number of previous
                                             % colorful to record.
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
%              TRecLen options


% 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 = [];
head = 1;
tail = ColorPartition(1);
for cnt=1:NumColor
    ColorMap(cnt).list = head : tail;
    head = head + ColorPartition(cnt);
    tail = tail + ColorPartition(cnt);
end
% Initialized: ColorMap


% Initialize some variables for iterations.
T = options.initT;         % Represents a colorful set of points.
TRec = zeros(TRecLen,d+1); % Initialize TRec.
TRec(1,:) = T;             % Record the initial colorful set.
TRecPtr = 1;               % The pointer to update TRec in the way of
                           % "Round Robin".
xk = Pts(:,T(1));          % Initialize xk to the first point in T.
color_out_list = 2:NumColor;
info.light = 0;            % Counter of light iterations.
info.heavy = 0;            % Counter of heavy iterations.
info.iter = 0;             % Counter of total iterations.
% Initialized: T TRec TRecPtr xk info


% 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 = 0;
    info.light = 0;
    info.heavy = 0;
    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.
  if (  ((info.light+info.heavy)>LoopLimit) ...
      &&(mod(info.light+info.heavy,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_hybrid');
        disp(sbuf);
        sbuf=sprintf('Loop limit: %f   Current loop count: %f\n', ...
                     LoopLimit, info.light+info.heavy');
        disp(sbuf);
        if (floor((info.light+info.heavy)/LoopLimit)>1000)
            info.feasbile = -1;
            x = zeros(1,NumPts);
            info.time = etime(clock,initTime);
            return;
        end
  end
  % Changed: info  
  
  % 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 = dot(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 = (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.
  end
  % Changed: T


  % Check if the current colorful set is previously reached, and decide
  % heavy or light iteration to perform in this iteration.
  IterType = 'L';                 % The default is light.
  for i=1:TRecLen
      if all(T==TRec(i,:))
          IterType = 'H';         % Perform heavy.
          break;
      end
  end
  % Initialized: IterType
  
  
  % Increment TRecPtr and record the current colorful set.
  if (TRecPtr<TRecLen)
      TRecPtr=TRecPtr+1;
  else
      TRecPtr=1;
  end
  TRec(TRecPtr,:)=T;
  % Changed: TRec TRecPtr

  
  % Update xk by performing a heavy or light iteration. 
  if (IterType=='L')
      
      % Increment the counter of light iterations.
      info.light = info.light + 1;
      % Changed: info
      
      % Shrink p to the surface of conv(T). 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,(d+1))]);  % 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)))
          % 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
          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.
              p = a*p;
          else
              % Terminate the algorithm.
              break;
          end
      end
      % Changed: p iFree


      % Find p0, the projection of origin on the affine hyperplane holding
      % p.
      t = Y(iFree,1:d)';
      p0 = (-Y(iFree,d+1))*t/dot(t,t);
      % Initialized: p0


      % Shrink p towards p0, such that p is on the boundary of the facet
      % holding it. Then update xk to p. This is done by solving the linear
      % optimization problem:
      %            min a
      % such that: Y(i,1:d)*(p0+a*(p-p0))<=Y(i,d+1) forall i
      % where    :  _
      %            |  the function Y(i,1:d)*x=Y(i,d+1) is the affine
      %            |_ hyperplane seperating p0 from conv(T).
      color_out_list = iFree;
      a = -Inf;
      t = p - p0;
      for i = find(Y*[p0;1]<(-TOLERANCE))'
          s = -(Y(i,1:d)*p0);
          m = -(Y(i,1:d)*t);
          if ((abs(m)>TOLERANCE)&&(((Y(i,d+1)-s)/m)>a))
              a = (Y(i,d+1)-s)/m;
              iFree = i;
          end
      end
      if (a>0)
          color_out_list = [color_out_list iFree];
          xk = p0 + a*t;
      else
          xk = p0;
      end
      % Changed: xk color_out_list

  else
      % Increment the counter of heavy iterations.
      info.heavy = info.heavy + 1;
      info.iter = info.iter + 1;
      % 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 = info.light+info.heavy;
          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

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,FeasibleFlag]...
        =linprog(sparse(zeros(d+1,1)),[],[],...
        [OriginPts(:,T); ones(1, NumColor)],[b;1],...
        zeros(d+1,1),[],[],optimset('Display','off'));
end
info.feasible = 1;
info.iter = info.light + info.heavy;
x = zeros(NumPts,1);
x(T) = tmp;
info.time = etime(clock,initTime);
% Initialized: info x

return
