//  ------------------------------------------------------------------------
//
//  Copyright (C) 2023 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 "bw8filter.h"


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


void Bw8filter::init1 (int i, float w, float d)
{
    float a, b, g, c0, c1;

    a = w * d;
    b = w * w;
    g = 1 + a + b;
    c0 = 2 * a + 4 * b;
    c1 = 4 * b / c0;
    c0 /= g;

    _g /= g;
    _cz [2 * i + 0] = c0;
    _cz [2 * i + 1] = c1;
    _a0 [i] = c0 * c1 / 4;
}


void Bw8filter::init (float f)
{
    float w;

    w = tanf (PIF * f);
    _g = 1.0f;
    init1 (0, w, 0.390181f);
    init1 (1, w, 1.111140f);
    init1 (2, w, 1.662939f);
    init1 (3, w, 1.961571f);
}


void Bw8filter::proclowpass (int n, const float *inp, float *out, float gain)
{
    float x;
       
    while (n--)
    {
        x = *inp++ * gain;

	x -= _z [0] + _z [1] + EPS;
        _z [1] += _cz [1] * _z [0];
        _z [0] += _cz [0] * x;
	x = _a0 [0] * x + _z [1];

	x -= _z [2] + _z [3] + EPS;
        _z [3] += _cz [3] * _z [2];
        _z [2] += _cz [2] * x;
	x = _a0 [1] * x + _z [3];

	x -= _z [4] + _z [5] + EPS;
        _z [5] += _cz [5] * _z [4];
        _z [4] += _cz [4] * x;
	x = _a0 [2] * x + _z [5];

	x -= _z [6] + _z [7] + EPS;
        _z [7] += _cz [7] * _z [6];
        _z [6] += _cz [6] * x;
	x = _a0 [3] * x + _z [7];

	*out++ = x;
    }
}


void Bw8filter::prochighpass (int n, const float *inp, float *out, float gain)
{
    float x;

    gain *= _g;
    while (n--)
    {
        x = *inp++ * gain;

	x -= _z [0] + _z [1] + EPS;
        _z [1] += _cz [1] * _z [0];
        _z [0] += _cz [0] * x;

	x -= _z [2] + _z [3] + EPS;
        _z [3] += _cz [3] * _z [2];
        _z [2] += _cz [2] * x;

	x -= _z [4] + _z [5] + EPS;
        _z [5] += _cz [5] * _z [4];
        _z [4] += _cz [4] * x;

	x -= _z [6] + _z [7] + EPS;
        _z [7] += _cz [7] * _z [6];
        _z [6] += _cz [6] * x;

	*out++ = x;
    }
}
