/* nag_kalman_sqrt_filt_info_invar (g13edc) Example Program.
 *
 * Copyright 1993 Numerical Algorithms Group
 *
 * Mark 3, 1993
 * Mark 7 revised, 2001.
 * Mark 8 revised, 2004.
 *
 */

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

typedef enum { read, print }   ioflag;

static int ex1(FILE *fpin, FILE *fpout);
static int ex2(FILE *fpin, FILE *fpout);

int main(int argc, char *argv[])
{
  FILE     *fpin, *fpout;
  Integer  exit_status_ex1 = 0;
  Integer  exit_status_ex2 = 0;
  NagError fail;

  INIT_FAIL(fail);

  /* Check for command-line IO options */
  fpin = nag_example_file_io(argc, argv, "-data", NULL);
  fpout = nag_example_file_io(argc, argv, "-results", NULL);

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

  fprintf(fpout, "nag_kalman_sqrt_filt_info_invar (g13edc) Example Program "
          "Results\n\n");

  exit_status_ex1 = ex1(fpin, fpout);
  exit_status_ex2 = ex2(fpin, fpout);

  if (fpin != stdin) fclose(fpin);
  if (fpout != stdout) fclose(fpout);
  return (exit_status_ex1 == 0 && exit_status_ex2 == 0) ? 0 : 1;
}

#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 AINVB(I, J) ainvb[(I) *tdainvb + J]
#define C(I, J)     c[(I) *tdc + J]

