% PLOT THE COLLISION EVENT DESCRIPTION FIGURES
function [currentFigure,figureSet] = GetFigure_objectCollision(SIM,objectIndex,DATA,currentFigure)
% This function is designed to move through the collsion event history and
% generate figure for each of the collisions.

figureSet = [];
% CHECK COLLISIONS OCCURED
if ~isfield(DATA,'uniqueCollisions') || isempty(DATA.uniqueCollisions)
    fprintf('[%s]\t...No collision data available.\n',SIM.phase);
    return
end

for collisionNumber = 1:numel(DATA.uniqueCollisions)
    [currentFigure,figureSet(collisionNumber)] = BuildObjectCollisionFigure(...
        SIM,...
        objectIndex,...
        DATA,...
        currentFigure,...
        DATA.uniqueCollisions(collisionNumber));
end

% ASSEMBLE TABBED FIGURE
windowHandle = GetTabbedFigure(figureSet,'OpenMAS Collision Overview');
set(windowHandle,'Position',DATA.figureProperties.windowSettings);        % Maximise the figure in the tab
savefig(windowHandle,[SIM.outputPath,'collision-overview']);              % Save the output figure
end

% Build the collision figure for the event
function [currentFigure,figureHandle] = BuildObjectCollisionFigure(SIM,objectIndex,DATA,currentFigure,collisionEvent)


% Generate the figure
figureHandle = figure('Name',sprintf('[t=%.0fs] %s & %s',collisionEvent.time,collisionEvent.name_A,collisionEvent.name_B));
set(figureHandle,'Position', DATA.figureProperties.windowSettings);        % [x y width height]
set(figureHandle,'Color',DATA.figureProperties.figureColor);               % Background colour
ax = axes(figureHandle);
hold on;
view([70 25]);
titlestr = sprintf('Collision Event between %s and %s at t=%s',...         % Declare title string for figure
           collisionEvent.name_A,collisionEvent.name_B,num2str(collisionEvent.time));

% /////////////////////////// ENTITY DATA /////////////////////////////////
% EVENT, OBJECT AND META DATA
META_A = SIM.OBJECTS(SIM.globalIDvector == collisionEvent.objectID_A);
META_B = SIM.OBJECTS(SIM.globalIDvector == collisionEvent.objectID_B);     % The associated META datas
object_A = objectIndex{SIM.globalIDvector == collisionEvent.objectID_A};
object_B = objectIndex{SIM.globalIDvector == collisionEvent.objectID_B};   % The associated object structures
geometry_A = object_A.GEOMETRY;
geometry_B = object_B.GEOMETRY;
[objectAStates] = OMAS_getTrajectoryData_mex(DATA.globalTrajectories,SIM.globalIDvector,META_A.objectID,inf);
[objectBStates] = OMAS_getTrajectoryData_mex(DATA.globalTrajectories,SIM.globalIDvector,META_B.objectID,inf);

% We need to plot:
% - The object geometries
% - The object hitboxes

% ////////////////////// PLOT THE OBJECT GEOMETRY /////////////////////////
% SUBSTITUTE A'S GEOMETRY IF NECESSARY
if size(object_A.GEOMETRY.vertices,1) < 1
    [geometry_A] = OMAS_graphics.defineSphere(zeros(3,1),META_A.radius,10);
