function [index_pairs, match_metric] = cvalgMatchFeatures(features1, ...
                                       features2, metric, match_percentage)
% Main algorithm used by the matchFeatures function. See matchFeatures
% for more details.

% Copyright 2010 The MathWorks, Inc.
% $Revision: 1.1.8.3 $  $Date: 2011/02/09 21:14:36 $
%#codegen

% Determine output class
if (isequal(class(features1), 'double'))
    output_class = 'double';
else
    output_class = 'single';
end

% Determine number of feature vectors in each set
N1 = cast(size(features1, 2), 'uint32');
N2 = cast(size(features2, 2), 'uint32');
if (N1 == 0 || N2 == 0)
    % Create 2x0 empty matrix to provide equivalence between MATLAB and
    % MATLAB Coder
    index_pairs = zeros(2, 0, 'uint32');
    match_metric = zeros(0, 0, output_class);
    return
end

% Generate correspondence metric matrix
switch metric
    case 'sad'
        % Generate correspondence matrix using Sum of Absolute Differences
        scores = metricSAD(features1, features2, N1, N2, output_class);
    case 'normxcorr'
        % Generate correspondence matrix using Normalized Cross-correlation
        scores = metricNormXCorr(features1, features2, N1, N2, ...
            output_class);
    otherwise % 'ssd'
        % Generate correspondence matrix using Sum of Squared Differences
        scores = metricSSD(features1, features2, N1, N2, output_class);
end

% Find matches
[index_pairs, match_metric] = findMatches(scores, N1, N2, metric);

% Convert match threshold percent to a numeric threshold
match_threshold = percentToLevel(match_percentage, size(features1, 1), ...
    metric, output_class);

% Remove weak matches
[index_pairs, match_metric] = removeWeakMatches(index_pairs, ...
    match_metric, match_threshold, metric);

%==========================================================================
% Convert match threshold percent to an numeric threshold
%==========================================================================
function match_threshold = percentToLevel(match_percentage, ...
    vector_length, metric, output_class)

match_percentage = cast(match_percentage, output_class);
vector_length = cast(vector_length, output_class);

if (strcmp(metric, 'normxcorr'))
    match_threshold = cast(0.01, output_class)*(cast(100, output_class) ...
        - match_percentage);
else    
    if (strcmp(metric, 'sad'))
        max_val = cast(2, output_class)*sqrt(vector_length);
    else % 'ssd'
        max_val = cast(4, output_class);
    end
    match_threshold = (match_percentage*cast(0.01, output_class))*max_val;       
end
        
%==========================================================================
% Find corresponding features
%==========================================================================
function [index_pairs, match_metric] = findMatches(scores, N1, N2, metric)
% SCORES is an N1-by-N2 correspondence metric matrix where the rows
% correspond to the feature vectors in FEATURES1, and the columns
% correspond to the feature vectors in FEATURES2.

% For each feature vector in FEATURES1 find the best match in FEATURES2.
% This is simply the entry along each row with the minimum or maximum
% metric value, depending on the metric.
row_index_pairs = [(1:N1); zeros(1, N1)];
if (strcmp(metric, 'normxcorr'))
    [row_scores, row_index_pairs(2, :)] = max(scores, [], 2);
else
    [row_scores, row_index_pairs(2, :)] = min(scores, [], 2);
end

% For each feature vector in FEATURES2 find the best match in FEATURES1.
% This is simply the entry down each col with the minimum or maximum metric
% value, depending on the metric.
col_index_pairs = [zeros(1, N2); (1:N2)];
if (strcmp(metric, 'normxcorr'))
    [col_scores, col_index_pairs(1, :)] = max(scores, [], 1);
else
    [col_scores, col_index_pairs(1, :)] = min(scores, [], 1);
end

