/* IO(1) */

/*
 * two linear tracks which cross in the middle
 */

/*
 * Reference: J. K. Wolf, A. M. Viterbi, and G. S. Dixon,
 * Finding the Best Set of K Paths Through a Trellis
 * with Application to Multitarget Tracking,
 * IEEE AES-25, No. 2, March 1989, pp. 287-296
 */

void get_x( int _K)
{
/* data setup for simple x/y/velocity targets */

  int i, j;
  vector jj;
  matrix ppp;

  if( narg == 0) {

    LOADED=10;

    N = 5;	/* N data samples */
    M = 4;	/* M symbol values */
    K = 1;	/* find K best completely unmerged paths */

    MM = M;	/* save original M, used when K > 1 */

/*** note: dt is global and is used in get_cost() below ***/

    dt = 4/(N-1);	/* time will go from 0 to 4 */

    xy1 = [ 0 0; 1 1; 2 2; 3 3; 4 4];
     v1 = [ 1 1; 1 1; 1 1; 1 1; 1 1];

    xy2 = [ 0  4; 1  3; 2  2; 3  1; 4  0];
     v2 = [ 1 -1; 1 -1; 1 -1; 1 -1; 1 -1];

   xyr1 = vr1 = xyr2 = vr2 = HUGE_VAL * ones( rows(xy1), cols(xy1));

/*
 * xv = [ xy1(1) v1(1); xy2(1) v2(1);  r  r  r  r;  r  r  r  r;	-- t = 1
 *	  xy1(2) v1(2); xy2(2) v2(2);  r  r  r  r;  r  r  r  r;	-- t = 2
 *	  ...
 *	  xy1(N) v1(N); xy2(N) v2(N);  r  r  r  r;  r  r  r  r;	-- t = N
 *	]
 */
    xv = reshape( [xy1 v1 xy2 v2 xyr1 vr1 xyr2 vr2], M*N, 4);

/* add some noise */

/* 0.01 stdev was used for the Asilomar summary */

/*
   xv += 0.2 * rand( rows(xv), cols(xv), "gauss");
*/

/* just add noise to the x/y positions, not velocities */

/*
   xv(:,[1 2]) = xv(:,[1 2]) + 0.2 * rand( rows(xv), 2, "gauss");
*/

/* randomize the observation rows */

/*
    ppp = permutations(1:M);

    for( i = 2; i <= N; ++i) {
      j = 1 + (i-1) * M;
      jj = [j:j+M-1];
      xv(jj) = xv( jj( ppp( rand(1,1,"int",M))));
    }
*/
    zstate = 0;

    z = zeros(1,N);	/* dummy */

    return;
  }

  /* narg > 0 */

  if( LOADED != 10) {
    print( "get_x: call get_x with no args first.");
    return;
  }

  if( _K > MM) {
    printf( "get_x: K > M not allowed!");
    return;
  }

  K = _K;
  combos = combinations( 1:MM, K);
  perms = permutations( 1:K);
  M = rows(combos);

  zstate = 0;
}


double get_cost( double _zz, int _cur, int _next, int _col, int _perm)
{
/*
 * 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;

  i_perm = undef(_perm) ? 1 : _perm;	/* permutation number, used when K>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;

  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;
}
