function [ detections, ground ] = rok_evaluation_assign_detections (detections, ground, threshold, identity)
    % [ detections, ground ] = ROK_EVALUATION_ASSIGN_DETECTIONS (detections, ground, threshold, identity)
    %
    % Find optimal assignment between given detections and ground truth
    % points.
    %
    % Input:
    %  - detections = array of structures describing detections. For each
    %     detection, it should contain at least fields 'x' and 'y', with
    %     detection's coordinates. Optionally, 'id' field can be contained
    %     as well
    %  - ground = array of structures describing ground truth points.
    %     Structure is same as for detections.
    %  - threshold = threshold that determines whether assignment is valid
    %     or not (default: Inf)
    %  - identity = whether to account for identities of detections and
    %     ground truth points (i.e. allow only assignments with same
    %     identities). This requires both detections and ground truth
    %     structures to contain field 'id'.
    %
    % Output:
    %   This function returns the input arrays fo detections and ground,
    %   with fields 'assigned' and 'distance' added, describing the
    %   assignment. If detection/ground truth point is left unassigned, the
    %   fields are left empty.
    %
    % Copyright (C) 2012, Rok Mandeljc <rok.mandeljc@fe.uni-lj.si>

    if ~exist('threshold', 'var') || isempty(threshold),
        threshold = Inf;
    end
    
    if ~exist('identity', 'var') || isempty(identity),
        identity = false;
    end
    
    
    D = numel(detections);
    G = numel(ground);
    
    % Add fields to the structures
    if D > 0,
        detections(1).assigned = [];
        detections(1).distance = [];
    end
    if G > 0,
        ground(1).assigned = [];                
        ground(1).distance = [];
    end
    
    % Nothing to do?
    if D == 0 || G == 0,
        return
    end
    
            
    %% Cost matrix - Euclidean distance part
    AX = repmat( [ detections.x ], G, 1 );
    AY = repmat( [ detections.y ], G, 1 );
        
    BX = repmat( [ ground.x ]', 1, D);
    BY = repmat( [ ground.y ]', 1, D);
        
    dist = sqrt( (AX - BX).^2 + (AY - BY).^2 );
    dist( dist > threshold) = Inf; % Mark invalid...

    %% Cost matrix - identity part (optional)
    if identity,
        idD = repmat( [ detections.id ], G, 1);
        idG = repmat( [ ground.id ]', 1, D);
    
        dist( idD ~= idG ) = Inf;
    end
    
    
    %% Find optimal assignment using Hungarian method
    [ assignment, ~ ] = Hungarian(dist);

    %% Update structures
    for d = 1:D,            
        idx = find( assignment(:,d) == 1 ); % Find the assigned ground truth point
        if ~isempty(idx)
            detections(d).assigned = idx;
            detections(d).distance = dist(idx,d);                
        end
    end
        
    for g = 1:G,
        idx = find( assignment(g,:) == 1 ); % Find the assigned detection
        if ~isempty(idx)
            ground(g).assigned = idx;
            ground(g).distance = dist(g,idx);                
        end
    end
end