/* filter/gaussian.c

 *

 * Gaussian smoothing 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>

#include <gsl/gsl_poly.h>

/* maximum derivative order allowed for Gaussian filter */

#define GSL_FILTER_GAUSSIAN_MAX_ORDER     10

typedef double gaussian_type_t;

typedef double ringbuf_type_t;

#include "ringbuf.c"

typedef struct

{

  size_t n;        /* window size */

  double * window; /* linear array with current window */

  ringbuf * rbuf;  /* ring buffer storing current window */

} gaussian_state_t;

static size_t gaussian_size(const size_t n);

static int gaussian_init(const size_t n, void * vstate);

static int gaussian_insert(const gaussian_type_t x, void * vstate);

static int gaussian_delete(void * vstate);

static int gaussian_get(void * params, gaussian_type_t * result, const void * vstate);

static const gsl_movstat_accum gaussian_accum_type;

/*

gsl_filter_gaussian_alloc()

  Allocate a workspace for Gaussian 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_gaussian_workspace *

gsl_filter_gaussian_alloc(const size_t K)

{

  const size_t H = K / 2;

  gsl_filter_gaussian_workspace *w;

  size_t state_size;

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

  if (w == 0)

    {

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

    }

  w->K = 2 * H + 1;

  w->kernel = malloc(w->K * sizeof(double));

  if (w->kernel == 0)

    {

      gsl_filter_gaussian_free(w);

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

      return NULL;

    }

  state_size = gaussian_size(w->K);

  w->movstat_workspace_p = gsl_movstat_alloc_with_size(state_size, H, H);

  if (!w->movstat_workspace_p)

    {

      gsl_filter_gaussian_free(w);

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

    }

  return w;

}

void

gsl_filter_gaussian_free(gsl_filter_gaussian_workspace * w)

{

  if (w->kernel)

    free(w->kernel);

  if (w->movstat_workspace_p)

    gsl_movstat_free(w->movstat_workspace_p);

  free(w);

}

/*

gsl_filter_gaussian()

  Apply a Gaussian filter to an input vector:

G_{sigma}(x) = exp [ -x^2 / (2 sigma^2) ]

Inputs: alpha - number of standard deviations to include in Gaussian kernel

        order - derivative order of Gaussian

        x     - input vector, size n

        y     - (output) filtered vector, size n

        w     - workspace

Notes:

1) If alpha = 3, then the Gaussian kernel will be a Gaussian of +/- 3 standard deviations

*/

int

gsl_filter_gaussian(const gsl_filter_end_t endtype, const double alpha, const size_t order, const gsl_vector * x,

                    gsl_vector * y, gsl_filter_gaussian_workspace * w)

{

  if (x->size != y->size)

    {

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

    }

  else if (alpha <= 0.0)

    {

      GSL_ERROR("alpha must be positive", GSL_EDOM);

    }

  else

    {

      int status;

      gsl_vector_view kernel = gsl_vector_view_array(w->kernel, w->K);

      /* construct Gaussian kernel of length K */

      gsl_filter_gaussian_kernel(alpha, order, 1, &kernel.vector);

      status = gsl_movstat_apply_accum(endtype, x, &gaussian_accum_type, (void *) w->kernel, y,

                                       NULL, w->movstat_workspace_p);

      return status;

    }

}

/*

gsl_filter_gaussian_kernel()

  Construct Gaussian kernel with given sigma and order

Inputs: alpha     - number of standard deviations to include in window

        order     - kernel order (0 = gaussian, 1 = first derivative, ...)

        normalize - normalize so sum(G) = 1

        kernel    - (output) Gaussian kernel

Return: success/error

Notes:

1) If alpha = 3, then the output kernel will contain a Gaussian with +/- 3 standard deviations

*/

int

gsl_filter_gaussian_kernel(const double alpha, const size_t order, const int normalize, gsl_vector * kernel)

