function [y,exitCode]=geometricMedian(x,eta,AbsTol,RelTol,maxIter,test4Duplicates)
%%GEOMETRICMEDIAN Compute the weighted geometric median of the (possibly
%          multivariate) points x. This finds the vector y that minimizes
%          sum_i eta(i)*norm(y-x(:,i),1) where all eta(i)>0.
%          The geometric median is also known as the L1-median and the
%          spatial median. 
%
%INPUTS: x The xDimXN set of N real vectors whose geometric median is
%          desired.
%      eta The NX1 or 1XN set of all positive weights. If this parameter is
%          omitted or an empty matrix is passed, then uniform weights are
%          used.
% AbsTol, RelTol Absolute and relative tolerances for determining
%          convergence. If diff is the difference between the estimate at
%          the current and the previous iteration, then convergence is
%          declared if diff<AbsTol||diff<RelTol*norm(y). These same
%          tolerances are used in a similar manner to determine whether y
%          equals any of the points in x during an iterative update and
%          whether any of the vectors in x are duplicates of each other.
%          The defaults if omitted or empty matrices are passed are
%          RelTol=1e-9, AbsTol=1e-12.
%  maxIter The maximum number of iterations to perform. The default if
%          ommitted or an empty matrix is passed is max(1000,20*xDim).
% test4Duplicates The algorithms assume that none of the vectors in x are
%          duplicated of each other. test4Duplicates can be set to false if
%          it is known that there are no duplicated vectors in x. The
%          default if omitted or an empty matrix is passed is true.
%
%OUTPUTS: y The xDimX1 geometric median of the vectors in x.
%  exitCode A value indicating how the iterative algorithm terminated.
%           Possible values are:
%           0 The algorithm converged.
%           1 The algorithm did not convergence withing maxiter iteration.
%
%This function uses the modified Weiszfeld algorithm, which is described in
%[1]. The algorithm is inistialized with the Convergence is linear. A
%quasi-Newton algorithm would presumably be better for very large problems.
%
%EXAMPLE:
%Here, we have a set of data with an outlier. one will see that the mean is
%pulled towards the outlier, whereas the geometric median is more robust.
% x=[-26,   5,   1,  -2,  13,  -1,   0,   3,  -2, -12,  120;
%      0,  12,  12,  19,  -8, -16,  -5,   3,   4,   6,  100];
% yGM=geometricMedian(x);
% yM=mean(x,2);
% costGM=sum(sqrt(sum((bsxfun(@minus,yGM,x)).^2,1)))
% costM=sum(sqrt(sum((bsxfun(@minus,yM,x)).^2,1)))
% figure(1)
% clf
% hold on
% scatter(x(1,:),x(2,:),'ok')
% scatter(yGM(1),yGM(2),'r','filled')
% scatter(yM(1),yM(2),'b','filled')
% legend('Points','Geometric Median','Mean')
% h1=xlabel('x');
% h2=ylabel('y');
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
%
%REFERENCES:
%[1] Y. Vardi and C.-H. Zhang, "The multivariate l1-median and associated
%    data depth," Proceedings of the National Academy of Sciences of the
%    United States of America, vol. 97, no. 4, pp. 1423-1426, 23 Feb. 2000.
%
%August 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

n=size(x,2);
xDim=size(x,1);

if(nargin<6||isempty(test4Duplicates))
    test4Duplicates=true;
end

if(nargin<5||isempty(maxIter))
    maxIter=max(1000,20*xDim);
end

if(nargin<4||isempty(RelTol))
    RelTol=1e-9;
end

if(nargin<3||isempty(AbsTol))
    AbsTol=1e-12;
end

if(nargin<2||isempty(eta))
    eta=ones(1,n);
end

%Get rid of duplicate values and adjust the weights.
if(test4Duplicates)
    xUnique=zeros(xDim,n);
    xUnique(:,1)=x(:,1);
    etaUnique=zeros(1,n);
    etaUnique(1)=eta(1);
    numUnique=1;
    
    xMag=sqrt(sum(x.*x,1));
    for curX=2:n

        d=x(:,curX)-xUnique(:,1:numUnique);
        dMag=sqrt(sum(d.^2,1));
        idx=find(dMag<AbsTol|dMag<RelTol*xMag(curX),1);
        
        if(isempty(idx))%It is unique.
          numUnique=numUnique+1;
          xUnique(:,numUnique)=x(:,curX);
          etaUnique(numUnique)=eta(curX);
        else%It is not unique.
            etaUnique(idx)=etaUnique(idx)+eta(curX);
        end
    end
    
    x=xUnique;
    eta=etaUnique;
    n=numUnique;
end

%The special case when given a single value.
if(n==1)
  y=x;
  exitCode=0;
  return
end

eta=eta(:).';
xMag=sqrt(sum(x.*x,1));%Magnitude

%Start with the weighted mean as the initial estimate.
y=sum(bsxfun(@times,(eta/sum(eta)),x),2);

exitCode=1;
for curIter=1:maxIter
  yPrev=y;

  %Duplicates were removed (weights combined) prior to beginning iterations, so
  %there should be at most 1 point coincident with x. We will use absolute and
  %relative tolerances to determine equality.
  d=bsxfun(@minus,y,x);
  dMag=sqrt(sum(d.^2,1));
  idx=find(dMag<AbsTol|dMag<RelTol*xMag,1);

  if(isempty(idx))
    %If no points coincide with x, then use Weiszfeld's original iteration.
    
    %Defined after Equation 2.3
    w=eta./dMag;
    w=w/sum(w);
    
    %Equation 2,3
    y=sum(bsxfun(@times,w,x),2);
  else
    %If any point in x coincides with y, then use the modified Weiszfeld
    %iteration.
    
    %Equation 2,5. The only nonzero weight is the one  where the values
    %coincide.
    etay=eta(idx); 
  
    %w as in Equation 23, but not normalized.
    w=eta./dMag;
    w(idx)=0;
    %The normalization constant.
    wNorm=sum(w);
    
    %Equation 2,4
    Ty=sum(bsxfun(@times,(w/wNorm),x),2);
    
    %Equation 2,7
    ry=norm(sum(bsxfun(@times,w,-d),2),1);
    
    %Equation 2.6
    y=max(0,(1-etay/ry))*Ty+min(1,etay/ry)*y;
  end
  
  yNorm=norm(y);
  diff=norm(y-yPrev);
  
  if(diff<AbsTol||diff<RelTol*yNorm)
      exitCode=0;
      break;
  end
end

end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.
