/* nag_kalman_sqrt_filt_info_var (g13ecc)  Example Program.
 *
 * Copyright 1993 Numerical Algorithms Group
 *
 * Mark 3, 1993
 * Mark 8 revised, 2004.
 */

#include <nag.h>
#include <stdio.h>
#include <nag_stdlib.h>
#include <nagg13.h>

#define AINV(I,J) ainv[(I)*tdainv + J]
#define QINV(I,J) qinv[(I)*tdqinv + J]
#define RINV(I,J) rinv[(I)*tdrinv + J]
#define T(I,J) t[(I)*tdt + J]
#define B(I,J) b[(I)*tdb + J]
#define C(I,J) c[(I)*tdc + J]
int main(void)
{

  Integer exit_status=0, i, istep, j, m, n, p, tdainv, tdb, tdc, tdqinv, tdrinv;
  Integer tdt;
  NagError fail;
  Nag_ab_input inp_ab;
  double *ainv=0, *b=0, *c=0, *qinv=0, *rinv=0, *rinvy=0, *t=0, tol, *x=0;
  double *z=0;

  INIT_FAIL(fail);
  Vprintf("nag_kalman_sqrt_filt_info_var (g13ecc) Example Program Results\n");

  /* Skip the heading in the data file   */
  Vscanf("%*[^\n]");

  Vscanf("%ld%ld%ld%lf",&n,&m,&p,&tol);
  if (n>=1 && m>=1 && p>=1)
    {
      if ( !( ainv = NAG_ALLOC(n*n, double)) ||
           !( qinv = NAG_ALLOC(m*m, double)) ||
           !( rinv = NAG_ALLOC(p*p, double)) ||
           !( t = NAG_ALLOC(n*n, double)) ||
           !( b = NAG_ALLOC(n*m, double)) ||
           !( c = NAG_ALLOC(p*n, double)) ||
           !( x = NAG_ALLOC(n, double)) ||
           !( z = NAG_ALLOC(m, double)) ||
           !( rinvy = NAG_ALLOC(p, double)) )
        {
          Vprintf("Allocation failure\n");
          exit_status = -1;
          goto END;
        }
      tdainv = n;
      tdqinv = m;
      tdrinv = p;
      tdt = n;
      tdb = m;
      tdc = n;
    }
  else
    {
      Vprintf("Invalid n or m or p.\n");
      exit_status = 1;
      return exit_status;
    }
  inp_ab = Nag_ab_prod;

  /* Read data */
  for (i=0; i<n; ++i)
    for (j=0; j<n; ++j)
      Vscanf("%lf", &AINV(i,j));
  for (i=0; i<p; ++i)
    for (j=0; j<n; ++j)
      Vscanf("%lf", &C(i,j));
  if (rinv)
    for (i=0; i<p; ++i)
      for (j=0; j<p; ++j)
        Vscanf("%lf", &RINV(i,j));
  for (i=0; i<n; ++i)
    for (j=0; j<m; ++j)
      Vscanf("%lf", &B(i,j));
  for (i=0; i<m; ++i)
    for (j=0; j<m; ++j)
      Vscanf("%lf", &QINV(i,j));
  for (i=0; i<n; ++i)
    for (j=0; j<n; ++j)
      Vscanf("%lf", &T(i,j));
  for (j=0; j<m; ++j)
    Vscanf("%lf", &z[j]);
  for (j=0; j<n; ++j)
    Vscanf("%lf", &x[j]);
  for (j=0; j<p; ++j)
    Vscanf("%lf", &rinvy[j]);

  /* Perform three iterations of the (Kalman) filter recursion
     (in square root information form). */
  for (istep=1; istep<=3; ++istep)
    /* nag_kalman_sqrt_filt_info_var (g13ecc).
     * One iteration step of the time-varying Kalman filter
     * recursion using the square root information
     * implementation
     */
    nag_kalman_sqrt_filt_info_var(n, m, p, inp_ab, t, tdt, ainv, tdainv, b, tdb,
                                  rinv, tdrinv, c, tdc, qinv, tdqinv, x, rinvy,
                                  z, tol, &fail);
  if (fail.code != NE_NOERROR)
    {
      Vprintf("Error from nag_kalman_sqrt_filt_info_var (g13ecc).\n%s\n",
              fail.message);
      exit_status = 1;
      goto END;
    }

  Vprintf("\nThe inverse of the square root of the state covariance "
          "matrix is\n\n");
  for (i=0; i<n; ++i)
    {
      for (j=0; j<n; ++j)
        Vprintf("%8.4f ", T(i,j));
      Vprintf("\n");
    }

  Vprintf("\nThe components of the estimated filtered state are\n\n");
  Vprintf("k       x(k)  \n");
  for (i=0; i<n; ++i)
    {
      Vprintf("%ld  ",  i);
      Vprintf("  %8.4f  \n", x[i]);
    }

 END:
  if (ainv) NAG_FREE(ainv);
  if (qinv) NAG_FREE(qinv);
  if (rinv) NAG_FREE(rinv);
  if (t) NAG_FREE(t);
  if (b) NAG_FREE(b);
  if (c) NAG_FREE(c);
  if (x) NAG_FREE(x);
  if (z) NAG_FREE(z);
  if (rinvy) NAG_FREE(rinvy);
  return exit_status;

}