/**
   * Pitch
   *
   * @param in
   * @return
   */
  private double[][] pitch2(double[][] in) {
    if (!state.get()) return in;

    OpCplx op =
        new OpCplx() {

          @Override
          public double magn(double re, double im) {
            return Math.sqrt(re * re + im * im);
          }

          @Override
          public double agl(double re, double im) {
            return Math.atan(im / re);
          }

          @Override
          public double real(double magnitude, double angle) {
            return magnitude * Math.cos(angle);
          }

          @Override
          public double imag(double magnitude, double angle) {
            return magnitude * Math.sin(angle);
          }
        };

    FastFourierTransformer fft = new FastFourierTransformer(DftNormalization.STANDARD);
    Complex[] freq, inverse, freqn;

    for (int c = 0; c < in.length; c++) {

      freq = fft.transform(in[c], TransformType.FORWARD);
      freqn = new Complex[freq.length];

      freqn[0] = Complex.valueOf(freq[0].getReal(), freq[0].getImaginary());

      for (int i = 1; i <= freq.length / 2; i++) {

        double fOrig = i / factor + shift;

        int left = (int) Math.floor(fOrig);
        int right = (int) Math.ceil(fOrig);
        double weighting = fOrig - left;

        double new_Re = 0, new_Im = 0;

        if (left > 0 && left < freq.length / 2 && right > 0 && right < freq.length / 2) {
          new_Re = interpolate(freq[left].getReal(), freq[right].getReal(), weighting);
          new_Im = interpolate(freq[left].getImaginary(), freq[right].getImaginary(), weighting);
        }
        freqn[i] = Complex.valueOf(new_Re, new_Im);
        freqn[freq.length - i] = Complex.valueOf(new_Re, new_Im);
      }
      inverse = fft.transform(freqn, TransformType.INVERSE);

      for (int i = 0; i < inverse.length; i++) {
        in[c][i] = inverse[i].getReal();
      }
    }
    return in;
  }
  protected void processData(final Context context, Bundle dataBundle) {
    if (dataBundle.containsKey(ContinuousProbe.EVENT_TIMESTAMP)
        && dataBundle.containsKey("X")
        && dataBundle.containsKey("Y")
        && dataBundle.containsKey("Z")) {
      double[] incomingTimes = dataBundle.getDoubleArray(ContinuousProbe.EVENT_TIMESTAMP);

      float[] incomingX = dataBundle.getFloatArray("X");
      float[] incomingY = dataBundle.getFloatArray("Y");
      float[] incomingZ = dataBundle.getFloatArray("Z");

      this.appendValues(incomingX, incomingY, incomingZ, incomingTimes);

      final long now = System.currentTimeMillis();

      final String key = this.featureKey();

      final SharedPreferences prefs = PreferenceManager.getDefaultSharedPreferences(context);
      long updateInterval =
          Long.parseLong(
                  prefs.getString(
                      "config_probe_" + key + "_frequency",
                      XYZBasicFrequencyFeature.DEFAULT_FREQUENCY))
              * 1000;

      if (now - this._lastUpdate > updateInterval) // add last updated
      // check for config
      {
        this._lastUpdate = now;

        LinearInterpolator interpolator = new LinearInterpolator();

        double[] xs = _xValues;
        double[] ys = _yValues;
        double[] zs = _zValues;
        double[] ts = _timestamps;

        if (this._currentIndex < BUFFER_SIZE - 1) {
          xs = Arrays.copyOfRange(_xValues, 0, this._currentIndex);
          ys = Arrays.copyOfRange(_yValues, 0, this._currentIndex);
          zs = Arrays.copyOfRange(_zValues, 0, this._currentIndex);
          ts = Arrays.copyOfRange(_timestamps, 0, this._currentIndex);
        }

        // Log.e("PR", "FIRST RAW TIME: " + ts[0] + " LAST RAW TIME: " +
        // ts[ts.length - 1]);

        // Log.e("PR", "RAW TIME[0]: " + ts[0]);
        // Log.e("PR", "RAW TIME[1]: " + ts[1]);

        PolynomialSplineFunction fX = interpolator.interpolate(ts, xs);
        PolynomialSplineFunction fY = interpolator.interpolate(ts, ys);
        PolynomialSplineFunction fZ = interpolator.interpolate(ts, zs);

        // double lowFreq = 0.6;
        // double highFreq = 7.0;

        double durationOffset = ts[0];
        double bufferDuration = ts[ts.length - 1] - durationOffset;

        double interval = 1.0 / 120.0;

        // Log.e("PR", "TS/0: " + ts[0] + " -- TS/-1: " + ts[ts.length -
        // 1] + " -- LEN TS: " + ts.length);
        // Log.e("PR", "BD: " + bufferDuration + " INT: " + interval);

        int twoPow = ts.length == 0 ? 0 : (32 - Integer.numberOfLeadingZeros(ts.length - 1));
        int bufferSize = (int) Math.pow(2, twoPow);

        // Log.e("PR", "BUFF SIZE: " + bufferSize);

        final double[] _interX = new double[bufferSize];
        final double[] _interY = new double[bufferSize];
        final double[] _interZ = new double[bufferSize];

        Arrays.fill(_interX, 0.0);
        Arrays.fill(_interY, 0.0);
        Arrays.fill(_interZ, 0.0);

        interTimes = new double[bufferSize];

        for (int i = 0; i < bufferSize; i++) {
          interTimes[i] = durationOffset + (i * interval);

          // Log.e("PR", "TIME REQUEST: " + time);
          // Log.e("PR", "TIME DIFFERENCE: " + (oldTime - time));

          if (interTimes[i] > ts[ts.length - 1]) // If the current
            // timestamp is
            // greater than the
            // last recorded
            // timestamp, set it
            // to the last
            // timestamp
            interTimes[i] = ts[ts.length - 1];

          _interX[i] = fX.value(interTimes[i]);
          _interY[i] = fY.value(interTimes[i]);
          _interZ[i] = fZ.value(interTimes[i]);
        }

        // double timeDifference = interTimes[bufferSize - 1] -
        // interTimes[0];

        // Log.e("PR", "INTERP TIME: " + timeDifference +
        // " BUFFER SIZE: " + bufferSize);
        // Log.e("PR", "FIRST INTERP TIME: " + interTimes[0] +
        // " LAST INTERP TIME: " + interTimes[interTimes.length - 1]);

        // Log.e("PR", "INTERP SAMPLE: " + interX[bufferSize - 1] +
        // " - " + interY[bufferSize - 1] + " - " + interZ[bufferSize -
        // 1]);

        final double[] _dynamicX = new double[_interX.length];
        final double[] _dynamicY = new double[_interY.length];
        final double[] _dynamicZ = new double[_interZ.length];

        final double[] _staticX = new double[_interX.length];
        final double[] _staticY = new double[_interY.length];
        final double[] _staticZ = new double[_interZ.length];

        for (int i = 0; i < _interX.length; i++) {
          if (i < 2) {
            _dynamicX[i] = 0;
            _dynamicY[i] = 0;
            _dynamicZ[i] = 0;

            _staticX[i] = 0;
            _staticY[i] = 0;
            _staticZ[i] = 0;
          } else {
            if (i == _dynamicX.length - 1) {
              _dynamicX[i] = XYZBasicFrequencyFeature.bpFilter(_interX, this._xBPHistory, i, "X");
              _dynamicY[i] = XYZBasicFrequencyFeature.bpFilter(_interY, this._yBPHistory, i, "Y");
              _dynamicZ[i] = XYZBasicFrequencyFeature.bpFilter(_interZ, this._zBPHistory, i, "Z");

              _staticX[i] = XYZBasicFrequencyFeature.lpFilter(_interX, this._xLPHistory, i, "X");
              _staticY[i] = XYZBasicFrequencyFeature.lpFilter(_interY, this._yLPHistory, i, "Y");
              _staticZ[i] = XYZBasicFrequencyFeature.lpFilter(_interZ, this._zLPHistory, i, "Z");
            } else {
              _dynamicX[i] = XYZBasicFrequencyFeature.bpFilter(_interX, this._xBPHistory, i, null);
              _dynamicY[i] = XYZBasicFrequencyFeature.bpFilter(_interY, this._yBPHistory, i, null);
              _dynamicZ[i] = XYZBasicFrequencyFeature.bpFilter(_interZ, this._zBPHistory, i, null);

              _staticX[i] = XYZBasicFrequencyFeature.lpFilter(_interX, this._xLPHistory, i, null);
              _staticY[i] = XYZBasicFrequencyFeature.lpFilter(_interY, this._yLPHistory, i, null);
              _staticZ[i] = XYZBasicFrequencyFeature.lpFilter(_interZ, this._zLPHistory, i, null);
            }

            this._xBPHistory[1] = this._xBPHistory[0];
            this._xBPHistory[0] = _dynamicX[i];

            this._yBPHistory[1] = this._yBPHistory[0];
            this._yBPHistory[0] = _dynamicY[i];

            this._zBPHistory[1] = this._zBPHistory[0];
            this._zBPHistory[0] = _dynamicZ[i];

            this._xLPHistory[1] = this._xLPHistory[0];
            this._xLPHistory[0] = _staticX[i];

            this._yLPHistory[1] = this._yLPHistory[0];
            this._yLPHistory[0] = _staticY[i];

            this._zLPHistory[1] = this._zLPHistory[0];
            this._zLPHistory[0] = _staticZ[i];
          }
        }

        // Log.e("PR", "Inter Sample: " + _interX[_interX.length - 1] +
        // " - " + _interY[_interX.length - 1] + " - " +
        // _interZ[_interX.length - 1]);
        // Log.e("PR", "DY Sample: " + _dynamicX[_dynamicX.length - 1] +
        // " - " + _dynamicY[_dynamicX.length - 1] + " - " +
        // _dynamicZ[_interX.length - 1]);
        // Log.e("PR", "GR Sample: " + _staticX[_staticX.length - 1] +
        // " - " + _staticY[_staticY.length - 1] + " - " +
        // _staticZ[_staticZ.length - 1]);

        double observedFreq = _interX.length / bufferDuration; // (((double)
        // this._currentIndex)
        // /
        // bufferDuration);

        // Log.e("PR", "IL: + " + _interX.length + " / BD: " +
        // bufferDuration);
        // Log.e("PR", "OBS HZ: " + observedFreq);

        FastFourierTransformer fft = new FastFourierTransformer(DftNormalization.STANDARD);

        Complex[] xFFT = fft.transform(_dynamicX, TransformType.FORWARD);
        Complex[] yFFT = fft.transform(_dynamicY, TransformType.FORWARD);
        Complex[] zFFT = fft.transform(_dynamicZ, TransformType.FORWARD);

        double[] frequencies =
            XYZBasicFrequencyFeature.calculateFreqArray(_interX.length, observedFreq);

        final double[] _xMaxFreqPowPair =
            XYZBasicFrequencyFeature.findPeakFrequency(xFFT, frequencies);
        final double[] _yMaxFreqPowPair =
            XYZBasicFrequencyFeature.findPeakFrequency(yFFT, frequencies);
        final double[] _zMaxFreqPowPair =
            XYZBasicFrequencyFeature.findPeakFrequency(zFFT, frequencies);

        // Log.e("PR", "FREQS & GEEKS: x:" + _xMaxFreqPowPair[0] + " - "
        // + _xMaxFreqPowPair[1] + " y:" + _yMaxFreqPowPair[0] + " - " +
        // _yMaxFreqPowPair[1] + " z:" + _zMaxFreqPowPair[0] + " - " +
        // _zMaxFreqPowPair[1] );

        final XYZBasicFrequencyFeature me = this;

        Runnable r =
            new Runnable() {
              public void run() {
                Bundle data = new Bundle();

                data.putDouble("TIMESTAMP", now / 1000);
                data.putString("PROBE", me.name(context));

                boolean incInterpolated =
                    prefs.getBoolean(
                        "config_probe_" + key + "_interpolated_enabled",
                        XYZBasicFrequencyFeature.INTERPOLATED_ENABLED);
                boolean incBandpass =
                    prefs.getBoolean(
                        "config_probe_" + key + "_bandpass_enabled",
                        XYZBasicFrequencyFeature.BANDPASS_ENABLED);
                boolean incLowpass =
                    prefs.getBoolean(
                        "config_probe_" + key + "_lowpass_enabled",
                        XYZBasicFrequencyFeature.LOWPASS_ENABLED);

                if (incInterpolated || incBandpass || incLowpass) {
                  Bundle sensorData = new Bundle();

                  synchronized (me) {
                    sensorData.putDoubleArray("INTERP_TIMESTAMPS", interTimes);

                    if (incInterpolated) {
                      sensorData.putDoubleArray("INTER_X", _interX);
                      sensorData.putDoubleArray("INTER_Y", _interY);
                      sensorData.putDoubleArray("INTER_Z", _interZ);
                    }

                    if (incBandpass) {
                      sensorData.putDoubleArray("DYNAMIC_X", _dynamicX);
                      sensorData.putDoubleArray("DYNAMIC_Y", _dynamicY);
                      sensorData.putDoubleArray("DYNAMIC_Z", _dynamicZ);
                    }

                    if (incLowpass) {
                      sensorData.putDoubleArray("STATIC_X", _staticX);
                      sensorData.putDoubleArray("STATIC_Y", _staticY);
                      sensorData.putDoubleArray("STATIC_Z", _staticZ);
                    }

                    data.putBundle("CALCULATIONS", sensorData);
                  }
                }

                data.putDouble("WINDOW_TIMESTAMP", interTimes[0]);

                data.putDouble("POWER_X", _xMaxFreqPowPair[1]);
                data.putDouble("POWER_Y", _yMaxFreqPowPair[1]);
                data.putDouble("POWER_Z", _zMaxFreqPowPair[1]);

                data.putDouble("FREQ_X", _xMaxFreqPowPair[0]);
                data.putDouble("FREQ_Y", _yMaxFreqPowPair[0]);
                data.putDouble("FREQ_Z", _zMaxFreqPowPair[0]);

                me.transmitData(context, data);
              }
            };

        Thread t = new Thread(r);

        t.start();
      }
    }
  }
  /**
   * Pitch
   *
   * @param in
   * @return
   */
  private double[][] pitch(double[][] in) {
    if (!state.get()) return in;

    OpCplx op =
        new OpCplx() {

          @Override
          public double magn(double re, double im) {
            return Math.sqrt(re * re + im * im);
          }

          @Override
          public double agl(double re, double im) {
            return Math.atan(im / re);
          }

          @Override
          public double real(double magnitude, double angle) {
            return magnitude * Math.cos(angle);
          }

          @Override
          public double imag(double magnitude, double angle) {
            return magnitude * Math.sin(angle);
          }
        };

    FastFourierTransformer fft = new FastFourierTransformer(DftNormalization.STANDARD);
    Complex[] freq, inverse;

    for (int c = 0; c < in.length; c++) {

      freq = fft.transform(in[c], TransformType.FORWARD);

      double[] ampl = new double[freq.length];
      double[] angl = new double[freq.length];

      double re, im;

      boolean[] unitRe = new boolean[freq.length];
      boolean[] unitIm = new boolean[freq.length];

      double fctr = factor;

      for (int f = 0; f < freq.length; f++) {
        re = freq[f].getReal();
        im = freq[f].getImaginary();
        unitRe[f] = re >= 0;
        unitIm[f] = im >= 0;

        ampl[f] = op.magn(re, im);
        angl[f] = op.agl(re, im);
      }

      for (int f = 0; f < freq.length; f++) {
        int val = f < freq.length / 2 ? f : freq.length / 2 - (f - freq.length / 2);
        double weighting = ((double) val / fctr + shift) % 1;

        int left = (int) Math.floor(val / fctr + shift);
        int right = (int) Math.ceil(val / fctr + shift);
        double new_ampl = 0, new_angl = 0, new_re = 0, new_im = 0;

        if (left >= 0 && left < freq.length / 2 && right >= 0 && right < freq.length / 2) {
          new_ampl = interpolate(ampl[left], ampl[right], weighting);
          new_angl = interpolate(angl[left], angl[right], weighting);
          new_re = interpolate(freq[left].getReal(), freq[right].getReal(), weighting);
          new_im = interpolate(freq[left].getImaginary(), freq[right].getImaginary(), weighting);
        }

        if (modus == Modus.Both) {
          re = op.real(new_ampl, new_angl);
          im = op.imag(new_ampl, new_angl);
        } else if (modus == Modus.Amplitude) {
          re = op.real(new_ampl, angl[f]);
          im = op.imag(new_ampl, angl[f]);
        } else if (modus == Modus.Phase) {
          re = op.real(ampl[f], new_angl);
          im = op.imag(ampl[f], new_angl);
        } else if (modus == Modus.Direct) {
          re = new_re;
          im = new_im;
        } else {
          re = 0;
          im = 0;
        }

        re = unitRe[f] ? Math.abs(re) : Math.abs(re) * -1;
        im = unitIm[f] ? Math.abs(im) : Math.abs(im) * -1;

        if (f >= freq.length) im = -im;

        freq[f] = Complex.valueOf(re, im);
      }

      inverse = fft.transform(freq, TransformType.INVERSE);

      for (int i = 0; i < inverse.length; i++) {
        in[c][i] = inverse[i].getReal();
      }
    }

    return in;
  }