/* vim: set expandtab shiftwidth=2 softtabstop=2 tw=70: */

// #define USE_CLONE 1
// #define USE_APPROX_EXP 1

// Comments like //t3 refer to trial t3 in git/oce-issues/18xx/1880/README.md

#include <Rcpp.h>
using namespace Rcpp;

// Cross-reference work:
// 1. update ../src/registerDynamicSymbol.c with an item for this
// 2. main code should use the autogenerated wrapper in ../R/RcppExports.R

#ifdef USE_APPROX_EXP
// Use exp(x)=exp(i)*exp(f), where i=floor(x) and f=x-i.  Compute
// exp(i) using a lookup table, and exp(f) using Taylor's rule.
//
// TO DO: test speed, to decide how many Taylor terms to retain.
double exp_approx(double x) {
  double rval;
  double ei[] = {
      // 51 entries, for i=0, -1, -2, ... -50
      1.00000000000000e+00, 3.67879441171442e-01, 1.35335283236613e-01,
      4.97870683678639e-02, 1.83156388887342e-02, 6.73794699908547e-03,
      2.47875217666636e-03, 9.11881965554516e-04, 3.35462627902512e-04,
      1.23409804086680e-04, 4.53999297624849e-05, 1.67017007902457e-05,
      6.14421235332821e-06, 2.26032940698105e-06, 8.31528719103568e-07,
      3.05902320501826e-07, 1.12535174719259e-07, 4.13993771878517e-08,
      1.52299797447126e-08, 5.60279643753727e-09, 2.06115362243856e-09,
      7.58256042791191e-10, 2.78946809286892e-10, 1.02618796317019e-10,
      3.77513454427910e-11, 1.38879438649640e-11, 5.10908902806333e-12,
      1.87952881653908e-12, 6.91440010694020e-13, 2.54366564737692e-13,
      9.35762296884017e-14, 3.44247710846998e-14, 1.26641655490942e-14,
      4.65888614510340e-15, 1.71390843154201e-15, 6.30511676014699e-16,
      2.31952283024357e-16, 8.53304762574407e-17, 3.13913279204803e-17,
      1.15482241730158e-17, 4.24835425529159e-18, 1.56288218933499e-18,
      5.74952226429356e-19, 2.11513103759108e-19, 7.78113224113380e-20,
      2.86251858054939e-20, 1.05306173575538e-20, 3.87399762868719e-21,
      1.42516408274094e-21, 5.24288566336346e-22, 1.92874984796392e-22};
  int i = (int)(floor(x));
  if (i > -51) {
    double I = ei[-i];
    double f = x - i;
    // Use Horner's rule for speed (and perhaps accuracy). Test
    // separately, over range e^(-50) to 1.
    // Order:        4     5          6        7
    // Max % error:  ?.??? ?.???      0.008273 ?.???
    double F =
        1.0 +
        f * (1.0 + f * (1.0 / 2.0 +
                        f * (1.0 / 6.0 + f * (1.0 / 24.0 + f / 120.0)))); // 5
    //. double F
    //= 1.0+f*(1.0+f*(1.0/2.0+f*(1.0/6.0+f*(1.0/24.0+f*(1.0/120.0+f/720.0)))));
    //// 6 . double F
    //= 1.0+f*(1.0+f*(1.0/2.0+f*(1.0/6.0+f*(1.0/24.0+f*(1.0/120.0+f*(1/720.0+f/5040.0))))));
    //// 7 printf("i=%d I=%.4f F=%.4f\n",i,I,F);
    rval = I * F;
  } else {
    // printf("using built-in exp()\n");
    rval = exp(x);
  }
  return rval;
}
#endif

#ifdef USE_APPROX_EXP_OLD
// Compute exp(-x) approximately, as efficiency measure.
// See Dan Kelley notebook [1997/1/25] for demonstration
// of factor of 3 speedup, with 1000 column data and a
// 10 by 10 grid, and demonstration that the
// error is < 0.1% in the final grid.
inline double exp_approx(double x) {
  return 1.0 / (0.999448 +
                x * (1.023820 +
                     x * (0.3613967 +
                          x * (0.4169646 + x * (-0.1292509 + x * 0.0499565)))));
}
#endif

static double interpolate_barnes(
    double xx, double yy,
    double zz, /* interpolate to get zz value at (xx,yy) */
    int skip,  /* value in (x,y,z) to skip, or -1 if no skipping */
    unsigned int nx, double *x, double *y, double *z,
    double *w,            /* data num, locations, values, weights */
    double *z_last,       /* last estimate of z at (x,y) */
    double xr, double yr) /* influence radii */
{
  double sum_w = 0.0, sum = 0.0;
  for (unsigned int k = 0; k < nx; k++) {
    // R trims NA (x,y values so no need to check here
    if ((int)k != skip) {
      double dx = (xx - x[k]) / xr;
      double dy = (yy - y[k]) / yr;
#ifdef USE_APPROX_EXP
      double weight = w[k] * exp_approx(-(dx * dx + dy * dy));
#else
      double weight = w[k] * exp(-(dx * dx + dy * dy));
#endif
      sum_w += weight;
      sum += weight * (z[k] - z_last[k]);
    }
  }
  return ((sum_w > 0.0) ? (zz + sum / sum_w) : NA_REAL);
}

