function [matchings, unassignedRows, unassignedCols] = ...
    matchpairs(Cost, costUnmatched, goal)
%MATLAB Code Generation Library Function

% Limitations:
%   Sparse Cost matrix is not supported for codegen.

%   Copyright 2018-2021 The MathWorks, Inc.
%#codegen

coder.inline('always');
coder.internal.assert(nargin >= 2, 'MATLAB:minrhs');

coder.internal.assert(isfloat(Cost) && isreal(Cost) && ismatrix(Cost), 'MATLAB:matchpairs:InvalidCost');
coder.internal.assert(~issparse(Cost),'Coder:toolbox:SparseNotSupported','matchpairs');
coder.internal.assert(~coder.internal.vAllOrAny('any', Cost, @isnan), 'MATLAB:matchpairs:NonFiniteCost');

coder.internal.assert(isfloat(costUnmatched) && isreal(costUnmatched) && isscalar(costUnmatched), 'MATLAB:matchpairs:InvalidCostUnmatched');
coder.internal.assert(isfinite(costUnmatched), 'MATLAB:matchpairs:NonFiniteCostUnmatched');
costUnmatched = costUnmatched(1); % make codegen know this is scalar.

minimize = true;
if nargin > 2
    coder.internal.prefer_const(goal);
    coder.internal.assert(coder.internal.isTextRow(goal), 'MATLAB:matchpairs:InvalidCost');
    isMax = strncmpi(goal, 'max', strlength(goal));
    isMin = strncmpi(goal, 'min', strlength(goal));
    % Flag must match exactly one of 'max' and 'min':
    coder.internal.assert(xor(isMax, isMin), 'MATLAB:matchpairs:InvalidOption');
    minimize = isMin;
end

if ~minimize
    Cost = -Cost;
    costUnmatched = -costUnmatched;
end

if nargout <= 1
    matchings = eml_matchpairs(Cost, costUnmatched);
    matchings = double(matchings);
elseif nargout <= 2
    [matchings, unassignedRows] = eml_matchpairs(Cost, costUnmatched);
    matchings = double(matchings);
    unassignedRows = double(unassignedRows);
else
    [matchings, unassignedRows, unassignedCols] = eml_matchpairs(Cost, costUnmatched);
    matchings = double(matchings);
    unassignedRows = double(unassignedRows);
    unassignedCols = double(unassignedCols);
end


function [matchings, unassignedRows, unassignedCols] = eml_matchpairs(Cost, costUnmatched)

% Create a larger cost matrix, with dummy rows and columns
% to account for the possibility of not matching.
cls = 'double';
if isa(Cost, 'single') && isa(costUnmatched, 'single')
    cls = 'single';
end

m = coder.internal.indexInt(size(Cost,1));
n = coder.internal.indexInt(size(Cost,2));

paddedCost = coder.internal.inf(m+n, m+n, cls);

for jj=1:n
    for ii=1:m
        paddedCost(ii, jj) = Cost(ii, jj);
    end
end
for jj=1:m
    for ii=1:n
        paddedCost(m+ii, n+jj) = Cost(jj, ii);
    end
end
for ii=1:m
    paddedCost(ii, n+ii) = 2*costUnmatched;
end
for jj=1:n
    paddedCost(m+jj, jj) = 2*costUnmatched;
end
[colToRow, rowToCol] = perfectMatching(paddedCost, cls);

if isempty(colToRow) && m+n > 0
    % No perfect matching found, must be because of an overflow inside
    % perfectMatching. Rescale the matrix and try again.
    maxVal = coder.internal.reduceToScalar(paddedCost, @finiteMaxAbs, zeros(cls));
    
    scale = 2^-nextpow2(maxVal);
    
    [colToRow, rowToCol] = perfectMatching(paddedCost * scale, cls);
end
% r = colToRow(c) gives row r matched to column c.
% c = rowToCol(r) gives column c matched to row r.

% Check for ambiguous cases, where a pair can be matched or unmatched
% with no effect on the goal function. Set these cases to be unmatched:
for c=1:n
    r = colToRow(c);
    if r <= m && Cost(r, c) == 2*costUnmatched
        colToRow(c) = m+n+1;
        rowToCol(r) = m+n+1;
    end
end

% Real row matched to real column: a matched pair in matrix C.
% The following code is a coder replacement for these three lines:
%matchedPairs = colToRow(1:n) <= m;
%matchings = [colToRow(matchedPairs), find(matchedPairs)];
%matchings = reshape(matchings, [], 2); % Correct size in empty case.

% Count the number of rows.
nr = coder.internal.indexInt(0);
for k = 1:n
    if colToRow(k) <= m
        nr = nr + 1;
    end
