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


float  Cmeterdsp::_w1;
float  Cmeterdsp::_w2; 



Cmeterdsp::Cmeterdsp (void) :
    _zl (0),
    _zr (0),
    _zlr (0),
    _zll (0),
    _zrr (0)
{
}


Cmeterdsp::~Cmeterdsp (void)
{
}


void Cmeterdsp::process (float *pl, float *pr, int n)
{
    // Called by JACK's process callback.
    //
    // pl : pointer to left channel sample buffer
    // pr : pointer to right channel sample buffer
    // n  : number of samples to process

    float zl, zr, zlr, zll, zrr;

    zl = _zl;
    zr = _zr;
    zlr = _zlr;
    zll = _zll;
    zrr = _zrr;
    while (n--)
    {
	zl += _w1 * (*pl++ - zl) + 1e-15f;
	zr += _w1 * (*pr++ - zr) + 1e-15f;
	zlr += _w2 * (zl * zr - zlr);
	zll += _w2 * (zl * zl - zll);
	zrr += _w2 * (zr * zr - zrr);
    }
    if (    isnormal (zl)
	 && isnormal (zr)
	 && isnormal (zlr)
	 && isnormal (zll)
	 && isnormal (zrr))
    {
        _zl = zl;
        _zr = zr;
        _zlr = zlr;
        _zll = zll;
        _zrr = zrr;
    }
    else reset ();
}


float Cmeterdsp::read (void)
{
    // Called by display process approx. 30 times per second.

    return _zlr / sqrtf (_zll * _zrr + 1e-12f);
}


void Cmeterdsp::init (int fsamp, float flp, float tcf)
{
    // Called by initialisation code.
    //
    // fsamp = sample frequency
    // flp   = lowpass frequency
    // tcf   = correlation filter time constant

    _w1 = 6.28f * flp / fsamp;
    _w2 = 1 / (tcf * fsamp);
}
