/* 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]]
NumericVector do_adv_vector_time(NumericVector vvdStart, NumericVector vsdStart,
                                 NumericVector vsdTime, NumericVector vvdhStart,
                                 NumericVector vvdhTime, NumericVector n,
                                 NumericVector f) {
  // This is called by read.adv.nortek(), in adv.nortek.R, with arguments as
  // follows
  //   vvdStart = indices of 'vector velocity data' (0xA5 ox10)
  //   vvdhStart = indices of headers for 'vector velocity data header' (0xA5
  //   ox10) vvdhTime = POSIX times of vvdh vsdStart = indices of headers for
  //   'vector system data' (0xA5 ox11) vsdTime = POSIX times of vsd n = samples
  //   expected (set to 0 for continous mode) f = sampling rate in Hz
  // and the result is a vector of times for the vvd items, which has length
  // matching that of vvdStart.  The method works by left-bracketing
  // velocity data with vsd headers, and stepping forward thereafter
  // in times dt=1/f.
  long int nvvd = vvdStart.size();
  long int nvsd = vsdStart.size();
  long int nvvdh = vvdhStart.size();
  NumericVector res(nvvd);
  long int ivvd, ivvdh = 0;
  double t = vvdhTime[0];
  int nn = (int)floor(0.5 + n[0]);
  if (nn < 0)
    ::Rcpp::stop("cannot have negative n (number of points), but got %d (after "
                 "rounding from %f)",
                 nn, n[0]);
  if (f[0] < 0)
    ::Rcpp::stop("cannot have negative f (sampling frequency), but got %f",
                 f[0]);
#ifdef DEBUG
  for (int iii = 0; iii < nvvdh; iii++)
    Rprintf("nvvdhTime[%d] = %f\n", iii, vvdhTime[iii]);
#endif
  double dt = 1.0 / f[0];
  if (nn == 0) {
    // Continuous sampling
    //
    // 1. Move vsd pointer to point at vsd entry just preceding first vvd entry.
    int ivvd, ivsd = 0;
#ifdef DEBUG
    Rprintf("continuous sampling\n");
    Rprintf("vvdStart[0]=%f\n", vvdStart[0]);
    Rprintf("vsdStart[0]=%f\n", vsdStart[0]);
#endif
    while (vvdStart[0] > vsdStart[ivsd]) {
      if (++ivsd >= nvsd)
        ::Rcpp::stop("cannot interpret times for velocities, because no Vector "
                     "System Data precede first velocity datum");
#ifdef DEBUG
      Rprintf("ivsd=%d\n", ivsd);
#endif
    }
    if (ivsd > 0)
      ivsd--;
#ifdef DEBUG
    Rprintf("got ivsd=%d\n", ivsd);
    Rprintf("vvdStart[0]=%f\n", vvdStart[0]);
    Rprintf("vsdStart[%d]=%f\n", ivsd, vsdStart[ivsd]);
#endif
    // 2. step through vvd, updating left-neighbor vsd when necessary
    double toffset = 0.0;
    for (ivvd = 0; ivvd < nvvd; ivvd++) {
      if (ivsd < (nvsd - 1) && vsdStart[ivsd + 1] < vvdStart[ivvd]) {
        ivsd++; // enter new time era
        toffset = 0.0;
      }
      res[ivvd] = vsdTime[ivsd] + toffset;
      toffset += dt;
    }
  } else {
    // Burst sampling
#ifdef DEBUG
    Rprintf("burst sampling\n");
#endif
    // Pin time to start, if vvd precede vvdh (perhaps not possible).
    for (ivvd = 0; ivvd < nvvd; ivvd++) {
      if (vvdStart[ivvd] < vvdhStart[ivvdh]) {
        res[ivvd] = NA_REAL;
      } else {
        break;
      }
    }
#ifdef DEBUG
    Rprintf("ivvd= %d (C notation)  dt= %.10f   f %f\n", ivvd, dt, f[0]);
#endif
    if (ivvd < nvvd) {
      for (; ivvd < nvvd; ivvd++) {
        // #ifdef DEBUG
        //       Rprintf("ivvd=%d (%f)     ivvdh=%d (%f)\n", ivvd,
        //       vvdStart[ivvd], ivvdh, vvdhStart[ivvdh]);
        // #endif
        //  use largest vvdh that is still has vvdhStart < vvdStart
        if (ivvdh < (nvvdh - 1) && vvdStart[ivvd] > vvdhStart[ivvdh + 1]) {
          ivvdh += 1;
          t = vvdhTime[ivvdh];
#ifdef DEBUG
          Rprintf("ivvd = %d ; update to ivvdh = %d, yielding t=%f)\n", ivvd,
                  ivvdh, t);
#endif
        }
        t += dt;
        res[ivvd] = t;
      }
    }
  }
  return (res);
}
