classdef HelperTrackMetrics < matlab.System & ...
        matlabshared.tracking.internal.SimulinkBusUtilities
    % Copyright 2019 The MathWorks, Inc.
    
    %#codegen
    properties(Nontunable, Logical)
        %HasTracks Enable tracks data
        HasTracks = false
        
        %HasActors Enable actor info
        HasActors = false
    end
    
    properties(Constant, Access=protected)
        pBusPrefix = 'TrackAssignment'
    end
    
    properties(Access = protected, Nontunable)
        TrackAssign
        Tracks
        ActProfiles
    end
    
    methods
        function obj = HelperTrackMetrics(varargin)
            % Constructor
            setProperties(obj,nargin,varargin{:});
        end
    end
    
    methods(Access=protected)
        function setupImpl(obj,varargin)
            % Function to return the distance between track and truth
            distFcn = @(track,truth)helperTargetDistance(track,truth);
            
            % Function to return the IDs from the ground truth. The default
            % identifier assumes that the truth is identified with PlatformID. In
            % drivingScenario, truth is identified with an ActorID.
            truthIdFcn = @(x)[x.ActorID];
            
            obj.TrackAssign = trackAssignmentMetrics(...
                'DistanceFunctionFormat','custom',...
                'AssignmentDistanceFcn',distFcn,...
                'DivergenceDistanceFcn',distFcn,...
                'TruthIdentifierFcn',truthIdFcn,...
                'AssignmentThreshold',5,...
                'DivergenceThreshold',8);
        end
        
        function assignment = stepImpl(obj,varargin)
            trackStruct = varargin{1};
            if isequal(trackStruct.NumTracks,0)
                obj.Tracks = repmat(trackStruct.Tracks,[0,1]);
            else
                obj.Tracks = trackStruct.Tracks(1:trackStruct.NumTracks);
            end
            
            % Obtain ground truth
            groundTruth = varargin{2}.Actors;
            
            % Update track assignment metrics
            obj.TrackAssign(obj.Tracks, groundTruth);
            
            % Update track metrics
            assignmentTable = obj.TrackAssign.trackMetricsTable;
            [TT, FT, RT] = helperFindAssignmentMetrics(assignmentTable);
            assignmentRes = convertToStruct(TT, FT, RT);
            assignment = sendToBus(obj,assignmentRes);
        end
        
        function status = isEndOfData(obj)
            % Return true if end of data has been reached
            status = (obj.pCurrentIndex == obj.pMaxIndex);
        end
        
        function s = saveObjectImpl(obj)
            s = saveObjectImpl@matlab.System(obj);
        end
        
        function loadObjectImpl(obj,s,wasLocked)
            loadObjectImpl@matlab.System(obj,s,wasLocked);
        end
    end
    
    % Simulink interface
    methods(Access=protected)
        function [out, argsToBus] = defaultOutput(obj)  %#ok<*MANU>
            out = struct('TargetTracks', 0, 'FalseTracks',0,...
                'RedundantTracks',0);
            argsToBus = {out};
        end
        % Currently only support simulink environment, so nothing to do
        % here
        function y = sendToBus(~,x,varargin)
            y = x;
        end
        
        function str = getIconImpl(~)
            str = sprintf('Helper Track\nAssignment\nMetrics');
        end
        
        function num = getNumInputsImpl(obj)
            num = 0;
            if obj.HasTracks
                num = num+1;
            end
            if obj.HasActors
                num = num+1;
            end
        end
        
        function num = getNumOutputsImpl(obj)
            num = 1;
        end
        
        function out = getOutputNamesImpl(obj)
            out = sprintf('Results');
        end
        
        function varargout = getOutputDataTypeImpl(obj)
            varargout{1} = getOutputDataTypeImpl@matlabshared.tracking.internal.SimulinkBusUtilities(obj);
        end
        
        function [out1] = isOutputFixedSizeImpl(obj)
            out1 = true;
        end
        
        function [out1] = getOutputSizeImpl(obj)
            out1 = [1 1];
        end
        
        function [out1] = isOutputComplexImpl(obj)
            out1 = false;
        end
        
        function varargout = getInputNamesImpl(obj)
            varargout = {};
            if obj.HasTracks
                varargout = {varargout{:} 'Tracks'};
            end
            if obj.HasActors
                varargout = {varargout{:} 'Actors'};
            end
        end
    end
    
    methods(Access = protected, Static)
        function header = getHeaderImpl
            % Define header panel for System block dialog
            header = matlab.system.display.Header(...
                'Title', 'TrackAssignmentMetrics',...
                'Text', getHeaderText());
        end
        
        function simMode = getSimulateUsingImpl
            % Return only allowed simulation mode in System block dialog
            simMode = 'Interpreted execution';
        end
        
        function flag = showSimulateUsingImpl
            % Return false if simulation mode hidden in System block dialog
            flag = false;
        end
        
        function groups = getPropertyGroupsImpl
            % Define property section(s) for System block dialog, only in
            % Simulink
            
            inputSection = matlab.system.display.Section(...
                'Title','Enable Inputs',...
                'PropertyList',{'HasTracks','HasActors'});
            slBusSection = getPropertyGroupsImpl@matlabshared.tracking.internal.SimulinkBusUtilities;
            groups = [inputSection slBusSection];
        end
        
    end
end

function str = getHeaderText
str = sprintf('TrackAssignmentMetrics compares tracks from a multi-object tracking \n system against known truth by automatic assignment of tracks \n to the known truths at each track update.');
end

function dist = helperTargetDistance(track,truth)
% This function computes the distance between track and a truth.

% Copyright 2019 The MathWorks, Inc.

% Errors in each aspect
[posError,velError] =  helperTargetError(track,truth);

% Distance is the sum of errors
dist = posError + velError;

end

function [posError,velError] = helperTargetError(track,truth)
% Errors as a function of target track and associated truth.
% Get true information from the ground truth.
truePos = truth.Position(1:2)';

% Position is at the rear axle for all vehicles. We would like to compute
% the error from the center of the vehicle
rot = [cosd(truth.Yaw) -sind(truth.Yaw);sind(truth.Yaw) cosd(truth.Yaw)];
truePos = truePos + rot*[2.8/2;0];

trueVel = truth.Velocity(1:2);

estPos = track.State([1 3]);
estVel = track.State([2 4]);

% Compute 2-norm of error for each attribute.
posError = norm(truePos(:) - estPos(:));
velError = norm(trueVel(:) - estVel(:));
end

function [TT, FT, RT] = helperFindAssignmentMetrics(targetMetrics)

% Extract data
trackerTgt = ~isnan(targetMetrics.AssignedTruthID);

TT = sum(trackerTgt);

trackerRedundant = targetMetrics.RedundancyStatus | (~targetMetrics.Surviving & targetMetrics.RedundancyCount > 0);

RT = sum(trackerRedundant);

trackerFalse = ~(trackerTgt | trackerRedundant);

FT = sum(trackerFalse);
end

function out = convertToStruct(tt, ft, rt)
out = struct('TargetTracks', tt, 'FalseTracks',ft, 'RedundantTracks',rt);
end