function [ClosestPoint,...
          Distance,...
          MinimumSubsetIndices, ...
          UsedCoefficients, ...
          ErrorFlag] = ClosestPointInSimplex(...
											SimplexVertices,...
											Point)
% Purpose		Find the point on a simplex which is closest to a given point.
%
% Description	This routine finds the point on a simplex that is closest to an input target
%				point.  The routine can work in an arbitrary number n of dimensions; the dimension
%				is inferred from the size of the input data.  The simplex is defined as the
%				convex hull of a set of input vertices.  While a simplex is often thought of as
%				being fully dimensional, it can also have smaller dimension than the ambient
%				space.  For example, it could be a line segment or a triangle in R^3.
%				Since a simplex is convex (and closed), 
%				it contains a unique point that is closest to the input point.  The closest
%				point is found by Johnson s sub-distance algorithm  ([Rabbitz1994]).
%
%				In addition to finding the closest point, Johnson s algorithm also finds the
%				smallest subsimplex that contains the nearest point.  For example, the closest
%				point on a tetrahedron in 3-space might actually be on an edge of the
%				tetrahedron.  In that case, the smallest subsimplex would be that edge, which
%				is generated by its two terminal vertices.  The indices of those vertices are
%				returned.  The closest point is a unique linear combination of those vertices,
%				so the coefficients of that combination are returned.  Coefficients of other
%				vertices are automatically 0, so they are not returned.
%
%				This routine follows [Rabbitz1994].  That reference mentions two possible
%				implementations of Johnson s algorithm.  One, that is followed in this routine,
%				uses an exhaustive search over all subsimplices, solving a linear system for
%				each subsimplex.  The second implementation uses only cofactors of linear
%				systems, and avoids repeated calculations, so it is more efficient than the
%				first.  The first implementation was chosen here because it is simpler, and
%				because execution time was not a problem.  This code is open source, so
%				programmers who are concerned about execution time are free to modify it.
%
%				Each subsimplex generates an affine hull.  The closest point to Point on that
%				affine hull is calculated by a linear system involving Lagrange multipliers
%				(see [Rabbitz1994]).  The closest point on the affine hull, however, might
%				be outside the convex hull, and thus outside the simplex.  In that case,
%				though, the closest point on the convex hull will occur on a boundary, and
%				that part of the boundary will be described by a smaller subsimplex.  Since
%				all subsimplices are evaluated, the closest point will eventually be found.
%
%				Since this source is open source, any suggestions or improvements that reduce
%				such instability are welcome.
%
%				[Rabbitz1994]	Rich Rabbitz, Fast Collision Detection of Moving Convex Polyhedra,
%								in Section I.8 of Graphics Gems IV (IBM Version), ed. Paul Heckbert,
%								Academic Press, 1994.
%
%				SimplexVertices	A set of up to n+1 affinely independent vectors in n dimensions.  
%								Their convex hull forms a simplex.  The vertices are expressed
%								as row vectors, stacked up to form the matrix SimplexVertices.
%								The number of rows is the number of vertices, and
%								the number of colums is the dimension of the ambient space.
%
%				Point			A point in n dimensions.  Point is a row vector with n entries.
%
%				ClosestPoint	The closest point on the simplex to Point.  ClosestPoint is a
%								row vector with n entries.
%
%				Distance		The distance between Point and the closest point on the simplex.
%
%				MinimumSubsetIndices	The indices of the vertices that generate the smallest
%								subsimplex that contains the closest point.  The indices refer
%								to the rows in the input SimplexVertices matrix.
%
%				UsedCoefficients	The coefficients of the linear combination of the minimal set
%								of vertices that produces the closest point.  The coefficients
%								of vertices that are not in the minimal set is automatically 0,
%								so they are not included here.  The ith entry of UsedCoefficients
%								is the coefficient of the ith vertex defined by MinimumSubsetIndices.
%
%				ErrorFlag		A Boolean variable that is true if the routine cannot return a
%								solution, and false otherwise.
%
% Author		Paul Centore (January 9, 2015)
%
% Copyright 2015 Paul Centore
%
%    This file is part of MunsellAndKubelkaMunkToolbox.
%
%    MunsellAndKubelkaMunkToolbox is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    MunsellAndKubelkaMunkToolbox is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with MunsellAndKubelkaMunkToolbox.  If not, see <http://www.gnu.org/licenses/>.

% Initialize variables
ClosestPoint         = []		;
Distance             = -99		;
MinimumSubsetIndices = []		;
UsedCoefficients     = []		;
ErrorFlag            = false	;

% Minimum distance for comparisons, to avoid numerical roundoff error
EpsDist = 1e-14	;	

% Infer the number of vertices and the dimension of the ambient space from the size of the
% input variables
[NumberOfVertices, DimensionOfSpace] = size(SimplexVertices)	;