{

  const size_t N = kernel->size;

  if (alpha <= 0.0)

    {

      GSL_ERROR("alpha must be positive", GSL_EDOM);

    }

  else if (order > GSL_FILTER_GAUSSIAN_MAX_ORDER)

    {

      GSL_ERROR("derivative order is too large", GSL_EDOM);

    }

  else

    {

      const double half = 0.5 * (N - 1.0); /* (N - 1) / 2 */

      double sum = 0.0;

      size_t i;

      /* check for quick return */

      if (N == 1)

        {

          if (order == 0)

            gsl_vector_set(kernel, 0, 1.0);

          else

            gsl_vector_set(kernel, 0, 0.0);

          return GSL_SUCCESS;

        }

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

        {

          double xi = ((double)i - half) / half;

          double yi = alpha * xi;

          double gi = exp(-0.5 * yi * yi);

          gsl_vector_set(kernel, i, gi);

          sum += gi;

        }

      /* normalize so sum(kernel) = 1 */

      if (normalize)

        gsl_vector_scale(kernel, 1.0 / sum);

      if (order > 0)

        {

          const double beta = -0.5 * alpha * alpha;

          double q[GSL_FILTER_GAUSSIAN_MAX_ORDER + 1];

          size_t k;

          /*

           * Need to calculate derivatives of the Gaussian window; define

           *

           * w(n) = C * exp [ p(n) ]

           *

           * p(n) = beta * n^2

           * beta = -1/2 * ( alpha / ((N-1)/2) )^2

           *

           * Then:

           *

           * d^k/dn^k w(n) = q_k(n) * w(n)

           *

           * where q_k(n) is a degree-k polynomial in n, which satisfies:

           *

           * q_k(n) = d/dn q_{k-1}(n) + q_{k-1}(n) * dp(n)/dn

           * q_0(n) = 1 / half^{order}

           */

          /* initialize q_0(n) = 1 / half^{order} */

          q[0] = 1.0 / gsl_pow_uint(half, order);

          for (i = 1; i <= GSL_FILTER_GAUSSIAN_MAX_ORDER; ++i)

            q[i] = 0.0;

          /* loop through derivative orders and calculate q_k(n) for k = 1,...,order */

          for (k = 1; k <= order; ++k)

            {

              double qm1 = q[0];

              q[0] = q[1];

              for (i = 1; i <= k; ++i)

                {

                  double tmp = q[i];

                  q[i] = (i + 1.0) * q[i + 1] + /* d/dn q_{k-1} */

                         2.0 * beta * qm1;      /* q_{k-1}(n) p'(n) */

                  qm1 = tmp;

                }

            }

          /* now set w(n) := q(n) * w(n) */

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

            {

              double xi = ((double)i - half) / half;

              double qn = gsl_poly_eval(q, order + 1, xi);

              double *wn = gsl_vector_ptr(kernel, i);

              *wn *= qn;

            }

        }

      return GSL_SUCCESS;

    }

}

static size_t

gaussian_size(const size_t n)

{

  size_t size = 0;

  size += sizeof(gaussian_state_t);

  size += n * sizeof(gaussian_type_t);

  size += ringbuf_size(n);

  return size;

}

static int

gaussian_init(const size_t n, void * vstate)

{

  gaussian_state_t * state = (gaussian_state_t *) vstate;

  state->n = n;

  state->window = (gaussian_type_t *) ((unsigned char *) vstate + sizeof(gaussian_state_t));

  state->rbuf = (ringbuf *) ((unsigned char *) state->window + n * sizeof(gaussian_type_t));

  ringbuf_init(n, state->rbuf);

  return GSL_SUCCESS;

}

static int

gaussian_insert(const gaussian_type_t x, void * vstate)

{

  gaussian_state_t * state = (gaussian_state_t *) vstate;

  /* add new element to ring buffer */

  ringbuf_insert(x, state->rbuf);

  return GSL_SUCCESS;

}

static int

gaussian_delete(void * vstate)

{

  gaussian_state_t * state = (gaussian_state_t *) vstate;

  if (!ringbuf_is_empty(state->rbuf))

    ringbuf_pop_back(state->rbuf);

  return GSL_SUCCESS;

}

static int

gaussian_get(void * params, gaussian_type_t * result, const void * vstate)

{

  const gaussian_state_t * state = (const gaussian_state_t *) vstate;

  const double * kernel = (const double *) params;

  size_t n = ringbuf_copy(state->window, state->rbuf);

  double sum = 0.0;

  size_t i;

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

    sum += state->window[i] * kernel[n - i - 1];

  *result = sum;

  return GSL_SUCCESS;

}

static const gsl_movstat_accum gaussian_accum_type =

{

  gaussian_size,

  gaussian_init,

  gaussian_insert,

  gaussian_delete,

  gaussian_get

};