// ----------------------------------------------------------------------------
//
//  Copyright (C) 2006-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 2 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, write to the Free Software
//  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
//
// ----------------------------------------------------------------------------


#include <string.h>
#include <math.h>
#include "decoder.h"



const float Decoder::_g_norm [16] = { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 };


const float Decoder::_g_sn3d [16] =
{
    1.0f,
    1.732051f,
    1.732051f,
    1.732051f,
    2.236068f, 
    2.236068f, 
    2.236068f, 
    2.236068f, 
    2.236068f, 
    2.645751f,
    2.645751f,
    2.645751f,
    2.645751f,
    2.645751f,
    2.645751f,
    2.645751f
};


const float Decoder::_g_fuma [16] =
{
    1.414214f,
    1.732051f,
    1.732051f,
    1.732051f,
    1.936492f,
    1.936492f,
    2.236068f,
    1.936492f,
    1.936492f,
    2.091650f,
    1.972026f,
    2.231093f,
    2.645751f,
    2.231093f,
    1.972026f,
    2.091650f,
};


Decoder::Decoder (void) :
    _test (0)
{
}


Decoder::~Decoder (void)
{
    delete[] _test;
}


void Decoder::init (int frate)
{
    int    i, k, f;
    float  w;

    // Create delay lines.
    _frate = frate;
    _dsize = FRAG_SIZE * ((int)(ceilf (frate / (34.0f * FRAG_SIZE))) + 1); 
    for (i = 0; i < MAXOP; i++) _output [i]._dline = new float [_dsize]; 
    reset ();

    // Create test signal.
    k = frate / 7;
    k -= k % FRAG_SIZE;
    _tsize = k;
    _test = new float[k];
    f = (800 * k) / frate;
    w = 2 * M_PI / k;
    for (i = 0; i < k; i++) _test [i] = 0.1f * sinf (i * f * w + sinf (i * w));
}


void Decoder::reset (void)
{
    int      i;
    Output  *Z;

    // Reset everything.
    memset (_sig0, 0, FRAG_SIZE * sizeof (float));
    memset (_sig1, 0, FRAG_SIZE * sizeof (float));
    memset (_sig2, 0, FRAG_SIZE * sizeof (float));
    memset (_sig3, 0, FRAG_SIZE * sizeof (float));
    memset (_sig4, 0, FRAG_SIZE * sizeof (float));
    for (i = 0, Z = _output; i < MAXOP; i++, Z++)
    {
	memset (Z->_dline, 0, _dsize * sizeof (float)); 
	Z->_delay = 0;
	Z->_gcorr = 1.0f;
	Z->_gain0 = 0.0f;
	Z->_nffilt1.reset ();
	Z->_nffilt2.reset ();
	Z->_nffilt3.reset ();
	Z->_nffilt4.reset ();
    }
    for (i = 0; i < 3; i++) _nffilt1 [i].reset ();
    for (i = 0; i < 5; i++) _nffilt2 [i].reset ();
    for (i = 0; i < 7; i++) _nffilt3 [i].reset ();
    for (i = 0; i < 9; i++) _nffilt4 [i].reset ();
    for (i = 0; i < MAXIP; i++)
    {
        _lr2filt [i].reset ();
	_scale [i] = 0;
    }
    _nsig0 = 0;
    _nsig1 = 0;
    _nsig2 = 0;
    _nsig3 = 0;
    _nsig4 = 0;
    _noutp = 0;
    _nband = 1;
    _comp  = 0;
    _inp0  = TEST_OFF;
    _inp1  = TEST_OFF;
    _solo  = 0;
    _mute  = 0;
    _gain  = 0;
    _idel  = 0;
    _itst  = 0;
}


