/*
***************************************************************************
*
* Author: Teunis van Beelen
*
* Copyright (C) 2010 - 2021 Teunis van Beelen
*
* Email: teuniz@protonmail.com
*
***************************************************************************
*
* 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, version 3 of the License.
*
* 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 "ravg_filter.h"




struct ravg_filter_settings * create_ravg_filter(int type, int size)
{
  struct ravg_filter_settings *settings;

  if((size < 2) || (size > 10000))  return NULL;

  if((type < 0) || (type > 1))  return NULL;

  settings = (struct ravg_filter_settings *) calloc(1, sizeof(struct ravg_filter_settings));
  if(settings==NULL)  return NULL;

  settings->size = size;
  settings->type = type;
  settings->first_sample = 1;
  settings->buf_pntr = 0;
  settings->buf_pntr2 = 0;
  settings->mean = 0.0;
  settings->mean2 = 0.0;
  settings->buf = (double *)calloc(1, sizeof(double) * size);
  if(settings->buf == NULL)
  {
    free(settings);
    return NULL;
  }
  settings->buf2 = (double *)calloc(1, sizeof(double) * size);
  if(settings->buf2 == NULL)
  {
    free(settings->buf);
    free(settings);
    return NULL;
  }

  return settings;
}


struct ravg_filter_settings * create_ravg_filter_copy(struct ravg_filter_settings *src)
{
  struct ravg_filter_settings *settings;

  settings = (struct ravg_filter_settings *) calloc(1, sizeof(struct ravg_filter_settings));
  if(settings==NULL)
  {
    return NULL;
  }
  memcpy(settings, src, sizeof(struct ravg_filter_settings));

  settings->buf = (double *)calloc(1, sizeof(double) * settings->size);
  if(settings->buf == NULL)
  {
    free(settings);
    return NULL;
  }
  memcpy(settings->buf, src->buf, settings->size);

  settings->buf2 = (double *)calloc(1, sizeof(double) * settings->size);
  if(settings->buf2 == NULL)
  {
    free(settings->buf);
    free(settings);
    return NULL;
  }
  memcpy(settings->buf2, src->buf2, settings->size);

  return settings;
}


void reset_ravg_filter(double offset, struct ravg_filter_settings *settings)
{
  int i;

  for(i=0; i<settings->size; i++)
  {
    settings->buf[i] = offset;
  }

  memcpy(settings->buf2, settings->buf, settings->size * sizeof(double));

  settings->mean = offset;

  settings->mean2 = offset;

  settings->buf_pntr = 0;

  settings->buf_pntr2 = 0;
}


void ravg_filter_restore_buf(struct ravg_filter_settings *settings)
{
  memcpy(settings->buf, settings->buf2, settings->size * sizeof(double));

  settings->mean = settings->mean2;

  settings->buf_pntr = settings->buf_pntr2;
}


void ravg_filter_save_buf(struct ravg_filter_settings *settings)
{
  memcpy(settings->buf2, settings->buf, settings->size * sizeof(double));

  settings->mean2 = settings->mean;

  settings->buf_pntr2 = settings->buf_pntr;
}


double run_ravg_filter(double new_input, struct ravg_filter_settings *settings)
{
  int i;


  if(settings->first_sample)
  {
    settings->first_sample = 0;

    for(i=0; i<settings->size; i++)
    {
      settings->buf[i] = new_input;
    }

    settings->mean = new_input;

    return new_input;
  }

  settings->mean -= (settings->buf[settings->buf_pntr] / settings->size);

  settings->buf[settings->buf_pntr] = new_input;

  settings->buf_pntr++;

  if(settings->buf_pntr >= settings->size)
  {
    settings->buf_pntr = 0;
  }

  settings->mean += (new_input / settings->size);

  if(settings->type == 0)  /* highpass */
  {
    return settings->buf[(settings->buf_pntr + (settings->size / 2)) % settings->size] - settings->mean;
  }

  if(settings->type == 1)  /* lowpass */
  {
    return settings->mean;
  }

  return 0.0;
}


void free_ravg_filter(struct ravg_filter_settings *settings)
{
  if(settings == NULL)
  {
    return;
  }

  free(settings->buf);
  free(settings->buf2);
  free(settings);
}


double ravg_filter_response(int type, int size, double freq)
{
  if((type < 0) || (type > 1) || (size < 2) || (freq > 0.5))
  {
    return 1.0;
  }

  if(type)
  {
    if(freq < 0.000001)
    {
      return 1.0;
    }

    return fabs((sin(M_PI * freq * (double)size)) / ((double)size * sin(M_PI * freq)));
  }

  if(freq < 0.000001)
  {
    return 0.0;
  }

  return 1.0 - fabs((sin(M_PI * freq * (double)size)) / ((double)size * sin(M_PI * freq)));
}