% OLD_get_perm( L, zz, vp, cur, next, col, el) - find best permutation
%
% Note: vp isn't used here because the kalman results
% are already stored in permuted form.
%
function [best, i_best] = OLD_get_perm( L, zz, vp, cur, next, col, el)

global DEBUG;
global HUGE_VAL;
global combos;
global perms;
global unique_perms;
global M;
global MP;
global K;
global MM;
global MM_max;
global kxv;
global XY;
global XY_offset;
global missed_cost;
global UNUSED_PENALTY;
global log2pi;
global Kv;

APRIORI = 1;
UNUSED_PENALTY = 1;

global Cmiss;
global Cfalse;

global Winc;			% precomputed incremental costs

extra = log2pi + log(Kv);	% added 28 Jan 1999

if( DEBUG>2) disp('get_perm:'), cur, next, end;

i_best = 1;
best = HUGE_VAL;
if( L > 1)
  best = HUGE_VAL * ones(MP,1);	% need to return all choices if L > 1
  i_best = [1:MP]';
end

if( col == 1)                   % handle dummy initial state
  if( cur == next & all(combos(:,next) <= MM(1)))
    if( APRIORI)
      best(1) = Cmiss(K+1,K) + Cfalse(MM(1)-K+1);
    else
      best(1) = 0;		% zero cost only for permutation number 1
    end
  end
  return;
end

s_next = combos(:,next);	% for next combinations

if( any( (s_next > MM(col)) & (s_next <= MM_max)))	% s_next contains non-existant measurement indices
  return;
end

i1 = ((col-1)*M + (cur-1))*K;	% offset for kxv

i2 = XY_offset(col);		% offset for XY

if( DEBUG>2) i1, i2, end;

if( APRIORI)
  n = sum(s_next <= MM(col));		% number of measurements used for tracks
  m = MM(col) - n;			% number of false detections
  apriori = Cmiss(n+1,K) + Cfalse(m+1);
else
  apriori = 0;
end

kel_offset = ((el-1)*M + (cur-1))*K;

for i = 1:MP			% for each permutation

  up = unique_perms(i,next);

  if( up == 0)	% no more unique permutations
    break;
  end

  s_tmp = s_next(perms(:,up));

  if( DEBUG>2) s_tmp, end;

  b = apriori;

  if( UNUSED_PENALTY | any(s_tmp > MM_max))
    b = b + (MM(col) - K) * missed_cost;
  end

  for j = 1:K

    if( s_tmp(j) > MM_max)	% missed target state
      b = b + missed_cost;
    else
%      ii1 = i1+j;
%      ii2 = i2 + s_tmp(j);
%
%      if( XY(1,ii2) == HUGE_VAL) % for testing with false detections never choosen
%        b = HUGE_VAL;
%	break;
%      end
%
%      rx = XY(1,ii2) - kxv(1+(el-1)*4,ii1);
%      ry = XY(2,ii2) - kxv(3+(el-1)*4,ii1);
%
%      Cost = (rx*rx + ry*ry)/(2*Kv) + extra;
%
%      b = b + Cost;
%
%      kel = j + kel_offset;
%
%      if( Cost != Winc( kel, s_tmp(j)))
%        disp( 'get_perm Cost != Winc');
%        Cost, Winc( kel, s_tmp(j))
%      end

      Cost = Winc(j+kel_offset,s_tmp(j));

      if( Cost == HUGE_VAL)
        b = HUGE_VAL;
        break;
      else
        b = b + Cost;
      end

    end

  end

  if( DEBUG>2) b, end;

  if( L > 1)
    best(i) = b;
  elseif( b < best)
    i_best = i;
    best = b;
  end

end
