#ifndef SVF_H
#define SVF_H

#include <math.h>
#include "defines.h"

/*
 * From <http://musicdsp.org/showone.php?id=92>
 * 
 * Posted by Andrew Simpler
 *  Thanks to Laurent de Soras for the stability limit
 *  and Steffan Diedrichsen for the correct notch output.
 */

enum {
    svf_l = 0,
    svf_h = 1,
    svf_b = 2,
    svf_p = 3,
    svf_n = 4
};


typedef struct {
    float rate;
    float fc;
    float res;
    float drive;
    float freq;
    float damp;
    float low;
    float high;
    float band;
    float peak;
    float notch;
} SVF;


SVF* svf () {
    SVF* f = (SVF*) malloc (sizeof (SVF));
    f->rate = 44100.0f;
    f->fc = 440.0f;
    f->res = 0.0f;
    f->drive = 0.0f;
    f->low = 0.0f;
    f->high = 0.0f;
    f->band = 0.0f;
    f->peak = 0.0f;
    f->notch = 0.0f;
    return f;
}


void svf_update (SVF* self, float fc, float res) {
    self->fc = fc;
    self->res = res;
    self->freq = 2.0f * sin (M_PI * fmin (0.25f, fc / (float)RATE));
    self->damp = fmin (2.0f * (1.0f - pow (res, 0.25f)), fmin (2.0f, (2.0f / self->freq) - (self->freq * 0.5f)));
}


float svf_process (SVF* f, float in, int mode, int clip) {
    float out = 0.0f;
    f->notch = in - (f->damp * f->band);
    f->low = f->low + (f->freq * f->band);
    f->high = f->notch - f->low;
    f->band = (f->freq * f->high) + f->band;
    switch (mode) {
        case svf_l: out += f->low; break;
        case svf_h: out += f->high; break;
        case svf_b: out += f->band; break;
        case svf_p: out += f->peak; break;
        case svf_n: out += f->notch; break;
        default: out += in; break;
    }
    f->notch = in - (f->damp * f->band);
    f->low = f->low + (f->freq * f->band);
    f->high = f->notch - f->low;
    f->band = (f->freq * f->high) + f->band;
    switch (mode) {
        case svf_l: out += f->low; break;
        case svf_h: out += f->high; break;
        case svf_b: out += f->band; break;
        case svf_p: out += f->peak; break;
        case svf_n: out += f->notch; break;
        default: out += in; break;
    }
    if (clip) {
        return atan (out / 2.0f);
    } else {
        return out / 2.0f;
    }
}

#endif
