//  ------------------------------------------------------------------------
//
//  Copyright (C) 2011-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 <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "lrxproc.h"


Lrxproc::Lrxproc (void) :
    _nchan (0),
    _nband (0),
    _buff1 (0),
    _buff2 (0)
{
    int i;

    for (i = 0; i < MAXCHAN * MAXFILT; i++) _xover [i] = 0;
    for (i = 0; i < MAXCHAN * MAXBAND; i++) _delay [i] = 0;
}


Lrxproc::~Lrxproc (void)
{
    clear ();
}


int Lrxproc::init (Lrxconf *C, float fsamp)
{
    int    i, j, k;
    float  v;
    Delayproc *D;

    clear ();
    _nchan = C->_nchan;
    _nband = C->_nband;
    _fsamp = fsamp;
    _buff1 = new float [PERIOD];
    _buff2 = new float [PERIOD];
    for (j = 0; j < _nband - 1; j++)
    {
	_xconf [j].calcpar (C->_xover [j]._freq / _fsamp, C->_xover [j]._shape);
    }
    for (j = 0; j < _nband; j++)
    {
	_band_gain [j] = powf (10.0f, 0.05f * C->_fband [j]._gain);
    }
    for (i = 0; i < _nchan; i++)
    {
	_chan_gain [i] = powf (10.0f, 0.05f * C->_chann [i]._gain);
	for (j = 0; j < _nband - 1; j++)
	{
            _xover [MAXFILT * i + j] = new LR4_filt;
	}
	for (j = 0; j < _nband; j++)
	{
	    v = C->_chann [i]._delay + C->_fband [j]._delay;
   	    k = int (1e-3f * v * _fsamp + 0.5f);
	    if (k) 
	    {
		D = new Delayproc (k, PERIOD);
		D->set_delay (k);
                _delay [MAXBAND * i + j] = D;
	    }
	}
    }
    reset ();
    return 0;
}


void Lrxproc::clear (void)
{
    _nchan = 0;
    _nband = 0;
    for (int i = 0; i < MAXCHAN * MAXFILT; i++)
    {
	delete _xover [i];
	_xover [i] = 0;
    }
    for (int i = 0; i < MAXCHAN * MAXBAND; i++)
    {
	delete _delay [i];
	_delay [i] = 0;
    }
    delete[] _buff1;
    delete[] _buff2;
    _buff1 = 0;
    _buff2 = 0;
}


void Lrxproc::reset (void)
{
    int i;

    for (i = 0; i < MAXCHAN * MAXFILT; i++)
    {
	if (_xover [i]) _xover [i]->reset ();
    }
    for (i = 0; i < MAXCHAN * MAXBAND; i++)
    {
	if (_delay [i]) _delay [i]->reset ();
    }
}


void Lrxproc::process (int nfr, float *inp[], float *out[])
{
    if      (_nband == 4)  process4 (nfr, inp, out);
    else if (_nband == 3)  process3 (nfr, inp, out);
    else                   process2 (nfr, inp, out);
}


void Lrxproc::process2 (int nfr, float *inp[], float *out[])
{
    int    i, n;
    float  g, **q;

    while (nfr)
    {
	n = (nfr < PERIOD) ? nfr : PERIOD;
        q = out;
	for (i = 0; i < _nchan; i++)
	{
            g = _chan_gain [i];
	    proctwin (i, 0, n, g * _band_gain [0], g * _band_gain [1], inp [i], q [0], q [1]);
            inp [i] += n;
            q [0] += n;			
            q [1] += n;			
	    q += 2;
	}
        nfr -= n;
    }
}


void Lrxproc::process3 (int nfr, float *inp[], float *out[])
{
    int    i, n;
    float  g, *p, **q;
    Delayproc  *D;

    while (nfr)
    {
	n = (nfr < PERIOD) ? nfr : PERIOD;
        q = out;
	for (i = 0; i < _nchan; i++)
	{
  	    D = _delay [MAXBAND * i];
	    p = D ? D->wr_ptr () : q [0];
	    g = _chan_gain [i];
	    _xover [MAXFILT * i]->process (_xconf + 0, n, g * _band_gain [0], g, inp [i], p, _buff1);
	    if (D)
	    {
		D->wr_commit (n);
		memcpy (q [0], D->rd_ptr (), n * sizeof (float));
		D->rd_commit (n);
	    }
	    proctwin (i, 1, n, _band_gain [1], _band_gain [2], _buff1, q [1], q [2]);
            inp [i] += n;
            q [0] += n;			
            q [1] += n;			
            q [2] += n;			
	    q += 3;
	}
        nfr -= n;
    }
}


void Lrxproc::process4 (int nfr, float *inp[], float *out[])
{
    int    i, n;
    float  g, **q;

    while (nfr)
    {
	n = (nfr < PERIOD) ? nfr : PERIOD;
        q = out;
	for (i = 0; i < _nchan; i++)
	{
	    g = _chan_gain [i];
	    _xover [MAXFILT * i + 1]->process (_xconf + 1, n, g, g, inp [i], _buff1, _buff2);
	    proctwin (i, 0, n, _band_gain [0], _band_gain [1], _buff1, q [0], q [1]);
	    proctwin (i, 2, n, _band_gain [2], _band_gain [3], _buff2, q [2], q [3]);
            inp [i] += n;
            q [0] += n;			
            q [1] += n;			
            q [2] += n;			
            q [3] += n;			
	    q += 4;
	}
        nfr -= n;
    }
}


void Lrxproc::proctwin (int i, int j, int n, float ga, float gb, float *inp, float *outa, float *outb)
{
    float  *a, *b;
    Delayproc *A, *B;

    A = _delay [MAXBAND * i + j];
    B = _delay [MAXBAND * i + j + 1];
    a = A ? A->wr_ptr () : outa;
    b = B ? B->wr_ptr () : outb;
    _xover [MAXFILT * i + j]->process (_xconf + j, n, ga, gb, inp, a, b);
    if (A)
    {
	A->wr_commit (n);
	memcpy (outa, A->rd_ptr (), n * sizeof (float));
	A->rd_commit (n);
    }
    if (B)
    {
	B->wr_commit (n);
	memcpy (outb, B->rd_ptr (), n * sizeof (float));
	B->rd_commit (n);
    }
}
