Moog resonant IIR voltage-controlled highpass filter.

#include <stdint.h>
#include <math.h>

typedef struct _filter_moog {
    float res_dB;
    float f, q, p; /* filter coefficients */
    float b0, b1, b2, b3, b4; /* filter state. TODO: make more sane. */
} MoogFilterContext;

static inline int av_clip(int val,int min, int max)
{
    if (val > max) val = max;
    if (val < min) val = min;
    return val;
}

void moog_filter_reset(MoogFilterContext *s)
{
    s->b0 = s->b1 = s->b2 = s->b3 = s->b4 = 0;
}

void moog_filter_init(MoogFilterContext *s, unsigned int freq, unsigned int samplerate, float res_dB)
{
    float res, fr, p, q;

    moog_filter_reset(s);

    freq = av_clip(freq, 75, samplerate>>1);
    cutoff = 2.0f * (float)freq / (float)samplerate;
    q = 1.0f - cutoff;
    p = cutoff + 0.8f * cutoff * q;
    s->f = p + p - 1.0f;
    s->p = p;

    res = expf((float)M_LN10 * (res_dB - 96) * 0.05f);
    s->q = res * (1.0f + 0.5f * q * (1.0f - q + 5.6f * q * q));
}

static inline float do_filter_moog(MoogFilterContext *s, float in)
{
    float t1, t2, t3, tb0 = s->b0, tb1 = s->b1, tb2 = s->b2, tb3 = s->b3, tb4 = s->b4;
    float f = s->f, p = s->p;
    t3 = in - (s->q * tb4);
    t1 = tb1;  tb1 = (tb0 + t3) * p - tb1 * f;
    t2 = tb2;  tb2 = (tb1 + t1) * p - tb2 * f;
    t1 = tb3;  tb3 = (tb2 + t2) * p - tb3 * f;
    tb4 = (tb3 + t1) * p - tb4 * f;
    tb0 = t3;
    /* *lpf_out = tb4; */
    s->b0 = tb0, s->b1 = tb1, s->b2 = tb2, s->b3 = tb3, s->b4 = tb4;
    return (t3 - tb4); /* Highpass output */
}

/* Mono for now, can just do stereo with a pair of MoogFilterContexts.
 * First can be initialized with moog_init() and the second with memcpy()
 */
void moog_filter_hpf_process(MoogFilterContext *s, float *dst, const float *src, size_t nb_samples)
{
    size_t i;
    for(i = 0; i < nb_samples; i++) {
        dst[i] = do_filter_moog(s, src[i]);
    }
}