/* IO(1) */

/*
 * 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=6;

    N = 40;	/* 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 */

    if( undef(example_num))
      example_num = 1;

    /* extra initial sample for initial state */

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

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

  if( example_num == 1) {	/* true paths intersect at 90 degree angle */

    xy1 = [ [0, 0:N-1]; [0, 0:N-1] ]';
     v1 = [ [0, ones(1,N)]; [0, ones(1,N)] ]';

    xy2 = [ [0, 0:N-1]; -[0, -(N-1):0] ]';
     v2 = [ [0, ones(1,N)]; [0, -ones(1,N)] ]';

    /* some random clutter samples, uniform over the same ranges as xy1, v1, xy2, v2 */

    xyr1 = (N-1) * rand( rows(xy1), cols(xy1));
     vr1 = 4 * rand( rows(v1), cols(v1)) - 2;

    xyr2 = (N-1) * rand( rows(xy1), cols(xy1));
     vr2 = 4 * rand( rows(v1), cols(v1)) - 2;

  } else if( example_num == 2) {	/* true paths intersect at a small angle */

    xy1 = [ [0, 0:N-1]; [0, 0:N-1]/(N-1) ]';
     v1 = [ [0, ones(1,N)]; [0, ones(1,N)]/(N-1) ]';

    xy2 = [ [0, 0:N-1]; -[0, -(N-1):0]/(N-1) ]';
     v2 = [ [0, ones(1,N)]; [0, -ones(1,N)]/(N-1) ]';

    /* some random clutter samples */

    xyr1 = [ (N-1)*rand(rows(xy1),1), rand(rows(xy1),1)];
     vr1 = [ 4*rand(rows(v1),1)-2, (4*rand(rows(v1),1)-2)/(N-1)];

    xyr2 = [ (N-1)*rand(rows(xy1),1), rand(rows(xy1),1)];
     vr2 = [ 4*rand(rows(v1),1)-2, (4*rand(rows(v1),1)-2)/(N-1)];

  } else if( example_num == 3) {	/* true paths intersect at 90 degree angle */

    xy1 = [ [0, 0:N-1]; [0, 0:N-1] ]';
     v1 = [ [0, ones(1,N)]; [0, ones(1,N)] ]';

    xy2 = [ [0, 0:N-1]; -[0, -(N-1):0] ]';
     v2 = [ [0, ones(1,N)]; [0, -ones(1,N)] ]';

    /* some random clutter samples based on adding random values to xy1, v1, xy2, v2 */

    /* with K=2, this can cause paths xy1 and xyr1 to be lower cost than xy1 and xy2 */

    xyr1 = xy1 + 2.0 * ( 2 * rand(rows(xy1),cols(xy1)) - 1);
     vr1 =  v1 + 0.5 * ( 2 * rand( rows(v1), cols(v1)) - 1);

    xyr2 = xy2 + 2.0 * ( 2 * rand(rows(xy1),cols(xy1)) - 1);
     vr2 =  v2 + 0.5 * ( 2 * rand( rows(v1), cols(v1)) - 1);

  } else {

    print( "bad example_num", example_num);
    return;

  }

/*
 * xv = [   0  0  0  0;   0  0  0  0;  r  r  r  r;  r  r  r  r;	-- t = 0
 *	  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+1), 4);

/* add some noise */

   xv += 0.01 * rand( rows(xv), cols(xv), "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 != 6) {
    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)
{
/*
 * 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;

  /* handle dummy initial state */

/* print( "get_yval: _cur, _next, _col, return = ", _cur, _next, _col); */

  if( _col == 1)  {
    rx = (_cur == _next) ? 0 : HUGE_VAL;
/* print( rx); */
    return rx;
  }

  /* xv row indices */

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

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

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

/* print( rx+ry); */

  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 j, j_max;
  vector r;

  j_max = cols(_index);
  r = zeros(1,j_max);

  for( j = 1; j <= j_max; ++j)
    r(j) = _index(j) + j * MM;

  return r;
}


matrix get_paths( vector _index, matrix _vp)
{
/*
 * 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) {
    combo = combos(_index(j),:);
    xex(:,j) = j*MM + combo(perms( _vp( _index(j), j+1)))';
  }

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

  return xex;
}
