function [assigned, unassignedrows, unassignedcols, costs] = murty(A, cost, k)
% MURTY  Finds the k best assignments
%
% [assigned, unassignedrows, unassignedcols, costs] = murty(A, cost, k)
% Uses Murty's algorithm together with the auction algorithm to compute the
% k best association hypotheses.  The 3 first outputs are cell arrays
% containing the output from the auction for the k best (if possible)
% assignments.  costs are the associated costs.  See auction for details.
%
% Based on (Blackman & Popoli, 1999)

% Copyright (2019) Gustaf Hendeby <gustaf.hendeby@liu.se>

  [nI, nC] = size(A);
  extra = inf(nI);  for i=1:nI; extra(i, i) = cost;  end
  testlist = struct('A', {}, 'assigned', {}, 'cost', []);
  testlist(1).A = [A-cost, extra];
  testlist(1).assigned = auction(testlist(1).A);
  testlist(1).cost = computecost(testlist(1).A, testlist(1).assigned, cost);
  A = cell(k, 1);
  assigned = cell(k, 1);
  costs = inf(1, k);
  while numel(testlist) > 0
    if costs(end) <= testlist(1).cost
      break;  % No more improvements possible
    end
    [assigned, costs, A] = addifnew(assigned, costs, A, testlist(1));
    I = testlist(1).assigned;
    for j = 1:size(I, 1)
      t.A = testlist(1).A;
      t.A(I(j, 1), I(j, 2)) = inf;
      t.assigned = auction(t.A);
      t.cost = computecost(t.A, t.assigned, cost);
      if t.cost<costs(end)
        testlist = addtolist(testlist, t);
      end
    end
    testlist(1) = [];
  end
  I = costs~=Inf;
  assigned = assigned(I);
  costs = costs(I);
  n = numel(costs);
  for i=1:n
    a = assigned{i};
    unassignedrows{i} = a(:, 2)>nC; %#ok<AGROW>
    a = uint32(a(~unassignedrows{i}, :));
    assigned{i} = a;
    unassignedrows{i} = uint32(find(unassignedrows{i})); %#ok<AGROW>
    unassignedcols{i} = uint32(setdiff((1:nC)', a(:, 2))); %#ok<AGROW>
  end
  unassignedrows = unassignedrows(:);
  unassignedcols = unassignedcols(:);
  costs = costs(1:n)';
end

function C = computecost(A, assigned, cost)
  [nI, nC] = size(A);
  C = (nC-nI)*cost;
  for i=1:size(assigned, 1)
    C = C + A(assigned(i, 1), assigned(i, 2));
  end
end

function [a, c, A] = addifnew(a, c, A, test)
  I = find(test.cost == c);
  for i=I
    if numel(test.assigned) == numel(a{i}) && all(test.assigned(:)==a{i}(:))
      return
    end
  end
  i = find(test.cost < c, 1);
  if ~isempty(i)
    a(i+1:end) = a(i:end-1);
    a{i} = test.assigned;
    c(i+1:end) = c(i:end-1);
    c(i) = test.cost;
    A(i+1:end) = A(i:end-1);
    A{i} = test.A;
  end
end

function testlist = addtolist(testlist, t)
  c = [testlist.cost];
  I = find(c==t.cost);
  for i=I
    tL = testlist(i);
    if all(t.A(:)==tL.A(:))
      return  % Element already exist
    end
  end

  testlist(end+1) = t;
  c(end+1) = t.cost;
  [~, I] = sort(c);
  testlist = testlist(I);
end

function [assigned] = auction(A)
  [nI, nC] = size(A);
  A = -A;  % Minimize instead of max in the book.
  eps = 0.05*1/nC;
  I2C = zeros(1, nI, 'uint16');  % 0 indicates unassigned item
  C2I = zeros(1, nC, 'uint16');  % 0 indicates unassigned customer
  P = zeros(1, nC);

  while any(I2C==0)  % Any unassigned item
    i = find(I2C==0, 1);
    [m, j] = max(A(i, :) - P);
    if C2I(j) ~= 0;  I2C(C2I(j)) = 0;  end
    C2I(j) = i;  I2C(i) = j;
    pj = P(j);
    P(j) = inf;
    P(j) = pj + m - max(A(i, :) - P) + eps;
  end

  assigned = uint32([1:nI; I2C]');
end