/* IO(1) */

load x9_10;

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

    MISSED = 1;         /* if non-zero, add extra state(s) for missed target events */

    UNUSED_PENALTY = 0;	/* if non-zero, add penalty to cost for unused measurements */

    N = 21;		/* N data samples */
    MM = 4;		/* MM measurements per unit time */
    K = 1;		/* find K best completely unmerged paths */

    TEST = 0;           /* for testing */

    M = MM + MISSED;    /* M states */

    M_clutter = MM - 2; /* number of clutter points per unit time */

    if( MISSED)
      print( "Using missed target events.");
    else
      print( "Not using missed target events.");

    if( UNUSED_PENALTY)
      print( "Using penalty for unused measurements.");
    else
      print( "Not using penalty for unused measurements.");

    print( M_clutter);

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

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

    xy1_orig = [ t; 2*(t.*(1-t/4))]';
     v1_orig = [ ones(1,N); 2-t]';

    xy2_orig = [ t; 4-2*(t.*(1-t/4))]';
     v2_orig = [ ones(1,N); t-2]';

/* add some noise */

    if(TEST)
      Kn = 0.0;
    else
      Kn = 0.01;	/* measurement noise variance */

    xy1 = xy1_orig + sqrt(Kn) * rand( rows(xy1_orig), cols(xy1_orig), "gauss");
     v1 =  v1_orig + sqrt(Kn) * rand( rows( v1_orig), cols( v1_orig), "gauss");
    xy2 = xy2_orig + sqrt(Kn) * rand( rows(xy2_orig), cols(xy2_orig), "gauss");
     v2 =  v2_orig + sqrt(Kn) * rand( rows( v2_orig), cols( v2_orig), "gauss");


    if( MISSED) {

      /* xy1([ 9 10 11 12 13],:) = HUGE_VAL*ones(5,2); */

      xy1([ 9 10 11 12 13],:) = 4 * rand(5,2);
    }

    if( MISSED || UNUSED_PENALTY) {
      missed_cost_1 = log(1);	/* modified version, log(1) instead of log(16). See clutter below */
      missed_cost_MM = MM * missed_cost_1;
    }

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

if( M_clutter > 0) {

 if(TEST) /*** testing with clutter as HUGE_VAL's ***/

  xyr1 = vr1 = xyr2 = vr2 = HUGE_VAL * ones( N, 2);

 else {
/* original, region x,y 0..4:
    xyr1 = 4 * rand( N, 2);
     vr1 = 2 * rand( N, 2) - 1;

    xyr2 = 4 * rand( N, 2);
     vr2 = 2 * rand( N, 2) - 1;

modified version, 28 Jan. 1999, region -.5 .. .5 around actual x,y positions:
*/
    xyr1 = xy1_orig + rand( N, 2) - 0.5;
     vr1 = 2 * rand( N, 2) - 1;	/* velocity doesn't matter */

    xyr2 = xy2_orig + rand( N, 2) - 0.5;
     vr2 = 2 * rand( N, 2) - 1;	/* velocity doesn't matter */
 }
}


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

  if( M_clutter > 0)
    xv = reshape( [xy1 v1 xy2 v2 xyr1 vr1 xyr2 vr2], M*N, 4);
  else
    xv = reshape( [xy1 v1 xy2 v2], M*N, 4);


/* randomize the observation rows */

if(0) {
    ppp = permutations(1:M);

    for( i = 1; 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 = [1:N];	/* use time index for z data */

    return;
  }

  /* narg > 0 */

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

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

  K = _K;

  setup_combos(); /* setup global variables: perms, combos, unique_perms, MP, and M */

  zstate = 0;
}
