//filter_highp.c:

/*
 *      Copyright (C) Philipp 'ph3-der-loewe' Schafft - 2008-2013
 *
 *  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

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

 (void)stream, (void)id;

 if ( self == NULL )
  return -1;

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

 filter->inst = (void*) self;

 return roardsp_filter_reset(filter, ROARDSP_RESET_FULL);
}

int roardsp_highp_uninit(struct roardsp_filter * filter) {

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

#define _calcX(bits,twobits) \
int roardsp_highp_calc##bits (struct roardsp_filter * filter, void * data, size_t samples) { \
 struct roardsp_highp * self = (struct roardsp_highp *) filter->inst; \
 int##bits##_t * samp = (int##bits##_t *) data; \
 register int##twobits##_t s, h; \
 size_t i, c; \
 size_t channels = filter->channels; \
\
 if ( channels > ROAR_MAX_CHANNELS ) \
  return -1; \
\
 samples /= channels; \
\
 ROAR_DBG("roardsp_highp_calc16(*): filtering %i frames of %i channels...", samples, channels); \
\
/* *      output[N] = A0 * input[N] + A1 * input[N-1] + B1 * output[N-1] */ \
\
\
 for (i = 0; i < samples; i++) { \
  for (c = 0; c < channels; c++) { \
   h = samp[i*channels + c]; \
   s = (int##twobits##_t)samp[i*channels + c] * self->a + (int##twobits##_t)self->oldin[c] * self->b + (int##twobits##_t)self->oldout[c] * self->c; \
\
   s /= 65536; \
\
   self->oldin[      c] = h; \
   samp[i*channels + c] = s; \
   self->oldout[     c] = s; \
  } \
 } \
\
 return 0; \
}

_calcX(8,32) /* we need 32 bit not 16 bit here as we multiplay with 65536 internally */
_calcX(16,32)
_calcX(32,64)

int roardsp_highp_ctl   (struct roardsp_filter * filter, int cmd, void * data) {
 struct roardsp_highp * self = (struct roardsp_highp *) filter->inst;
 float lp;
 float oldfreq;
 float newfreq;

 if ( cmd != ROARDSP_FCTL_FREQ )
  return -1;

 newfreq = *(float*)data;

 lp = exp(-2.0 * M_PI * newfreq / (float) filter->rate) * 65536.;

 self->a =  (65536 + lp)/2.0;
 self->b = -(65536 + lp)/2.0;
 self->c =           lp;


 oldfreq = self->freq / 1000;
 self->freq = newfreq * 1000;

 *(float*)data = oldfreq;

 ROAR_DBG("roardsp_highp_ctl(*): oldfreq=%f, newfreq=%f", oldfreq, newfreq);
 ROAR_DBG("roardsp_highp_ctl(*): a=%i, b=%i, c=%i", self->a, self->b, self->c);

 return 0;
}

int roardsp_highp_reset (struct roardsp_filter * filter, int what) {
 struct roardsp_highp * self;
 float freq = 25.;

 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:
    roardsp_highp_ctl(filter, ROARDSP_FCTL_FREQ, &freq);
  case ROARDSP_RESET_STATE:
    memset(self->oldin,  0, sizeof(int32_t)*ROAR_MAX_CHANNELS);
    memset(self->oldout, 0, sizeof(int32_t)*ROAR_MAX_CHANNELS);
    return  0;
   break;
  default:
    return -1;
 }

 return -1;
}

#endif

//ll