function result = getAngleDifference(angle1, angle2, unsigned)
%GETANGLEDIFFERENCE Difference between two angles.
%   A = GETANGLEDIFFERENCE(ANGLE1, ANGLE2, UNSIGNED) computes the shortest
%   (signed by default) difference from ANGLE1 to ANGLE2.
%
%   Input angles must be in [0, 2 * pi[.
%   The returned difference is in [-pi, pi[ (signed) or [0, pi[ (unsigned).
%
%   Also works with vectors of angles (of the same size).

%   Author: Damien Teney

% Check parameters in case they are vectors
angle1 = angle1(:);
angle2 = angle2(:);
assert(length(angle1) == length(angle2));

% Make sure the input angles are in the correct range
assert(all(angle1 >= 0));
assert(all(angle2 >= 0));
assert(all(angle1 < 2 * pi));
assert(all(angle2 < 2 * pi));

nAngles = length(angle1);

if nargin > 2 && unsigned
  % Unsigned difference
  tmp = abs(angle2 - angle1);
  result = min(tmp, 2 * pi - tmp);
else
  % Signed difference (default case)
  tmp = angle2 - angle1;
  valuesToCompare = [(tmp) (tmp + 2*pi) (tmp - 2*pi)];
  [minValue minValueIndex] = min(abs(valuesToCompare'));
  result = diag(valuesToCompare(1:nAngles, minValueIndex));
  %result = mod(result, 2 * pi);
end