// next is modelled on interpolate_barnes()
static double weight_barnes(double xx, double yy, int skip, unsigned int n,
                            double *x, double *y, double *z, double *w,
                            double xr, double yr) {
  double sum_w = 0.0;
  for (unsigned int k = 0; k < n; k++) {
    if ((int)k != skip) {
      double dx = (xx - x[k]) / xr;
      double dy = (yy - y[k]) / yr;
#ifdef USE_APPROX_EXP
      double weight = w[k] * exp_approx(-(dx * dx + dy * dy));
#else
      double weight = w[k] * exp(-(dx * dx + dy * dy));
#endif
      sum_w += weight;
    }
  }
  return ((sum_w > 0.0) ? sum_w : NA_REAL);
}

// [[Rcpp::export]]
List do_interp_barnes(NumericVector x, NumericVector y, NumericVector z,
                      NumericVector w, NumericVector xg, NumericVector yg,
                      NumericVector xr, NumericVector yr, NumericVector gamma,
                      NumericVector iterations) {
  int nx = x.size();
  int nxg = xg.size();
  int nyg = yg.size();
  double rgamma = gamma[0]; // gamma
  if (rgamma < 0.0)
    Rcpp::stop("cannot have gamma < 0, but got gamma=%f", rgamma);
  int niter = floor(0.5 + iterations[0]); // number of iterations
  if (niter < 1)
    Rcpp::stop("cannot have fewer than 1 iteration, but got niter=%d ", niter);
  if (niter > 20)
    Rcpp::stop("cannot have more than 20 iterations, but got niter=%d ", niter);
  if (xr[0] <= 0)
    Rcpp::stop("cannot have xr<=0 but got xr=%f", xr[0]);
  if (yr[0] <= 0)
    Rcpp::stop("cannot have yr<=0 but got yr=%f", yr[0]);
  double xr2 = xr[0]; // local radius, which will vary with iteration
  double yr2 = yr[0]; // local radius, which will vary with iteration
  // Get storage
  NumericMatrix zg(nxg, nyg); // predictions on grid
  NumericMatrix wg(nxg, nyg); // weights on the grid
  NumericVector zd(nx);       // predictions at the data
  NumericVector z_last(nx);   // previous (last) values at data points
  NumericMatrix zz(nxg, nyg); // working matrix on grid
  // initialize storage to 0.  I don't know if this is needed, because
  // https://teuder.github.io/rcpp4everyone_en/080_vector.html
  // implies not, but without saying so explicitly.
  std::fill(zz.begin(), zz.end(), 0.0);
  std::fill(z_last.begin(), z_last.end(), 0.0);

  // t3 double *zzp = &zz[0];
  for (int iter = 0; iter < niter; iter++) {
    // Rprintf("iter=%d xr2=%f yr2=%f\n", iter, xr2, yr2);
    /* update grid */
    for (int i = 0; i < nxg; i++) {
      for (int j = 0; j < nyg; j++) {
        // t3 *(zzp+i+j*nxg) = interpolate_barnes(xg[i], yg[j], *(zzp+i+j*nxg),
        zz(i, j) = interpolate_barnes(xg[i], yg[j], zz(i, j), -1, /* no skip */
                                      nx, &x[0], &y[0], &z[0], &w[0],
                                      &z_last(0), xr2, yr2);
      }
      R_CheckUserInterrupt();
    }
    /* interpolate grid back to data locations */
    for (int k = 0; k < nx; k++) {
      // Rprintf("  zd[%d] = %f (iter %d)\n", k, zd[k], iter);
      zd[k] = interpolate_barnes(
          x[k], y[k], z_last[k], -1, /* BUG: why not skip? */
          nx, &x[0], &y[0], &z[0], &w[0], &z_last(0), xr2, yr2);
      // Rprintf("  -> zd[%d] = %f (iter %d)\n", k, zd[k], iter);
    }
    R_CheckUserInterrupt();
    // Note that we have to clone, or the final z_last results will be wrong.
    // I think the error stems from Rcpp using pointers sometimes.
#ifdef USE_CLONE
    z_last = clone(zd);
#else
    for (int k = 0; k < nx; k++)
      z_last[k] = zd[k];
#endif

    if (rgamma > 0.0) {
      // refine search range for next iteration
      xr2 *= sqrt(rgamma);
      yr2 *= sqrt(rgamma);
    }
  }

  // copy matrix to return value
#ifdef USE_CLONE
  zg = clone(zz);
#else
  for (int i = 0; i < nxg; i++)
    for (int j = 0; j < nyg; j++)
      zg(i, j) = zz(i, j);
#endif

  // weights at final region-of-influence radii
  for (int i = 0; i < nxg; i++) {
    for (int j = 0; j < nyg; j++) {
      wg(i, j) = weight_barnes(xg[i], yg[j], -1, /* no skip */
                               nx, &x[0], &y[0], &z[0], &w[0], xr2, yr2);
    }
    R_CheckUserInterrupt();
  }
  return (List::create(Named("zg") = zg, Named("wg") = wg, Named("zd") = zd));
}
