/* filter/impulse.c

 *

 * Impulse detecting filters

 * 

 * Copyright (C) 2018 Patrick Alken

 * 

 * 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; either version 3 of the License, or (at

 * your option) any later version.

 * 

 * 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, write to the Free Software

 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.

 */

#include <config.h>

#include <stdlib.h>

#include <math.h>

#include <gsl/gsl_math.h>

#include <gsl/gsl_vector.h>

#include <gsl/gsl_filter.h>

static int filter_impulse(const double scale, const double epsilon, const double t, const gsl_vector * x, const gsl_vector * xmedian,

                          gsl_vector * y, gsl_vector * xsigma, size_t * noutlier, gsl_vector_int * ioutlier);

/*

gsl_filter_impulse_alloc()

  Allocate a workspace for impulse detection filtering.

Inputs: K - number of samples in window; if even, it is rounded up to

            the next odd, to have a symmetric window

Return: pointer to workspace

*/

gsl_filter_impulse_workspace *

gsl_filter_impulse_alloc(const size_t K)

{

  gsl_filter_impulse_workspace *w;

  w = calloc(1, sizeof(gsl_filter_impulse_workspace));

  if (w == 0)

    {

      GSL_ERROR_NULL ("failed to allocate space for workspace", GSL_ENOMEM);

    }

  w->movstat_workspace_p = gsl_movstat_alloc(K);

  if (w->movstat_workspace_p == 0)

    {

      gsl_filter_impulse_free(w);

      return NULL;

    }

  return w;

}

void

gsl_filter_impulse_free(gsl_filter_impulse_workspace * w)

{

  if (w->movstat_workspace_p)

    gsl_movstat_free(w->movstat_workspace_p);

  free(w);

}

/*

gsl_filter_impulse()

  Apply an impulse detection filter to an input vector. The filter output is

y_i = { x_i, |x_i - m_i| <= t * S_i

      { m_i, |x_i - m_i| > t * S_i

where m_i is the median of the window W_i^H and S_i is the scale estimate (MAD, IQR, S_n, Q_n)

Inputs: endtype    - how to handle signal end points

        scale_type - which statistic to use for scale estimate (MAD, IQR, etc)

        t          - number of standard deviations required to identity outliers (>= 0)

        x          - input vector, size n

        y          - (output) filtered vector, size n

        xmedian    - (output) vector of median values of x, size n

                     xmedian_i = median of window centered on x_i

        xsigma     - (output) vector of estimated local standard deviations of x, size n

                     xsigma_i = sigma for i-th window: scale*MAD

        noutlier   - (output) number of outliers detected

        ioutlier   - (output) boolean array indicating outliers identified, size n; may be NULL

                     ioutlier_i = 1 if outlier detected, 0 if not

        w          - workspace

Notes:

*/

int

gsl_filter_impulse(const gsl_filter_end_t endtype, const gsl_filter_scale_t scale_type, const double t,

                   const gsl_vector * x, gsl_vector * y, gsl_vector * xmedian, gsl_vector * xsigma, size_t * noutlier,

                   gsl_vector_int * ioutlier, gsl_filter_impulse_workspace * w)

{

  const size_t n = x->size;

  if (n != y->size)

    {

      GSL_ERROR("input and output vectors must have same length", GSL_EBADLEN);

    }

  else if (xmedian->size != n)

    {

      GSL_ERROR("xmedian vector must match input size", GSL_EBADLEN);

    }

  else if (xsigma->size != n)

    {

      GSL_ERROR("xsigma vector must match input size", GSL_EBADLEN);

    }

  else if ((ioutlier != NULL) && (ioutlier->size != n))

    {

      GSL_ERROR("ioutlier vector must match input size", GSL_EBADLEN);

    }

  else if (t < 0.0)

    {

      GSL_ERROR("t must be non-negative", GSL_EDOM);

    }

  else

    {

      int status;

      double scale = 1.0;

      switch (scale_type)

        {

          case GSL_FILTER_SCALE_MAD:

            {

              /* compute window medians and MADs */

              gsl_movstat_mad(endtype, x, xmedian, xsigma, w->movstat_workspace_p);

              break;

            }

          case GSL_FILTER_SCALE_IQR:

            {

              /* multiplication factor for IQR to estimate stddev for Gaussian signal */

              scale = 0.741301109252801;

              /* calculate the window medians */

              gsl_movstat_median(endtype, x, xmedian, w->movstat_workspace_p);

              /* calculate window IQRs */

              gsl_movstat_qqr(endtype, x, 0.25, xsigma, w->movstat_workspace_p);

              break;

            }

          case GSL_FILTER_SCALE_SN:

            {

              /* calculate the window medians */

              gsl_movstat_median(endtype, x, xmedian, w->movstat_workspace_p);

              /* calculate window S_n values */

              gsl_movstat_Sn(endtype, x, xsigma, w->movstat_workspace_p);

              break;

            }

          case GSL_FILTER_SCALE_QN:

            {

              /* calculate the window medians */

              gsl_movstat_median(endtype, x, xmedian, w->movstat_workspace_p);

              /* calculate window Q_n values */

              gsl_movstat_Qn(endtype, x, xsigma, w->movstat_workspace_p);

              break;

            }

          default:

            GSL_ERROR("unknown scale type", GSL_EDOM);

            break;

        }

      /* apply impulse detecting filter using previously computed scale estimate */

      status = filter_impulse(scale, 0.0, t, x, xmedian, y, xsigma, noutlier, ioutlier);

      return status;

    }

}

