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

#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
//
// [[Rcpp::export]]
NumericMatrix do_fill_gap_2d(NumericMatrix m, IntegerVector Gap,
                             IntegerVector Debug) {
  int jnum = m.ncol();
  int inum = m.nrow();
  int igap = Gap[0];
  int jgap = Gap[1];
  int debug = Debug[0];
  // For binMean2D(), fillgap=-1 means to fill all gaps that have
  // non-NA neighbours, no matter how far away.
  if (igap < 0) {
    igap = inum;
  }
  if (jgap < 0) {
    jgap = jnum;
  }
  if (debug) {
    Rprintf("gap=c(%d,%d) so using igap=%d and jgap=%d\n", Gap[0], Gap[1], igap,
            jgap);
  }
  if (debug) {
    Rprintf("inum(=nrow)=%d jnum=(=ncol)=%d\n", inum, jnum);
  }
  // Rprintf("inum=%d\n", inum);
  NumericMatrix r(m); // initialize return value to input value
  int N = 0;
  double SUM = 0.0;
  int im, ip, jm, jp;
  // FIXME: should we iterate?
  for (int i = 0; i < inum; i++) {
    for (int j = 0; j < jnum; j++) {
      // Rprintf("m[%d,%d] (R notation) is %g\n", i + 1, j + 1, m(i, j));
      if (ISNA(m(i, j))) {
        if (debug) {
          Rprintf("m[%d,%d] is NA\n", i + 1, j + 1);
        }
        // find im,ip (indices of good neighbours in i), and similarly jm,jp
        for (im = i - 1; im >= 0; im--) {
          if (!ISNA(m(im, j))) {
            break; // this defines im (<i)
          }
        }
        for (jm = j - 1; jm >= 0; jm--) {
          if (!ISNA(m(i, jm))) {
            break; // this defines jm (<j)
          }
        }
        // FIXME: is the limit correct on next ... maybe nxbreaks-1 ???
        for (ip = i + 1; ip < inum; ip++) {
          if (!ISNA(m(ip, j))) {
            break; // this defines ip (>i)
          }
        }
        for (jp = j + 1; jp < jnum; jp++) {
          if (!ISNA(m(i, jp))) {
            break; // this defines jp (>j)
          }
        }
        if (debug) {
          Rprintf("  got im=%d ip=%d jm=%d jp=%d (in R index-1 notation)\n",
                  im + 1, ip + 1, jm + 1, jp + 1);
        }
        // can only fill if good neighbours exist (not at edges)
        N = 0;
        SUM = 0.0;
        if (-1 < im && ip < inum) {
          if (debug) {
            Rprintf("  can we interpolate along index 1 from im=%d to ip=%d?\n",
                    im + 1, ip + 1);
          }
          if ((ip - im) <= igap + 1) {
            if (debug) {
              Rprintf("  long enough since igap=%d\n", igap);
            }
            double M = m(im, j) + (m(ip, j) - m(im, j)) * (i - im) / (ip - im);
            SUM += M;
            N++;
            if (debug) {
              Rprintf("m[%d,%d]=NA but m[c(%d,%d),%d]=(%.4g,%.4g) "
                      "so interpolating along index 1, to get %.4g\n",
                      i + 1, j + 1, im + 1, ip + 1, j + 1, m(im, j), m(ip, j),
                      M);
            }
          }
        }
        if (-1 < jm && jp < jnum) {
          if (debug) {
            Rprintf("  can we interpolate along 2ffrom jm=%d to jp=%d?\n",
                    jm + 1, jp + 1);
          }
          if ((jp - jm) <= jgap + 1) {
            if (debug) {
              Rprintf("  long enough since jgap=%d\n", jgap);
            }
            double M = m(i, jm) + (m(i, jp) - m(i, jm)) * (j - jm) / (jp - jm);
            SUM += M;
            N++;
            if (debug) {
              Rprintf("m[%d,%d]=NA but m[%d,c(%d,%d)]=(%.4g,%.4g) "
                      "so interpolating along index 2, to get %.4g\n",
                      i + 1, j + 1, i + 1, jm + 1, jp + 1, m(i, jm), m(i, jp),
                      M);
            }
          }
        }
        if (N > 0) {
          r(i, j) = SUM / N;
          if (debug) {
            Rprintf("setting return value r[%d,%d] to %.4g\n", i + 1, j + 1,
                    r(i, j));
          }
        }
      }
    }
  }
  return (r);
}