% Translate the input point to the origin; translate the simplex by the same amount.  This
% translation will simplify calculations
TranslatedSimplexVertices = SimplexVertices - repmat(Point,NumberOfVertices,1)		;

% For diagnostic information, if interested
if false
	Pairs = combinator(NumberOfVertices, 2, 'c')	;
	NumberOfPairs = size(Pairs,1)					;
	DistancesBetweenVertices = []	;
	for ctr = 1:NumberOfPairs
		DistanceBetweenVertices = norm(SimplexVertices(Pairs(ctr,1),:) - SimplexVertices(Pairs(ctr,2),:))	;
		DistancesBetweenVertices = [DistancesBetweenVertices, DistanceBetweenVertices]	;
	end
	%DistancesBetweenVertices
	DistancesToVertices = []	;
	for ctr = 1:NumberOfVertices
		DistanceToVertex = norm(TranslatedSimplexVertices(ctr,:))	;
		DistancesToVertices = [DistancesToVertices, DistanceToVertex]	;
	end
	%DistancesToVertices
end

% Any subset of vertices will generate a subsimplex, and every subsimplex is generated by
% a subset of vertices.  To find the set of all subsimplices, find the power set of all
% the vertices.  
IndicesOfVertices = 1:NumberOfVertices			;
PowerSetOfIndices = nchoose(IndicesOfVertices)	;

% This implementation loops exhaustively through the set of subsimplices.  For each
% subsimplex, the algorithm finds the closest point to Point on the affine hull of that
% subsimplex.  That closest point is expressed as a linear combination of the subsimplex s
% vertices, whose coefficients are given in a vector denoted lambda.  If all the entries
% in lambda are between 0 and 1, then the closest point on the affine hull is also in the
% convex hull, and is therefore a candidate for the closest point on the whole simplex (which
% is the convex hull of its generating vertices).  
% The following loop produces three matrices.  The first matrix gives the closest point on
% the affine hull for each subsimplex; each row of the matrix is a point in n-space, and there
% is a row for each susbsimplex.  The second matrix is a column vector of distances from Point
% to that subsimplex; an entry is NaN if the closest point on the affine hull is not contained
% in the convex hull of the subsimplex.  The third matrix gives the coefficients of the
% generating vertices, that lead to the closest point in the affine hull.  Each row corresponds
% to one subsimplex; each column corresponds to one generating vector.  If a particular
% generating vector does not appear in a particular simplex, then the entry in lambdas is NaN.
% A point is in the convex hull of the subsimplex if and only if all its entries are between
% 0 and 1.
ClosestPointsInSubSimplices = NaN * ones(length(PowerSetOfIndices),DimensionOfSpace)	;
MinDistancesToSubSimplices  = NaN * ones(length(PowerSetOfIndices),1)					;
lambdas                     = NaN * ones(length(PowerSetOfIndices),NumberOfVertices)	;
for SubSimplexCtr = 1:length(PowerSetOfIndices)

	% Extract the vertices for a particular subsimplex.
	SubSimplexIndices         = PowerSetOfIndices{SubSimplexCtr}	;
	NumberOfSubSimplexIndices = length(SubSimplexIndices)			;
	
	% The Johnson Sub-Distance Algorithm defines a linear system A x lambda = b.  The
	% vector lambda gives coefficients in the linear combination of the subsimplex s
	% vertices.  The point defined by these coefficients is the closest point to the origin,
	% on the affine hull of that subsimplex.  See [Rabbitz1994] for details.
	
	% Construct the matrix A in the system A x lambda = b.
	JohnsonMatrixA      = NaN * ones(NumberOfSubSimplexIndices,NumberOfSubSimplexIndices)	;
	JohnsonMatrixA(1,:) = ones(1,NumberOfSubSimplexIndices)									;
	for rowctr = 2:NumberOfSubSimplexIndices
		for colctr = 1:NumberOfSubSimplexIndices
			JohnsonMatrixA(rowctr,colctr) = dot( ...
							(TranslatedSimplexVertices(SubSimplexIndices(rowctr),:) - ...
							 TranslatedSimplexVertices(SubSimplexIndices(1),:)), ...
							 TranslatedSimplexVertices(SubSimplexIndices(colctr),:))	;
		end
	end
	
	% Construct the vector b in the system A x lambda = b.
	JohnsonVectorb = [1; zeros(NumberOfSubSimplexIndices-1,1)]	;

	% Numerical instability will result if the matrix A is ill-conditioned.  The top row
	% of A and the first entry of b are all ones, to insure that the lambdas sum to 1.
	% Multiplying the top row of A and the first entry of b by the same constant should
	% not change the system solution.  Choose a constant that increases the condition
	% number (though at the expense of increasing the determinant).  
	% This method of handling ill-conditioning is awkward and not always effective; more
	% elegant and effective handling methods would be welcome.
	RcondThreshold = 1e-5	;
	if rcond(JohnsonMatrixA) < RcondThreshold
