/* nag_kalman_sqrt_filt_cov_var(g13eac)  Example Program
 *
 * Copyright 1996 Numerical Algorithms Group
 *
 * Mark 4, 1996.
 * Mark 5 revised, 1998.
 * Mark 6 revised, 2000.
 * Mark 7 revised, 2001.
 *
 */

#include <nag.h>
#include <math.h>
#include <stdio.h>
#include <nag_stdlib.h>
#include <nage04.h>
#include <nagf06.h>
#include <nagg05.h>
#include <nagg13.h>
#include <nagx02.h>

#ifdef __cplusplus
extern "C" {
#endif
static void objfun(Integer n, double theta_phi[], double *objf,
                   double g[], Nag_Comm *comm);
#ifdef __cplusplus
}
#endif

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

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

#define NY 2000

static void objfun(Integer n, double theta_phi[], double *objf,
                   double g[], Nag_Comm *comm)
     /* Routine to evaluate objective function. */
{
  double  ak[2][1], h[1][1];
  double tol = 0.0;
  double xp[2];
  double q[1][1];
  double c[1][2];
  double r[1][1];          /* There is no measurement noise */
  double a[2][2];
  double s[2][2];
  double b[2][1];
  double hs, v, ss = 0.0, logdet = 0.0;
  Integer k, nsum = 0, ione = 1, itwo = 2;
  Integer nsteps = NY, n1 = 2, m1 = 1, p1 = 1;
  double phi, theta, temp1, temp2, k11;
  double *y;

  y = comm->user;

  xp[0]= 0.0;   /* The expectation of the mean of an
                 * ARMA(1,1) is 0.0 */
  xp[1] = 0.0;
  q[0][0] = 1.0;

  c[0][0] = 1.0;
  c[0][1] = 0.0;

  r[0][0] = 0.0; /* There is no measurement noise */
  theta = theta_phi[0];
  phi = theta_phi[1];

  b[0][0] = 1.0;
  b[1][0] = - theta;

  a[0][0] = phi;
  a[1][0] = 0.0;
  a[0][1] = 1.0;
  a[1][1] = 0.0;

  /* set value for cholesky factor of state covariance matrix  */
  temp1 = 1.0 + (theta * theta) - (2.0 * theta * phi);
  temp2 = 1.0 - (phi * phi);
  k11   = temp1/temp2;
  s[0][0] = sqrt(k11);
  s[0][1] = 0.0;
  s[1][0] = - theta /s[0][0];
  s[1][1] = theta * sqrt(1.0 - (1.0/k11));


  /* iterate kalman filter for number of observations  */
  for (k=1; k <= nsteps; ++k)
    {
      g13eac(n1, m1, p1, &s[0][0], itwo, &a[0][0],
             itwo, &b[0][0], ione, &q[0][0], ione, &c[0][0],
             itwo, &r[0][0], ione, &ak[0][0], ione, &h[0][0], ione,
             tol, NAGERR_DEFAULT);

      v   = y[k-1] - c[0][0]*xp[0];
      hs   = h[0][0] * h[0][0];
      logdet = logdet +  log(hs);
      ss     = ss + (v * v/ hs);
      nsum = nsum + 1;

      xp[0] = a[0][0]* xp[0] +  a[0][1] * xp[1] + ak[0][0] * v;
      xp[1] = ak[1][0] * v;
    }
  *objf = nsum * log (ss/nsum) + logdet;
}                               /* objfun */

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

static void ex1(void)
{
  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;
  Integer m, n, p;
  Integer istep;
  double tol;
  Integer nmax, mmax, pmax, tradim;

  Vprintf("g13eac Example Program Results\n\n");
  Vprintf("Example 1\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)
    g13eac(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 ex2 (void)
{
  /*
     Example program to iluustrate the use of the kalman filter to estimate the
     parameters of an ARMA(1,1) time series model.
     Note : theta_phi[0] contains theta (moving average coefficient), and
     theta_phi[1] contains phi (autoregressive coefficient)
  */

  double theta_phi[2], g[2], bl[2], bu[2];
  double objf;
  Integer n = 2;
  Nag_BoundType bound;
  Integer ip = 1, iq = 1;
  double sphi[1], stheta[1], sy[NY];
  double mean = 0.0, vara = 1.0;
  double ref[20];
  Boolean start;
  Integer seed = 1238;
  Integer nterms = NY;
  static NagError fail;
  static Nag_Comm comm;
  static Nag_E04_Opt options;

  Vprintf("\n\nExample 2\n\n");
  g05cbc(seed);

  stheta[0] = 0.9;
  sphi[0]   = 0.4;

  start = TRUE;
  g05hac(start, ip, iq, sphi, stheta, mean, vara, nterms,
         sy, ref, NAGERR_DEFAULT);

  theta_phi[0] = 0.5;    /* initial guess */
  theta_phi[1] = 0.5;

  fail.print = TRUE;
  bound = Nag_Bounds;
  bl[0] = -1.0;
  bu[0] = 1.0;
  bl[1] = -1.0;
  bu[1] = 1.0;
  comm.user = &sy[0];
  e04xxc(&options);
  options.print_level = Nag_NoPrint;
  options.list = FALSE;

  e04jbc(n, objfun, bound, bl, bu, theta_phi, &objf, g, &options, &comm, &fail);

  e04xzc(&options, "all", NAGERR_DEFAULT);
  if(fail.code != NE_NOERROR && fail.code != NW_COND_MIN) {
    Vprintf("An error has occurred \n");
    exit(EXIT_FAILURE);
  }

  Vprintf("The estimates are :  theta = %7.3f, phi = %7.3f \n", theta_phi[0],
          theta_phi[1]);
  exit(EXIT_SUCCESS);
}