/* nag_kalman_unscented_state_revcom (g13ejc) Example Program.
 *
 * Copyright 2014 Numerical Algorithms Group.
 *
 * Mark 25, 2014.
 */
/* Pre-processor includes */
#include <stdio.h>
#include <math.h>
#include <nag.h>
#include <nag_stdlib.h>
#include <nagg13.h>
#include <nagx01.h>

#define LY(I,J) ly[(J) * pdly + (I)]
#define LX(I,J) lx[(J) * pdlx + (I)]
#define ST(I,J) st[(J) * pdst + (I)]
#define XT(I,J) xt[(J) * pdxt + (I)]
#define FXT(I,J) fxt[(J) * pdfxt + (I)]

typedef struct g13_problem_data {
  double delta, a, r, d;
  double phi_rt, phi_lt;
} g13_problem_data;

const Integer mx = 3, my = 2;
void f(Integer n, double *xt, Integer pdxt, double *fxt, Integer pdfxt,
       g13_problem_data dat);
void h(Integer n, double *xt, Integer pdxt, double *fxt, Integer pdfxt,
       g13_problem_data dat);
void read_problem_dat(Integer t, g13_problem_data *dat);


int main(void)
{
  /* Integer scalar and array declarations */
  Integer i, irevcm, pdfxt, pdlx, pdly, pdst, pdxt, licomm, lrcomm, lropt,
    n, ntime, t, j;
  Integer *icomm = 0;
  Integer exit_status = 0;

  /* NAG structures and types */
  NagError fail;

  /* Double scalar and array declarations */
  double *fxt = 0, *lx = 0, *ly = 0, *rcomm = 0, *ropt = 0,
    *st = 0, *x = 0, *xt = 0, *y = 0;

  /* Other structures */
  g13_problem_data dat;

  /* Initialise the error structure */
  INIT_FAIL(fail);

  printf("nag_kalman_unscented_state_revcom (g13ejc) "
         "Example Program Results\n\n");

  /* Skip heading in data file */
  scanf("%*[^\n] ");

  /* Using default optional arguments */
  lropt = 0;

  /* Allocate arrays */
  n = 2*mx + 1;
  if (lropt >= 1 && fabs(ropt[0]-2.0)<=0.0) {
    n += 2*mx;
  }
  pdlx = pdst = pdxt = mx;
  pdly = my;
  pdfxt = (mx > my) ? mx : my;
  licomm = 30;
  lrcomm = 30 + my + mx*my + 2*((mx > my) ? mx : my);
  if (!(lx = NAG_ALLOC(pdlx*mx, double)) ||
      !(ly = NAG_ALLOC(pdly*my, double)) ||
      !(x = NAG_ALLOC(mx, double)) ||
      !(st = NAG_ALLOC(pdst*mx, double)) ||
      !(xt = NAG_ALLOC(pdxt*(my > n ? my : n), double)) ||
      !(fxt = NAG_ALLOC(pdfxt*(n+(mx > my ? mx : my)), double)) ||
      !(icomm = NAG_ALLOC(licomm, Integer)) ||
      !(rcomm = NAG_ALLOC(lrcomm, double)) ||
      !(y = NAG_ALLOC(my, double))) {
    printf("Allocation failure\n");
    exit_status = -1;
    goto END;
  }

  /* Read in the cholesky factorisation of the covariance matrix for the
     process noise */
  for (i = 0; i < mx; i++) {
    for (j = 0; j <= i; j++) {
      scanf("%lf",&LX(i,j));
    }
    scanf("%*[^\n] ");
  }

  /* Read in the cholesky factorisation of the covariance matrix for the
     observation noise */
  for (i = 0; i < my; i++) {
    for (j = 0; j <= i; j++) {
      scanf("%lf",&LY(i,j));
    }
    scanf("%*[^\n] ");
  }

  /* Read in the initial state vector */
  for (i = 0; i < mx; i++) {
    scanf("%lf",&x[i]);
  }
  scanf("%*[^\n] ");

  /* Read in the cholesky factorisation of the initial state covariance
     matrix */
  for (i = 0; i < mx; i++) {
    for (j = 0; j <= i; j++) {
      scanf("%lf",&ST(i,j));
    }
    scanf("%*[^\n] ");
  }

  /* Read in the number of time points to run the system for */
  scanf("%ld%*[^\n] ",&ntime);

  /* Read in any problem specific data that is constant */
  read_problem_dat(0, &dat);

  /* Title for first set of output */
  printf("   Time  ");
  for (i = 0; i < (11*mx- 16)/2; i++) putchar(' ');
  printf("Estimate of State\n ");
  for (i = 0; i < 7+11*mx; i++) putchar('-');
  printf("\n");

  /* Loop over each time point */
  irevcm = 0;
  for (t = 0; t < ntime; t++) {

    /* Read in any problem specific data that is time dependent */
    read_problem_dat(t+1, &dat);

    /* Read in the observed data for time t */
    for (i = 0; i < my; i++) {
      scanf("%lf",&y[i]);
    }
    scanf("%*[^\n] ");

    /* Call Unscented Kalman Filter routine (g13ejc) */
    do {
      nag_kalman_unscented_state_revcom(&irevcm, mx, my, y, lx, pdlx, ly, pdly,
                                        x, st, pdst, &n, xt, pdxt, fxt, pdfxt,
                                        ropt, lropt, icomm, licomm, rcomm,
                                        lrcomm, &fail);
      switch(irevcm) {
      case 1:
        /* Evaluate F(X) */
        f(n,xt,pdxt,fxt,pdfxt,dat);
        break;

      case 2:
        /* Evaluate H(X) */
        h(n,xt,pdxt,fxt,pdfxt,dat);
        break;

      default:
        /* irevcm = 3, finished */
        if (fail.code != NE_NOERROR) {
          printf("Error from nag_kalman_unscented_state_revcom (g13ejc).\n%s\n",
                 fail.message);
          exit_status = 1;
          goto END;
        }
        break;
      }
    } while(irevcm != 3);

    /* Display the some of the current state estimate */
    printf(" %3ld    ",t+1);
    for (i = 0; i < mx; i++) {
      printf(" %10.3f",x[i]);
    }
    printf("\n");
  }

  printf("\n");
  printf("Estimate of Cholesky Factorisation of the State\n");
  printf("Covariance Matrix at the Last Time Point\n");
  for (i = 0; i < mx; i++) {
    for (j = 0; j <= i; j++) {
      printf(" %10.2e",ST(i,j));
    }
    printf("\n");
  }

 END:
  NAG_FREE(icomm);
  NAG_FREE(fxt);
  NAG_FREE(lx);
  NAG_FREE(ly);
  NAG_FREE(rcomm);
  NAG_FREE(ropt);
  NAG_FREE(st);
  NAG_FREE(x);
  NAG_FREE(xt);
  NAG_FREE(y);

  return(exit_status);
}

