/* nag_kalman_sqrt_filt_cov_invar(g13ebc)  Example Program
 *
 * Copyright 1993 Numerical Algorithms Group
 *
 * Mark 3, 1993
 * Mark 7, revised, 2001.
 *
 */

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

typedef enum {read, print} ioflag;

static void ex1(void);
static void ex2(void);


int main(void)
{
  ex1();
  ex2();
  return EXIT_SUCCESS;
}

#define NMAX 10
#define MMAX 10
#define PMAX 10
#define TRADIM 10

static void ex1(void)
{ /* simple example (matrices A and C are supplied in lower observer
     Hessenberg form) */

  double a[NMAX][TRADIM], b[NMAX][TRADIM], c[PMAX][TRADIM], k[NMAX][TRADIM],
  q[MMAX][TRADIM], r[PMAX][TRADIM], s[NMAX][TRADIM], h[NMAX][TRADIM];
  Integer i, j, m, n, p, istep;
  double tol;
  Integer nmax, mmax, pmax, tradim;

  Vprintf("g13ebc Example 1 Program Results\n");
  /* Skip the heading in the data file  */
  Vscanf(" %*[^\n]");

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

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

      exit(EXIT_FAILURE);
    }

  /* Read data */
  for (i=0; i<n; ++i)
    for (j=0; j<n; ++j)
      Vscanf("%lf",&s[i][j]);
  for (i=0; i<n; ++i)
    for (j=0; j<n; ++j)
      Vscanf("%lf",&a[i][j]);
  for (i=0; i<n; ++i)
    for (j=0; j<m; ++j)
      Vscanf("%lf",&b[i][j]);

  if (q)
    {
      for (i=0; i<m; ++i)
        for (j=0; j<m; ++j)
          Vscanf("%lf", &q[i][j]);
    }
  for (i=0; i<p; ++i)
    for (j=0; j<n; ++j)
      Vscanf("%lf",&c[i][j]);
  for (i=0; i<p; ++i)
    for (j=0; j<p; ++j)
      Vscanf("%lf", &r[i][j]);

  /* Perform three iterations of the Kalman filter recursion  */
  for (istep=1; istep<=3; ++istep)
    g13ebc(n, m, p, &s[0][0], tradim, &a[0][0],
           tradim, &b[0][0], tradim, &q[0][0], tradim,
           &c[0][0], tradim, &r[0][0], tradim,
           &k[0][0], tradim, &h[0][0], tradim, tol, NAGERR_DEFAULT);

  Vprintf("\nThe 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 ", s[i][j]);
      Vprintf("\n");
    }
  if (k)
    {
      Vprintf("\nThe matrix AK (the product of the Kalman gain\n");
      Vprintf("matrix with the state transition matrix) is\n\n");
      for (i=0; i<n; ++i)
        {
          for (j=0; j<p; ++j)
            Vprintf("%8.4f ", k[i][j]);
          Vprintf("\n");
        }
    }
}

static void mat_io(Integer n, Integer m, double mat[], Integer tdmat,
                   ioflag flag, const char *message);


