function clusteredDetections = mergeDetections(detectionsIn, clusterIdx, varargin)
% mergeDetections - Merge detections into clustered detections
% 
%    clusteredDetections = mergeDetections(detections, clusterIdx) merges
%    detections sharing the same cluster labels. By default, the function
%    merges detections in the same cluster using a Gaussian mixture merging
%    algorithm. The function assumes that all detections in the same
%    cluster share the same Time, SensorIndex, ObjectClassID,
%    MeasurementParameters, and ObjectAttributes.
% 
%    detections is one of the following:
%
%    * A cell array of objectDetection objects
%    * An array of objectDetection objects
%    * A struct array with same fields as properties of objectDetection
%
%    clusterIdx is a vector with the same number of elements as the
%    detections. clusterIdx(i) is the cluster label for i-th detection.
% 
%    clusteredDetections are the clustered detections. The number of
%    elements in clusteredDetections is equal to the number of unique
%    labels in the input, clusterIdx. The format of the clusteredDetections
%    is the same as input detections.
% 
%    clusteredDetections = mergeDetections(..,'MergingFcn',mergeFcn)
%    specifies a function for merging detections sharing the same cluster
%    index.
%
%    mergeFcn must have one of the following signatures:
% 
%    1. Detection input and output
%       detectionOut = mergeFcn(detectionsIn) 
% 
%       detectionsIn is a cell array of objectDetection objects in the same
%       cluster. 
%
%       detectionOut is a scalar objectDetection object representing the
%       merged detection.
% 
%    2. Mean and Covariance input and output
%       [mergedMean, mergedCovariance] = mergeFcn(means, covariances)
% 
%       means is an M-by-N matrix representing measurements in the cluster.
%       M is the size of the input measurement and N is the number of
%       measurements in the cluster.
% 
%       covariances is an M-by-M-by-N matrix representing the associated
%       uncertainty covariance matrix. 
% 
%       mergedMean is a P-by-1 vector representing the merged measurement.
%       Note that the size of the merged measurement (P) may or may not be
%       same as the size of the input measurement (M). This allows you to
%       merge many detections into parameterized forms such as rectangular
%       or cuboid detections.
%
%       mergedCovariance is a P-by-P vector representing the uncertainty
%       covariance in the merged measurement.
% 
%    Note: You can use covariance fusion functions such as fusexcov,
%    fusecovint and fusecovunion in the Sensor Fusion and Tracking Toolbox
%    as MergingFcn.
% 
%    Example: Create clusters from positional detections in Rectangular frame
%    % Generate 10 detections from 2 clusters with 2 false alarms
%    x1 = [5;5;0] + randn(3,4); % 4 detections in cluster 1
%    x2 = [5;-5;0] + randn(3,4); % 4 detections in cluster 2
%    xFalse = 30*randn(3,2); % 2 false alarms
%    x = [x1 x2 xFalse];
%
%    % Assemble as objectDetection array
%    detections = repmat({objectDetection(0,[0;0;0])},10,1);
%    for i = 1:10
%      detections{i}.Measurement = x(:,i);
%    end
%
%    % Define cluster index according to the scenario defined above. This
%    % is typically obtained by a clustering algorithm
%    clusterIndex = [1;1;1;1;2;2;2;2;3;4]; 
% 
%    clusteredDetections = mergeDetections(detections, clusterIndex);
% 
%    % Visualize the results
%    tp = theaterPlot;
%
%    % Colors for plotters
%    clrs = lines(2);
%
%    % Unclustered detection plotter
%    detPlotterUn = detectionPlotter(tp,...
%                 'DisplayName','Unclustered Detections',...
%                 'MarkerFaceColor',clrs(1,:),...
%                 'MarkerEdgeColor',clrs(1,:));
%
%    % Clustered detection plotter
%    detPlotterC = detectionPlotter(tp,...
%                 'DisplayName','Clustered Detections',...
%                 'MarkerFaceColor',clrs(2,:),...
%                 'MarkerEdgeColor',clrs(2,:));
% 
%    % Concatenate measurements and covariances from unclustered detections
%    detArray = [detections{:}];
%    xUn = horzcat(detArray.Measurement)';
%    PUn = cat(3,detArray.MeasurementNoise);
% 
%    % Plot unclustered measurements with covariances
%    plotDetection(detPlotterUn,xUn,PUn);
%
%    % Concatenate measurements and covariance from clustered detections
%    clusteredDetArray = [clusteredDetections{:}];
%    xC = horzcat(clusteredDetArray.Measurement)';
%    PC = cat(3,clusteredDetArray.MeasurementNoise);
% 
%    % Plot clustered detections
%    plotDetection(detPlotterC,xC,PC);
%
%    See also: objectDetection, theaterPlot

% Copyright 2021-2022 The MathWorks, Inc.

%#codegen

% Validate detections input
validateattributes(detectionsIn,{'cell','struct','objectDetection'},{},mfilename,'Detections',1);

if coder.target('MATLAB') && isempty(detectionsIn)
    clusteredDetections = detectionsIn;
    return;
end

% Validate the cell or struct and convert to cell array of detections to
% make code readable with less branches
if ~iscell(detectionsIn)
    detections = matlabshared.tracking.internal.fusion.makeDetectionCells(detectionsIn); 
else
    detections = detectionsIn;
end

% Validate clusterIdx
validateattributes(clusterIdx,{'numeric'},{'real','finite','positive','integer','vector','numel',numel(detections)},mfilename,'clusterIdx',2);

% Parse inputs
opArgs = {};
poptions = struct('CaseSensitivity', true,...
    'IgnoreNulls',true,...
    'SupportOverrides', false);
