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


Jmatrix::Jmatrix (const char *client_name, const char *server_name,
		  int ninp, int nout, float maxdel) :
    _ginp (0),
    _gout (0),
    _gmatr (0),
    _gcurr (0),
    _dinp (0),
    _dout (0),
    _dmatr (0),
    _dcurr (0),
    _dproc (0)
{
    if (ninp > MAXINP) ninp = MAXINP;
    else if (ninp < 0) ninp = 0;
    if (nout > MAXOUT) nout = MAXOUT;
    else if (nout < 0) nout = 0;
    if (maxdel < 0.0f) maxdel = 0.0f;
    if (maxdel > 3.0f) maxdel = 3.0f;
    if (   open_jack (client_name, server_name, ninp, nout)
        || create_inp_ports ("in_%d")
        || create_out_ports ("out_%d"))
    {
        _state = FAILED;
        return;
    }
    init (maxdel);
}


Jmatrix::~Jmatrix (void)
{
    fini ();
}


void Jmatrix::init (float maxdel)
{
    int  i;

    _ginp  = new float [_ninp];
    _gout  = new float [_nout];
    _gmatr = new float [_ninp * _nout];
    _gcurr = new float [_ninp * _nout];
    for (i = 0; i < _ninp; i++)	_ginp [i] = 1.0f;
    for (i = 0; i < _nout; i++)	_gout [i] = 1.0f;
    memset (_gmatr, 0, _ninp * _nout * sizeof (float));
    memset (_gcurr, 0, _ninp * _nout * sizeof (float));

    _maxdel = (int)(maxdel * jack_rate () + 0.5f);
    if (_maxdel)
    {
        _dinp  = new int32_t [_ninp];
        _dout  = new int32_t [_nout];
        _dmatr = new int32_t [_ninp * _nout];
        _dcurr = new int32_t [_ninp * _nout];
	_dproc = new Delay [_ninp];
        memset (_dinp,  0, _ninp * sizeof (int32_t));
        memset (_dout,  0, _nout * sizeof (int32_t));
        memset (_dmatr, 0, _ninp * _nout * sizeof (int32_t));
        memset (_dcurr, 0, _ninp * _nout * sizeof (int32_t));
	for (i = 0; i < _ninp; i++) _dproc [i].init (_maxdel, jack_size ());
    }

    _state = PROCESS;
}


void Jmatrix::fini (void)
{
    _state = INITIAL;
    close_jack ();
    delete[] _ginp;
    delete[] _gout;
    delete[] _gmatr;
    delete[] _gcurr;
    delete[] _dinp;
    delete[] _dout;
    delete[] _dmatr;
    delete[] _dcurr;
    delete[] _dproc;
}


void Jmatrix::set_gain (int inp, int out, float gain)
{
    if (inp >= _ninp) return;
    if (out >= _nout) return;
    if (inp < 0)
    {
	if (out >= 0) _gout [out] = gain;
	return;
    }
    if (out < 0)
    {
	if (inp >= 0) _ginp [inp] = gain;
	return;
    }
    _gmatr [_ninp * out + inp] = gain;
}


void Jmatrix::set_delay (int inp, int out, float delay)
{
    int32_t d;
    
    if (inp >= _ninp) return;
    if (out >= _nout) return;
    d = (int)(delay * jack_rate () + 0.5f);
    if (inp < 0)
    {
	if (out >= 0) _dout [out] = d;
	return;
    }
    if (out < 0)
    {
	if (inp >= 0) _dinp [inp] = d;
	return;
    }
    _dmatr [_ninp * out + inp] = d;
}


int Jmatrix::jack_process (int nframes)
{
    const float  *p, *inp [MAXINP];
    float        *out;
    float        g0, g1, g, s;
    int          d0, d1;
    int          i, j, k, m;
    
    if (_state < PROCESS) return 0;

    for (i = 0; i < _ninp; i++)
    {
        p = (float *) jack_port_get_buffer (_inp_ports [i], nframes);
	if (_maxdel) _dproc [i].write (p);
	else inp [i] = p;
    }

    for (j = m = 0; j < _nout; j++, m += _ninp)
    {
        out = (float *) jack_port_get_buffer (_out_ports [j], nframes);
	memset (out, 0, nframes * sizeof (float));
	for (i = 0; i < _ninp; i++)
	{
	    g0 = _gcurr [m + i];
	    g1 = _gmatr [m + i] * _ginp [i] * _gout [j];
	    _gcurr [m + i] = g1;
	    if (_maxdel)
            {
		d0 = _dcurr [m + i];
		d1 = _dmatr [m + i] + _dinp [i] + _dout [j];;
		if (d1 < 0) d1 = 0;
		if (d1 > _maxdel) d1 = _maxdel;
		_dcurr [m + i] = d1;
	    }
	    if (_maxdel && (d1 != d0))
	    {
		p = _dproc [i].readp (d0);
		g = g0;
		s = g0 / nframes;
		for (k = 0; k < nframes; k++)
		{
		    g -= s;
		    out [k] += g * p [k];
		}
		p = _dproc [i].readp (d1);
		g = 0.0f;
		s = g1 / nframes;
		for (k = 0; k < nframes; k++)
		{
		    g += s;
		    out [k] += g * p [k];
		}
	    }
	    else
	    {
		p = _maxdel ? _dproc [i].readp (d0) : inp [i];
		s = g1 - g0;
		if (fabsf (s) < 1e-3f * (fabsf (g0) + fabsf (g1)))
		{
		    if (fabsf (g1) >= 1e-15f)
		    {
			for (k = 0; k < nframes; k++)
			{
			    out [k] += g1 * p [k];
			}
		    }
		}
		else
		{
		    g = g0;
		    s /= nframes;
		    for (k = 0; k < nframes; k++)
		    {
			g += s;
			out [k] += g * p [k];
		    }
		}
	    }
	}
    }

    return 0;
}

