/* simple digital lowpass filter for puredata
 * 2005-08-01	Thomas Strathmann <thomas@pdp7.org>
 */
#include <math.h>
#include "m_pd.h"

#define TWOPI             (M_PI * 2.0)


static t_class *biquadlpf_tilde_class;

typedef struct _biquadlpf_tilde {
  t_object  x_obj;
  t_sample f_cutoff;
  t_sample f_resonance;
  t_sample f;
} t_biquadlpf_tilde;

t_int *biquadlpf_tilde_perform(t_int *w) {
  // ringbuffers
  static t_sample x_n[3] = {0, 0, 0};
  static t_sample y_n[3] = {0, 0, 0};

  // parameters
  t_biquadlpf_tilde *x = (t_biquadlpf_tilde *)(w[1]);
  t_sample  *in  =    (t_sample *)(w[2]);
  t_sample  *out =    (t_sample *)(w[3]);
  int          n =           (int)(w[4]);
  t_float cutoff = x->f_cutoff;
  t_float resonance = 0.1 * x->f_resonance;

  static t_float cutoff_old = 1;
  static t_float resonance_old = 1;  
  static t_float c, c2, a1, a2, b1, b2;

  // filter coeffs
  if(cutoff != cutoff_old || resonance != resonance_old) {
    c = 1.0f / tanf(M_PI/44100.0 * cutoff);
    c2 = c*c;
    a1 = 1.0f / (1.0f + resonance * c + c2);
    a2 = 2.0f * a1;
    b1 = 2.0f * (1.0f - c2) * a1;
    b2 = (1.0f - resonance * c + c2) * a1;
    cutoff_old = cutoff;
    resonance_old = resonance;
  }

  // process block
  while (n--) {
    x_n[0] = *(in++);
    y_n[0] = a1*x_n[0] + a2*x_n[1] + a1*x_n[2] - b1*y_n[1] - b2*y_n[2];
    *(out++) = y_n[0];

    // shift ringbuffers
    x_n[2] = x_n[1];
    x_n[1] = x_n[0];
    y_n[2] = y_n[1];
    y_n[1] = y_n[0];
  }

  return (w+5);
}

void biquadlpf_tilde_dsp(t_biquadlpf_tilde *x, t_signal **sp) {
  dsp_add(biquadlpf_tilde_perform, 4, x,
          sp[0]->s_vec, sp[1]->s_vec, sp[0]->s_n);
}

void *biquadlpf_tilde_new(t_floatarg cutoff, t_floatarg resonance) {
  t_biquadlpf_tilde *x = (t_biquadlpf_tilde *)pd_new(biquadlpf_tilde_class);

  x->f_cutoff = cutoff;
  x->f_resonance = resonance;

  floatinlet_new (&x->x_obj, &x->f_cutoff);
  floatinlet_new (&x->x_obj, &x->f_resonance);
  outlet_new(&x->x_obj, &s_signal);

  return (void *)x;
}

void biquadlpf_tilde_setup(void) {
  biquadlpf_tilde_class = class_new(gensym("biquadlpf~"),
        (t_newmethod)biquadlpf_tilde_new,
        0, sizeof(t_biquadlpf_tilde),
        CLASS_DEFAULT, 
        A_DEFFLOAT, 0);

  class_addmethod(biquadlpf_tilde_class,
        (t_method)biquadlpf_tilde_dsp, gensym("dsp"), 0);
  CLASS_MAINSIGNALIN(biquadlpf_tilde_class, t_biquadlpf_tilde, f);
}