% GET THE STANDARD 2D TRAJECTORY
function [currentFigure,figureHandle] = GetFigure_plan(SIM,objectIndex,DATA,currentFigure)

% CONFIGURE THE PLOT ATTRIBUTES
figureHandle = figure('Name','OpenMAS top-down image');
set(figureHandle,'Position',DATA.figureProperties.windowSettings);         % [x y width height]
set(figureHandle,'Color',DATA.figureProperties.figureColor);               % Background colour 
ax = axes(figureHandle);
% setappdata(figureHandle, 'SubplotDefaultAxesLocation', [0.08, 0.08, 0.90, 0.88]); % MAXIMISE GRAPH SIZE IN WINDOW

legendCounter = 1; legendEntries = cell(DATA.totalObjects,1);

hold on; grid on; box on; grid minor;
for ID1 = 1:DATA.totalObjects    
    % GET OBJECT OVERVIEW DATA
    legendString = sprintf('[ID-%s] %s',num2str(SIM.OBJECTS(ID1).objectID),SIM.OBJECTS(ID1).name);
    legendEntries(legendCounter) = {legendString};
    % THE OBJECT HANDLE
    objectHandle = objectIndex{SIM.OBJECTS(ID1).objectID == SIM.globalIDvector};
    % EXTRACT FINAL POSITION DATA FROM THE TRAJECTORY MATRIX
    [finalStates] = OMAS_getTrajectoryData_mex(...
        DATA.globalTrajectories,...
        SIM.globalIDvector,...
        SIM.OBJECTS(ID1).objectID,...
        SIM.TIME.endStep);
    finalPosition = finalStates(1:3,:);
    % LOCAL FIXED TO GLOBAL ROTATED
    R_final = OMAS_geometry.quaternionToRotationMatrix(finalStates(7:10)); 
    % DISPLAY THE OBJECT
    if numel(objectHandle.GEOMETRY.vertices) > 0
        patch(ax,'Vertices',objectHandle.GEOMETRY.vertices*R_final + finalPosition',...
            'Faces',objectHandle.GEOMETRY.faces,...
            'FaceColor',SIM.OBJECTS(ID1).colour,...
            'EdgeColor',DATA.figureProperties.edgeColor,...
            'EdgeAlpha',DATA.figureProperties.edgeAlpha,...  
            'FaceLighting',DATA.figureProperties.faceLighting,...
            'FaceAlpha',DATA.figureProperties.faceAlpha,...
            'LineWidth',DATA.figureProperties.patchLineWidth);             % Patch properties
    else
        % PLOT THE TERMINAL POSITIONS
        plot3(ax,finalPosition(1),finalPosition(2),finalPosition(3),...
              'Marker',SIM.OBJECTS(ID1).symbol,...
              'MarkerSize',DATA.figureProperties.markerSize,...
              'MarkerFaceColor',SIM.OBJECTS(ID1).colour,...
              'MarkerEdgeColor',DATA.figureProperties.markerEdgeColor,...
              'LineWidth',DATA.figureProperties.lineWidth,...
              'LineStyle',DATA.figureProperties.lineStyle,...
              'Color',SIM.OBJECTS(ID1).colour); 
    end  
    legendCounter = legendCounter + 1;
end
legend('off')
hold on;
for ID1 = 1:DATA.totalObjects 
    % EXTRACT TIME-STATE DATA FROM THE TRAJECTORY MATRIX
    idleFlag = NaN('double');
    [objectStates] = OMAS_getTrajectoryData_mex(DATA.globalTrajectories,SIM.globalIDvector,SIM.OBJECTS(ID1).objectID,idleFlag);
    positions = objectStates(1:3,:);
    plot3(positions(1,:),positions(2,:),positions(3,:),...
          'LineStyle',DATA.figureProperties.lineStyle,...
          'LineWidth',DATA.figureProperties.lineWidth,...
          'Color',SIM.OBJECTS(ID1).colour);
      
    % ADD THE INTIAL POSITION REFERENCE POSITION 
    if SIM.OBJECTS(ID1).type == OMAS_objectType.agent
        initialPlotPosition = positions(1:2,1) - SIM.OBJECTS(ID1).radius; % The first position of the object
        rectangle('Position',[initialPlotPosition(1) initialPlotPosition(2) (2*SIM.OBJECTS(ID1).radius) (2*SIM.OBJECTS(ID1).radius)],...
                  'Curvature',[1 1],...
                  'FaceColor',SIM.OBJECTS(ID1).colour,...
                  'EdgeColor',DATA.figureProperties.edgeColor,...
                  'LineWidth',DATA.figureProperties.patchLineWidth)  
    end  
end

% Title
title(ax,...
    sprintf('Object trajectories over a period of %ss',num2str(SIM.TIME.endTime)),...
    'Interpreter',DATA.figureProperties.interpreter,...
    'fontname',DATA.figureProperties.fontName,...
    'Fontweight',DATA.figureProperties.fontWeight,...
    'FontSize',DATA.figureProperties.titleFontSize,...
    'FontSmoothing','on');
% X-Label
xlabel(ax,'x(m)',...
    'Interpreter',DATA.figureProperties.interpreter,...
    'fontname',DATA.figureProperties.fontName,...
    'Fontweight',DATA.figureProperties.fontWeight,...
    'FontSize',DATA.figureProperties.axisFontSize,...
    'FontSmoothing','on');
% Y-Label
ylabel(ax,'y(m)',...
    'Interpreter',DATA.figureProperties.interpreter,...
    'fontname',DATA.figureProperties.fontName,...
    'Fontweight',DATA.figureProperties.fontWeight,...
    'FontSize',DATA.figureProperties.axisFontSize,...
    'FontSmoothing','on');
% Axes
set(ax,...
    'TickLabelInterpreter',DATA.figureProperties.interpreter,...
    'fontname',DATA.figureProperties.fontName,...
    'Fontweight',DATA.figureProperties.fontWeight,...
    'FontSize',DATA.figureProperties.axisFontSize,...
    'FontSmoothing','on',...
    'Color',DATA.figureProperties.axesColor,...
    'GridLineStyle','--',...
    'GridAlpha',0.25,...
    'GridColor','k');
% Legend
legend(legendEntries,...
    'location','northeast',...
    'interpreter',DATA.figureProperties.interpreter,...
    'fontname',DATA.figureProperties.fontName);
axis square equal;

% axis square;     
view([0 90]);
% set(ax,'outerposition',[0.05 0.15 1 0.68]);                               % Set the axes offset position in the figure window
hold off;

figurePath = strcat(SIM.outputPath,'plan');
% SAVE THE OUTPUT FIGURE
savefig(figureHandle,figurePath);   

% Publish as .pdf if requested
if DATA.figureProperties.publish
	GetFigurePDF(figureHandle,figurePath);   
end

% FIGURE COMPLETE
DATA.figureProperties.alignment = DATA.figureProperties.alignment + DATA.figureProperties.spacing;
currentFigure = currentFigure + 1;
end