% Concatenate both row and column lists
M = [row_scores', col_scores];
index_pairs = [row_index_pairs, col_index_pairs];

% Remove duplicate entries in the matches list
[trimmed_index_pairs, I, ~] = unique(index_pairs', 'rows');
match_metric = M(I);
index_pairs = trimmed_index_pairs';

%==========================================================================
% Generate correspondence metric matrix using Sum of Absolute Differences
%==========================================================================
function scores = metricSAD(features1, features2, N1, N2, output_class)
% Convert feature vectors to unit vectors
D1 = normalizeX(cast(features1, output_class));
D2 = normalizeX(cast(features2, output_class));

% Need to obtain feature vector length to perform explicit row indexing 
% needed for code generation of variable sized inputs
vector_length = size(features1, 1);
scores = zeros(N1, N2, output_class);
for c = 1:N2
    for r = 1:N1
        scores(r, c) = sum(abs(D1(1:vector_length, r) - ...
            D2(1:vector_length, c)));
    end
end

%==========================================================================
% Generate correspondence metric matrix using Sum of Squared Differences
%==========================================================================
function scores = metricSSD(features1, features2, N1, N2, output_class)
% Convert feature vectors to unit vectors
D1 = normalizeX(cast(features1, output_class));
D2 = normalizeX(cast(features2, output_class));

% Need to obtain feature vector length to perform explicit row indexing 
% needed for code generation of variable sized inputs
vector_length = size(features1, 1);
scores = zeros(N1, N2, output_class);
for c = 1:N2
    for r = 1:N1
        scores(r, c) = sum((D1(1:vector_length, r) - ...
            D2(1:vector_length, c)).^2);
    end
end
        
%==========================================================================
% Generate correspondence metric matrix using Normalized Cross-Correlation
%==========================================================================
function scores = metricNormXCorr(features1, features2, N1, N2, ...
        output_class)
D1 = cast(features1, output_class);
D2 = cast(features2, output_class);

% Compute vector means
mean_D1 = mean(D1, 1);
mean_D2 = mean(D2, 1);

% Subtract vector means from each feature vector
D1_minus_mean = zeros(size(D1), class(D1));
D2_minus_mean = zeros(size(D2), class(D2));
vector_length = cast(size(features1, 1), 'uint32');
for c = 1:N1
    for r = 1:vector_length
        D1_minus_mean(r, c) = D1(r, c) - mean_D1(c);
    end    
end
for c = 1:N2
    for r = 1:vector_length
        D2_minus_mean(r, c) = D2(r, c) - mean_D2(c);
    end    
end

sum_D1_minus_mean_squared = sum(D1_minus_mean.^2, 1);
sum_D2_minus_mean_squared = sum(D2_minus_mean.^2, 1);

% Populate correspondence metric matrix
scores = zeros(N1, N2, output_class);
for c = 1:N2
    for r = 1:N1
        normx_num = D1_minus_mean(:, r)' * D2_minus_mean(:, c);
        normx_denom = sqrt(sum_D1_minus_mean_squared(r) .* ...
            sum_D2_minus_mean_squared(c));
        if (normx_denom == 0)
            scores(r, c) = 0;
        else
            scores(r, c) = normx_num/normx_denom;
        end
    end
end

%==========================================================================
% Remove weak matches
%==========================================================================
function [indices, match_metric] = removeWeakMatches(indices, ...
    match_metric, match_threshold, metric)

sub_indices = indices;
sub_match_metric = match_metric;
count = 0;
if (strcmp(metric, 'normxcorr'))    
    for k = 1:length(match_metric)
        if (match_metric(k) >= match_threshold)
            % Save acceptable match
            count = count + 1;
            sub_indices(:, count) = indices(:, k);
            sub_match_metric(:, count) = match_metric(:, k);
        end
    end
else
    for k = 1:length(match_metric)
        if (match_metric(k) <= match_threshold)
            % Save acceptable match
            count = count + 1;
            sub_indices(:, count) = indices(:, k);
            sub_match_metric(:, count) = match_metric(:, k);
        end
    end
end

if count > 0
    indices = sub_indices(:, 1:count);
    match_metric = sub_match_metric(:, 1:count);
else
    indices = zeros(2, 0, 'uint32');
    match_metric = zeros(0, 0, class(match_metric));
end

%==========================================================================
% Normalize the columns in X to have unit norm.
%==========================================================================
function X = normalizeX(X)
Xnorm = sqrt(sum(X.^2, 1));
X = bsxfun(@rdivide, X, Xnorm);

% Set effective zero length vectors to zero
X(:, (Xnorm <= eps(single(1))) ) = 0;
