// ----------------------------------------------------------------------------
//
//  Copyright (C) 2013-2019 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 "lr4filter.h"


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


Lr4filter::Lr4filter (void) :
    _c1 (0),
    _c2 (0),
    _c3 (0),
    _c4 (0),
    _g (0)
{
    reset ();
}


Lr4filter::~Lr4filter (void)
{
}


void Lr4filter::reset (void)
{
    _z1 = _z2 = _z3 = _z4 = 0;
}


void Lr4filter::setpars (float f, float s)
{
    float a, b, w, g1, g2;

    // Limit f to safe range.
    if (f < 1e-6f) f = 1e-6f;
    if (f > 0.48f) f = 0.48f;
    // Limit s to safe range.
    if (s < -6) s = -6;
    if (s > -3) s = -3;
    // Almost perfect mapping for s in [-3...-6] dB 
    s = powf ((s + 6) / 3, 0.465f);
    w = tanf (PIF * f);
    b = w * w;
    // First section.
    a = w * 2 * cosf ((2 + s) * PIF / 8);
    g1 = 1 + a + b;
    _c1 = 2 * a + 4 * b;
    _c2 = 4 * b / _c1;
    _c1 /= g1;
    // Second section.
    a = w * 2 * cosf ((2 - s) * PIF / 8);
    g2 = 1 + a + b;
    _c3 = 2 * a + 4 * b;
    _c4 = 4 * b / _c3;
    _c3 /= g2;
    // Highpass gain factor.
    _g = 1.0f / (g1 * g2);
}


void Lr4filter::process_lopass (int nsamp, const float *inp, float *out)
{
    float x, z1, z2, z3, z4, c12, c34;

    // Get filter state.
    z1 = _z1;
    z2 = _z2;
    z3 = _z3;
    z4 = _z4;
    // Local constants.
    c12 = _c1 * _c2 / 4;
    c34 = _c3 * _c4 / 4;
    // Run filter for n samples.
    while (nsamp--)
    {
        x = *inp++;
	x -= z1 + z2 + EPS;
        z2 += _c2 * z1;
        z1 += _c1 * x;
	x = c12 * x + z2;
	x -= z3 + z4 + EPS;
        z4 += _c4 * z3;
        z3 += _c3 * x;
	*out++ = c34 * x + z4;
    }
    // Save filter state.
    _z1 = z1;
    _z2 = z2;
    _z3 = z3;
    _z4 = z4;
}


void Lr4filter::process_hipass (int nsamp, const float *inp, float *out)
{
    float x, z1, z2, z3, z4;

    // Get filter state.
    z1 = _z1;
    z2 = _z2;
    z3 = _z3;
    z4 = _z4;
    // Run filter for n samples.
    while (nsamp--)
    {
        x = *inp++ * _g;
	x -= z1 + z2 + EPS;
        z2 += _c2 * z1;
        z1 += _c1 * x;
	x -= z3 + z4 + EPS;
        z4 += _c4 * z3;
        z3 += _c3 * x;
	*out++ = x;
    }
    // Save filter state.
    _z1 = z1;
    _z2 = z2;
    _z3 = z3;
    _z4 = z4;
}
