% compute -log(P(k|Z)) for k=0..K using results of K-path Viterbi
%
% Wmin will be used as a scale factor to prevent underflow in P
%
% Note that for unreachable final states, exp(-HUGE_VAL) == 0,
% so these states do not affect the probability calculations here.
%
% P will include all final cost results for k=K, and all costs for
% unique paths from the K-path L-best for 0<k<K
%
% P1 will include just the single lowest costs.
%
P  = zeros( K, 1);
P1 = zeros( K, 1);

% P(K) and P1(K) come directly from K-path Viterbi results
%
W = sort(w(:));	% all costs, not just the L-best
Wmin = W(1);
P(K) = sum( exp( -(W - Wmin)));
P1(K) = exp( -(W(1) - Wmin));

K1 = [1:K];

% for 0<k<K, extract the unique tracks from the L-best to estimate P and P1
%
for k = 1:K-1

  cc = combinations(K1,k);
  ncc = size(cc,2);		% number of combinations

  x = zeros(k*N,ncc*L);
  c = zeros(ncc*L,1);

% old version:
%
%  for i = 1:L
%    p = paths(i); 	<- need SortCols() here?
%    offset = (i-1)*ncc;
%    pi = (path_index(N,i)-1)*K;
%    el = el_index(N,i);
%    for j = 1:ncc
%      q = j + offset;
%      r = p(:,cc(:,j));
%      x(:,q) = r(:);
%      c(q) = sum( Wtot( pi+cc(:,j), el));
%    end
%  end
%
% I haven't seen any cases where SortCols(paths(i)) != paths(i)
% Perhaps that will never happen due to the order in which state
% combinations are searched and sorted in the Viterbi algorithm?
% Nevertheless, if paths(i) isn't sorted by columns, then certain
% redundant paths may not be detected.  For example, with k=2, N=3,
% if paths(1) contains [1;2;3] and [4;5;6], and paths(2) contains
% [4;5;6] and [1;2;3] in those orders, then both [1;2;3;4;5;6]
% and [4;5;6;1;2;3] will be included in the unique sets of tracks
% even though they represent the same pair of tracks.
%
% Therefore, I rewrote the above using SortCols(paths(i)),
% which also involves permuting the indices of the associated
% costs from Wtot.

  % L-best paths taken k at a time, and associated Kalman costs
  %
  for i = 1:L
    [p t] = SortCols(paths(i));
    tc = Wtot( K1+(path_index(i,N)-1)*K, el_index(i,N));
    tc = tc(t);
    offset = (i-1)*ncc;
    for j = 1:ncc
      q = j + offset;
      t = cc(:,j);
      r = p(:,t);
      x(:,q) = r(:);
      c(q) = sum(tc(t));
    end
  end

  % indices and costs for the unique sets of tracks
  %
  u = Unique(x);
  uc = c(u);
  nc = size(uc,1);

  for j = 1:nc
    xj = reshape( x(:,u(j)), N, k);
    for i = 1:N
      n = sum(xj(i,:) <= MM_max);			% number of measurements used for tracks
      m = MM(i) - n;					% number of false detections
      uc(j) = uc(j) + Cmiss(n+1,k) + Cfalse(m+1);	% apriori costs
      if( i > 1)					% missed target costs only for i>1
        uc(j) = uc(j) + m*missed_cost;
      end
    end
  end

  W = sort(uc);
  P(k) = sum( exp( -(W - Wmin)));
  P1(k) = exp( -(W(1) - Wmin));

end


% compute P(0|Z) manually, Viterbi not needed
%
if( N > 1)
  W0 = missed_cost * sum(MM(2:N));
else
  W0 = 0
end

for i=1:N
  W0 = W0 - log( p_poisson( MM(i), false_alarm_rate) );
end

P0=exp(-(W0-Wmin));


% Compute sum over tau' and normalize.
%
% The normalization is done only for convenience and does not affect the
% analysis of which P(k|Z) is maximum.
%
PKZ = [ P0; P];

PKZ = -log(PKZ/sum(PKZ));

disp( 'K    PKZ');

[ [0:K]' PKZ]


% Compute PKZ1 using just the single lowest costs
%
PKZ1 = [P0; P1];

PKZ1 = -log(PKZ1/sum(PKZ1));

disp( 'K    PKZ1');

[ [0:K]' PKZ1]
