/* functions used with x9 and x10 */

double get_cost( double _zz, int _cur, int _next, int _col, int _perm, int _el)
{
/* for K=1 and Kalman cost function
 * or K>=1 and energy cost function
 *
 * this function needs to be adjusted to handle truncation of survivors
 * and the resulting shift in index origin of the samples
 */
  int i1, i2;	/* row indices for xv */
  double t, rx, ry, dv;

  /* handle dummy initial state */

  if( _col == 1)  {

    if( _perm == 1)
      rx = (_cur == _next) ? 0 : HUGE_VAL;
    else
      rx = HUGE_VAL;

    return rx;
  }

if(KALMAN) {

  if( _col == 2 && _cur > MM)   /* don't follow initial missed target event state */
    return HUGE_VAL;

  if( _next > MM)               /* _next is missed target state */
    return missed_cost_MM;

  i2 = _next + (_col - 1) * MM;	/* index for xv */

  if( xv(i2,1) == HUGE_VAL)	/* for testing with clutter points never chosen */
    return HUGE_VAL;

  i1 = _cur  + (_col - 1) * M;

  rx = xv(i2,1) - kxv(i1,1+(_el-1)*4);
  ry = xv(i2,2) - kxv(i1,3+(_el-1)*4);

  t = (rx*rx + ry*ry)/(2*Kv) + log2pi + log(Kv);	/* added log2pi+log(Kv) 28 Jan 1999 */

  if( UNUSED_PENALTY)
    t += (MM-1)*missed_cost_1;

  return t;

} else { /* energy cost */

  i2 = _next + (_col - 1) * MM;	/* index for xv */

  i1 = _cur  + (_col - 2) * MM;

  rx = xv(i1,3) + xv(i2,3) - 2 * (xv(i2,1) - xv(i1,1)) / dt;
  rx = 3 * rx * rx;
  dv = xv(i2,3) - xv(i1,3);
  rx = (rx + dv*dv) / dt;

  ry = xv(i1,4) + xv(i2,4) - 2 * (xv(i2,2) - xv(i1,2)) / dt;
  ry = 3 * ry * ry;
  dv = xv(i2,4) - xv(i1,4);
  ry = (ry + dv*dv) / dt;

  return rx + ry;
 }
}

vector get_ex( vector _index)
{
/*
 * this function needs to be adjusted to handle truncation of survivors
 * and the resulting shift in index origin of the samples
 */
  int i, j, i_max, j_max;
  vector r;

  i_max = rows(_index);
  j_max = cols(_index);

  r = zeros(i_max,j_max);

  for( i = 1; i <= i_max; ++i)
    for( j = 1; j <= j_max; ++j)
      r(i,j) = _index(i,j) + (j-1) * M;

  return r;
}


matrix get_paths( vector _index, vector _perm, int _el)
{
/* for K>1
 *
 * this function needs to be adjusted to handle truncation of survivors
 * and the resulting shift in index origin of the samples
 *
 * This functions isn't right for MISSED > 0
 */

  int j, c;
  matrix xex;
  vector combo;

  c = cols(_index);
  xex = zeros( K, c);

  for( j = 1; j <= c; ++j) {
    if( VIT_NEW) {
      combo = combos(_index(_el,j),:);
      xex(:,j) = (j-1)*MM + combo( perms( _perm(_el,j), :))';
/*
      xex(:,j) = (j-1)*MM + combo( perms( _vp(_index(_el,j)+j*M,_el) ))';
*/
    } else {
      combo = combos(_index(j),:);
      xex(:,j) = (j-1)*MM + combo(perms( _vp( _index(j), j+1)))';
    }
  }

  /* exy1 = xv(xex(1));	/* assuming K == 2 here */
  /* exy2 = xv(xex(2)); */

  return xex;
}


vector get_exx( int _el, int _k)
{
/* get x values from xv using ex for K>1 */

  matrix x;

  if( K == 1) {
    print( "get_exx: don't call me when K=1 !!!");
    return 0;
  }

  x = ex( 1+(_el-1)*K:_el*K);
  x = xv(x(_k));
  x = x(:,1);	/* x values are in 1st column of xv */

  return x;
}

vector get_exy( int _el, int _k)
{
/* get y values from xv using ex for K>1 */

  matrix x;

  if( K == 1) {
    print( "get_exy: don't call me when K=1 !!!");
    return 0;
  }

  x = ex( 1+(_el-1)*K:_el*K);
  x = xv(x(_k));
  x = x(:,2);	/* y values are in 2nd column of xv */

  return x;
}


vector get_kex( int _el, int _k)
{
/* get x and y values from kalman kxv for K>1 */

  int i, i1;
  vector j;
  matrix r;

  if( K == 1) {
    print( "get_kex: don't call me when K=1 !!!");
    return 0;
  }

  r = zeros(N,2);

  for( i = 1; i <= N; ++i) {
    j = [1 3] + (el_index(_el,i)-1)*4;
    i1 = _k + ((i-1)*M + (path_index(_el,i)-1))*K;
    r(i,:) = kxv(i1,j);
  }

  return r;
}


matrix get_kxv( int _el)
{
/*
 * get _el'th best kalman path for K=1
 */
  int i;
  vector j;
  matrix r;

  if( K != 1) {
    print( "get_kxv: don't call me when K>1 !!!");
    return 0;
  }

  r = zeros(N,4);

  for( i = 1; i <= N; ++i) {
    j = [1:4] + (el_index(_el,i)-1)*4;
    r(i,:) = kxv( path_index(_el,i)+(i-1)*M, j);
  }

  return r;
}
