/* nag_kalman_sqrt_filt_info_invar(g13edc)  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 20
#define MMAX 20
#define PMAX 20
#define TRADIM 20

static void ex1(void)
{
  double ainv[NMAX][TRADIM],qinv[MMAX][TRADIM],rinv[PMAX][TRADIM],
  t[NMAX][TRADIM],ainvb[NMAX][TRADIM];
  double c[PMAX][TRADIM],x[NMAX],z[MMAX],rinvy[PMAX];
  Integer i,j,m,n,p,istep;
  double tol;
  Integer nmax, mmax, pmax, tradim;

  Vprintf("g13edc 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", &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", &ainvb[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  */

  for (istep=1; istep<=3; ++istep)
    g13edc(n, m, p, &t[0][0], tradim, &ainv[0][0],
           tradim, &ainvb[0][0], tradim, &rinv[0][0],
           tradim, &c[0][0], tradim, &qinv[0][0],
           tradim, 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]);
    }
}

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

static void ex2(void)
{

  double ainv[NMAX][TRADIM], ainvb[NMAX][TRADIM], c[PMAX][TRADIM],
  ainvu[NMAX][TRADIM], ainvbu[NMAX][TRADIM];
  double qinv[MMAX][TRADIM], rinv[PMAX][TRADIM], t[NMAX][TRADIM], x[NMAX],
  z[NMAX];
  double rwork[NMAX][TRADIM], tu[NMAX][TRADIM], rinvy[PMAX], ig[NMAX][TRADIM],
  ih[NMAX][TRADIM];
  double cu[PMAX][TRADIM], u[PMAX][TRADIM], ux[PMAX];
  double diag[NMAX];
  double detf, tol, zero = 0.0, one = 1.0;
  Integer dete, i, j, m, n, p, istep, ione = 1;
  Nag_ab_input inp_ab = Nag_ab_prod;
  Nag_ControllerForm reduceto  = Nag_UH_Controller;
  Integer nmax, mmax, pmax, tradim;

  Vprintf("\n\ng13edc Example 2 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 */
  mat_io(n, n, &ainv[0][0], tradim, read, "");
  mat_io(p, n, &c[0][0], tradim, read, "");
  if (rinv)
    mat_io(p, p, &rinv[0][0], tradim, read,"");
  mat_io(n, m, &ainvb[0][0], tradim, read, "");
  mat_io(m, m, &qinv[0][0], tradim, read, "");
  mat_io(n, n, &t[0][0], tradim, read, "");
  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]);

  for (i=0; i<n; ++i)     /* Initialise the identity matrix u */
    {
      for (j=0; j<n; ++j)
        u[i][j] = zero;
      u[i][i] = one;
    }

  /* Copy the arrays ainv[] and ainvb[] into ainvu[] and ainvbu[] */
  for (i=0; i< n; ++i)
    f06efc(n, &ainv[0][i], tradim, &ainvu[0][i], tradim);
  for (j=0; j<m; ++j)
    f06efc(n, &ainvb[0][j], tradim, &ainvbu[0][j], tradim);

  /* Transform (ainvu[],ainvbu[]) to reduceto controller Hessenberg form */
  g13exc(n, m, reduceto, &ainvu[0][0], tradim, &ainvbu[0][0], tradim,
         &u[0][0], tradim, NAGERR_DEFAULT);

  /* Calculate the matrix cu = c*u'    */
  f06yac(NoTranspose, Transpose, p, n, n, one, &c[0][0], tradim,
         &u[0][0], tradim, zero, &cu[0][0], tradim);

  /* Calculate the vector ux = u*x     */
  f06pac(NoTranspose, n, n, one, &u[0][0], tradim, x, ione,
         zero, ux, ione);

  /* Form the information matrices ih = u*ig*u' and ig = t'*t     */
  f06yac(Transpose, NoTranspose, n, n, n, one, &t[0][0], tradim,
         &t[0][0], tradim, zero, &ig[0][0], tradim);
  f06yac(NoTranspose, Transpose, n, n, n, one, &ig[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, &ih[0][0], tradim);

  /* Now find the reduceto triangular (right) cholesky factor of ih */
  f03aec(n, &ih[0][0], tradim, diag, &detf, &dete, NAGERR_DEFAULT);
  for (i=0; i< n; ++i)
    {
      tu[i][i] = one/diag[i];
      for (j=0; j<i; ++j)
        {
          tu[j][i] = ih[i][j];
          tu[i][j] = zero;
        }
    }

  /* Do three iterations of the Kalman filter recursion  */
  for (istep=1; istep<=3; ++istep)
    {
      g13ecc(n, m, p, inp_ab, &t[0][0], tradim, &ainv[0][0],
             tradim, &ainvb[0][0], tradim, &rinv[0][0], tradim,
             &c[0][0], tradim, &qinv[0][0], tradim, x,
             rinvy, z, tol,NAGERR_DEFAULT);
      g13edc(n, m, p, &tu[0][0], tradim, &ainvu[0][0], tradim,
             &ainvbu[0][0], tradim, &rinv[0][0], tradim, &cu[0][0],
             tradim, &qinv[0][0], tradim, ux, rinvy,
             z, tol, NAGERR_DEFAULT);
    }

  /* Print Results */
  Vprintf("\nResults from g13ecc \n\n");     /* let ig = t' * t */
  f06yac(Transpose, NoTranspose, n, n, n, one, &t[0][0], tradim,
         &t[0][0], tradim, zero, &ig[0][0], tradim);
  mat_io(n, n, &ig[0][0], tradim, print,"The information matrix ig is\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   %8.4f \n", i, x[i]);
  Vprintf("\nResults from g13edc \n\n");    /* let ih = tu' * tu */
  f06yac(Transpose, NoTranspose, n, n, n, one, &tu[0][0], tradim,
         &tu[0][0], tradim, zero, &ih[0][0], tradim);
  mat_io(n, n, &ih[0][0], tradim, print,"The information matrix ih is\n");

  /* Calculate ih = u'*ih*u   */
  f06yac(NoTranspose, NoTranspose, n, n, n, one, &ih[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, &ih[0][0], tradim);
  mat_io(n, n, &ih[0][0], tradim, print,"\nThe matrix u' * ih * u is\n");

  /* Calculate x = u' * ux */
  f06pac(Transpose, n, n, one, &u[0][0], tradim, ux, ione,
         zero, x, ione);
  Vprintf("\nThe components of the estimated filtered state are \n\n");
  Vprintf("  k       x(k)  \n");
  for (i=0; i<n; ++i)
    Vprintf("  %ld   %8.4f \n", i, x[i]);
}

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");
     }
 } /* mat_io */