NVPairNames = {'MergingFcn'};
pstruct = coder.internal.parseInputs(opArgs,NVPairNames,...
    poptions,varargin{:});

% Get value from parser
mergeFcnIn = coder.internal.getParameterValue(pstruct.MergingFcn,@fusion.internal.gaussianMergeDetections,varargin{:});

% Validate mergeFcn
validateattributes(mergeFcnIn, {'char','string','function_handle'}, {'nonempty'}, mfilename, 'MergingFcn', 4);

if isa(mergeFcnIn,'function_handle')
    mergeFcn = mergeFcnIn;
else
    mergeFcn = str2func(mergeFcnIn);
end

% Check if the merging function signature uses detections input or
% measurements and covariances as input.
nInputs = nargin(mergeFcn);
isMeasurementFusionSignature = nInputs >= 2; % Allow fusion functions with 3 inputs

% Unique clusters
uqClusters = unique(clusterIdx(:));
nClusters = numel(uqClusters);

% Allocate memory for clusteredDetections
clusteredDetectionsCell = cell(nClusters,1);

isValidatedMergeFcn = false;

for i = 1:nClusters
    thisClusterLabel = uqClusters(i);
    thisClusterIdx = clusterIdx == thisClusterLabel;
    if ~isMeasurementFusionSignature
        [detOut,isValidatedMergeFcn] = mergeUsingDetectionSignature(mergeFcn, detections, thisClusterIdx,isValidatedMergeFcn);
    else
        [detOut,isValidatedMergeFcn] = mergeUsingMeasurementFusionSignature(mergeFcn, detections, thisClusterIdx,isValidatedMergeFcn);
    end
    clusteredDetectionsCell{i} = detOut;
end

% Send output in same format as input
if ~iscell(detectionsIn) 
    clusteredDetections = cell2Array(clusteredDetectionsCell);
else
    clusteredDetections = clusteredDetectionsCell;
end

end

function detOut = cell2Array(detections)
    detOut = repmat(detections{1},numel(detections),1);
    for i = 1:numel(detOut)
        detOut(i) = detections{i};
    end
end

function [detOut, isValidatedMergeFcn] = mergeUsingDetectionSignature(mergeFcn, detections, thisClusterIdx,isValidatedMergeFcn)
thisClusterDetections = fusion.internal.selectDetections(detections, thisClusterIdx);
detOut = mergeFcn(thisClusterDetections);
if ~isValidatedMergeFcn
    validateattributes(detOut,{class(thisClusterDetections{1})},{'scalar'},'MergingFcn','output');
    isValidatedMergeFcn = true;
end
end

function [detOut,isValidatedMergeFcn] = mergeUsingMeasurementFusionSignature(mergeFcn, detections, thisClusterIdx,isValidatedMergeFcn)
% Merge measurement and covariance in a cluster
x = matlabshared.tracking.internal.fusion.concatenateDetectionData(detections,'Measurement',thisClusterIdx);
P = matlabshared.tracking.internal.fusion.concatenateDetectionData(detections,'MeasurementNoise',thisClusterIdx);
[xMerged, PMerged] = mergeFcn(x, P);

% The size of a merged measurement may be different than the size of input
% measurements. Don't assert any size constraints here. Just verify that
% the sizes of mean and covariance match each other.
if ~isValidatedMergeFcn
    n = size(xMerged,1);
    validateattributes(xMerged,{class(x)},{'nonempty','real','finite','nonsparse','column'},'MergingFcn','output');
    validateattributes(PMerged,{class(P)},{'real','finite','nonsparse','size',[n n]},'MergingFcn','output');
    isValidatedMergeFcn = true;
end

% Preserve other fields of the detections in this cluster.
% Extract sample detection from the cluster
f = find(thisClusterIdx,1,'first');
sampleInputDetection = detections{f(1)};

% Create output detection
detOut = createOutputDetection(sampleInputDetection, xMerged, PMerged);

end

function detOut = createOutputDetection(detIn, xMerged, PMerged)
    if coder.target('MATLAB')
        % In MATLAB, just use the same detection, its size can change. 
        detOut = detIn;
        detOut.Measurement = xMerged;
        detOut.MeasurementNoise = PMerged;
    else
        % In coder, we need to create a struct or objectDetection of the
        % right size measurement
        n = size(xMerged,1);
        classToUse = class(xMerged);
        if isa(detIn,'objectDetection')           
            detOut = objectDetection(detIn.Time,...
                zeros(n,1,classToUse),...
                'MeasurementParameters',detIn.MeasurementParameters,...
                'ObjectAttributes',detIn.ObjectAttributes,...
                'SensorIndex',detIn.SensorIndex,...
                'ObjectClassID',detIn.ObjectClassID);
            detOut.Measurement = xMerged(:,1); % Assist Coder
            detOut.MeasurementNoise = PMerged(:,:,1); % Assist Coder
        else
            detOut = struct;
            fNames = fieldnames(detIn);
            coder.unroll();
            for i = 1:numel(fNames)
                if (strcmpi(fNames{i},'Measurement'))
                    detOut.Measurement = zeros(n,1,classToUse);
                elseif (strcmpi(fNames{i},'MeasurementNoise'))
                    detOut.MeasurementNoise = eye(n,classToUse);
                else
                    detOut.(fNames{i}) = detIn.(fNames{i});
                end
            end
            detOut.Measurement = xMerged(:,1); % Assist Coder
            detOut.MeasurementNoise = PMerged(:,:,1); % Assist Coder
        end
    end
end
