goertzel.c

/*
* Copyright (c) 2015-2019 Sergey Bakhurin
* Digital Signal Processing Library [http://dsplib.org]
*
* This file is part of libdspl-2.0.
*
* is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser    General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* DSPL 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 Lesser General Public License
* along with Foobar.    If not, see <http://www.gnu.org/licenses/>.
*/
 
#include <stdlib.h>
#include <string.h>
#include "dspl.h"
 
 
#ifdef DOXYGEN_ENGLISH
 
#endif
#ifdef DOXYGEN_RUSSIAN
 
#endif
int DSPL_API goertzel(double *x, int n, int *ind, int k, complex_t *y)
{
 
    int m, p;
    double wR, wI;
    double alpha;
    double v[3];
 
    if(!x || !y || !ind)
        return ERROR_PTR;
 
    if(n < 1 || k < 1)
        return ERROR_SIZE;
 
    for(p = 0; p < k; p++)
    {
        wR = cos(M_2PI * (double)ind[p] / (double)n);
        wI = sin(M_2PI * (double)ind[p] / (double)n);
 
        alpha = 2.0 * wR;
        v[0] = v[1] = v[2] = 0.0;
 
        for(m = 0; m < n; m++)
        {
            v[2] = v[1];
            v[1] = v[0];
            v[0] = x[m]+alpha*v[1] - v[2];
        }
        RE(y[p]) = wR * v[0] - v[1];
        IM(y[p]) = wI * v[0];
    }
    return RES_OK;
}
 
 
 
 
 
#ifdef DOXYGEN_ENGLISH
 
#endif
#ifdef DOXYGEN_RUSSIAN
 
#endif
int DSPL_API goertzel_cmplx(complex_t *x, int n, int *ind, int k, complex_t *y)
{
 
    int m, p;
    complex_t w;
    double alpha;
    complex_t v[3];
 
    if(!x || !y || !ind)
        return ERROR_PTR;
 
    if(n < 1 || k < 1)
        return ERROR_SIZE;
 
    for(p = 0; p < k; p++)
    {
        RE(w) = cos(M_2PI * (double)ind[p] / (double)n);
        IM(w) = sin(M_2PI * (double)ind[p] / (double)n);
 
        alpha = 2.0 * RE(w);
        memset(v, 0, 3*sizeof(complex_t));
 
        for(m = 0; m < n; m++)
        {
            RE(v[2]) = RE(v[1]);
            RE(v[1]) = RE(v[0]);
            RE(v[0]) = RE(x[m]) + alpha * RE(v[1]) - RE(v[2]);
 
            IM(v[2]) = IM(v[1]);
            IM(v[1]) = IM(v[0]);
            IM(v[0]) = IM(x[m]) + alpha * IM(v[1]) - IM(v[2]);
        }
 
        RE(y[p]) = CMRE(w, v[0]) - RE(v[1]);
        IM(y[p]) = CMIM(w, v[0]) - IM(v[1]);
    }
 
    return RES_OK;
}