/* functions used with x9 and x10 */

double get_cost( double _zz, int _cur, int _next, int _col, int _perm, int _el)
{
/*
 * 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 rx, ry, dv;
  int i_perm;
  int i_el;

  i_perm = undef(_perm) ? 1 : _perm;	/* permutation number, used when K>1 */

  i_el = undef(_el) ? 1 : _el;		/* "l" number, useful when L>1 */

  /* handle dummy initial state */

  if( _col == 1)  {

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

    return rx;
  }

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

if(KALMAN) {

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

  return (rx*rx + ry*ry)/(2*Kv);

} else { /* energy cost */

  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) * MM;

  return r;
}


matrix get_paths( vector _index, vector _perm, int _el)
{
/*
 * this function needs to be adjusted to handle truncation of survivors
 * and the resulting shift in index origin of the samples
 */

  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 */

  matrix x;

  if( narg != 2 || _el < 1 || _el > L || _k < 1 || _k > K) {
    print( "get_exx: bad argument");
    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 */

  matrix x;

  if( narg != 2 || _el < 1 || _el > L || _k < 1 || _k > K) {
    print( "get_exx: bad argument");
    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;
}


matrix get_kxv( int _el)
{
/*
 * Get _el'th best Kalman path
 */
  int i, i_max;
  vector j;
  matrix r;

  i_max = cols(path_index);

  r = zeros(N,4);

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

  return r;
}
