//  ------------------------------------------------------------------------
//
//  Copyright (C) 2009-2012 Fons Adriaensen <fons@linuxaudio.org>
//
//  This program is free software; you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation; either version 3 of the License, or
//  (at your option) any later version.
//
//  This program is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details.
//
//  You should have received a copy of the GNU General Public License
//  along with this program.  If not, see <http://www.gnu.org/licenses/>.
//
//  ------------------------------------------------------------------------


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


#define PIF 3.141592f
#define P2F 6.283185f
#define EPS 1e-20f;


// State variable version of 4th order crossover filters.
// The 'f' parameter is the crossover frequency divided by
// the sample rate. The 's' parameter defines the filter
// shape, s = 0 for Linkwitz-Riley, s = 1 for Butterworth.
// Outputs are exactly in phase, and sum to unity magnitude
// for s = 0.


void LR4_conf::calcpar (float f, float s)
{
    float a, b, d1, d2, q, p;
    
    // This provides an almost linear mapping from s to gain in dB. 
    s = powf ((s + 6) / 3, 0.465f);
    // Damping for first and second sections, from pole locations.
    d1 = 2 * cosf ((2 + s) * PIF / 8);
    d2 = 2 * cosf ((2 - s) * PIF / 8);
    // Basic coefficients.
    a = tanf (PIF * f);
    b = a * a;
    // First section.
    q = a / d1;
    p = q * b;
    s = p + q + b;
    _c1 = (4 * p + 2 * b) / s;
    _c2 = (4 * p) / s;
    _gl = p / s;
    _gh = q / s;
    // Second section.
    q = a / d2;
    p = q * b;
    s = p + q + b;
    _c3 = (4 * p + 2 * b) / s;
    _c4 = (4 * p) / s;
    _gl *= p / s;
    _gh *= q / s;
}


void LR4_filt::process (const LR4_conf *C, int n, float glo, float ghi, float *inp, float *oplo, float *ophi)
{
    float x, y, z1, z2, z3, z4, z5, z6;

    // Get filter state.
    z1 = _z1;
    z2 = _z2;
    z3 = _z3;
    z4 = _z4;
    z5 = _z5;
    z6 = _z6;
    // Get gain factors.
    glo *= C->_gl;
    ghi *= C->_gh;
    // Run filter for n samples.
    while (n--)
    {
        x = *inp++;
        x -= C->_c1 * z1 + C->_c2 * z2 + EPS;
	y = x + 4 * (z1 + z2);
        z2 += z1;
        z1 += x;
        x -= C->_c3 * z3 + C->_c4 * z4 - EPS;
	*ophi++ = ghi * x;
        z4 += z3;
        z3 += x;
        y -= C->_c3 * z5 + C->_c4 * z6 - EPS;
	x = y + 4 * (z5 + z6);
	*oplo++ = glo * x;
        z6 += z5;
        z5 += y;
    }
    // Save filter state.
    _z1 = z1;
    _z2 = z2;
    _z3 = z3;
    _z4 = z4;
    _z5 = z5;
    _z6 = z6;
}