%		disp(['Ill-conditioned matrix in ClosestPointInSimplex: adjusting'])
%		SubSimplexIndices
%		JohnsonMatrixA
		JohnsonMatrixA(1,:) = (1/RcondThreshold) * JohnsonMatrixA(1,:)	;
		JohnsonVectorb(1) = (1/RcondThreshold) 							;
	end
	% Solve the system for lambda, if the condition number is not too low
	if rcond(JohnsonMatrixA) > 1e-15
		lambda = JohnsonMatrixA \ JohnsonVectorb	;
	else
		if false	% Use this code to output information for analysis
			disp(['ERROR in routine ClosestPointInSimplex: ill-conditioned matrix:'])
			JohnsonMatrixA
			RConditionNumber = rcond(JohnsonMatrixA)
			ranktest = rank(SimplexVertices(reshape(SubSimplexIndices,length(SubSimplexIndices),1),:))	;
			lengthrank = [length(SubSimplexIndices),ranktest]
		end
		ErrorFlag = true	;
		return
	end
	
	% Save the lambda values for this subsimplex in the matrix lambdas.  Leave an entry
	% in lambdas as NaN if a particular generating vector is not contained in the
	% current subsimplex.  
	for ctr = 1:length(lambda)
		lambdas(SubSimplexCtr,SubSimplexIndices(ctr)) = lambda(ctr)	;
	end

	% The closest point in the affine hull of the subsimplex is the linear combination
	% of that subsimplex s vertices, using the coefficients in lambda
	ClosestPointInSubSimplex = zeros(1,DimensionOfSpace)	;
	for ctr = 1:length(lambda)
		ClosestPointInSubSimplex = ClosestPointInSubSimplex + ...
				(lambda(ctr) * TranslatedSimplexVertices(SubSimplexIndices(ctr),:))	;
	end
	% The closest point is in the convex hull of the subsimplex if and only if all the
	% coefficients in lambda are between 0 and 1.  Check this condition, and switch the
	% closest point to NaNs if it is not satisfied.
	if min(min(lambda)) < 0 || max(max(lambda)) > 1
		ClosestPointInSubSimplex = NaN * ones(1,DimensionOfSpace)	;
	end
	ClosestPointsInSubSimplices(SubSimplexCtr,:) = ClosestPointInSubSimplex			;
	
	% Record the minimum distance from the origin (the translation of Point) to the
	% subsimplex, using an NaN if the closest point is not in the convex hull of the
	% subsimplex s vertices
	MinDistancesToSubSimplices(SubSimplexCtr)    = norm(ClosestPointInSubSimplex)	;
end

% We are interested in the point on the simplex that is nearest to Point, so sort the
% distances from smallest to greatest.
[SortedDistances, SortedIndices] = sort(MinDistancesToSubSimplices)		;
% Numerical issues might cause multiple subsimplices, with different numbers of vertices,
% both to return the same minimum distance point.  In that case, select the subsimplex
% with the smallest number of vertices.  The following loop identifies all points, on any
% subsimplices, which give the minimum distance.
BestIndex        = SortedIndices(1)						;
BestDistance     = SortedDistances(1)					;
NumberOfVertices = length(PowerSetOfIndices{BestIndex})	;
SigDifFound = false	;
SubCtr      = 2		;
while ~SigDifFound && SubCtr <= size(MinDistancesToSubSimplices,1)
	NewIndex            = SortedIndices(SubCtr)								;
	NewDistance         = SortedDistances(SubCtr)							;
	NewNumberOfVertices = length(PowerSetOfIndices{SortedIndices(SubCtr)})	;
	% Check if the next distance is identical (to within negligible numerical error) to
	% the best distance.  If it is, then check whether the new distance comes from a
	% subsimplex with smaller vertices.  Update it if it does.
	if abs(NewDistance - BestDistance) < EpsDist
		if NewNumberOfVertices < NumberOfVertices	;
			BestIndex        = NewIndex				;
			NumberOfVertices = NewNumberOfVertices	;
		end
	else
		% The distances have been sorted, so once one distance is larger than the minimum
		% distance, the loop can terminate.
		SigDifFound = true	;
	end
	SubCtr = SubCtr + 1	;
end
% The point in the smallest subsimplex is returned, along with the indices of that 
% subsimplex.  The closest point is now translated back.
MinimumSubsetIndices = PowerSetOfIndices{BestIndex}						;		
ClosestPoint         = ClosestPointsInSubSimplices(BestIndex,:) + Point	;
Distance             = norm(ClosestPoint - Point)						;

% The lambdas are the coefficients of the generating vertices.  Return them to provide
% more information to the user.  If any lambda is NaN, then it is automatically 0,
% so it is not returned.
Coefficients     = lambdas(BestIndex,:) ;	
UsedCoefficients = []					;	
for ctr = 1:length(Coefficients)
	if ~isnan(Coefficients(ctr))
		UsedCoefficients = [UsedCoefficients, Coefficients(ctr)]	;
	end
end