function P = computeTrackProb(V, A, betaFA)
% COMPUTETRACKPROB Probability of associating measurements and tracks
%
% P = computeTrackProb(V, A, betaFA)
% Computes the probabilities P(T_j <-> y_i|Y, {T_k}) as needed to implement
% JPDA.  The resulting probabilities are given in the matrix P
% (# measurements+1 x # tracks), where the element P_ij contains P(T_j <->
% y_i|Y, {T_k}) and the last row is used to indicate no associated
% measurement.  V is the validation matrix for the measurements and tracks,
% and A the association matrix (without EX columns).  betaFA the false
% alarm rate.

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

  A = A - log(betaFA);
  P = zeros(size(A, 1)+1, size(A, 2));
  [P, ptot] = recurseComputeTrackProb(V, A, 1, P, 0, zeros(0, 2));
  P = P/ptot;
end

% No claim of being efficient, but gets the job done in relatively few
% lines of code.  Many tricka would apply to speed up the implementation.

function [P, ptot] = recurseComputeTrackProb(V, A, t, P, ptot, ind)
  if t > size(A, 2)
    p = 0;
    for i=1:size(ind, 1)
      if ind(i, 1) > size(A, 1); continue; end  % No association is free in this formulation
      p = p + A(ind(i, 1), ind(i, 2));
    end
    for i=1:size(ind, 1)
      P(ind(i, 1), ind(i, 2)) = P(ind(i, 1), ind(i, 2)) + exp(p);
    end
    ptot = ptot + exp(p);
    return
  end
  for m=1:size(V, 1)
    if ~V(m, t); continue; end  % Skip if not compatible
    v = V(m, :);
    V(m, :) = 0;
    [P, ptot] = recurseComputeTrackProb(V, A, t+1, P, ptot, [ind; m, t]);
    V(m, :) = v;
  end
  [P, ptot] = recurseComputeTrackProb(V, A, t+1, P, ptot, [ind; size(A, 1)+1, t]);
end