/* IO(1) */

/*
 * kalman_gain_update: update Ke, Kv, and G
 */
void kalman_gain_update(void)
{
  Ke = A*Ke*AT + Ku;		/* Ke(l|l-1) */

  Kv = Ke(1,1) + Kn;		/* assuming C=[1 0] */

  G = Ke(:,1) / Kv;		/* Ke * C' * inv(Kv) */

  GT = G';

  Ke = Ke - G * Ke(1,:);	/* Ke(l|l) = Ke(l|l-1) - G * C * Ke(l|l-1) */
}


/*
 * kalman_predict: compute innovations for next step
 */
void kalman_predict( int _cur)
{
  int el;
  vector i1, i2;
  vector el12, el34;
  int size;

  size = (K > 1) ? M*K : M;

  i1 = [ 1+(_cur-1)*size:_cur*size];
  i2 = i1 + size;

  for( el=0; el<L; ++el) {
    el12 = [1:2] + el*4;
    el34 = el12 + 2;
    kxv(i2,el12) = kxv(i1,el12)*AT;	/* x's and xv's */
    kxv(i2,el34) = kxv(i1,el34)*AT;	/* y's and yv's */
  }
}


/*
 * kalman_init: initialize kalman stuff
 */
void kalman_init(void)
{
  int i;
  vector i1;
  matrix txv;

  A = [ 1 dt; 0 1];	/* for one dimension, position and velocity */
  AT = A';

  C = [ 1 0];
  Ke = eye(2,2);
  S2a = 2.00;		/* S2a*dt is noise acceleration spectral height */

  Ku = [ dt*dt*dt*dt/3  dt*dt*dt/2;  dt*dt*dt/2  dt*dt] * S2a;

  /* each row of kxv has [x xv y yv] repeated L times */

  if( K > 1) {
    kxv = zeros( M*K*N, 4*L);
    txv = zeros( max(M,MM), 4);	/* changed M to max(M,MM) to handle K==MM case, 11/24/1998 */
    txv(1:MM,:) = xv(1:MM,:);
    for( i = 1; i <= M; ++i) {
      i1 = 1 + (i-1)*K;
      i1 = [i1:i1+K-1];
      kxv(i1,1:4) = txv(combos(i,:),[1 3 2 4]);
    }
  } else {
    kxv = zeros( M*N, 4*L);
    kxv(1:MM,1:4) = xv(1:MM,[1 3 2 4]);
  }

 for(i=0; i<10; ++i) /* try to get close to convergence */
  kalman_gain_update();		/* initial update */

  kalman_predict(1);
}


/*
 * kalman_update: update predicted values and gain for K=1
 *
 * _v comes from the v array in viterbi and specifies
 * the indices of the previous states representing the
 * lowest cost paths.
 *
 * _vl comes from the vl array in viterbi and specifies
 * the "l" values associated with the previous states.
 */
void kalman_update( int _j, matrix _v, matrix _vl)
{
  vector ii_m, ii_mm;
  matrix k1, k2;
  int iii, i, j;
  vector el12, el34;
  vector j12, j34;
  vector iv;

  if( _j == 1)	/* kalman_init() already handled this case */
    return;

  ii_m =  [ 1+(_j-1)*M:_j*M];	/* row indices for kxv */
  ii_mm = [ 1+(_j-1)*MM:_j*MM];	/* row indices for xv */

  k2 = k1 = kxv(ii_m,:);

  for( i = 1; i <= M; ++i)
    for( j = 1; j <= L; ++j) {

      if( _v(i,j) == -1)
        continue;

      iv = _v(i,j);
      
      el12 = [1:2] + (_vl(i,j)-1)*4;
      el34 = el12 + 2;
      
      j12 = [1:2] + (j-1)*4;
      j34 = j12 + 2;

      if( i > MM) {	/* for missed target event, no update needed */

        k2(i,j12) = k1(iv,el12);

        k2(i,j34) = k1(iv,el34);

      } else {

        iii = ii_mm(i);

        k2(i,j12) = k1(iv,el12) + (xv(iii,1) - k1(iv,el12(1))) * GT;	/* update x position and velocity */

        k2(i,j34) = k1(iv,el34) + (xv(iii,2) - k1(iv,el34(1))) * GT;	/* update y position and velocity */
      }
    }

  kxv(ii_m,:) = k2;	/* update kxv */

  if( _j < N) {
    kalman_gain_update();
    kalman_predict(_j);
  }
}


/*
 * kalman_perm_update: update predicted values and gain for K>1
 *
 * _v comes from the v array in viterbi and specifies
 * the indices of the previous states representing the
 * lowest cost paths.
 *
 * _vl comes from the vl array in viterbi and specifies
 * the "l" values associated with the previous states.
 *
 * _vp comes from the vp array in viterbi and specifies
 * the permutation values associated with the current states.
 */