void f(Integer n, double *xt, Integer pdxt, double *fxt, Integer pdfxt,
       g13_problem_data dat) {
  double t1, t3;
  Integer i;

  t1 = 0.5 * dat.r * (dat.phi_rt+dat.phi_lt);
  t3 = (dat.r/dat.d)*(dat.phi_rt-dat.phi_lt);

  for (i = 0; i < n; i++) {
    FXT(0,i) = XT(0,i) + cos(XT(2,i))*t1;
    FXT(1,i) = XT(1,i) + sin(XT(2,i))*t1;
    FXT(2,i) = XT(2,i) + t3;
  }
}

void h(Integer n, double *xt, Integer pdxt, double *fxt, Integer pdfxt,
       g13_problem_data dat) {
  Integer i;

  for (i = 0; i < n; i++) {
    FXT(0,i) = dat.delta - XT(0,i)*cos(dat.a) - XT(1,i)*sin(dat.a);
    FXT(1,i) = XT(2,i) - dat.a;

    /* Make sure that the theta is in the same range as the observed data,
       which in this case is [0, 2*pi) */
    if (FXT(1,i) < 0.0)
      FXT(1,i) += 2 * X01AAC;
  }
}
void read_problem_dat(Integer t, g13_problem_data *dat) {
  /* Read in any data specific to the f and h functions */
  Integer tt;

  if (t==0) {
    /* Read in the data that is constant across all time points */
    scanf("%lf%lf%lf%lf%*[^\n] ",&(dat->r), &(dat->d), &(dat->delta),
          &(dat->a));

  } else {
    /* Read in data for time point t */
    scanf("%ld%lf%lf%*[^\n] ",&tt, &(dat->phi_rt), &(dat->phi_lt));
    if (tt!=t) {
      /* Sanity check */
      printf("Expected to read in data for time point %ld\n",t);
      printf("Data that was read in was for time point %ld\n",tt);
    }
  }
}