function [cost,accumCost] = exampleHelperComputeTrajectoryCost(Q)
%exampleHelperComputeTrajectoryCost Calculates cost of a trajectory

% Copyright 2021 The MathWorks Inc.

    % Calculate incremental distances
    trajDiff = diff(Q(:,[1 2 4]),1);
    
    % Check whether motions were forward or reverse
    motionMask = double(Q(1:end-1,5) < 0);
    
    % Calculate cost across trajectory
    trajCost = (trajDiff.*[repmat(10,size(trajDiff,1),2) 10^2*motionMask]).^2;
    if nargout == 2
        accumCost = [0;cumsum(sum(trajCost,2))];
        cost = accumCost(end);
    else
        cost = sum(trajCost,'all');
    end
end