function varargout = fixPosition(A, inputConstraintMatrix, zerotol)
% calculate constraints to fix the position of the polytope
%
% Syntax: constraintMatrix = fixPosition(A)
%
% The position of polytopes is non-relevant for the topology (existence of
%   vertices, edges or facets). Hence, all algorithms connected to
%   Minkowski decomposition or the 'morphological' decomposition of the
%   state space require the position of each valid polytope to be fixed.
%   Whenever this is not the case, additional constraints are employed
%   without loss of generality.
%
% This function fixes certain facets to a facet distance of 0 so that:
%     constraintMatrix * h = 0
%   ensures a fixed position of the polytope.
%
% fixPosition(A, originalConstraintMatrix) searches for additional 
%   constraints to fix the position of the polytope. Only the additional 
%   constaints are returned. The condition above is then:
%     [constraintMatrix; originalConstraintMatrix] * h = 0
%
% With [~, mappingMatrix] = fixPosition(A), a mappingMatrix is returned
%   that allows to move a given polytope (vector h) such that the new
%   polytope (vector hNew) fulfills the fixed position constraint. Hence,
%   the code for adopting a given polytope is:
%     [constraintMatrix, mappingMatrix] = fixPosition(hrep.A);
%     hrepPositionFixed = moveHrep(hrep, mappingMatrix * hrep.h);
%   It then holds:
%     constraintMatrix * hrepPositionFixed.h = 0
%
% fixPosition(.., zerotol) sets the tolerance for underlying algorithms. 
%   Default: elkZerotol
%
% See also: obtainDec, decomposeMinkowski

% The elk-library: convex geometry applied to crystallization modeling.
%   Copyright (C) 2012 Alexander Reinhold
%
% This program 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.
%
% This program 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 this program.  If not, see <http://www.gnu.org/licenses/>.

nFacet = size(A, 1);
nDim = size(A, 2);

if ~exist('inputConstraintMatrix', 'var')
    inputConstraintMatrix = zeros(0, nFacet);
end
if ~exist('zerotol', 'var')
    zerotol = elkZerotol;
end

%% subsequently increase the rank of Mc*A
% ..by fixing selected facet distances to zero.

thisRank = rank(inputConstraintMatrix * A, zerotol);
positionConstraintMatrix = zeros(nFacet);
while thisRank < nDim
    
    % singular value decomposition
    [U S V] = svd([inputConstraintMatrix; positionConstraintMatrix] * A);
    S(isZero(S(:), zerotol)) = 0;
    thisRank = sum(S(:) ~= 0);
    
    % check rank
    if thisRank == nDim
        break;
    end
    
    % in which direction can I move?
    thisDirection = V(:, end);
    
    % which facet normals point into this direction?
    facetNormalDotProduct = A * thisDirection;
    if isAllZero(facetNormalDotProduct, zerotol)
        error('elk:decomposition:numericalProblem', ...
            'Failed to fix position. This should not happen.');
    end
    
    % take one of them as new constraint
    thisFacetIndex = find(...
        abs(facetNormalDotProduct) == max(abs(facetNormalDotProduct)), ...
        1, 'first');
    
    % add constraint
    positionConstraintMatrix(thisRank + 1, thisFacetIndex) = 1;
    
end
varargout{1} = positionConstraintMatrix;

%% Offset generation
% Now the polytope must be moved to fulfill that additional constraint.
%   This means we search an x0 with: Mc * (h + A*x0) = 0, which translates
%   to: Mc*A*x0 = -Mc*h. This can be solved by: x0 = (Mc*A)\(-Mc*h).
%   However, the output of this will be a matrix that transforms directly
%   from h to x0, which is: -1*pinv(Mc*A)*Mc.
             
if nargout == 2
    varargout{2} = -1 * pinv(positionConstraintMatrix * A) * ...
        positionConstraintMatrix;
end