void Decoder::set_config (AD_conf *C)
{
    int           i, j, k, m;
    float         dm, da, fp;
    const float   *B;
    Speaker       *S;

    reset ();
    _noutp = C->_dec.nspkr;
    _nband = C->_dec.nband;

    switch (C->_opt.scale)
    {
    case AD_conf::SN3D: B = _g_sn3d; break;
    case AD_conf::FUMA: B = _g_fuma; break;
    default: B = _g_norm;
    }

    // Determine number of channels per degree and scale factors.
    for (i = j = 0, m = C->_dec.cmask; m; i++, m >>= 1)
    {
        if (m & 1)
	{
            _scale [j++] = B [i];
	    if      (i < 1)  _nsig0++;
	    else if (i < 4)  _nsig1++;
	    else if (i < 9)  _nsig2++;
	    else if (i < 16) _nsig3++;
	    else             _nsig4++;
	}    
    }

    // Set crossover filter frequency.
    if (_nband > 1)
    {
        _lr2conf.init (C->_opt.xfreq / _frate);
    }

    // Find maximum and average speaker distance.
    dm = da = 0;
    for (i = 0, S = C->_speakers; i < _noutp; i++, S++)
    {
	da += S->_r;
	if (dm < S->_r) dm = S->_r;
    } 
    da /= _noutp;

    // Ininitialise delay lines.
    if (C->_opt.delay) 
    {
	_comp |= COMP_DELAY;
	for (i = 0, S = C->_speakers; i < _noutp; i++, S++)
	{
	    k = int ((dm - S->_r) * _frate / 340.0f + 0.5f);
	    if (k > _dsize - FRAG_SIZE) k = _dsize - FRAG_SIZE;
	    _output [i]._delay = k;
	}
    }

    // Correct gains for speaker distance.
    if (C->_opt.level) 
    {
	_comp |= COMP_LEVEL;
	for (i = 0, S = C->_speakers; i < _noutp; i++, S++)
	{
	    _output [i]._gcorr = S->_r / dm;
	}
    }

    // Initialise near field compensation.
    // If on inputs, use average distance.
    if (C->_opt.nfeff == AD_conf::INPUT) 
    {
	_comp |= COMP_NFE_IP;
	fp = 340.0f / (da * _frate);
	for (i = 0; i < _nsig1; i++) _nffilt1 [i].init (fp);
	for (i = 0; i < _nsig2; i++) _nffilt2 [i].init (fp);
	for (i = 0; i < _nsig3; i++) _nffilt3 [i].init (fp);
    }
    // If on outputs, use correct distance.
    else if (C->_opt.nfeff == AD_conf::OUTPUT) 
    {
	_comp |= COMP_NFE_OP;
	for (i = 0, S = C->_speakers; i < _noutp; i++, S++)
	{
	    fp = 340.0f / (S->_r * _frate);
	    _output [i]._nffilt1.init (fp);
	    _output [i]._nffilt2.init (fp);
	    _output [i]._nffilt3.init (fp);
	}
    }
}


void Decoder::set_matrix (AD_conf *C)
{
    int      i, j, k, m;
    float    g, b;
    Output   *X;
    Matrow   *R;

    if (_nband == 2) b = powf (10.0f, C->_opt.ratio / 40.0f);
    else b = 1;
    g = 0;
    for (k = 0, X = _output, R = C->_lfmatrix; k < _noutp; k++, X++, R++)
    {
	g = C->_lfgain [0] / b;
        for (i = j = 0, m = C->_dec.cmask; m; i++, m >>= 1)
        {
	    if      (i == 1) g = C->_lfgain [1] / b;
	    else if (i == 4) g = C->_lfgain [2] / b;
	    else if (i == 9) g = C->_lfgain [3] / b;
            if (m & 1) X->_matlf [j++] = R->_coeff [i] * g;
	}
    }
    if (_nband == 1) return;
    for (k = 0, X = _output, R = C->_hfmatrix; k < _noutp; k++, X++, R++)
    {
	g = C->_hfgain [0] * b;
        for (i = j = 0, m = C->_dec.cmask; m; i++, m >>= 1)
        {
	    if      (i == 1) g = C->_hfgain [1] * b;
	    else if (i == 4) g = C->_hfgain [2] * b;
	    else if (i == 9) g = C->_hfgain [3] * b;
            if (m & 1) X->_mathf [j++] = R->_coeff [i] * g;
	}
    }
}


