/* 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.
 * Mark 8 revised, 2004.
 *
 */

#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 int ex1(void);
static int ex2(void);

int main(void)
{
  Integer exit_status_ex1=0;
  Integer exit_status_ex2=0;
  exit_status_ex1 = ex1();
  exit_status_ex2 = ex2();
  return exit_status_ex1 == 0 && exit_status_ex2 == 0 ? 0 : 1;

}

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

  INIT_FAIL(fail);

  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)
    {
      /* nag_kalman_sqrt_filt_cov_var (g13eac).
       * One iteration step of the time-varying Kalman filter
       * recursion using the square root covariance implementation
       */
      nag_kalman_sqrt_filt_cov_var(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 A(I,J) a[(I)*tda + J]
#define B(I,J) b[(I)*tdb + J]
#define C(I,J) c[(I)*tdc + J]
#define K(I,J) k[(I)*tdk + J]
#define Q(I,J) q[(I)*tdq + J]
#define R(I,J) r[(I)*tdr + J]
#define S(I,J) s[(I)*tds + J]
#define H(I,J) h[(I)*tdh + J]
static int ex1(void)
{
  double *a=0, *b=0, *c=0, *k=0, *q=0, *r=0, *s=0, *h=0;
  Integer exit_status=0;
  Integer i, j;
  Integer m, n, p;
  Integer istep;
  double tol;
  Integer tda, tdb, tdc, tdk, tdq, tdr, tds, tdh;
  NagError fail;

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

  Vscanf("%ld%ld%ld%lf",&n,&m,&p,&tol);
  if (n>=1 && m>=1 && p>=1)
    {
      if ( !( a = NAG_ALLOC(n*n, double)) ||
           !( b = NAG_ALLOC(n*m, double)) ||
           !( c = NAG_ALLOC(p*n, double)) ||
           !( k = NAG_ALLOC(n*p, double)) ||
           !( q = NAG_ALLOC(m*m, double)) ||
           !( r = NAG_ALLOC(p*p, double)) ||
           !( s = NAG_ALLOC(n*n, double)) ||
           !( h = NAG_ALLOC(n*p, double)) )
        {
          Vprintf("Allocation failure\n");
          exit_status = -1;
          goto END;
        }
      tda = n;
      tdb = m;
      tdc = n;
      tdk = p;
      tdq = m;
      tdr = p;
      tds = n;
      tdh = p;
    }
  else
    {
      Vprintf("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)
      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)
    {
      /* nag_kalman_sqrt_filt_cov_var (g13eac), see above. */
      nag_kalman_sqrt_filt_cov_var(n, m, p, s, tds, a, tda, b, tdb, q, tdq,
                                   c, tdc, r, tdr, k, tdk, h, tdh, tol, &fail);
      if (fail.code != NE_NOERROR)
        {
          Vprintf("Error from nag_kalman_sqrt_filt_cov_var (g13eac).\n%s\n",
                  fail.message);
          exit_status = 1;
          goto END;
        }
    }
  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");
        }
    }
 END:
  if (a) NAG_FREE(a);
  if (b) NAG_FREE(b);
  if (c) NAG_FREE(c);
  if (k) NAG_FREE(k);
  if (q) NAG_FREE(q);
  if (r) NAG_FREE(r);
  if (s) NAG_FREE(s);
  if (h) NAG_FREE(h);
  return exit_status;
}


static int 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 exit_status=0;
  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];
  Nag_Boolean start;
  Integer seed = 1238;
  Integer nterms = NY;
  Nag_Comm comm;
  Nag_E04_Opt options;
  NagError fail;

  INIT_FAIL(fail);

  Vprintf("\n\nExample 2\n\n");
  /* nag_random_init_repeatable (g05cbc).
   * Initialize random number generating functions to give
   * repeatable sequence
   */
  nag_random_init_repeatable(seed);

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

  start = Nag_TRUE;
  /* nag_arma_time_series (g05hac).
   * ARMA time series of n terms
   */
  nag_arma_time_series(start, ip, iq, sphi, stheta, mean, vara, nterms,
                       sy, ref, &fail);
  if (fail.code != NE_NOERROR)
    {
      Vprintf("Error from nag_arma_time_series (g05hac).\n%s\n", fail.message);
      exit_status = 1;
      goto END;
    }

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

  bound = Nag_Bounds;
  bl[0] = -1.0;
  bu[0] = 1.0;
  bl[1] = -1.0;
  bu[1] = 1.0;
  comm.user = &sy[0];
  /* nag_opt_init (e04xxc).
   * Initialization function for option setting
   */
  nag_opt_init(&options);
  options.print_level = Nag_NoPrint;
  options.list = Nag_FALSE;

  /* nag_opt_bounds_no_deriv (e04jbc).
   * Bound constrained nonlinear minimization (no derivatives
   * required)
   */
  nag_opt_bounds_no_deriv(n, objfun, bound, bl, bu, theta_phi, &objf, g,
                          &options, &comm, &fail);
  if(fail.code != NE_NOERROR && fail.code != NW_COND_MIN)
    {
      Vprintf("Error from nag_opt_bounds_no_deriv (e04jbc).\n%s\n",
              fail.message);
      exit_status = 1;
      goto END;
    }

  /* nag_opt_free (e04xzc).
   * Memory freeing function for use with option setting
   */
  nag_opt_free(&options, "all", &fail);
  if (fail.code != NE_NOERROR)
    {
      Vprintf("Error from nag_opt_free (e04xzc).\n%s\n", fail.message);
      exit_status = 1;
      goto END;
    }

  Vprintf("The estimates are :  theta = %7.3f, phi = %7.3f \n", theta_phi[0],
          theta_phi[1]);
 END:
  return exit_status;

}