end
% GET A'S ROTATIONS
R_A = OMAS_geometry.quaternionToRotationMatrix(collisionEvent.state_A(7:10)); 
% OBJECT A'S GEOMETRY
patch(ax,...
    'Vertices',(geometry_A.vertices*R_A + collisionEvent.state_A(1:3)'),...
    'Faces',geometry_A.faces,...
    'FaceColor',META_A.colour,...
    'EdgeColor','k',...
    'EdgeAlpha',DATA.figureProperties.edgeAlpha,...
    'FaceAlpha',DATA.figureProperties.faceAlpha,...
    'FaceLighting','gouraud',...
    'LineWidth',DATA.figureProperties.lineWidth);

% SUBSTITUTE B'S GEOMETRY IF NECESSARY
if size(object_B.GEOMETRY.vertices,1) < 1
    [geometry_B] = OMAS_graphics.defineSphere(zeros(3,1),META_B.radius,10); 
end
% GET B'S ROTATIONS
R_B = OMAS_geometry.quaternionToRotationMatrix(collisionEvent.state_B(7:10)); 
% OBJECT B'S GEOMETRY
patch(ax,...
    'Vertices',(geometry_B.vertices*R_B + collisionEvent.state_B(1:3)'),...
    'Faces',geometry_B.faces,...
    'FaceColor',META_B.colour,...
    'EdgeColor','k',...
    'EdgeAlpha',DATA.figureProperties.edgeAlpha,...
    'FaceAlpha',DATA.figureProperties.faceAlpha,...
    'FaceLighting','gouraud',...
    'LineWidth',DATA.figureProperties.lineWidth);

% ////////////////////// PLOT THE OBJECT'S HITBOX /////////////////////////
% Get the hit-box geometry
[hitBoxGeometry_A] = OMAS_graphics.getHitBoxGeometry(object_A.GetGLOBAL(),object_A.GEOMETRY);
% DEFINE A's HITBOX
patch(ax,...
    'Vertices',(hitBoxGeometry_A.vertices + collisionEvent.state_A(1:3)'),...
    'Faces',hitBoxGeometry_A.faces,...
    'FaceColor','r',...
    'EdgeColor','k',...
    'EdgeAlpha',DATA.figureProperties.edgeAlpha*0.8,...
    'FaceAlpha',DATA.figureProperties.faceAlpha*0.8,...
    'FaceLighting','gouraud',...
    'LineWidth',DATA.figureProperties.lineWidth);

% Get the hit-box geometry
[hitBoxGeometry_B] = OMAS_graphics.getHitBoxGeometry(object_B.GetGLOBAL(),object_B.GEOMETRY);
% DEFINE B's HITBOX
patch(ax,...
    'Vertices',(hitBoxGeometry_B.vertices + collisionEvent.state_B(1:3)'),...
    'Faces',hitBoxGeometry_B.faces,...
    'FaceColor','r',...
    'EdgeColor','k',...
    'EdgeAlpha',DATA.figureProperties.edgeAlpha*0.8,...
    'FaceAlpha',DATA.figureProperties.faceAlpha*0.8,...
    'FaceLighting','gouraud',...
    'LineWidth',DATA.figureProperties.lineWidth);

% ////////////////////// PLOT THE TRAJECTORY TRAILS ///////////////////////
plot3(ax,objectAStates(1,:),objectAStates(2,:),objectAStates(3,:),...
    'LineStyle',DATA.figureProperties.lineStyle,...
    'LineWidth',DATA.figureProperties.lineWidth,...
    'Color',META_A.colour);
plot3(ax,objectBStates(1,:),objectBStates(2,:),objectBStates(3,:),...
    'LineStyle',DATA.figureProperties.lineStyle,...
    'LineWidth',DATA.figureProperties.lineWidth,...
    'Color',META_B.colour);

% ////////////////////// PLOT THE VELOCITY VECTORS ////////////////////////
% PLOT THE OBJECT VELOCITY VECTORS
q = quiver3(collisionEvent.state_A(1),...
    collisionEvent.state_A(2),...
    collisionEvent.state_A(3),...
    collisionEvent.state_A(4),...
    collisionEvent.state_A(5),...
    collisionEvent.state_A(6),'r');
q.AutoScaleFactor = 1;
q.LineWidth = DATA.figureProperties.lineWidth;
q = quiver3(collisionEvent.state_B(1),...
    collisionEvent.state_B(2),...
    collisionEvent.state_B(3),...
    collisionEvent.state_B(4),...
    collisionEvent.state_B(5),...
    collisionEvent.state_B(6),'g');
q.AutoScaleFactor = 1;
q.LineWidth = DATA.figureProperties.lineWidth;

% ///////////////////////// ADD LABEL ANNOTATIONS /////////////////////////
annotationText = sprintf('    %s [ID-%d]',META_A.name,META_A.objectID);
text(collisionEvent.state_A(1),collisionEvent.state_A(2),collisionEvent.state_A(3),annotationText);
annotationText = sprintf('    %s [ID-%d]',META_B.name,META_B.objectID);
text(collisionEvent.state_B(1),collisionEvent.state_B(2),collisionEvent.state_B(3),annotationText);

% DEFINE THE PLOT LAYOUT
% Title
title(ax,titlestr,...
    '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');
% Z-Label
zlabel(ax,'z(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');
axisLimit = (DATA.figureProperties.objectMaximalRadii + DATA.figureProperties.maxAbsPosition);
xlim(ax,[-axisLimit,axisLimit]);
ylim(ax,[-axisLimit,axisLimit]);
zlim(ax,[-axisLimit,axisLimit]);
axis vis3d equal;    
grid on;  box on;
hold off;

% SAVE THE OUTPUT FIGURE
figurePath = [SIM.outputPath,...
    sprintf('collision-event-id-%d-%s-id-%d-%s.fig',...
    collisionEvent.objectID_A,collisionEvent.name_A,...
    collisionEvent.objectID_B,collisionEvent.name_B)];
    
savefig(figureHandle,figurePath);
% ITERATE PLOT
currentFigure = currentFigure + 1;
end
