/* IO(1) */


/*
 * one linear track with missed target events
 * for use with Kalman cost, K=1
 */

void get_x( int _K)
{
  int i, j;
  vector jj;
  matrix ppp;

  if( narg == 0) {

    LOADED=12;

    MISSED = 1;		/* add extra state for missed target event */

    TEST = 1;		/* for testing */

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

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

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


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

    print( M_clutter);


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

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

    /*  x = 0 ... 4
     *  y = 0 ... 4
     */
    xy1 = dt * [0:(N-1); 0:(N-1)]';
     v1 = ones(N,2);

    /*** missed target events ***/

    if( MISSED) {

/*
      xy1([6 11 16],:) = 4*rand(3,2);
*/
	xy1(2,:) = [10 10];

      missed_cost_1 = log(16);
      missed_cost_MM = MM * missed_cost_1;
    }


    /* clutter: x,y uniform 0 .. 4, v uniform -1 .. 1
     */

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

    xyr = vr = HUGE_VAL * ones( M_clutter*N, 2);

} else {

    xyr = 4 * rand( M_clutter*N, 2);
     vr = 2 * rand( M_clutter*N, 2) - 1;
}

    xyvr = reshape( [xyr vr], N, 4*M_clutter);


    xv = reshape( [xy1 v1 xyvr], MM*N, 4);

    /* add some noise */

    Kn = 0.01;	/* measurement noise variance */

if(TEST)	/* testing... */
 Kn=0;

     xv += sqrt(Kn) * rand( rows(xv), cols(xv), "gauss");


/* randomize the observation rows */

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

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

    zstate = 0;

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

    return;
  }


  /* narg > 0 */

  print( "get_x: K>1 not implemented for this example");
}



double get_cost( double _zz, int _cur, int _next, int _col, int _perm, int _el)
{
/* for K=1, kalman 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 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( _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;

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

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

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

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

}


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