static int ex1(FILE *fpin, FILE *fpout)
{
  Integer  exit_status = 0, i, istep, j, m, n, p, tdainv, tdainvb, tdc, tdqinv;
  Integer  tdrinv, tdt;
  NagError fail;
  double   *ainv = 0, *ainvb = 0, *c = 0, *qinv = 0, *rinv = 0, *rinvy = 0;
  double   *t = 0, tol, *x = 0, *z = 0;

  INIT_FAIL(fail);

  fprintf(fpout, "Example 1\n");

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


  fscanf(fpin, "%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)) ||
          !(ainvb = 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)))
        {
          fprintf(fpout, "Allocation failure\n");
          exit_status = -1;
          goto END;
        }
      tdainv = n;
      tdqinv = m;
      tdrinv = p;
      tdt = n;
      tdainvb = m;
      tdc = n;
    }
  else
    {
      fprintf(fpout, "Invalid n or m or p.\n");
      exit_status = 1;
      return exit_status;
    }

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

  /* Perform three iterations of the Kalman filter recursion  */

  for (istep = 1; istep <= 3; ++istep)
    /* nag_kalman_sqrt_filt_info_invar (g13edc).
     * One iteration step of the time-invariant Kalman filter
     * recursion using the square root information
     * implementation with (A^(~-~1)(A^(~-~1)B)) in upper
     * controller Hessenberg form
     */
    nag_kalman_sqrt_filt_info_invar(n, m, p, t, tdt, ainv,
                                    tdainv, ainvb, tdainvb, rinv,
                                    tdrinv, c, tdc, qinv,
                                    tdqinv, x, rinvy, z, tol, &fail);
  if (fail.code != NE_NOERROR)
    {
      fprintf(fpout,
              "Error from nag_kalman_sqrt_filt_info_invar (g13edc).\n%s\n",
              fail.message);
      exit_status = 1;
      goto END;
    }
  fprintf(fpout, "\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)
        fprintf(fpout, "%8.4f ", T(i, j));
      fprintf(fpout, "\n");
    }
  fprintf(fpout, "\nThe components of the estimated filtered state are\n\n");
  fprintf(fpout, "   k       x(k)  \n");
  for (i = 0; i < n; ++i)
    {
      fprintf(fpout, "   %ld  ", i);
      fprintf(fpout, "  %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 (ainvb) NAG_FREE(ainvb);
  if (c) NAG_FREE(c);
  if (x) NAG_FREE(x);
  if (z) NAG_FREE(z);
  if (rinvy) NAG_FREE(rinvy);

  return exit_status;
}

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

#define AINV(I, J)   ainv[(I) *tdainv + J]
#define AINVB(I, J)  ainvb[(I) *tdainvb + J]
#define C(I, J)      c[(I) *tdc + J]
#define AINVU(I, J)  ainvu[(I) *tdainvu + J]
#define AINVBU(I, J) ainvbu[(I) *tdainvbu + 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 RWORK(I, J)  rwork[(I) *tdrwork + J]
#define TU(I, J)     tu[(I) *tdtu + J]
#define IG(I, J)     ig[(I) *tdig + J]
#define IH(I, J)     ih[(I) *tdih + J]
#define CU(I, J)     cu[(I) *tdcu + J]
#define U(I, J)      u[(I) *tdu + J]

static int ex2(FILE *fpin, FILE *fpout)
{
  Integer            dete, exit_status = 0, i, ione = 1, istep, j, m, n, p,
                     tdainv, tdainvb;
  Integer            tdainvbu, tdainvu, tdc, tdcu, tdig, tdih, tdqinv, tdrinv,
                     tdrwork;
  Integer            tdt, tdtu, tdu;
  NagError           fail;
  Nag_ControllerForm reduceto = Nag_UH_Controller;
  Nag_ab_input       inp_ab = Nag_ab_prod;
  double             *ainv = 0, *ainvb = 0, *ainvbu = 0, *ainvu = 0, *c = 0;
  double             *cu = 0, detf, *diag = 0, *ig = 0, *ih = 0, one = 1.0;
  double             *qinv = 0, *rinv = 0, *rinvy = 0, *rwork = 0, *t = 0;
  double             tol, *tu = 0, *u = 0, *ux = 0, *x = 0, *z = 0, zero = 0.0;

  INIT_FAIL(fail);

  fprintf(fpout, "\n\nExample 2\n");

  /* skip the heading in the data file */
  fscanf(fpin, " %*[^\n]");

  fscanf(fpin, "%ld%ld%ld%lf", &n, &m, &p, &tol);
  if (n >= 1 || m >= 1 || p >= 1)
    {
      if (!(ainv = NAG_ALLOC(n*n, double)) ||
          !(ainvb = NAG_ALLOC(n*m, double)) ||
          !(c = NAG_ALLOC(p*n, double)) ||
          !(ainvu = NAG_ALLOC(n*n, double)) ||
          !(ainvbu = NAG_ALLOC(n*m, double)) ||
          !(qinv = NAG_ALLOC(m*m, double)) ||
          !(rinv = NAG_ALLOC(p*p, double)) ||
          !(t = NAG_ALLOC(n*n, double)) ||
          !(x = NAG_ALLOC(n, double)) ||
          !(z = NAG_ALLOC(n, double)) ||
          !(rwork = NAG_ALLOC(n*n, double)) ||
          !(tu = NAG_ALLOC(n*n, double)) ||
          !(rinvy = NAG_ALLOC(p, double)) ||
          !(ig = NAG_ALLOC(n*n, double)) ||
          !(ih = NAG_ALLOC(n*n, double)) ||
          !(cu = NAG_ALLOC(p*n, double)) ||
          !(u = NAG_ALLOC(n*n, double)) ||
          !(ux = NAG_ALLOC(n, double)) ||
          !(diag = NAG_ALLOC(n, double)))
        {
          fprintf(fpout, "Allocation failure\n");
          exit_status = -1;
          goto END;
        }
      tdainv = n;
      tdainvb = m;
      tdc = n;
      tdainvu = n;
      tdainvbu = m;
      tdqinv = m;
      tdrinv = p;
      tdt = n;
      tdrwork = n;
      tdtu = n;
      tdig = n;
      tdih = n;
      tdcu = n;
      tdu = n;
    }
  else
    {
      fprintf(fpout, "Invalid n or m or p.\n");
      exit_status = 1;
      return exit_status;
    }

  /* Read data */
  mat_io(n, n, ainv, tdainv, read, "", fpin, fpout);
  mat_io(p, n, c, tdc, read, "", fpin, fpout);
  if (rinv)
    mat_io(p, p, rinv, tdrinv, read, "", fpin, fpout);
  mat_io(n, m, ainvb, tdainvb, read, "", fpin, fpout);
  mat_io(m, m, qinv, tdqinv, read, "", fpin, fpout);
  mat_io(n, n, t, tdt, read, "", fpin, fpout);
  for (j = 0; j < m; ++j)
    fscanf(fpin, "%lf", &z[j]);
  for (j = 0; j < n; ++j)
    fscanf(fpin, "%lf", &x[j]);
  for (j = 0; j < p; ++j)
    fscanf(fpin, "%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), tdainv, &AINVU(0, i), tdainvu);
  for (j = 0; j < m; ++j)
    f06efc(n, &AINVB(0, j), tdainvb, &AINVBU(0, j), tdainvbu);

  /* Transform (ainvu[],ainvbu[]) to reduceto controller Hessenberg form */
  /* nag_trans_hessenberg_controller (g13exc).
   * Unitary state-space transformation to reduce (BA) to
   * lower or upper controller Hessenberg form
   */
  nag_trans_hessenberg_controller(n, m, reduceto, ainvu, tdainvu, ainvbu,
                                  tdainvbu, u, tdu, &fail);
  if (fail.code != NE_NOERROR)
    {
      fprintf(fpout,
              "Error from nag_trans_hessenberg_controller (g13exc).\n%s\n",
              fail.message);
      exit_status = 1;
      goto END;
    }

  /* Calculate the matrix cu = c*u'    */
  old_dgemm(NoTranspose, Transpose, p, n, n, one, c, tdc,
            u, tdu, zero, cu, tdcu);

  /* Calculate the vector ux = u*x     */
  f06pac(NoTranspose, n, n, one, u, tdu, x, ione,
         zero, ux, ione);

  /* Form the information matrices ih = u*ig*u' and ig = t'*t     */
  old_dgemm(Transpose, NoTranspose, n, n, n, one, t, tdt,
            t, tdt, zero, ig, tdig);
  old_dgemm(NoTranspose, Transpose, n, n, n, one, ig, tdig,
            u, tdu, zero, rwork, tdrwork);
  old_dgemm(NoTranspose, NoTranspose, n, n, n, one, u, tdu,
            rwork, tdrwork, zero, ih, tdih);

  /* Now find the reduceto triangular (right) cholesky factor of ih */
  /* nag_real_cholesky (f03aec).
   * LL^T factorization and determinant of real symmetric
   * positive-definite matrix
   */
  nag_real_cholesky(n, ih, tdih, diag, &detf, &dete, &fail);
  if (fail.code != NE_NOERROR)
    {
      fprintf(fpout, "Error from nag_real_cholesky (f03aec).\n%s\n",
              fail.message);
      exit_status = 1;
      goto END;
    }
  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)
    {
      /* 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, ainvb, tdainvb, rinv, tdrinv,
                                    c, tdc, qinv, tdqinv, x,
                                    rinvy, z, tol, &fail);
      if (fail.code != NE_NOERROR)
        {
          fprintf(fpout,
                  "Error from nag_kalman_sqrt_filt_info_var (g13ecc).\n%s\n",
                  fail.message);
          exit_status = 1;
          goto END;
        }
      /* nag_kalman_sqrt_filt_info_invar (g13edc), see above. */
      nag_kalman_sqrt_filt_info_invar(n, m, p, tu, tdtu, ainvu, tdainvu,
                                      ainvbu, tdainvbu, rinv, tdrinv, cu,
                                      tdcu, qinv, tdqinv, ux, rinvy,
                                      z, tol, &fail);
      if (fail.code != NE_NOERROR)
        {
          fprintf(fpout,
                  "Error from nag_kalman_sqrt_filt_info_invar (g13edc).\n%s\n",
                  fail.message);
          exit_status = 1;
          goto END;
        }
    }

  /* Print Results */
  fprintf(fpout, "\nResults from nag_kalman_sqrt_filt_info_var (g13ecc) \n\n");
  /* let ig = t' * t */
  old_dgemm(Transpose, NoTranspose, n, n, n, one, t, tdt,
            t, tdt, zero, ig, tdig);
  mat_io(n, n, ig, tdig, print, "The information matrix ig is\n", fpin, fpout);
  fprintf(fpout, "\nThe components of the estimated filtered state are\n\n");
  fprintf(fpout, "  k       x(k)  \n");
  for (i = 0; i < n; ++i)
    fprintf(fpout, "  %ld   %8.4f \n", i, x[i]);
  fprintf(fpout,
          "\nResults from nag_kalman_sqrt_filt_info_invar (g13edc) \n\n");
  /* let ih = tu' * tu */
  old_dgemm(Transpose, NoTranspose, n, n, n, one, tu, tdtu,
            tu, tdtu, zero, ih, tdih);
  mat_io(n, n, ih, tdih, print, "The information matrix ih is\n", fpin, fpout);

  /* Calculate ih = u'*ih*u   */
  old_dgemm(NoTranspose, NoTranspose, n, n, n, one, ih, tdih,
            u, tdu, zero, rwork, tdrwork);
  old_dgemm(Transpose, NoTranspose, n, n, n, one, u, tdu,
            rwork, tdrwork, zero, ih, tdih);
  mat_io(n, n, ih, tdih, print, "\nThe matrix u' * ih * u is\n", fpin, fpout);

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

 END:
  if (ainv) NAG_FREE(ainv);
  if (ainvb) NAG_FREE(ainvb);
  if (c) NAG_FREE(c);
  if (ainvu) NAG_FREE(ainvu);
  if (ainvbu) NAG_FREE(ainvbu);
  if (qinv) NAG_FREE(qinv);
  if (rinv) NAG_FREE(rinv);
  if (t) NAG_FREE(t);
  if (x) NAG_FREE(x);
  if (z) NAG_FREE(z);
  if (rwork) NAG_FREE(rwork);
  if (tu) NAG_FREE(tu);
  if (rinvy) NAG_FREE(rinvy);
  if (ig) NAG_FREE(ig);
  if (ih) NAG_FREE(ih);
  if (cu) NAG_FREE(cu);
  if (u) NAG_FREE(u);
  if (ux) NAG_FREE(ux);
  if (diag) NAG_FREE(diag);

  return exit_status;
}

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