/*
  Copyright (C) 2003-2009 Paul Brossier <piem@aubio.org>

  This file is part of aubio.

  aubio 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.

  aubio 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 aubio.  If not, see <http://www.gnu.org/licenses/>.

*/


/* Requires lsmp_t to be long or double. float will NOT give reliable 
 * results */

#include "aubio_priv.h"
#include "fvec.h"
#include "lvec.h"
#include "mathutils.h"
#include "temporal/filter.h"

struct _aubio_filter_t
{
  uint_t order;
  uint_t samplerate;
  lvec_t *a;
  lvec_t *b;
  lvec_t *y;
  lvec_t *x;
};

void
aubio_filter_do_outplace (aubio_filter_t * f, fvec_t * in, fvec_t * out)
{
  fvec_copy (in, out);
  aubio_filter_do (f, out);
}

void
aubio_filter_do (aubio_filter_t * f, fvec_t * in)
{
  uint_t i, j, l, order = f->order;
  lsmp_t *x;
  lsmp_t *y;
  lsmp_t *a = f->a->data[0];
  lsmp_t *b = f->b->data[0];

  for (i = 0; i < in->channels; i++) {
    x = f->x->data[i];
    y = f->y->data[i];
    for (j = 0; j < in->length; j++) {
      /* new input */
      x[0] = KILL_DENORMAL (in->data[i][j]);
      y[0] = b[0] * x[0];
      for (l = 1; l < order; l++) {
        y[0] += b[l] * x[l];
        y[0] -= a[l] * y[l];
      }
      /* new output */
      in->data[i][j] = y[0];
      /* store for next sample */
      for (l = order - 1; l > 0; l--) {
        x[l] = x[l - 1];
        y[l] = y[l - 1];
      }
    }
    /* store for next run */
    f->x->data[i] = x;
    f->y->data[i] = y;
  }
}

/* The rough way: reset memory of filter between each run to avoid end effects. */
void aubio_filter_do_filtfilt(aubio_filter_t * f, fvec_t * in, fvec_t * tmp) {
  uint_t j,i=0;
  uint_t length = in->length;
  /* apply filtering */
  aubio_filter_do(f,in);
  aubio_filter_do_reset(f);
  /* mirror */
  for (j = 0; j < length; j++)
    tmp->data[i][length-j-1] = in->data[i][j];
  /* apply filtering on mirrored */
  aubio_filter_do(f,tmp);
  aubio_filter_do_reset(f);
  /* invert back */
  for (j = 0; j < length; j++)
    in->data[i][j] = tmp->data[i][length-j-1];
}

lvec_t *
aubio_filter_get_feedback (aubio_filter_t * f)
{
  return f->a;
}

lvec_t *
aubio_filter_get_feedforward (aubio_filter_t * f)
{
  return f->b;
}

uint_t
aubio_filter_get_order (aubio_filter_t * f)
{
  return f->order;
}

uint_t
aubio_filter_get_samplerate (aubio_filter_t * f)
{
  return f->samplerate;
}

uint_t
aubio_filter_set_samplerate (aubio_filter_t * f, uint_t samplerate)
{
  f->samplerate = samplerate;
  return AUBIO_OK;
}

void
aubio_filter_do_reset (aubio_filter_t * f)
{
  lvec_zeros(f->x);
  lvec_zeros(f->y);
}

aubio_filter_t *
new_aubio_filter (uint_t order, uint_t channels)
{
  aubio_filter_t *f = AUBIO_NEW (aubio_filter_t);
  f->x = new_lvec (order, channels);
  f->y = new_lvec (order, channels);
  f->a = new_lvec (order, 1);
  f->b = new_lvec (order, 1);
  /* by default, samplerate is not set */
  f->samplerate = 0;
  f->order = order;
  /* set default to identity */
  f->a->data[0][1] = 1.;
  return f;
}

void
del_aubio_filter (aubio_filter_t * f)
{
  del_lvec (f->a);
  del_lvec (f->b);
  del_lvec (f->x);
  del_lvec (f->y);
  AUBIO_FREE (f);
  return;
}