void kalman_perm_update( int _j, matrix _v, matrix _vl, matrix _vp)
{
  vector ii, kk, i1, i2;
  vector combo, perm, cperm;
  matrix txv, k1, k2;
  int i, j, k, ki, size;
  vector el14, j14;

  if( _j == 1)	/* kalman_init() already handled this case */
    return;

if(DEBUG>2) {
  print( "kalman_perm_update:");
  print( "_v = ", _v);
  print( "_vl = ", _vl);
  print( "_vp = ", _vp);
}

  size = M*K;

  kk = [1+(_j-1)*size:_j*size];		/* row indices for kxv */

  ii = [1+(_j-1)*MM:_j*MM];		/* row indices for xv */

  k2 = k1 = kxv(kk,:);

  for( i = 1; i <= M; ++i) {

    combo = combos(i,:);

    i2 = 1 + (i-1)*K;		/* index range for k2 */
    i2 = [i2:i2+K-1];

    for( j = 1; j <= L; ++j) {

      if( _v(i,j) == -1)
        continue;

      i1 = 1 + (_v(i,j)-1)*K;	/* index range for k1 */
      i1 = [i1:i1+K-1];

      perm = perms(_vp(i,j));

      cperm = combo(perm);

      el14 = [1:4] + (_vl(i,j)-1)*4;
      j14  = [1:4] + (j-1)*4;

if( DEBUG>2) {
  print( "i1 = ", i1);
  print( "i2 = ", i2);
  print( "el14 = ", el14);
  print( "j14 = ", j14);
}

      txv = zeros(K,4);

      for( k = 1; k <= K; ++k)	{ /* only update for non-missed target events */
	ki = cperm(k);
	if( ki <= MM) {
	  ki = ii(ki);
	  txv(k,1:2) = (xv(ki,1) - k1(i1(k),el14(1))) * GT;
	  txv(k,3:4) = (xv(ki,2) - k1(i1(k),el14(3))) * GT;
	}
      }

      k2(i2,j14) = k1(i1,el14) + txv;		/* update x,y positions and velocities */

    }
  }

  kxv(kk,:) = k2;	/* update kxv */

  if( _j < N) {
    kalman_gain_update();
    kalman_predict(_j);
  }
}

/*
 * get_kalman_perm: used instead of get_perm() if K>1
 *
 * Note: _vp isn't used here because the kalman results
 * are already stored in permuted form.
 */
vector get_kalman_perm( int _L, double _zz, int _vp, int _cur, int _next, int _j, int _el)
{
  int i, j, i1, i2, ii1, ii2, i_best;
  double rx, ry, b, best;
  double extra = log2pi + log(Kv);	/* added 28 Jan 1999 */
  vector s_next, s_tmp;
  int up;

if( DEBUG>2) print( "get_kalman_perm:", "_cur = ", _cur, "_next = ", _next);

  i_best = 1;
  best = HUGE_VAL;
  if( _L > 1) {
    best *= ones(1,MP);		/* need to return all choices if _L > 1 */
    i_best = [1:MP];
  }


  /* handle dummy initial state */

  if( _j == 1)  {

    if( _cur == _next)
      best(1) = 0;	/* zero cost only for permutation number 1 */

    return [best; i_best];
  }


  if( _j == 2 && any(combos(_cur,:) > MM))   /* don't follow initial missed target event states */
    return [best; i_best];

 
  s_next = combos(_next,:);	/* for _next combinations */

  i1 = ((_j-1)*M + (_cur-1))*K;	/* offset for kxv */

  i2 = (_j-1)*MM;		/* offset for xv */

if( DEBUG>2) print( "i1 = ", i1, "i2 = ", i2);

  for( i = 1; i <= MP; ++i) {

    up = unique_perms(_next,i);

    if( up == 0)	/* no more unique permutations */
      break;

    s_tmp = s_next(perms(up,:));

if( DEBUG>2) print( "s_tmp = ", s_tmp);

    b = 0;

    if( UNUSED_PENALTY || any(s_tmp > MM))
      b = (MM - K) * missed_cost_1;

    for( j = 1; j <= K; ++j) {

      if( s_tmp(j) > MM) {		/* missed target state */
        b += missed_cost_1;
	continue;
      }

      ii1 = i1+j;
      ii2 = i2 + s_tmp(j);

      if( xv(ii2,1) == HUGE_VAL) {	/* for testing with clutter never chosen */
        b = HUGE_VAL;
	break;
      }

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

      b += (rx*rx + ry*ry)/(2*Kv) + extra;
    }

if( DEBUG>2) print( "b = ", b);

    if( _L > 1)
      best(i) = b;
    else if( b < best) {
      i_best = i;
      best = b;
    }
  }

  return [best; i_best];
}
