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

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

#define NMAX 20
#define MMAX 20
#define PMAX 20

#define TDA NMAX
#define TDB MMAX
#define TDC NMAX
#define TDQ MMAX
#define TDR PMAX
#define TDT NMAX

int main(void)
{

  double ainv[NMAX][TDA];
  double qinv[MMAX][TDQ];
  double rinv[PMAX][TDR];
  double t[NMAX][TDT];
  double b[NMAX][TDB];
  double c[PMAX][TDC];

  Integer i, j, m, n, p;
  double x[NMAX], z[MMAX];
  Integer istep;
  double rinvy[PMAX];
  Nag_ab_input inp_ab;
  double tol;
  Integer nmax, mmax, pmax;

  Vprintf("g13ecc Example Program Results\n");

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

  nmax = NMAX;
  mmax = MMAX;
  pmax = PMAX;

  Vscanf("%ld%ld%ld%lf",&n,&m,&p,&tol);
  if (n<=0 || m<=0 || p<=0 ||
      n>nmax || m>mmax || p>pmax)
    {
      Vfprintf(stderr, "One of n m or p is out of range "
               "n = %ld, m = %ld, p = %ld\n", n, m, p);

      return EXIT_FAILURE;
    }

  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)
    g13ecc(n, m, p, inp_ab, &t[0][0], (Integer)TDT,
           &ainv[0][0], (Integer)TDA, &b[0][0],
           (Integer)TDB, &rinv[0][0], (Integer)TDR,
           &c[0][0], (Integer)TDC, &qinv[0][0],
           (Integer)TDQ, x, rinvy, z,
           tol, NAGERR_DEFAULT);

  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]);
    }

  return EXIT_SUCCESS;
}