#include "filter.h"

void Resonator::clear()
{
  xp = xpp = yp = ypp = 0;
}

inline float Resonator::process(float x)
{
  // Apply biquad filter.
  float y = scale * x - xpp - app * ypp + ap * yp;
  // Update biquad memory.
  xpp = xp;
  xp = x;
  ypp = yp;
  yp = y;
  return y;
}

void Resonator::set(double _app, double _ap, double _scale)
{
  app = _app;
  ap = _ap;
  scale = _scale;
}

Filter::Filter(){};

void Filter::process(float **outputs, const float **inputs, uint32_t frames)
{
  for (uint chn = 0; chn < 2; ++chn)
  {
    for (auto &r : res[chn])
    {
      for (uint32_t j = 0; j < frames; ++j)
      {
        outputs[chn][j] += r.process(inputs[chn][j]);
      }
    }
  }
}

// Compute filter coefficients from
// frequency as a proportion of the sample rate
// REMEMBER: THAT'S DOUBLE THE Nyquist RATE, dummy
// and resonance in [0, 1]
void Filter::set(double x, double y, double p)
{
  double r = p * 0.049 + 0.95; // Filter unstable at r = 1.
  double app = r * r;
  double xap = (1.0 + r * r) * cos(M_PI * x);
  double yap = (1.0 + r * r) * cos(M_PI * y);
  double scale = sqrt(0.5 - 0.5 * r * r);
  for (auto &channelFilter : res)
  {
    channelFilter[0].set(app, xap, scale);
    channelFilter[1].set(app, yap, scale);
  }
}