void Decoder::process (int nf, float *ip[], float *op[], float *ts, float *lm)
{
    int       i, j, k, n, n1, n2;
    uint64_t  m;
    float     g0, g1, d, s, t;
    float     *p1, *p2, *p3;
    Output    *Z;

    while (nf)
    {
	n = (nf > FRAG_SIZE) ? FRAG_SIZE : nf;
	k = 0;
	// Scale and bandsplit zero degree.
	if (_nsig0)
	{
	    p1 = ip [k];
            p2 = _ip_lf [k];
	    p3 = _ip_hf [k];
	    for (i = 0; i < n; i++) p2 [i] = _scale [k] * p1 [i];
	    if (_nband > 1) _lr2filt [k].process (&_lr2conf, n, p2, p2, p3);
	    ip [k++] += n;
	}
	// Scale, filter and bandsplit first degree.
	for (j = 0; j < _nsig1; j++)
	{
	    p1 = ip [k];
            p2 = &(_ip_lf [k][0]);
            p3 = &(_ip_hf [k][0]);
	    if (_comp & COMP_NFE_IP) _nffilt1 [j].process (n, p1, p2, _scale [k]);
            else for (i = 0; i < n; i++) p2 [i] = _scale [k] * p1 [i];
	    if (_nband > 1) _lr2filt [k].process (&_lr2conf, n, p2, p2, p3);
 	    ip [k++] += n;
	}	    
	// Scale, filter and bandsplit second degree.
	for (j = 0; j < _nsig2; j++)
	{
	    p1 = ip [k];
            p2 = &(_ip_lf [k][0]);
            p3 = &(_ip_hf [k][0]);
	    if (_comp & COMP_NFE_IP) _nffilt2 [j].process (n, p1, p2, _scale [k]);
            else for (i = 0; i < n; i++) p2 [i] = _scale [k] * p1 [i];
	    if (_nband > 1) _lr2filt [k].process (&_lr2conf, n, p2, p2, p3);
 	    ip [k++] += n;
	}	    
	// Scale, filter and bandsplit third degree.
	for (j = 0; j < _nsig3; j++)
	{
	    p1 = ip [k];
            p2 = &(_ip_lf [k][0]);
            p3 = &(_ip_hf [k][0]);
	    if (_comp & COMP_NFE_IP) _nffilt3 [j].process (n, p1, p2, _scale [k]);
            else for (i = 0; i < n; i++) p2 [i] = _scale [k] * p1 [i];
	    if (_nband > 1) _lr2filt [k].process (&_lr2conf, n, p2, p2, p3);
 	    ip [k++] += n;
	}	    
	// Scale, filter and bandsplit degree 4 inputs.
	for (j = 0; j < _nsig4; j++)
	{
	    p1 = ip [k];
            p2 = &(_ip_lf [k][0]);
            p3 = &(_ip_hf [k][0]);
	    if (_comp & COMP_NFE_IP) _nffilt4 [j].process (n, p1, p2, _scale [k]);
            else for (i = 0; i < n; i++) p2 [i] = _scale [k] * p1 [i];
	    if (_nband > 1) _lr2filt [k].process (&_lr2conf, n, p2, p2, p3);
 	    ip [k++] += n;
	}	    

	// Get current signal selection.
        switch (_inp0)
        {
	case TEST_INT:
	    p1 = _test + _itst;
	    break;
	case TEST_EXT:
	    p1 = ts;
	    break;
	default:
	    p1 = _sig0;
        }

	// Loop over all outputs.
	for (j = 0, Z = _output; j < _noutp; j++, Z++)
	{
	    if (op [j] == 0) continue;
	    k = 0;
	    if (_nsig0)
	    { 
  	        if (_nband > 1) matr2band (n, k, 1, _sig0, Z);
	        else            matr1band (n, k, 1, _sig0, Z);
		k++;
	    }
	    else memset (_sig0, 0, n * sizeof (float));
	    if (_nsig1)
	    { 
  	        if (_nband > 1) matr2band (n, k, k + _nsig1, _sig1, Z);
	        else            matr1band (n, k, k + _nsig1, _sig1, Z);
	        if (_comp & COMP_NFE_OP) Z->_nffilt1.process_add (n, _sig1, _sig0, 1);
		else for (i = 0; i < n; i++) _sig0 [i] += _sig1 [i];
	        k += _nsig1;
	    }
	    if (_nsig2)
	    { 
  	        if (_nband > 1) matr2band (n, k, k + _nsig2, _sig2, Z);
	        else            matr1band (n, k, k + _nsig2, _sig2, Z);
	        if (_comp & COMP_NFE_OP) Z->_nffilt2.process_add (n, _sig2, _sig0, 1);
		else for (i = 0; i < n; i++) _sig0 [i] += _sig2 [i];
	        k += _nsig2;
	    }
	    if (_nsig3)
	    { 
  	        if (_nband > 1) matr2band (n, k, k + _nsig3, _sig3, Z);
	        else            matr1band (n, k, k + _nsig3, _sig3, Z);
	        if (_comp & COMP_NFE_OP) Z->_nffilt3.process_add (n, _sig3, _sig0, 1);
		else for (i = 0; i < n; i++) _sig0 [i] += _sig3 [i];
	        k += _nsig3;
	    }
	    if (_nsig4)
	    { 
  	        if (_nband > 1) matr2band (n, k, k + _nsig4, _sig4, Z);
	        else            matr1band (n, k, k + _nsig4, _sig4, Z);
	        if (_comp & COMP_NFE_OP) Z->_nffilt4.process_add (n, _sig4, _sig0, 1);
		else for (i = 0; i < n; i++) _sig0 [i] += _sig4 [i];
	        k += _nsig4;
	    }

	    // Mute, solo and volume logic.
	    g0 = Z->_gain0;
	    g1 = 0;
            m = 1LL << j;
	    if ((_inp1 == _inp0) && (m & ~_mute) && ((m & _solo) || (!_inp0 && !_solo)))
	    {		
		g1 = _gain;
		if (_comp & COMP_LEVEL) g1 *= Z->_gcorr;
	    }
	    Z->_gain0 = g1;
            d = (g1 - g0) / n;
            if (fabsf (d) < 1e-9f) d = 0;

	    // Copy to delay line with gain fade.
	    p2 = Z->_dline + _idel;
	    s = 0;
   	    for (i = 0; i < n; i++)
	    {
		g0 += d;
  	        p2 [i] = t = g0 * p1 [i];
		t = fabsf (t);
		if (t > s) s = t;
	    }

	    // Update level measurement.
	    lm [j] = 0.98f * lm [j] + 1e-10f;
	    if (s > lm [j]) lm [j] = s;

	    // Find read indices and block sizes for delay.
	    if (_comp & COMP_DELAY)
	    {
		i = _idel - Z->_delay;
	        if (i < 0) i += _dsize;
		p2 = Z->_dline + i;
		n1 = _dsize - i;
		if (n1 > n) n1 = n;
   	        n2 = n - n1;
	    }
	    else
            {
		n1 = n;
		n2 = 0;
	    }

            // Copy delayed signal to output buffer.
	    p3 = op [j];
	    memcpy (p3, p2, n1 * sizeof (float));
	    p2 = Z->_dline;
	    p3 += n1;
	    memcpy (p3, p2, n2 * sizeof (float));
	    p3 += n2;
	    op [j] = p3;
	}

	// Prepare for next fragment.
	nf -= n;
	ts += n;
	_idel += n;
	if (_idel >= _dsize) _idel = 0;
	_itst += n;
	if (_itst >= _tsize) _itst = 0;
        _inp0 = _inp1;
    }
}


void Decoder::matr1band (int nf, int ind0, int ind1, float *out, Output *Z)
{
    int   i;
    float a, *p;

    a = Z->_matlf [ind0];
    p = &_ip_lf [ind0][0];
    for (i = 0; i < nf; i++) out [i] = a * p [i];
    while (++ind0 < ind1)
    {
        a = Z->_matlf [ind0];
        p = &_ip_lf [ind0][0];
        for (i = 0; i < nf; i++) out [i] += a * p [i];
    }
}


void Decoder::matr2band (int nf, int ind0, int ind1, float *out, Output *Z)
{
    int   i;
    float a, b, *p, *q;

    a = Z->_matlf [ind0];
    b = Z->_mathf [ind0];
    p = &_ip_lf [ind0][0];
    q = &_ip_hf [ind0][0];
    for (i = 0; i < nf; i++) out [i] = a * p [i] + b * q [i];
    while (++ind0 < ind1)
    {
        a = Z->_matlf [ind0];
        b = Z->_mathf [ind0];
        p = &_ip_lf [ind0][0];
        q = &_ip_hf [ind0][0];
        for (i = 0; i < nf; i++) out [i] += a * p [i] + b * q [i];
    }
}


