function displayOrientedPoints2D(points, markerStyle, markerSize, colors)
%DISPLAYORIENTEDPOINTS2D Plot oriented 2D points.
%   DISPLAYORIENTEDPOINTS2D(POINTS, MARKERSTYLE, LINELENGTH, COLORS) plots
%   2D points together with their orientation.
%
%   POINTS is a Nx3 matrix: position (2), orientation angle in radians (1)
%   MARKERSTYLE can be 'points', 'hollowPoints', 'bars', or 'tees'
%   MARKERSIZE is the length of the markers displayed
%   COLORS is an optional Nx3 matrix, containing a RGB color for each point

%   Author: Damien Teney

% Assign default values to missing input parameters
if nargin < 2
  markerStyle = 'bars';
end
if nargin < 3
  markerSize = 3;
end

if nargin < 4 % No colors given
  color = getColor(0); % Set default color
else % Colors given
  assert(size(colors, 1) == size(points, 1)); % Check we have as many colors as points
  if size(unique(colors, 'rows'), 1) == 1 % Same color for all
    color = unique(colors, 'rows');
  else
    color = []; % Color will be different for each point
  end
end

switch markerStyle
  %------------------------------------------------------------------------
  case 'points'
    for i = 1:size(points, 1)
      % Get the color
      if isempty(color)
        color = colors(i, 1:3);
      end

      plot(points(i, 1), points(i, 2), ...
        'MarkerSize', markerSize, ...
        'MarkerEdgeColor', color, ...
        'MarkerFaceColor', color, ...
        'Marker', 'o');
    end
  %------------------------------------------------------------------------
  case 'hollowPoints'
    for i = 1:size(points, 1)
      % Get the color
      if isempty(color)
        color = colors(i, 1:3);
      end

      plot(points(i, 1), points(i, 2), ...
        'MarkerSize', markerSize, ...
        'MarkerEdgeColor', [.7 .7 .7], ...
        'MarkerFaceColor', color, ...
        'Marker', 'o');
    end
  %------------------------------------------------------------------------
  case 'bars'
    if ~isempty(color)
      % Faster version, but only works if same color for all points
      nPoints = size(points, 1);
      linePointsX = zeros(nPoints * 3, 1);
      linePointsY = zeros(nPoints * 3, 1);
      linePointsX(1:3:end) = points(:, 1) - markerSize * cos(points(:, 3)); % Line starts
      linePointsY(1:3:end) = points(:, 2) - markerSize * sin(points(:, 3));
      linePointsX(2:3:end) = points(:, 1) + markerSize * cos(points(:, 3)); % Line ends
      linePointsY(2:3:end) = points(:, 2) + markerSize * sin(points(:, 3));
      linePointsX(3:3:end) = NaN;
      linePointsY(3:3:end) = NaN;
      line(linePointsX, linePointsY, 'Color', color, ...
                   'Marker', 'none', ...
                   'LineStyle', '-', ...
                   'LineWidth', 1.2);
    else
      for i = 1:size(points, 1)
        % Get the color
        color = colors(i, 1:3);

        % Rename for clarity
        point = points(i, 1:2);
        orientation = points(i, 3); % Orientation (angle in radians)

        lineStart = point - markerSize * [cos(orientation) sin(orientation)];
        lineEnd = point + markerSize * [cos(orientation) sin(orientation)];

        plot([lineStart(1) lineEnd(1)], ...
             [lineStart(2) lineEnd(2)], ...
             'Color', color, ...
             'Marker', 'none', ...
             'LineStyle', '-', ...
             'LineWidth', 1.2);
      end
    end
  %------------------------------------------------------------------------
  case 'tees'
    for i = 1:size(points, 1)
      % Get the color
      if isempty(color)
        color = colors(i, 1:3);
      end

      % Rename for clarity
      point = points(i, 1:2);
      orientation = points(i, 3); % Orientation (angle in radians)

      lineStart = point - markerSize * [cos(orientation) sin(orientation)];
      lineEnd = point + markerSize * [cos(orientation) sin(orientation)];

      plot([lineStart(1) lineEnd(1)], ...
           [lineStart(2) lineEnd(2)], ...
           'Color', color, ...
           'Marker', 'none', ...
           'LineStyle', '-', ...
           'LineWidth', 1.2);

      % Add the "bottom" of the T to show the direction of the point (the orientation is not modulo pi, but (2 * pi))
      % Rename for clarity
      point = points(i, 1:2);
      orientation = points(i, 3); % Orientation (angle in radians)

      lineStart = point;
      lineEnd = point + 0.75 * markerSize * [cos(orientation + pi/2) sin(orientation + pi/2)];

      plot([lineStart(1) lineEnd(1)], ...
           [lineStart(2) lineEnd(2)], ...
           'Color', color, ...
           'Marker', 'none', ...
           'LineStyle', '-', ...
           'LineWidth', 1.2);
    end
  %------------------------------------------------------------------------
  otherwise
    error('Unknown markerStyle.');
  %------------------------------------------------------------------------
end
