/* 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)
{
  vector i1 = [ 1+(_cur-1)*MM:_cur*MM];
  vector i2 = i1 + MM;
  int el;
  vector el12, el34;

  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)
{
  if( K > 1) {
    print("*** kalman is not yet implemented for K>1 ***");
    return;
  }

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

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

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

  kxv = zeros( MM*N, 4*L);	/* each row: [x xv y yv] L times */

  kxv(1:MM,1:4) = xv(1:MM,[1 3 2 4]);

  kalman_gain_update();		/* initial update */

  kalman_predict(1);
}

/*
 * kalman_update: update predicted values and gain
 *
 * _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;
  matrix k1, k2;
  int i, j;
  int r, c;
  vector el12, el34;
  vector j12, j34;
  vector iv;

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

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

  k2 = k1 = kxv(ii,:);

  r = rows(_v);
  c = cols(_v);

  for( i = 1; i <= r; ++i)
    for( j = 1; j <= c; ++j) {

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

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

      iv = _v(i,j);
      
      iii = ii(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,:) = k2;	/* update kxv */

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