static void ex2(void)
{ /* more general example which requires the data to be transformed. The
     results produced by g13eac and g13ebc are compared */

  double a[NMAX][TRADIM], b[NMAX][TRADIM], c[PMAX][TRADIM], ke[NMAX][TRADIM],
  kf[NMAX][TRADIM], ub[NMAX][TRADIM], q[MMAX][TRADIM], r[PMAX][TRADIM],
  rwork[NMAX][TRADIM], sf[NMAX][TRADIM], se[NMAX][TRADIM], h[NMAX][TRADIM];
  double pf[NMAX][TRADIM], pe[NMAX][TRADIM], uaut[NMAX][TRADIM],
  cut[PMAX][TRADIM], u[NMAX][TRADIM];
  double diag[NMAX];
  Integer i, j, m, n, p, istep;
  Integer nmax, mmax, pmax,tradim;

  Nag_ObserverForm reduceto  = Nag_LH_Observer;
  double detf, tol, zero = 0.0, one = 1.0;
  Integer dete, ione = 1;

  Vprintf("\ng13ebc Example 2 Program Results \n\n");
  /* skip the heading in the data file */
  Vscanf(" %*[^\n]");

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

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

      exit(EXIT_FAILURE);
    }
  mat_io(n, n, &se[0][0], tradim, read, "");
  mat_io(n, n, &a[0][0], tradim, read, "");
  mat_io(n, m, &b[0][0], tradim, read, "");
  if (q)
    mat_io(m, m, &q[0][0], tradim, read,"");
  mat_io(p, n, &c[0][0], tradim, read, "");
  mat_io(p, p, &r[0][0], tradim, read, "");
  for (i=0; i<n; ++i)
    {
      for (j=0; j<n; ++j)
        {
          if (i<p)
            cut[i][j] = c[i][j];
          sf[i][j] = se[i][j];
          uaut[i][j] = a[i][j];
          u[i][j] = zero;
        }
      u[i][i] = one;
    }
  /* Set up the matrix pair (A,C) in the lower observer hessenberg form */
  g13ewc(n, p,  reduceto, &uaut[0][0], tradim, &cut[0][0], tradim,
         &u[0][0], tradim, NAGERR_DEFAULT);
  for (j=0; j<m; ++j)
    for (i=0; i<n; ++i)
      ub[i][j] = f06eac(n, &u[i][0], ione, &b[0][j], tradim);

  /* Generate noise covariance matrices PE and PF = U * PE * U' */
  f06yac(NoTranspose, Transpose, n, n, n, one, &se[0][0], tradim,
         &se[0][0], tradim, zero, &pe[0][0], tradim);
  f06yac(NoTranspose, Transpose, n, n, n, one, &pe[0][0], tradim,
         &u[0][0], tradim, zero, &rwork[0][0], tradim);
  f06yac(NoTranspose, NoTranspose, n, n, n, one, &u[0][0], tradim,
         &rwork[0][0], tradim, zero, &pf[0][0], tradim);

  /* Now find the lower triangular (left) cholesky factor of PF. */
  f03aec(n, &pf[0][0], tradim, diag, &detf, &dete, NAGERR_DEFAULT);
  for (i=0; i<n; ++i)
    {
      sf[i][i] = one/diag[i];
      for (j=0; j<i; ++j)
        sf[i][j] = pf[i][j];
    }
  /* Perform three steps of the Kalman filter recursion */
  for (istep=1; istep<=3; ++istep)
    {
      g13eac(n, m, p, &se[0][0], tradim, &a[0][0],
             tradim, &b[0][0], tradim, &q[0][0],
             tradim, &c[0][0], tradim, &r[0][0],
             tradim, &ke[0][0], tradim, &h[0][0],
             tradim, tol, NAGERR_DEFAULT);
      g13ebc(n, m, p, &sf[0][0], tradim, &uaut[0][0],
             tradim, &ub[0][0], tradim, &q[0][0],
             tradim, &cut[0][0], tradim, &r[0][0],
             tradim, &kf[0][0], tradim, &h[0][0],
             tradim, tol, NAGERR_DEFAULT);
    }
  f06yac(NoTranspose, Transpose, n, n, n, one, &se[0][0], tradim,
         &se[0][0], tradim, zero, &pe[0][0], tradim);
  f06yac(NoTranspose, Transpose, n, n, n, one, &sf[0][0], tradim,
         &sf[0][0], tradim, zero, &pf[0][0], tradim);
  mat_io(n,n,&pe[0][0],tradim,print,"Covariance matrix PE from g13eac is\n");
  mat_io(n,n,&pf[0][0],tradim,print,"Covariance matrix PF from g13ebc is\n");

  /* Calculate PF = U' * PF * U */
  f06yac(NoTranspose, NoTranspose, n, n, n, one, &pf[0][0], tradim,
         &u[0][0], tradim, zero, &rwork[0][0], tradim);
  f06yac(Transpose, NoTranspose, n, n, n, one, &u[0][0], tradim,
         &rwork[0][0], tradim, zero, &pf[0][0], tradim);
  mat_io(n, n, &pf[0][0], tradim, print, "Matrix U' * PF * U is \n");
  mat_io(n, p, &ke[0][0], tradim, print, "The matrix KE from g13eac is\n");
  mat_io(n, p, &kf[0][0], tradim, print, "The matrix KF from g13ebc is\n");

  /* calculate U' * K */
  f06yac(Transpose, NoTranspose, n, p, n, one, &u[0][0], tradim,
         &kf[0][0], tradim, zero, &rwork[0][0], tradim);
  mat_io(n, p, &rwork[0][0], tradim, print, "U' * KF is\n");
}


static void mat_io(Integer n, Integer m, double mat[], Integer tdmat,
                   ioflag flag, const char *message)
{
  Integer i, j;
#define MAT(I,J) mat[((I)-1)*tdmat + (J)-1]
  if (flag==print) Vprintf("%s \n", message);
  for (i=1; i<=n; ++i)
    {
      for (j=1; j<=m; ++j)
        {
          if (flag==read) Vscanf("%lf", &MAT(i,j));
          if (flag==print) Vprintf("%8.4f ", MAT(i,j));
        }
      if (flag==print) Vprintf("\n");
    }
  if (flag==print) Vprintf("\n");
} /* mat_io */