end
assert(nr <= n); % Always true. This is a hint to compiler for bounded variable sizing.
matchings = coder.nullcopy(zeros(nr,2,coder.internal.indexIntClass));
j = coder.internal.indexInt(0);
for k = 1:n
    if colToRow(k) <= m
        j = j + 1;
        matchings(j,1) = colToRow(k);
        matchings(j,2) = k;
    end
end

if nargout >= 2
    % Real row matched to dummy column: an unassigned row.
    unassignedRows = findAGreaterThanB(rowToCol(1:m,1),n); % find(rowToCol(1:m,1) > n)
end

if nargout >= 3
    % Real column matched to dummy row: an unassigned column.
    unassignedCols = findAGreaterThanB(colToRow(1:n,1),m); % find(colToRow(1:n,1) > m)
end


function currentMaxAbs = finiteMaxAbs(currentMaxAbs, newVal)
absNewVal = abs(newVal);
if absNewVal > currentMaxAbs && ~isinf(absNewVal)
    currentMaxAbs = absNewVal;
end


function [m1, m2] = perfectMatching(A, cls)
% This requires A to be dense. Automatically the case based on the
% construction before passing it here.

[m1, m2, ~, ~, success] = matlabPerfectMatching(A, cls);

if ~success
    % No perfect matching was found.
    m1 = zeros(0,1,coder.internal.indexIntClass);
    m2 = zeros(0,1,coder.internal.indexIntClass);
end


function [matchCtoR, matchRtoC, rowWeight, colWeight, success] = matlabPerfectMatching(matrixRep, cls)
% This function mirrors the built-in function matlab.internal.graph.perfectMatching.

n = size(matrixRep, 1); % Input matrixRep must be square

matchCtoR = zeros(n, 1, coder.internal.indexIntClass);
matchRtoC = zeros(n, 1, coder.internal.indexIntClass);
colWeight = coder.internal.inf(n, 1, cls);
UNINIT = coder.internal.indexInt(0);

if n == 0
    % Early return.
    rowWeight = coder.internal.inf(n, 1, cls);
    success = true;
    return
end

[rowWeight, minIndices] = min(matrixRep, [], 2);

if ~coder.internal.vAllOrAny('all',rowWeight,@isfinite) % any(isinf(rowWeight))
    % No perfect matching exists, one row contains only Inf.
    success = false;
    return
end

% Allocate matching vector
pairWeightR = zeros(n, 1, cls); % stores C(r, c) for each matched pair


for r=1:n
    c = minIndices(r);
    if matchCtoR(c) == UNINIT
        matchRtoC(r) = c;
        matchCtoR(c) = r;
        pairWeightR(r) = rowWeight(r);
    end
end

% Weight of columns:
for c=1:n
    for r=1:n
        edge_weight = matrixRep(r, c);
        edge_weight_shifted = edge_weight - rowWeight(r);
        if edge_weight_shifted < colWeight(c)
            colWeight(c) = edge_weight_shifted;
        end
    end
end

% Initialize internal helps for augmentedShortestPath, so that they are not
% reallocated on each iteration.
newPairWeightR = zeros(n, 1, cls);
predecessorsC = zeros(n, 1, coder.internal.indexIntClass);
distancesR = coder.internal.inf(n, 1, cls);
colorsR = zeros(n, 1, 'uint8');

queue = matlab.internal.coder.minPriorityQueue(n);

% Invoke Dijkstra
for colStart=1:n
    
    % Column is already matched.
    if matchCtoR(colStart) ~= 0
        continue
    end
    
    % Try to find an augmenting path from column colStart to a currently
    % unmatched row:
    [foundRow, rowWeight, colWeight, matchCtoR, matchRtoC, pairWeightR] = ...
        augmentedShortestPath(matrixRep, colStart, rowWeight, colWeight, ...
        matchCtoR, matchRtoC, pairWeightR, cls, ...
        newPairWeightR, predecessorsC, distancesR, colorsR, queue);
    
    if ~foundRow
        % There is no perfect matching
        success = false;
        return
    end
end
success = true;


function [foundRow, rowWeight, colWeight, matchCtoR, matchRtoC, pairWeightR] = ...
    augmentedShortestPath(matrixRep, colStart, rowWeight, colWeight, matchCtoR, matchRtoC, pairWeightR, cls, ...
    newPairWeightR, predecessorsC, distancesR, colorsR, queue)

n = coder.internal.indexInt(size(matrixRep, 1));
one = coder.internal.indexInt(1);
UNINIT = coder.internal.indexInt(0);

% Weight buffer for edges in the augmenting tree (initiailization does not
% matter).
newPairWeightR(:) = 0;

