A smart data smoothing function.

void SmoothData(mglData * cX, mglData * cY,
                mglData * cXAvg, mglData * cYAvg,
                mglData * cXStdDev, mglData * cYStdDev,
                double dThresholdX, double dThresholdY) {

  bool bAcquire = false;

  float fXSum = 0;
  float fXSumSqr = 0;
  float fXAvg = 0;
  float fXStdDev = 0;

  float fYSum = 0;
  float fYSumSqr = 0;
  float fYAvg = 0;
  float fYStdDev = 0;

  long nNumTempSamples = 0;
  long nNumDataPoints = 0;

  float fStartX = cX->a[0];
  float fStartY = cY->a[0];

  for (int nIndex = 0; nIndex <= cX->nx; nIndex++) {

    nNumTempSamples++;

    fXSum += cX->a[nIndex];
    fYSum += cY->a[nIndex];

    fXSumSqr += cX->a[nIndex] * cX->a[nIndex];
    fYSumSqr += cY->a[nIndex] * cY->a[nIndex];

    //printf("  Sample %ld: Pair: (%f, %f) Sum: (%f, %f) SumSqr: (%f, %f)\n",
    //                                                            nNumTempSamples,
    //                                                            cX->a[nIndex], cY->a[nIndex],
    //                                                            fXSum, fYSum,
    //                                                            fXSumSqr, fYSumSqr);

    if (nNumTempSamples == 1) {
      fXAvg = cX->a[nIndex];
      fYAvg = cY->a[nIndex];
    }

    if (nNumTempSamples > 1) {
      fXAvg = fXSum / nNumTempSamples;
      fYAvg = fYSum / nNumTempSamples;
    }
    if (nNumTempSamples > 2) {
      fXStdDev = sqrt((fXSumSqr - (fXSum * fXSum)/nNumTempSamples) / (nNumTempSamples - 1) );
      fYStdDev = sqrt((fYSumSqr - (fYSum * fYSum)/nNumTempSamples) / (nNumTempSamples - 1) );
    }

    //printf("  Avg: (%f, %f) StdDev(%f, %f) Start: (%f, %f)\n", fXAvg, fYAvg,
    //                                                           fXStdDev, fYStdDev,
    //                                                           fStartX, fStartY);
    if (fXAvg > 1) {
      if ( fabs((fXAvg - fStartX) ) > dThresholdX*10) {
        bAcquire = true;
      }
    }
    else {
      if ( fabs((fXAvg - fStartX) ) > dThresholdX) {
        bAcquire = true;
      }
    }
    if ( fabs((fYAvg - fStartY) / fStartY) > dThresholdY) {
      bAcquire = true;
    }

    if (bAcquire) {
      bAcquire = false;
      nNumDataPoints++;

      cXAvg->Put(fXAvg, (nNumDataPoints -1) );
      cXStdDev->Put(fXStdDev, (nNumDataPoints -1) );

      cYAvg->Put(fYAvg, (nNumDataPoints -1) );
      cYStdDev->Put(fYStdDev, (nNumDataPoints -1) );

      //printf("  * New datapoint: (%f+-%f, %f+-%f)\n", fXAvg, fXStdDev, fYAvg, fYStdDev);

      nNumTempSamples = 0;

      fStartX = fXAvg;
      fStartY = fYAvg;

      fXAvg = 0;
      fXStdDev = 0;
      fXSum = 0;
      fXSumSqr = 0;

      fYAvg = 0;
      fYStdDev = 0;
      fYSum = 0;
      fYSumSqr = 0;
    }
  }
  cXAvg->Resize(nNumDataPoints);
  cYAvg->Resize(nNumDataPoints);
  cXStdDev->Resize(nNumDataPoints);
  cXStdDev->Resize(nNumDataPoints);
}