/* nag_tsa_kalman_sqrt_filt_info_var (g13ecc) Example Program.
 *
 * Copyright 2021 Numerical Algorithms Group
 *
 * Mark 27.2, 2021.
 */

#include <nag.h>
#include <stdio.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;
  Nag_ab_input inp_ab;
  double *ainv = 0, *b = 0, *c = 0, *qinv = 0, *rinv = 0, *rinvy = 0;
  double *t = 0, tol, *x = 0, *z = 0;
  NagError fail;

  INIT_FAIL(fail);

  printf(
      "nag_tsa_kalman_sqrt_filt_info_var (g13ecc) Example Program Results\n");

  /* Skip the heading in the data file   */
  scanf("%*[^\n]");
  scanf("%" NAG_IFMT "%" NAG_IFMT "%" NAG_IFMT "%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))) {
      printf("Allocation failure\n");
      exit_status = -1;
      goto END;
    }
    tdainv = n;
    tdqinv = m;
    tdrinv = p;
    tdt = n;
    tdb = m;
    tdc = n;
  } else {
    printf("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)
      scanf("%lf", &AINV(i, j));
  for (i = 0; i < p; ++i)
    for (j = 0; j < n; ++j)
      scanf("%lf", &C(i, j));
  if (rinv)
    for (i = 0; i < p; ++i)
      for (j = 0; j < p; ++j)
        scanf("%lf", &RINV(i, j));
  for (i = 0; i < n; ++i)
    for (j = 0; j < m; ++j)
      scanf("%lf", &B(i, j));
  for (i = 0; i < m; ++i)
    for (j = 0; j < m; ++j)
      scanf("%lf", &QINV(i, j));
  for (i = 0; i < n; ++i)
    for (j = 0; j < n; ++j)
      scanf("%lf", &T(i, j));
  for (j = 0; j < m; ++j)
    scanf("%lf", &z[j]);
  for (j = 0; j < n; ++j)
    scanf("%lf", &x[j]);
  for (j = 0; j < p; ++j)
    scanf("%lf", &rinvy[j]);

  /* Perform three iterations of the (Kalman) filter recursion
     (in square root information form). */
  for (istep = 1; istep <= 3; ++istep)
    /* nag_tsa_kalman_sqrt_filt_info_var (g13ecc).
     * One iteration step of the time-varying Kalman filter
     * recursion using the square root information
     * implementation
     */
    nag_tsa_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) {
    printf("Error from nag_tsa_kalman_sqrt_filt_info_var (g13ecc).\n%s\n",
           fail.message);
    exit_status = 1;
    goto END;
  }

  printf("\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)
      printf("%8.4f ", T(i, j));
    printf("\n");
  }

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

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