% Predecessor map (points to the previous column on the path back to colStart
% - we skip the rows).
predecessorsC(:) = 0;
predecessorsC(colStart) = UNINIT;

% Distance array for each row (we skip the columns, since weight of current
% matched edges is zero).
distancesR(:) = coder.internal.inf;

% Color for each row (see Dijkstra algorithm)
% 0 == WHITE (undiscovered), 1 == GREY (discovered), 2 == BLACK (finished)
colorsR(:) = 0;

queue = clear(queue);

% Length of shortest path to current node in the algorithm.
lsp = zeros(cls);

% Length of shortest augmenting path (path to an unmatched row)
lsap = coder.internal.inf(cls);

% Last row, column and edge weight along augmenting path.
rlast = UNINIT;
clast = UNINIT;
last_weight_sap = coder.internal.inf(cls);

c = coder.internal.indexInt(colStart);
myRealmax = realmax(cls); % Always compare <= realmax instead of < Inf,
% because otherwise most time is spent in that comparison (Infs are weird
% in codegen).

while true
    for r = one:n
        if colorsR(r) == 2 % BLACK
            continue
        end
        edge_weight = matrixRep(r, c);
        dnew = lsp + (edge_weight - rowWeight(r) - colWeight(c));
        if dnew < lsap
            if matchRtoC(r) == UNINIT
                lsap = dnew;
                rlast = r;
                clast = c;
                last_weight_sap = edge_weight;
            elseif dnew < distancesR(r)
                distancesR(r) = dnew;
                predecessorsC(matchRtoC(r)) = c;
                newPairWeightR(r) = edge_weight;
                if colorsR(r) == 0
                    queue = push(queue, r, distancesR);
                    colorsR(r) = 1; % GRAY
                else
                    queue = update(queue, r, distancesR);
                end
            end
        elseif ~(-myRealmax <= dnew && dnew <= myRealmax) && ...
                (-myRealmax <= edge_weight && edge_weight <= myRealmax)
            % Overflow occurred, result is not reliable, return unsuccessfully.
            foundRow = false;
            return;
        end
    end
    if isempty(queue)
        % No rows left in queue, all paths have been explored.
        break
    else
        % Get new row candidate at minimal distance.
        [queue, rCandidate] = pop(queue, distancesR);
        
        lsp = distancesR(rCandidate);
        if lsap <= lsp
            % The shortest distance is larger than the current shortest augmenting path.
            % Successful early exit.
            break
        else
            % Choose the column matched to rCandidate, and grow the tree along its edges.
            colorsR(rCandidate) = 2; % BLACK
            c = matchRtoC(rCandidate);
        end
    end
end

foundRow = lsap < coder.internal.inf(cls);
if ~foundRow
    % No augmenting path found, return unsuccessfully.
    return
end

% Augment the matching along the shortest augmenting path
% Travel back up the discovered shortest path. Remove old matched
% edges, and replace them with the alternating new matched edges.

riter = rlast;
citer = clast;
witer = last_weight_sap;

while true
    % Determine next row/column pair
    rnext = matchCtoR(citer);
    cnext = predecessorsC(citer);
    
    % Add new matched edge (implicitly removing old matched pair)
    matchRtoC(riter) = citer;
    matchCtoR(citer) = riter;
    pairWeightR(riter) = witer;
    
    % Check if start of shortest path has been reached.
    if cnext == UNINIT
        break
    end
    
    % Advance
    riter = rnext;
    citer = cnext;
    witer = newPairWeightR(rnext);
end

for r=one:n
    if colorsR(r) == 2 % BLACK
        rowWeight(r) = rowWeight(r) - lsap + distancesR(r);
    end
end

for c=one:n
    if matchCtoR(c) ~= UNINIT
        matchingNode = matchCtoR(c);
        colWeight(c) = pairWeightR(matchingNode) - rowWeight(matchingNode);
    end
end

function out = findAGreaterThanB(a,b)
% Avoid traversing data twice to find the indices of a that correspond to
% values greater than b

coder.inline('always')
nIn = coder.internal.indexInt(numel(a));

% Force out to be variable-sized while permitting nIn to change at runtime
out = coder.nullcopy(zeros(0, 1, coder.internal.indexIntClass)); %#ok<NASGU>
out = coder.nullcopy(zeros(nIn, 1, coder.internal.indexIntClass));

nOut = coder.internal.indexInt(0);
for ii = coder.internal.indexInt(1) : nIn
    if a(ii) > b
        nOut = coder.internal.indexPlus(nOut, 1);
        out(nOut) = ii;
    end
end

out = out(1:nOut,1);