/*

filter_impulse()

  Apply an impulse detection filter to an input vector. The filter output is

y_i = { x_i, |x_i - m_i| <= t * S_i OR S_i < epsilon

      { m_i, |x_i - m_i| > t * S_i

where m_i is the median of the window W_i^H and S_i is the scale estimate (MAD, IQR, etc)

Inputs: scale    - scale factor to multiply xsigma to get unbiased estimate of stddev for Gaussian data

        epsilon  - minimum allowed scale estimate for identifying outliers

        t        - number of standard deviations required to identity outliers (>= 0)

        x        - input vector, size n

        xmedian  - vector of median values of x, size n

                   xmedian_i = median of window centered on x_i

        y        - (output) filtered vector, size n

        xsigma   - (output) vector of estimated local standard deviations of x, size n

                   xsigma_i = S_n for i-th window

        noutlier - (output) number of outliers detected

        ioutlier - (output) boolean array indicating outliers identified, size n; may be NULL

                   ioutlier_i = 1 if outlier detected, 0 if not

Notes:

1) If S_i = 0 or is very small for a particular sample, then the filter may erroneously flag the

sample as an outlier, since it will act as a standard median filter. To avoid this scenario, the

parameter epsilon specifies the minimum value of S_i which can be used in the filter test. Any

samples for which S_i < epsilon are passed through unchanged.

*/

static int

filter_impulse(const double scale, const double epsilon, const double t, const gsl_vector * x, const gsl_vector * xmedian,

               gsl_vector * y, gsl_vector * xsigma, size_t * noutlier, gsl_vector_int * ioutlier)

{

  const size_t n = x->size;

  if (n != y->size)

    {

      GSL_ERROR("input and output vectors must have same length", GSL_EBADLEN);

    }

  else if (xmedian->size != n)

    {

      GSL_ERROR("xmedian vector must match input size", GSL_EBADLEN);

    }

  else if (xsigma->size != n)

    {

      GSL_ERROR("xsigma vector must match input size", GSL_EBADLEN);

    }

  else if ((ioutlier != NULL) && (ioutlier->size != n))

    {

      GSL_ERROR("ioutlier vector must match input size", GSL_EBADLEN);

    }

  else if (t < 0.0)

    {

      GSL_ERROR("t must be non-negative", GSL_EDOM);

    }

  else

    {

      size_t i;

      *noutlier = 0;

      /* build output vector */

      for (i = 0; i < n; ++i)

        {

          double xi = gsl_vector_get(x, i);

          double xmedi = gsl_vector_get(xmedian, i);

          double absdevi = fabs(xi - xmedi); /* absolute deviation for this sample */

          double *xsigmai = gsl_vector_ptr(xsigma, i);

          /* multiply by scale factor to get estimate of standard deviation */

          *xsigmai *= scale;

          /*

           * If the absolute deviation for this sample is more than t stddevs

           * for this window (and S_i is sufficiently large to avoid scale implosion),

           * set the output value to the window median, otherwise use the original sample

           */

          if ((*xsigmai >= epsilon) && (absdevi > t * (*xsigmai)))

            {

              gsl_vector_set(y, i, xmedi);

              ++(*noutlier);

              if (ioutlier)

                gsl_vector_int_set(ioutlier, i, 1);

            }

          else

            {

              gsl_vector_set(y, i, xi);

              if (ioutlier)

                gsl_vector_int_set(ioutlier, i, 0);

            }

        }

      return GSL_SUCCESS;

    }

}