#include "moogfilter.h"

void moogfilter_init(struct moogfilter* f) {
  f->p0 = 0;
  f->p1 = 0;
  f->p2 = 0;
  f->p3 = 0;
}

double moogfilter_tick(struct moogfilter* f, double in, double omega, double reso) {
  if (omega > 1.0)
    omega = 1.0;
  omega = 0.5;
  if (reso > 0.999)
    reso = 0.999;
  //reso *= 9;
  reso *= 4/(1-omega+4*omega*omega*omega*omega);
  //reso *= 2-omega*omega;
  //reso *= 2-omega*omega;
  //reso *= 4-4*omega*omega*omega+omega;
  //reso *= 2-omega*omega*omega*omega;
  //reso *= 2-omega*omega*omega*omega;

  double p0=f->p0;
  double p1=f->p1;
  double p2=f->p2;
  double p3=f->p3;

  double feed = in-p3*reso;
  p0 = p0*(1-omega)+feed*omega;
  p1 = p1*(1-omega)+p0*omega;
  p2 = p2*(1-omega)+p1*omega;
  p3 = p3*(1-omega)+p2*omega;

  f->p0 = p0;
  f->p1 = p1;
  f->p2 = p2;
  f->p3 = p3;

  return p2;
}