function [centers, index, options, post, errlog] = sp_kmeans(centers, data, options)
% KMEANS	Trains a k means cluster model.
% Adapted from Netlab neural network software:
% http://www.ncrg.aston.ac.uk/netlab/index.php
%
%	Description
%	 CENTRES = KMEANS(CENTRES, DATA, OPTIONS) uses the batch K-means
%	algorithm to set the centres of a cluster model. The matrix DATA
%	represents the data which is being clustered, with each row
%	corresponding to a vector. The sum of squares error function is used.
%	The point at which a local minimum is achieved is returned as
%	CENTRES.  The error value at that point is returned in OPTIONS(8).
%
%	[CENTRES, OPTIONS, POST, ERRLOG] = KMEANS(CENTRES, DATA, OPTIONS)
%	also returns the cluster number (in a one-of-N encoding) for each
%	data point in POST and a log of the error values after each cycle in
%	ERRLOG.    The optional parameters have the following
%	interpretations.
%
%	OPTIONS(1) is set to 1 to display error values; also logs error
%	values in the return argument ERRLOG. If OPTIONS(1) is set to 0, then
%	only warning messages are displayed.  If OPTIONS(1) is -1, then
%	nothing is displayed.
%
%	OPTIONS(2) is a measure of the absolute precision required for the
%	value of CENTRES at the solution.  If the absolute difference between
%	the values of CENTRES between two successive steps is less than
%	OPTIONS(2), then this condition is satisfied.
%
%	OPTIONS(3) is a measure of the precision required of the error
%	function at the solution.  If the absolute difference between the
%	error functions between two successive steps is less than OPTIONS(3),
%	then this condition is satisfied. Both this and the previous
%	condition must be satisfied for termination.
%
%	OPTIONS(14) is the maximum number of iterations; default 100.
%
%	Copyright (c) Ian T Nabney (1996-2001)

[ndata, data_dim] = size(data);
[ncentres, dim] = size(centers);

if dim ~= data_dim
  error('Data dimension does not match dimension of centres')
end

if (ncentres > ndata)
  error('More centres than data')
end

% Sort out the options
if (options(14))
  niters = options(14);
else
  niters = 100;
end

store = 0;
if (nargout > 3)
  store = 1;
  errlog = zeros(1, niters);
end

% Check if centres and posteriors need to be initialised from data
if (options(5) == 1)
  % Do the initialisation
  perm = randperm(ndata);
  perm = perm(1:ncentres);

  % Assign first ncentres (permuted) data points as centres
  centers = data(perm, :);
end

 %%%%%%%%%%%%%%%
% Matrix to make unit vectors easy to construct
% id = eye(ncentres);

% Main loop of algorithm
for n = 1:niters

  % Save old centres to check for termination
  old_centres = centers;
  
%   % Calculate posteriors based on existing centres
%   d2 = sp_dist2(data, centres);
%   % Assign each point to nearest centre
%   [minvals, index] = min(d2', [], 1);   d2 = [];
%   post = id(index,:);
% 
%   num_points = sum(post, 1);
%   % Adjust the centres based on new posteriors
%   for j = 1:ncentres
%     if (num_points(j) > 0)
%       centres(j,:) = sum(data(find(post(:,j)),:), 1)/num_points(j);
%     end
%   end

  %%%%%%%%%%%%%%%
  %[minvals, index] = pdist2(centres, data, 'euclidean','Smallest',1) ;
  [minvals, index] = lph_pdist2(centers, data);
  for j = 1:ncentres
      curr_idx   = find(index ==j);
      curr_num = length(curr_idx);
      if curr_num > 0
          centers(j, :) = sum(data(curr_idx, :), 1) / curr_num;
      end
  end
  %%%%%%%%%%%%%%%

  % Error value is total squared distance from cluster centres
  e = sum(minvals);
  if store
    errlog(n) = e;
  end
  if options(1) > 0
    fprintf(1, 'Cycle %4d  Error %11.6f\n', n, e);
  end

  if n > 1
    % Test for termination
    if max(max(abs(centers - old_centres))) < options(2) & ...
        abs(old_e - e) < options(3)
      options(8) = e;
      return;
    end
  end
  old_e = e;
end

% If we get here, then we haven't terminated in the given number of 
% iterations.
options(8) = e;
if (options(1) >= 0)
  disp('Warning: Maximum number of iterations has been exceeded');
end

end

function [minvals, index] = lph_pdist2(centres, data) 

    num_per_iter = 50000;
    num_iter = size(data, 1) / num_per_iter;

    minvals = zeros(size(data, 1), 1, 'single');
    index    = zeros(size(data, 1), 1, 'single');
    for i = 1:num_iter
        start2end = (i-1)*num_per_iter +1: i * num_per_iter;
        dist2 = sp_dist2(centres, data(start2end, :));
        [Y, I] = sort(dist2, 1, 'ascend');
        minvals(start2end) = Y(1, :);
        index(start2end) = I(1, :);
    end

end
 
 function n2 = sp_dist2(x, c)
% DIST2	Calculates squared distance between two sets of points.
% Adapted from Netlab neural network software:
% http://www.ncrg.aston.ac.uk/netlab/index.php
%
%	Description
%	D = DIST2(X, C) takes two matrices of vectors and calculates the
%	squared Euclidean distance between them.  Both matrices must be of
%	the same column dimension.  If X has M rows and N columns, and C has
%	L rows and N columns, then the result has M rows and L columns.  The
%	I, Jth entry is the  squared distance from the Ith row of X to the
%	Jth row of C.
%
%
%	Copyright (c) Ian T Nabney (1996-2001)

[ndata, dimx] = size(x);
[ncentres, dimc] = size(c);
if dimx ~= dimc
	error('Data dimension does not match dimension of centres')
end

n2 = (ones(ncentres, 1) * sum((x.^2)', 1))' + ...
  ones(ndata, 1) * sum((c.^2)',1) - ...
  2.*(x*(c'));

% Rounding errors occasionally cause negative entries in n2
if any(any(n2<0))
  n2(n2<0) = 0;
end

end
