//filter_goertzel.c:

/*
 *      Copyright (C) Philipp 'ph3-der-loewe' Schafft - 2008-2012
 *
 *  This file is part of libroardsp a part of RoarAudio,
 *  a cross-platform sound system for both, home and professional use.
 *  See README for details.
 *
 *  This file is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License version 3
 *  as published by the Free Software Foundation.
 *
 *  libroardsp 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 software; see the file COPYING.  If not, write to
 *  the Free Software Foundation, 51 Franklin Street, Fifth Floor,
 *  Boston, MA 02110-1301, USA.
 *
 */

#include "libroardsp.h"

#ifdef ROAR_HAVE_LIBM

#define DEFAULT_FREQ 1000.0

struct roardsp_goertzel {
 float   freq;
 int32_t coeff;
 int32_t old[ROAR_MAX_CHANNELS][2];
};

int roardsp_goertzel_init  (struct roardsp_filter * filter, struct roar_stream * stream, int id) {
 struct roardsp_goertzel * self = roar_mm_malloc(sizeof(struct roardsp_goertzel));

 (void)stream, (void)id;

 if ( self == NULL )
  return -1;

 memset(self, 0, sizeof(struct roardsp_goertzel));

 filter->inst = (void*) self;

 roardsp_goertzel_reset(filter, ROARDSP_RESET_FULL);

 return 0;
}

int roardsp_goertzel_uninit(struct roardsp_filter * filter) {

 roar_mm_free(filter->inst);
 return 0;
}

#define _calcX(bits,twobits) \
int roardsp_goertzel_calc##bits  (struct roardsp_filter * filter, void * data, size_t samples) { \
 struct roardsp_goertzel * self = (struct roardsp_goertzel *) filter->inst; \
 int##bits##_t * samp = (int##bits##_t *) data; \
 /*register int##twobits##_t s, g, h;*/ \
 register float s, g, h; \
 /*register int64_t s, g, h;*/ \
 int channel; \
 size_t i; \
\
 roardsp_goertzel_reset(filter, ROARDSP_RESET_STATE); \
\
 for (i = 0, channel = 0; i < samples; i++, channel = channel == (filter->channels - 1) ? 0 : channel + 1) { \
  g        = self->old[channel][0]; \
  g       *= self->coeff; \
  g       /= 32767; \
  s        = samp[i]; \
  ROAR_DBG("roardsp_goertzel_calc*(*): channel=%i, g=%f, s=%f, old[0]=%f, old[1]=%f", channel, g, s, self->old[channel][0], self->old[channel][1]); \
  s       += g; \
  s       -= self->old[channel][1]; \
  self->old[channel][1] = self->old[channel][0]; \
  self->old[channel][0] = s; \
  /* samp[i]  = s; */ \
  g  = s; \
  g *= self->old[channel][1]; \
  g *= self->coeff; \
  g /= 32767; \
  h  = self->old[channel][1]; \
  h *= self->old[channel][1]; \
  s  *= s; \
  s  += h; \
  s  -= g; \
  s  *= 0.0001;
  s  /= (float)((i+1)*(i+1));
  samp[i]  = s; \
 }; \
\
 return 0; \
}

_calcX(8,16)
_calcX(16,32)
_calcX(32,64)

int roardsp_goertzel_ctl   (struct roardsp_filter * filter, int cmd, void * data) {
 struct roardsp_goertzel * self = (struct roardsp_goertzel *) filter->inst;
 float old;

 switch (cmd) {
  case ROARDSP_FCTL_FREQ:
   old = self->freq;
   self->freq = *(float*)data;
   *(float*)data = old;
   old = 2.*cos(2.*M_PI*self->freq/(float)filter->rate);
   self->coeff = old*32767.;
   ROAR_DBG("roardsp_goertzel_ctl(filter=%p, cmd=%i, data=%p): self->coeff=%li (%f)", filter, cmd, data, (long int)self->coeff, old);
   break;
  default:
    roar_err_set(ROAR_ERROR_BADRQC);
    return -1;
   break;
 }

 return 0;
}

int roardsp_goertzel_reset (struct roardsp_filter * filter, int what) {
 struct roardsp_goertzel * self;
 float freq;

 if ( filter == NULL )
  return -1;

 if ( filter->inst == NULL )
  return -1;

 self = filter->inst;

 switch (what) {
  case ROARDSP_RESET_NONE:
    return 0;
   break;
  case ROARDSP_RESET_FULL:
    freq = self->freq = DEFAULT_FREQ;
    roardsp_goertzel_ctl(filter, ROARDSP_FCTL_FREQ, &freq);
  case ROARDSP_RESET_STATE:
    memset(self->old, 0, sizeof(self->old));
    return 0;
   break;
  default:
    roar_err_set(ROAR_ERROR_BADRQC);
    return -1;
   break;
 }

 return -1;
}

#endif

//ll