Carla/source/modules/zita-resampler/cresampler.cc

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


CResampler::CResampler (void) :
    _nchan (0),
    _buff  (0)
{
    reset ();
}


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


int CResampler::setup (double       ratio,
                       unsigned int nchan)
{
    if (! nchan) return 1; 
    clear ();
    _inmax = 50;
    _buff = new float [nchan * (3 + _inmax)];
    _nchan = nchan;
    _pstep = 1 / ratio;
    return reset ();
}


void CResampler::clear (void)
{
    delete[] _buff;
    _buff  = 0;
    _nchan = 0;
    _inmax = 0;
    _pstep = 0;
    reset ();
}


void CResampler::set_phase (double p)
{
    _phase = p - floor (p);
}


void CResampler::set_ratio (double r)
{
    _pstep = 1.0 / r;
}


double CResampler::inpdist (void) const
{
    return (int)(3 - _nread) - _phase;
}


int CResampler::inpsize (void) const
{
    return 4;
}


int CResampler::reset (void)
{
    inp_count = 0;
    out_count = 0;
    inp_data = 0;
    out_data = 0;
    _index = 0;
    _phase = 0; 
    _nread = 4;
    _nzero = 0;
    return 0;
}


int CResampler::process (void)
{
    unsigned int   in, nr, n, c;
    int            nz;
    double         ph; 
    float          *pb, a, b, d, m0, m1, m2, m3;

    in = _index;
    nr = _nread;
    nz = _nzero;
    ph = _phase;
    pb = _buff + in * _nchan;

    while (out_count)
    {
        if (nr)
        {
            if (inp_count == 0) break;
            n = (4 - nr) * _nchan;
            if (inp_data)
            {
                for (c = 0; c < _nchan; c++) pb [n + c] = inp_data [c];
                inp_data += _nchan;
                nz = 0;
            }
            else
            {
                for (c = 0; c < _nchan; c++) pb [n + c] = 0;
                if (nz < 4) nz++;
            }
            nr--;
            inp_count--;
        }
        else
        {
            n = _nchan;
            if (out_data)
            {
                if (nz < 4)
                {
                    a = ph;
                    b = 1 - a;
                    d = a * b / 2;
                    m0 = -d * b;
                    m1 = b + (3 * b - 1) * d;
                    m2 = a + (3 * a - 1) * d;
                    m3 = -d * a;
                    for (c = 0; c < n; c++)
                    {
                        *out_data++ = m0 * pb [0]
                                    + m1 * pb [n]
                                    + m2 * pb [2 * n]
                                    + m3 * pb [3 * n];
                        pb++;
                    }
                    pb -= n;
                }
                else
                {
                    for (c = 0; c < n; c++) *out_data++ = 0;
                }
            }
            out_count--;

            ph += _pstep;
            if (ph >= 1.0)
            {
                nr = (unsigned int) floor (ph);
                ph -= nr;
                in += nr;
                pb += nr * _nchan;
                if (in >= _inmax)
                {
                    memcpy (_buff, pb, (4 - nr) * _nchan * sizeof (float));
                    in = 0;
                    pb = _buff;
                }
            }
        }
    }

    _index = in;
    _nread = nr;
    _nzero = nz;
    _phase = ph;

    return 0;
}