//  ------------------------------------------------------------------------
//
//  Copyright (C) 2009-2017 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 "xover2.h"


#define P2F 6.283185f
#define EPS 1e-20f


void LR2conf::init (float f)
{
    float w, c, s;

    w = P2F * f;
    c = cosf (w);
    s = sinf (w);
    _c0 = (c < 1e-3f) ? (-0.5f * c) : ((s - 1) / c);
    _c1 = 0.5f * (1 + _c0);
}


void LR2filt::process (const LR2conf *C, int k,
		       const float *in, float *lp, float *hp)
{
    float d, t, x0, x1, z0, z1, z2;

    // Get filter state.
    z0 = _z0;
    z1 = _z1;
    z2 = _z2;
    // Run filter for n samples.
    while (k--)
    {
	x1 = x0 = *in++ + EPS;
        d = C->_c1 * (x1 - z1);
        x1 = z1 + d;
        z1 = x1 + d;      
        d = C->_c1 * (x1 - z2);
        x1 = z2 + d;
        z2 = x1 + d;      
        *lp++ = x1;
        t = x0 - C->_c0 * z0;
        x0 = z0 + C->_c0 * t;        
        z0 = t;
	*hp++ = x0 - x1;
    }
    // Save filter state.
    _z0 = z0;
    _z1 = z1;
    _z2 = z2;
}


void AP1conf::init (float f)
{
    float w, c, s;

    w = P2F * f;
    c = cosf (w);
    s = sinf (w);
    _c0 = (c < 1e-3f) ? (-0.5f * c) : ((s - 1) / c);
}


void AP1filt::process (const AP1conf *C, int k, float *sig)
{
    float t, z0;

    z0 = _z0;
    while (k--)
    {
        t = *sig - C->_c0 * z0 + EPS;
        *sig++ = z0 + C->_c0 * t;        
        z0 = t;
    }
    _z0 = z0;
}

