text: // Part of the Soundplane client software by Madrona Labs.
  // Copyright (c) 2017 Madrona Labs LLC. http://www.madronalabs.com
  // Distributed under the MIT license: http://madrona-labs.mit-license.org/
  
  #include <algorithm>
  #include <array>
  #include <bitset>
  #include <cmath>
  #include <iostream>
  #include <limits>
  #include <list>
  #include <mutex>
  #include <thread>
  
  #include "TouchTracker.h"
  
  using namespace std::chrono;
  using namespace soundplane;
  
  constexpr float kTwoPi = 3.1415926535f * 2.f;
  
  template <class c> inline c(clamp)(const c &x, const c &min, const c &max) {
    return (x < min) ? min : (x > max ? max : x);
  }
  
  // within range, including start, excluding end value.
  template <class c> inline bool(within)(const c &x, const c &min, const c &max) {
    return ((x >= min) && (x < max));
  }
  
  inline float lerp(const float a, const float b, const float m) {
    return (a + m * (b - a));
  }
  
  inline float unlerp(const float a, const float b, const float x) {
    return (x - a) / (b - a);
  }
  
  inline float mapRange(const float a, const float b, const float c,
                        const float d, const float x) {
    return lerp(c, d, unlerp(a, b, x));
  }
  
  inline float mapAndClipRange(const float a, const float b, const float c,
                               const float d, const float x) {
    return lerp(c, d, clamp(unlerp(a, b, x), 0.f, 1.f));
  }
  
  // return city block distance between two touches with a "z importance"
  // parameter. if z scale is too small, zero touches will get matched with new
  // active ones in the same position. if too big, z is more important than
  // position and nothing works.
  inline float cityBlockDistanceXYZ(Touch a, Touch b, float zScale) {
    return fabs(a.x - b.x) + fabs(a.y - b.y) + zScale * fabs(a.z - b.z);
  }
  
  // TouchTracker
  
  TouchTracker::TouchTracker()
      : mSampleRate(1000.f), mMaxTouchesPerFrame(0), mLopassZ(50.),
        mRotate(false) {
    setThresh(0.1);
  
    for (int i = 0; i < kMaxTouches; i++) {
      mRotateShuffleOrder[i] = i;
    }
  }
  
  TouchTracker::~TouchTracker() {}
  
  void TouchTracker::setMaxTouches(int t) {
    int kmax = kMaxTouches;
    int newT = clamp(t, 0, kmax);
    if (newT != mMaxTouchesPerFrame) {
      mMaxTouchesPerFrame = newT;
  
      // reset shuffle order
      for (int i = 0; i < kMaxTouches; i++) {
        mRotateShuffleOrder[i] = i;
      }
  
      mClearNextFrame = true;
    }
  }
  
  void TouchTracker::setRotate(bool b) {
    mRotate = b;
    for (int i = 0; i < kMaxTouches; i++) {
      mRotateShuffleOrder[i] = i;
    }
  }
  
  void TouchTracker::clear() {
    for (int i = 0; i < kMaxTouches; i++) {
      mTouches[i] = Touch();
    }
  }
  
  // set the threshold of curvature that will cause a touch. Note that this will
  // not correspond with the pressure (z) values reported by touches.
  void TouchTracker::setThresh(float f) {
    mOnThreshold = clamp(f, 0.005f, 1.f);
    mFilterThreshold = mOnThreshold * 0.5f;
    mOffThreshold = mOnThreshold * 0.75f;
  }
  
  void TouchTracker::setLopassZ(float k) { mLopassZ = k; }
  
  SensorFrame smoothPressureX(const SensorFrame &in) {
    int i, j;
    const float *pr2;
    float *prOut;
  
    SensorFrame out{};
  
    for (j = 0; j < SensorGeometry::height; j++) {
      // row ptrs
      pr2 = (in.data() + (j * SensorGeometry::width));
      prOut = (out.data() + (j * SensorGeometry::width));
  
      i = 0; // left side
      { prOut[i] = (pr2[i] + pr2[i + 1]); }
  
      for (i = 1; i < SensorGeometry::width - 1; i++) // center
      {
        prOut[i] = (pr2[i - 1] + pr2[i] + pr2[i + 1]);
      }
  
      i = SensorGeometry::width - 1; // right side
      { prOut[i] = (pr2[i - 1] + pr2[i]); }
    }
    return out;
  }
  
  SensorFrame smoothPressureY(const SensorFrame &in) {
    int i, j;
    const float *pr1, *pr2, *pr3; // input row ptrs
    float *prOut;
  
    SensorFrame out{};
  
    j = 0; // top row
    {
      pr2 = (in.data() + j * SensorGeometry::width);
      pr3 = (in.data() + (j + 1) * SensorGeometry::width);
      prOut = (out.data() + j * SensorGeometry::width);
  
      for (i = 0; i < SensorGeometry::width; i++) {
        (prOut[i] = pr2[i] + pr3[i]);
      }
    }
    for (j = 1; j < SensorGeometry::height - 1; j++) // center rows
    {
      pr1 = (in.data() + (j - 1) * SensorGeometry::width);
      pr2 = (in.data() + j * SensorGeometry::width);
      pr3 = (in.data() + (j + 1) * SensorGeometry::width);
      prOut = (out.data() + j * SensorGeometry::width);
  
      for (i = 0; i < SensorGeometry::width; i++) {
        (prOut[i] = pr1[i] + pr2[i] + pr3[i]);
      }
    }
    j = SensorGeometry::height - 1; // bottom row
    {
      pr1 = (in.data() + (j - 1) * SensorGeometry::width);
      pr2 = (in.data() + j * SensorGeometry::width);
      prOut = (out.data() + j * SensorGeometry::width);
  
      for (i = 0; i < SensorGeometry::width; i++) {
        (prOut[i] = pr1[i] + pr2[i]);
      }
    }
    return out;
  }
  
  SensorFrame TouchTracker::preprocess(const SensorFrame &in) {
    SensorFrame y;
  
    // fixed IIR filter input
    float k = 0.25f;
    y = multiply(in, k);
    mInputZ1 = multiply(mInputZ1, 1.0 - k);
    y = add(y, mInputZ1);
    mInputZ1 = y;
  
    // filter out any negative values. negative values can show up from capacitive
    // coupling near edges, from motion or bending of the whole instrument, from
    // the elastic layer deforming and pushing up on the sensors near a touch.
    y = max(y, 0.f);
  
    // a lot of filtering is needed here for Soundplane A to make sure peaks are
    // in centers of touches. it also reduces noise. the down side is, contiguous
    // touches are harder to tell apart. a smart blob-shape algorithm can make up
    // for this later, with this filtering still intact.
    y = smoothPressureX(smoothPressureX(smoothPressureX(smoothPressureX(y))));
    y = smoothPressureY(smoothPressureY(smoothPressureY(y)));
    y = getCurvatureXY(multiply(y, 1.f / 64.f));
  
    return y;
  }
  
  // to clear the next frame, all touch z values must be set to 0 and states to
  // kTouchStateOff so that the frame is guaranteed to be sent.
  void TouchTracker::clearAndSendNextFrameIfNeeded() {
    if (mClearNextFrame) {
      mClearNextFrame = false;
      mTouches = TouchArray{};
      for (int i = 0; i < kMaxTouches; ++i) {
        mTouches[i].state = kTouchStateOff;
      }
    }
  }
  
  TouchArray TouchTracker::process(const SensorFrame &in, int maxTouches) {
    setMaxTouches(maxTouches);
  
    mTouches = TouchArray{};
  
    if (mMaxTouchesPerFrame > 0) {
      mTouches = findTouches(in);
  
      // match -> position filter -> feedback
      mTouches = matchTouches(mTouches, mTouchesMatch1);
      mTouches = filterTouchesXYAdaptive(mTouches, mTouchesMatch1);
      mTouchesMatch1 = mTouches;
  
      // asymmetrical z filter from user setting. Ages are created here.
      mTouches =
          filterTouchesZ(mTouches, mTouches2, mLopassZ * 2.f, mLopassZ * 0.25f);
      mTouches2 = mTouches;
  
      // after variable filter, exile decayed touches so they are not matched.
      // Note this affects match feedback!
      mTouchesMatch1 = exileUnusedTouches(mTouchesMatch1, mTouches);
  
      // TODO hysteresis after matching to prevent glitching when there are more
      // physical touches than mMaxTouchesPerFrame and touches are stolen
  
      if (mRotate) {
        mTouches = rotateTouches(mTouches);
      }
  
      mTouches = clampAndScaleTouches(mTouches);
    }
    clearAndSendNextFrameIfNeeded();
    return mTouches;
  }
  
  Touch correctPeakX(Touch pos, const SensorFrame &in) {
    Touch newPos = pos;
    const float maxCorrect = 0.5f;
    const int w = SensorGeometry::width;
    int x = pos.x;
    int y = pos.y;
  
    if (within(x, 1, w - 1)) {
      float a = in[y * w + x - 1];
      float b = in[y * w + x];
      float c = in[y * w + x + 1];
      float p = ((a - c) / (a - 2.f * b + c)) * 0.5f;
      float fx = x + clamp(p, -maxCorrect, maxCorrect);
      newPos = Touch{.x = fx, .y = pos.y, .z = pos.z};
    }
  
    return newPos;
  }
  
  Touch correctPeakY(Touch pos, const SensorFrame &in) {
    const float maxCorrect = 0.5f;
    const int w = SensorGeometry::width;
    const int h = SensorGeometry::height;
    int x = pos.x;
    int y = pos.y;
  
    float a, b, c;
    if (y <= 0) {
      a = 0;
      b = in[y * w + x];
      c = in[(y + 1) * w + x];
    } else if (y >= h - 1) {
      a = in[(y - 1) * w + x];
      b = in[y * w + x];
      c = 0;
    } else {
      a = in[(y - 1) * w + x];
      b = in[y * w + x];
      c = in[(y + 1) * w + x];
    }
    float p = ((a - c) / (a - 2.f * b + c)) * 0.5f;
    float fy = y + clamp(p, -maxCorrect, maxCorrect);
    return Touch{.x = pos.x, .y = fy, .z = pos.z};
  }
  
  float sensorToKeyY(float sy) {
    float ky = 0.f;
  
    constexpr int mapSize = 6;
  
    // Soundplane A as measured.
    // NOTE: these y locations depend on the amount of smoothing done in
    // smoothPressureX / smoothPressureY.
    constexpr std::array<float, mapSize> sensorMap{
        {0.7, 1.2, 2.7, 4.3, 5.8, 6.3}};
    constexpr std::array<float, mapSize> keyMap{{0.01, 1., 2., 3., 4., 4.99}};
  
    if (sy < sensorMap[0]) {
      ky = keyMap[0];
    } else if (sy > sensorMap[mapSize - 1]) {
      ky = keyMap[mapSize - 1];
    } else {
      // piecewise linear map
      for (int i = 1; i < mapSize; ++i) {
        if (sy <= sensorMap[i]) {
          // piecewise linear
          float m = (sy - sensorMap[i - 1]) / (sensorMap[i] - sensorMap[i - 1]);
          ky = lerp(keyMap[i - 1], keyMap[i], m);
          break;
        }
      }
    }
  
    return ky;
  }
  
  Touch peakToTouch(Touch p) {
    return Touch{.x = mapRange(3.5f, 59.5f, 1.f, 29.f, p.x),
                 .y = sensorToKeyY(p.y),
                 .z = p.z};
  }
  
  // quick touch finder based on peaks of curvature.
  // this works well, but a different approach based on blob sizes / shapes could
  // do a much better job with contiguous keys.
  TouchArray TouchTracker::findTouches(const SensorFrame &in) {
    constexpr int kMaxPeaks = kMaxTouches * 2;
    constexpr int w = SensorGeometry::width;
    constexpr int h = SensorGeometry::height;
    const float *pIn = in.data();
    int i, j;
  
    std::array<Touch, kMaxPeaks> peaks;
    TouchArray touches{};
    std::array<std::bitset<w>, h> map;
  
    // get peaks
    float f11, f12, f13;
    float f21, f22, f23;
    float f31, f32, f33;
  
    j = 0;
    {
      auto &row = map[j];
      const float *pRow2 = pIn + (j)*w;
      const float *pRow3 = pIn + (j + 1) * w;
      for (i = 1; i < w - 1; ++i) {
        f21 = pRow2[i - 1];
        f22 = pRow2[i];
        f23 = pRow2[i + 1];
        f31 = pRow3[i - 1];
        f32 = pRow3[i];
        f33 = pRow3[1 + 1];
        row[i] = (f22 > f21) && (f22 > mFilterThreshold) && (f22 > f23) &&
                 (f22 > f31) && (f22 > f32) && (f22 > f33);
      }
    }
  
    for (j = 1; j < h - 1; ++j) {
      auto &row = map[j];
      const float *pRow1 = pIn + (j - 1) * w;
      const float *pRow2 = pIn + (j)*w;
      const float *pRow3 = pIn + (j + 1) * w;
      for (i = 1; i < w - 1; ++i) {
        f11 = pRow1[i - 1];
        f12 = pRow1[i];
        f13 = pRow1[i + 1];
        f21 = pRow2[i - 1];
        f22 = pRow2[i];
        f23 = pRow2[i + 1];
        f31 = pRow3[i - 1];
        f32 = pRow3[i];
        f33 = pRow3[1 + 1];
        row[i] = (f22 > f11) && (f22 > f12) && (f22 > f13) && (f22 > f21) &&
                 (f22 > mFilterThreshold) && (f22 > f23) && (f22 > f31) &&
                 (f22 > f32) && (f22 > f33);
      }
    }
  
    j = h - 1;
    {
      auto &row = map[j];
      const float *pRow1 = pIn + (j - 1) * w;
      const float *pRow2 = pIn + (j)*w;
      for (i = 1; i < w - 1; ++i) {
        f11 = pRow1[i - 1];
        f12 = pRow1[i];
        f13 = pRow1[i + 1];
        f21 = pRow2[i - 1];
        f22 = pRow2[i];
        f23 = pRow2[i + 1];
        row[i] = (f22 > f11) && (f22 > f12) && (f22 > f13) && (f22 > f21) &&
                 (f22 > mFilterThreshold) && (f22 > f23);
      }
    }
  
    // gather all peaks.
    int nPeaks = 0;
    for (int j = 0; j < h; j++) {
      auto &mapRow = map[j];
      for (int i = 0; i < w; i++) {
        // if peak
        if (mapRow[i]) {
          float z = in[j * w + i];
          peaks[nPeaks++] = Touch{
              .x = static_cast<float>(i), .y = static_cast<float>(j), .z = z};
          if (nPeaks >= kMaxPeaks)
            break;
        }
      }
    }
  
    if (nPeaks > 1) {
      std::sort(peaks.begin(), peaks.begin() + nPeaks,
                [](Touch a, Touch b) { return a.z > b.z; });
    }
  
    // correct and clip
    int nTouches = std::min(nPeaks, (int)kMaxTouches);
    for (int i = 0; i < nTouches; ++i) {
      Touch p = peaks[i];
      if (p.z < mFilterThreshold)
        break;
  
      Touch px = correctPeakX(p, in);
      Touch pxy = correctPeakY(px, in);
      touches[i] = peakToTouch(pxy);
    }
  
    return touches;
  }
  
  // match incoming touches in x with previous frame of touches in x1.
  // for each possible touch slot, output the touch x closest in location to the
  // previous frame. if the incoming touch is a continuation of the previous one,
  // set its age (w) to 1, otherwise to 0. if there is no incoming touch to match
  // with a previous one at index i, and no new touch needs index i, the position
  // at index i will be maintained.
  
  // TODO first touch below filter threshold(?) is on one index, then active touch
  // switches index?! investigate.
  
  TouchArray TouchTracker::matchTouches(const TouchArray &x,
                                        const TouchArray &x1) {
    const float kMaxConnectDist = 2.f;
  
    TouchArray newTouches{};
  
    std::array<int, kMaxTouches> forwardMatchIdx;
    forwardMatchIdx.fill(-1);
  
    std::array<int, kMaxTouches> reverseMatchIdx;
    reverseMatchIdx.fill(-1);
  
    // for each previous touch, find minimum distance to a current touch.
    for (int i = 0; i < mMaxTouchesPerFrame; ++i) {
      float minDist = std::numeric_limits<float>::max();
      Touch prev = x1[i];
  
      for (int j = 0; j < mMaxTouchesPerFrame; ++j) {
        Touch curr = x[j];
        if ((curr.z > mFilterThreshold) && (prev.z > mFilterThreshold)) {
          float distToCurrentTouch = cityBlockDistanceXYZ(prev, curr, 20.f);
          if (distToCurrentTouch < minDist) {
            forwardMatchIdx[i] = j;
            minDist = distToCurrentTouch;
          }
        }
      }
    }
  
    // for each current touch, find minimum distance to a previous touch
    for (int i = 0; i < mMaxTouchesPerFrame; ++i) {
      float minDist = std::numeric_limits<float>::max();
      Touch curr = x[i];
      for (int j = 0; j < mMaxTouchesPerFrame; ++j) {
        Touch prev = x1[j];
        if ((curr.z > mFilterThreshold) && (prev.z > mFilterThreshold)) {
          float distToPreviousTouch = cityBlockDistanceXYZ(prev, curr, 20.f);
          if (distToPreviousTouch < minDist) {
            reverseMatchIdx[i] = j;
            minDist = distToPreviousTouch;
          }
        }
      }
    }
  
    // get mutual matches
    std::array<int, kMaxTouches> mutualMatches;
    mutualMatches.fill(0);
    for (int i = 0; i < mMaxTouchesPerFrame; ++i) {
      int prevIdx = reverseMatchIdx[i];
      if (prevIdx >= 0) {
        if (forwardMatchIdx[prevIdx] == i) {
          mutualMatches[i] = true;
        }
      }
    }
  
    std::array<bool, kMaxTouches> currWrittenToNew;
    currWrittenToNew.fill(false);
  
    // first, continue mutually matched  touches
    for (int i = 0; i < mMaxTouchesPerFrame; ++i) {
      if (mutualMatches[i]) {
        int j = reverseMatchIdx[i];
        Touch curr = x[i];
        Touch prev = x1[j];
        {
          // touch is continued, mark as connected and write to new touches
          curr.age = (cityBlockDistanceXYZ(prev, curr, 0.f) < kMaxConnectDist);
          newTouches[j] = curr;
          currWrittenToNew[i] = true;
        }
      }
    }
  
    // now take care of any remaining nonzero current touches
    for (int i = 0; i < mMaxTouchesPerFrame; ++i) {
      Touch curr = x[i];
      if ((!currWrittenToNew[i]) && (curr.z > mFilterThreshold)) {
        int freeIdx = -1;
        float minDist = std::numeric_limits<float>::max();
  
        // first, try to match same touch index (important for decay!)
        Touch prev = x1[i];
        if (prev.z <= mFilterThreshold) {
          freeIdx = i;
        }
  
        // then try closest free touch
        if (freeIdx < 0) {
          for (int j = 0; j < mMaxTouchesPerFrame; ++j) {
            Touch prev = x1[j];
            if (prev.z <= mFilterThreshold) {
              float d = cityBlockDistanceXYZ(curr, prev, 0.f);
              if (d < minDist) {
                minDist = d;
                freeIdx = j;
              }
            }
          }
        }
  
        // if a free index was found, write the current touch
        if (freeIdx >= 0) {
          Touch free = x1[freeIdx];
          curr.age = (cityBlockDistanceXYZ(free, curr, 0.f) < kMaxConnectDist);
          newTouches[freeIdx] = curr;
        }
      }
    }
  
    // fill in any free touches with previous touches at those indices. This will
    // allow old touches to re-link if not reused.
    for (int i = 0; i < mMaxTouchesPerFrame; ++i) {
      Touch t = newTouches[i];
      if (t.z <= mFilterThreshold) {
        newTouches[i].x = (x1[i].x);
        newTouches[i].y = (x1[i].y);
      }
    }
  
    return newTouches;
  }
  
  // input: vec4<x, y, z, k> where k is 1 if the touch is connected to the
  // previous touch at the same index.
  //
  TouchArray TouchTracker::filterTouchesXYAdaptive(const TouchArray &in,
                                                   const TouchArray &inz1) {
    // these filter settings have a big and sort of delicate impact on play feel,
    // so they are not user settable
    const float kFixedXYFreqMax = 20.f;
    const float kFixedXYFreqMin = 1.f;
  
    TouchArray out{};
  
    for (int i = 0; i < mMaxTouchesPerFrame; ++i) {
      float x = in[i].x;
      float y = in[i].y;
      float z = in[i].z;
      int age = in[i].age;
  
      float x1 = inz1[i].x;
      float y1 = inz1[i].y;
  
      // filter, or not, based on w from matchTouches
      float newX, newY;
      if (age > 0) {
        // get xy coeffs, adaptive based on z
        float freq =
            mapAndClipRange(0., 0.02, kFixedXYFreqMin, kFixedXYFreqMax, z);
        float omegaXY = freq * kTwoPi / mSampleRate;
        float kXY = expf(-omegaXY);
        float a0XY = 1.f - kXY;
        float b1XY = kXY;
  
        // onepole filters
        newX = (x * a0XY) + (x1 * b1XY);
        newY = (y * a0XY) + (y1 * b1XY);
      } else {
        newX = x;
        newY = y;
      }
  
      out[i] = Touch{.x = newX, .y = newY, .z = z, .dz = 0, .age = age};
    }
  
    return out;
  }
  
  TouchArray TouchTracker::filterTouchesZ(const TouchArray &in,
                                          const TouchArray &inz1, float upFreq,
                                          float downFreq) {
    const float omegaUp = upFreq * kTwoPi / mSampleRate;
    const float kUp = expf(-omegaUp);
    const float a0Up = 1.f - kUp;
    const float b1Up = kUp;
    const float omegaDown = downFreq * kTwoPi / mSampleRate;
    const float kDown = expf(-omegaDown);
    const float a0Down = 1.f - kDown;
    const float b1Down = kDown;
  
    TouchArray out{};
  
    for (int i = 0; i < mMaxTouchesPerFrame; ++i) {
      float x = in[i].x;
      float y = in[i].y;
      float z = in[i].z;
  
      float z1 = inz1[i].z;
      int age1 = inz1[i].age;
  
      float newZ;
  
      // filter z variable
      float dz = z - z1;
      if (dz > 0.f) {
        newZ = (z * a0Up) + (z1 * b1Up);
      } else {
        newZ = (z * a0Down) + (z1 * b1Down);
      }
  
      // gate with hysteresis
      bool gate1 = (age1 > 0);
      bool newGate = gate1;
      if (newZ > mOnThreshold) {
        newGate = true;
      } else if (newZ < mOffThreshold) {
        newGate = false;
      }
  
      // increment age
      int newAge = newGate ? (age1 + 1) : 0;
  
      // set state
      int newState = newGate ? (gate1 ? kTouchStateContinue : kTouchStateOn)
                             : (gate1 ? kTouchStateOff : kTouchStateInactive);
  
      out[i] = Touch{
          .x = x, .y = y, .z = newZ, .dz = dz, .age = newAge, .state = newState};
    }
  
    return out;
  }
  
  // if a touch has decayed below the filter threshold after z filtering, move it
  // off the scene so it won't match to other nearby touches.
  TouchArray TouchTracker::exileUnusedTouches(const TouchArray &preFiltered,
                                              const TouchArray &postFiltered) {
    TouchArray out(preFiltered);
    for (int i = 0; i < mMaxTouchesPerFrame; ++i) {
      Touch a = preFiltered[i];
      Touch b = postFiltered[i];
  
      if (b.x > 0.f) {
        if (b.z <= mFilterThreshold) {
          a.x = (-1.f);
          a.y = (-10.f);
          a.z = (0.f);
        }
      }
  
      out[i] = a;
    }
    return out;
  }
  
  // rotate order of touches, changing order every time there is a new touch in a
  // frame. side effect: writes to mRotateShuffleOrder
  TouchArray TouchTracker::rotateTouches(const TouchArray &in) {
    TouchArray touches(in);
    if (mMaxTouchesPerFrame > 1) {
      bool doRotate = false;
      for (int i = 0; i < mMaxTouchesPerFrame; ++i) {
        if (in[i].age == 1) {
          // we have a new touch at index i.
          doRotate = true;
          break;
        }
      }
  
      if (doRotate) {
        // rotate the indices of all free or new touches in the shuffle order
        int nFree = 0;
        std::array<int, kMaxTouches> freeIndexes;
  
        for (int i = 0; i < mMaxTouchesPerFrame; ++i) {
          if ((in[i].z < mFilterThreshold) || (in[i].age == 1)) {
            freeIndexes[nFree++] = i;
          }
        }
  
        if (nFree > 1) {
          int first = mRotateShuffleOrder[freeIndexes[0]];
          for (int i = 0; i < nFree - 1; ++i) {
            mRotateShuffleOrder[freeIndexes[i]] =
                mRotateShuffleOrder[freeIndexes[i + 1]];
          }
          mRotateShuffleOrder[freeIndexes[nFree - 1]] = first;
        }
      }
  
      // shuffle
      for (int i = 0; i < mMaxTouchesPerFrame; ++i) {
        touches[mRotateShuffleOrder[i]] = in[i];
      }
    }
    return touches;
  }
  
  TouchArray TouchTracker::clampAndScaleTouches(const TouchArray &in) {
    const float kTouchOutputScale = 4.f;
    TouchArray out{};
    for (int i = 0; i < mMaxTouchesPerFrame; ++i) {
      Touch t = in[i];
  
      if (t.x != t.x) {
        t.x = (0.f);
      }
      if (t.y != t.y) {
        t.y = (0.f);
      }
  
      out[i] = t;
      float newZ = (clamp((t.z - mOnThreshold) * kTouchOutputScale, 0.f, 8.f));
      if (t.age == 0) {
        newZ = 0.f;
      }
      out[i].z = (newZ);
    }
    return out;
  }
  
  TouchArray TouchTracker::getTestTouches(time_point<system_clock> now,
                                          int maxTouches) {
    TouchArray t{};
  
    setMaxTouches(maxTouches);
  
    system_clock::duration dtn = now.time_since_epoch();
    auto m = std::chrono::duration_cast<std::chrono::milliseconds>(dtn).count();
    auto c = m % 60000;
    int ic = c;
    float fc = ic;
  
    for (int i = 0; i < maxTouches; ++i) {
      float minusI = 16 - i;
      float rpm = 2.f * (i + 1);
      float theta = fc * rpm * kTwoPi / 60000.f;
      float rx = 12.f / 16. * minusI;
      float ry = 2.f / 16. * minusI;
      float cx = 15.f;
      float cy = 2.5f;
  
      float x = cosf(theta) * rx + cx;
      float y = sinf(theta) * ry + cy;
  
      float theta2 = theta * 8.f;
      float amp = sinf(theta2) * 0.25f;
  
      t[i] = Touch{.x = x, .y = y, .z = amp};
    }
  
    mTouches = t;
    // asymmetrical z filter from user setting. Ages are created here.
    mTouches =
        filterTouchesZ(mTouches, mTouches2, mLopassZ * 2.f, mLopassZ * 0.25f);
    mTouches2 = mTouches;
    mTouches = clampAndScaleTouches(mTouches);
  
    clearAndSendNextFrameIfNeeded();
    return mTouches;
  }
  