/* multiroots/test.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Brian Gough
 *
 * 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 2 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., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

#include <config.h>
#include <stdlib.h>
#include <stdio.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_multiroots.h>

#include <gsl/gsl_ieee_utils.h>

#include "test_funcs.h"
int test_fdf (const char * desc, gsl_multiroot_function_fdf * function, initpt_function initpt, double factor, const gsl_multiroot_fdfsolver_type * T);
int test_f (const char * desc, gsl_multiroot_function_fdf * fdf, initpt_function initpt, double factor, const gsl_multiroot_fsolver_type * T);

int
main (void)
{
  const gsl_multiroot_fsolver_type * fsolvers[5] ;
  const gsl_multiroot_fsolver_type ** T1 ;

  const gsl_multiroot_fdfsolver_type * fdfsolvers[5] ;
  const gsl_multiroot_fdfsolver_type ** T2 ;

  double f;

  fsolvers[0] = gsl_multiroot_fsolver_dnewton;
  fsolvers[1] = gsl_multiroot_fsolver_broyden;
  fsolvers[2] = gsl_multiroot_fsolver_hybrid;
  fsolvers[3] = gsl_multiroot_fsolver_hybrids;
  fsolvers[4] = 0;

  fdfsolvers[0] = gsl_multiroot_fdfsolver_newton;
  fdfsolvers[1] = gsl_multiroot_fdfsolver_gnewton;
  fdfsolvers[2] = gsl_multiroot_fdfsolver_hybridj;
  fdfsolvers[3] = gsl_multiroot_fdfsolver_hybridsj;
  fdfsolvers[4] = 0;

  gsl_ieee_env_setup();

  f = 1.0 ;

  T1 = fsolvers ;

  while (*T1 != 0)
    {
      test_f ("Rosenbrock", &rosenbrock, rosenbrock_initpt, f, *T1);
      test_f ("Roth", &roth, roth_initpt, f, *T1);
      test_f ("Powell badly scaled", &powellscal, powellscal_initpt, f, *T1);
      test_f ("Brown badly scaled", &brownscal, brownscal_initpt, f, *T1);
      test_f ("Powell singular", &powellsing, powellsing_initpt, f, *T1);
      test_f ("Wood", &wood, wood_initpt, f, *T1);
      test_f ("Helical", &helical, helical_initpt, f, *T1);
      test_f ("Discrete BVP", &dbv, dbv_initpt, f, *T1);
      test_f ("Trig", &trig, trig_initpt, f, *T1);
      T1++;
    }

  T2 = fdfsolvers ;

  while (*T2 != 0)
    {
      test_fdf ("Rosenbrock", &rosenbrock, rosenbrock_initpt, f, *T2);
      test_fdf ("Roth", &roth, roth_initpt, f, *T2);
      test_fdf ("Powell badly scaled", &powellscal, powellscal_initpt, f, *T2);
      test_fdf ("Brown badly scaled", &brownscal, brownscal_initpt, f, *T2);
      test_fdf ("Powell singular", &powellsing, powellsing_initpt, f, *T2);
      test_fdf ("Wood", &wood, wood_initpt, f, *T2);
      test_fdf ("Helical", &helical, helical_initpt, f, *T2);
      test_fdf ("Discrete BVP", &dbv, dbv_initpt, f, *T2);
      test_fdf ("Trig", &trig, trig_initpt, f, *T2);
      T2++;
    }

  exit (gsl_test_summary ());
}

void scale (gsl_vector * x, double factor);

void
scale (gsl_vector * x, double factor)
{
  size_t i, n = x->size;

  if (gsl_vector_isnull(x))
    {
      for (i = 0; i < n; i++)
        {
          gsl_vector_set (x, i, factor);
        }
    }
  else
    {
      for (i = 0; i < n; i++)
        {
          double xi = gsl_vector_get(x, i);
          gsl_vector_set(x, i, factor * xi);
        }
    }
}

int
test_fdf (const char * desc, gsl_multiroot_function_fdf * function,
          initpt_function initpt, double factor,
          const gsl_multiroot_fdfsolver_type * T)
{
  int status;
  double residual = 0;
  size_t i, n = function->n, iter = 0;

  gsl_vector *x = gsl_vector_alloc (n);
  gsl_matrix *J = gsl_matrix_alloc (n, n);

  gsl_multiroot_fdfsolver *s;

  (*initpt) (x);

  if (factor != 1.0) scale(x, factor);

  s = gsl_multiroot_fdfsolver_alloc (T, n);
  gsl_multiroot_fdfsolver_set (s, function, x);

  do
    {
      iter++;
      status = gsl_multiroot_fdfsolver_iterate (s);

      if (status)
        break ;

      status = gsl_multiroot_test_residual (s->f, 0.0000001);
    }
  while (status == GSL_CONTINUE && iter < 1000);

#ifdef DEBUG
  printf("x "); gsl_vector_fprintf (stdout, s->x, "%g"); printf("\n");
  printf("f "); gsl_vector_fprintf (stdout, s->f, "%g"); printf("\n");
#endif

#ifdef TEST_JACOBIAN
 {
    double r,sum; size_t j;

    gsl_multiroot_function f1 ;
    f1.f = function->f ;
    f1.n = function->n ;
    f1.params = function->params ;

    gsl_multiroot_fdjacobian (&f1, s->x, s->f, GSL_SQRT_DBL_EPSILON, J);

    /* compare J and s->J */

    r=0;sum=0;
    for (i = 0; i < n; i++)
      for (j = 0; j< n ; j++)
        {
          double u = gsl_matrix_get(J,i,j);
          double su = gsl_matrix_get(s->J, i, j);
          r = fabs(u - su)/(1e-6 + 1e-6 * fabs(u)); sum+=r;
          if (fabs(u - su) > 1e-6 + 1e-6 * fabs(u))
            printf("broken jacobian %g\n", r);
        }
    printf("avg r = %g\n", sum/(n*n));
  }
#endif

  for (i = 0; i < n ; i++)
    {
      residual += fabs(gsl_vector_get(s->f, i));
    }

  gsl_multiroot_fdfsolver_free (s);
  gsl_matrix_free(J);
  gsl_vector_free(x);

  gsl_test(status, "%s on %s (%g), %u iterations, residual = %.2g", T->name, desc, factor, iter, residual);

  return status;
}

int
test_f (const char * desc, gsl_multiroot_function_fdf * fdf,
        initpt_function initpt, double factor,
        const gsl_multiroot_fsolver_type * T)
{
  int status;
  size_t i, n = fdf->n, iter = 0;
  double residual = 0;

  gsl_vector *x;

  gsl_multiroot_fsolver *s;
  gsl_multiroot_function function;

  function.f = fdf->f;
  function.params = fdf->params;
  function.n = n ;

  x = gsl_vector_alloc (n);

  (*initpt) (x);

  if (factor != 1.0) scale(x, factor);

  s = gsl_multiroot_fsolver_alloc (T, n);
  gsl_multiroot_fsolver_set (s, &function, x);

/*   printf("x "); gsl_vector_fprintf (stdout, s->x, "%g"); printf("\n"); */
/*   printf("f "); gsl_vector_fprintf (stdout, s->f, "%g"); printf("\n"); */

  do
    {
      iter++;
      status = gsl_multiroot_fsolver_iterate (s);

      if (status)
        break ;

      status = gsl_multiroot_test_residual (s->f, 0.0000001);
    }
  while (status == GSL_CONTINUE && iter < 1000);

#ifdef DEBUG
  printf("x "); gsl_vector_fprintf (stdout, s->x, "%g"); printf("\n");
  printf("f "); gsl_vector_fprintf (stdout, s->f, "%g"); printf("\n");
#endif

  for (i = 0; i < n ; i++)
    {
      residual += fabs(gsl_vector_get(s->f, i));
    }

  gsl_multiroot_fsolver_free (s);
  gsl_vector_free(x);

  gsl_test(status, "%s on %s (%g), %u iterations, residual = %.2g", T->name, desc, factor, iter, residual);

  return status;
}


/* bspline/bspline.c
 *
 * Copyright (C) 2006 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 2 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 <gsl/gsl_errno.h>
#include <gsl/gsl_bspline.h>

/*
 * This module contains routines related to calculating B-splines.
 * The algorithms used are described in
 *
 * [1] Carl de Boor, "A Practical Guide to Splines", Springer
 *     Verlag, 1978.
 */

static int bspline_eval_all(const double x, gsl_vector *B, size_t *idx,
                            gsl_bspline_workspace *w);

static inline size_t bspline_find_interval(const double x, int *flag,
                                           gsl_bspline_workspace *w);

/*
gsl_bspline_alloc()
  Allocate space for a bspline workspace. The size of the
workspace is O(5k + nbreak)

Inputs: k      - spline order (cubic = 4)
        nbreak - number of breakpoints

Return: pointer to workspace
*/

gsl_bspline_workspace *
gsl_bspline_alloc(const size_t k, const size_t nbreak)
{
  if (k == 0)
    {
      GSL_ERROR_NULL("k must be at least 1", GSL_EINVAL);
    }
  else if (nbreak < 2)
    {
      GSL_ERROR_NULL("nbreak must be at least 2", GSL_EINVAL);
    }
  else
    {
      gsl_bspline_workspace *w;

      w = (gsl_bspline_workspace *)
          calloc(1, sizeof(gsl_bspline_workspace));
      if (w == 0)
        {
          GSL_ERROR_NULL("failed to allocate space for workspace", GSL_ENOMEM);
        }

      w->k = k;
      w->km1 = k - 1;
      w->nbreak = nbreak;
      w->l = nbreak - 1;
      w->n = w->l + k - 1;

      w->knots = gsl_vector_alloc(w->n + k);
      if (w->knots == 0)
        {
          gsl_bspline_free(w);
          GSL_ERROR_NULL("failed to allocate space for knots vector", GSL_ENOMEM);
        }

      w->deltal = gsl_vector_alloc(k);
      w->deltar = gsl_vector_alloc(k);
      if (!w->deltal || !w->deltar)
        {
          gsl_bspline_free(w);
          GSL_ERROR_NULL("failed to allocate space for delta vectors", GSL_ENOMEM);
        }

      w->B = gsl_vector_alloc(k);
      if (w->B == 0)
        {
          gsl_bspline_free(w);
          GSL_ERROR_NULL("failed to allocate space for temporary spline vector", GSL_ENOMEM);
        }

      return (w);
    }
} /* gsl_bspline_alloc() */

/* Return number of coefficients */
size_t
gsl_bspline_ncoeffs (gsl_bspline_workspace * w)
{
  return w->n;
}

/* Return order */
size_t
gsl_bspline_order (gsl_bspline_workspace * w)
{
  return w->k;
}

/* Return number of breakpoints */
size_t
gsl_bspline_nbreak (gsl_bspline_workspace * w)
{
  return w->nbreak;
}

double
gsl_bspline_breakpoint (size_t i, gsl_bspline_workspace * w)
{
  size_t j = i + w->k - 1;
  return gsl_vector_get(w->knots, j);
}

/*
gsl_bspline_free()
  Free a bspline workspace

Inputs: w - workspace to free

Return: none
*/

void
gsl_bspline_free(gsl_bspline_workspace *w)
{
  if (!w)
    return;

  if (w->knots)
    gsl_vector_free(w->knots);

  if (w->deltal)
    gsl_vector_free(w->deltal);

  if (w->deltar)
    gsl_vector_free(w->deltar);

  if (w->B)
    gsl_vector_free(w->B);

  free(w);
} /* gsl_bspline_free() */

/*
gsl_bspline_knots()
  Compute the knots from the given breakpoints:

knots(1:k) = breakpts(1)
knots(k+1:k+l-1) = breakpts(i), i = 2 .. l
knots(n+1:n+k) = breakpts(l + 1)

where l is the number of polynomial pieces (l = nbreak - 1) and
n = k + l - 1
(using matlab syntax for the arrays)

The repeated knots at the beginning and end of the interval
correspond to the continuity condition there. See pg. 119
of [1].

Inputs: breakpts - breakpoints
        w        - bspline workspace

Return: success or error
*/

int
gsl_bspline_knots(const gsl_vector *breakpts, gsl_bspline_workspace *w)
{
  if (breakpts->size != w->nbreak)
    {
      GSL_ERROR("breakpts vector has wrong size", GSL_EBADLEN);
    }
  else
    {
      size_t i; /* looping */

      for (i = 0; i < w->k; ++i)
        gsl_vector_set(w->knots, i, gsl_vector_get(breakpts, 0));

      for (i = 1; i < w->l; ++i)
        {
          gsl_vector_set(w->knots, w->k - 1 + i,
                         gsl_vector_get(breakpts, i));
        }

      for (i = w->n; i < w->n + w->k; ++i)
        gsl_vector_set(w->knots, i, gsl_vector_get(breakpts, w->l));

      return GSL_SUCCESS;
    }
} /* gsl_bspline_knots() */

/*
gsl_bspline_knots_uniform()
  Construct uniformly spaced knots on the interval [a,b] using
the previously specified number of breakpoints. 'a' is the position
of the first breakpoint and 'b' is the position of the last
breakpoint.

Inputs: a - left side of interval
        b - right side of interval
        w - bspline workspace

Return: success or error

Notes: 1) w->knots is modified to contain the uniformly spaced
          knots

       2) The knots vector is set up as follows (using octave syntax):

          knots(1:k) = a
          knots(k+1:k+l-1) = a + i*delta, i = 1 .. l - 1
          knots(n+1:n+k) = b
*/

int
gsl_bspline_knots_uniform(const double a, const double b,
                          gsl_bspline_workspace *w)
{
  size_t i;     /* looping */
  double delta; /* interval spacing */
  double x;

  delta = (b - a) / (double) w->l;

  for (i = 0; i < w->k; ++i)
    gsl_vector_set(w->knots, i, a);

  x = a + delta;
  for (i = 0; i < w->l - 1; ++i)
    {
      gsl_vector_set(w->knots, w->k + i, x);
      x += delta;
    }

  for (i = w->n; i < w->n + w->k; ++i)
    gsl_vector_set(w->knots, i, b);

  return GSL_SUCCESS;
} /* gsl_bspline_knots_uniform() */

/*
gsl_bspline_eval()
  Evaluate the basis functions B_i(x) for all i. This is
basically a wrapper function for bspline_eval_all()
which formats the output in a nice way.

Inputs: x - point for evaluation
        B - (output) where to store B_i(x) values
            the length of this vector is
            n = nbreak + k - 2 = l + k - 1 = w->n
        w - bspline workspace

Return: success or error

Notes: The w->knots vector must be initialized prior to calling
       this function (see gsl_bspline_knots())
*/

int
gsl_bspline_eval(const double x, gsl_vector *B,
                 gsl_bspline_workspace *w)
{
  if (B->size != w->n)
    {
      GSL_ERROR("B vector length does not match n", GSL_EBADLEN);
    }
  else
    {
      size_t i;     /* looping */
      size_t idx = 0;
      size_t start; /* first non-zero spline */

      /* find all non-zero B_i(x) values */
      bspline_eval_all(x, w->B, &idx, w);

      /* store values in appropriate part of given vector */

      start = idx - w->k + 1;
      for (i = 0; i < start; ++i)
        gsl_vector_set(B, i, 0.0);

      for (i = start; i <= idx; ++i)
        gsl_vector_set(B, i, gsl_vector_get(w->B, i - start));

      for (i = idx + 1; i < w->n; ++i)
        gsl_vector_set(B, i, 0.0);

      return GSL_SUCCESS;
    }
} /* gsl_bspline_eval() */

/****************************************
 *          INTERNAL ROUTINES           *
 ****************************************/

/*
bspline_eval_all()
  Evaluate all non-zero B-splines B_i(x) using algorithm (8)
of chapter X of [1]

The idea is something like this. Given x \in [t_i, t_{i+1}]
with t_i < t_{i+1} and the t_i are knots, the values of the
B-splines not automatically zero fit into a triangular
array as follows:
                         0
            0
0                        B_{i-2,3}
            B_{i-1,2}
B_{i,1}                  B_{i-1,3}       ...
            B_{i,2}
0                        B_{i,3}
            0
                         0

where B_{i,k} is the ith B-spline of order k. The boundary 0s
indicate that those B-splines not in the table vanish at x.

To compute the non-zero B-splines of a given order k, we use
Eqs. (4) and (5) of chapter X of [1]:

(4) B_{i,1}(x) = { 1, t_i <= x < t_{i+1}
                   0, else }

(5) B_{i,k}(x) =     (x - t_i)
                 ----------------- B_{i,k-1}(x)
                 (t_{i+k-1} - t_i)

                      t_{i+k} - x
                 + ----------------- B_{i+1,k-1}(x)
                   t_{i+k} - t_{i+1}

So (4) gives us the first column of the table and we can use
the recurrence relation (5) to get the rest of the columns.

Inputs: x   - point at which to evaluate splines
        B   - (output) where to store B-spline values (length k)
        idx - (output) B-spline function index of last output
              value (B_{idx}(x) is stored in the last slot of 'B')
        w   - bspline workspace

Return: success or error

Notes: 1) the w->knots vector must be initialized before calling
          this function

       2) On output, B contains:

       B = [B_{i-k+1,k}, B_{i-k+2,k}, ..., B_{i-1,k}, B_{i,k}]

          where 'i' is stored in 'idx' on output

       3) based on PPPACK bsplvb
*/

static int
bspline_eval_all(const double x, gsl_vector *B, size_t *idx,
                 gsl_bspline_workspace *w)
{
  if (B->size != w->k)
    {
      GSL_ERROR("B vector not of length k", GSL_EBADLEN);
    }
  else
    {
      size_t i;  /* spline index */
      size_t j;  /* looping */
      size_t ii; /* looping */
      int flag = 0;  /* error flag */
      double saved;
      double term;

      i = bspline_find_interval(x, &flag, w);

      if (flag == -1)
        {
          GSL_ERROR("x outside of knot interval", GSL_EINVAL);
        }
      else if (flag == 1)
        {
          if (x <= gsl_vector_get(w->knots, i) + GSL_DBL_EPSILON)
            {
              --i;
            }
          else
            {
              GSL_ERROR("x outside of knot interval", GSL_EINVAL);
            }
        }

      *idx = i;

      gsl_vector_set(B, 0, 1.0);

      for (j = 0; j < w->k - 1; ++j)
        {
          gsl_vector_set(w->deltar, j,
                         gsl_vector_get(w->knots, i + j + 1) - x);
          gsl_vector_set(w->deltal, j,
                         x - gsl_vector_get(w->knots, i - j));

          saved = 0.0;

          for (ii = 0; ii <= j; ++ii)
            {
              term = gsl_vector_get(B, ii) /
                     (gsl_vector_get(w->deltar, ii) +
                      gsl_vector_get(w->deltal, j - ii));

              gsl_vector_set(B, ii,
                             saved +
                             gsl_vector_get(w->deltar, ii) * term);

              saved = gsl_vector_get(w->deltal, j - ii) * term;
            }

          gsl_vector_set(B, j + 1, saved);
        }

      return GSL_SUCCESS;
    }
} /* bspline_eval_all() */

/*
bspline_find_interval()
  Find knot interval such that

  t_i <= x < t_{i + 1}

where the t_i are knot values.

Inputs: x    - x value
        flag - (output) error flag
        w    - bspline workspace

Return: i (index in w->knots corresponding to left limit of interval)

Notes: The error conditions are reported as follows:

Condition             Return value        Flag
---------             ------------        ----
x < t_0                    0               -1
t_i <= x < t_{i+1}         i                0
t_{n+k-1} <= x           l+k-1             +1
*/

static inline size_t
bspline_find_interval(const double x, int *flag,
                      gsl_bspline_workspace *w)
{
  size_t i;

  if (x < gsl_vector_get(w->knots, 0))
    {
      *flag = -1;
      return 0;
    }

  /* find i such that t_i <= x < t_{i+1} */
  for (i = w->k - 1; i < w->k + w->l - 1; ++i)
    {
      const double ti = gsl_vector_get(w->knots, i);
      const double tip1 = gsl_vector_get(w->knots, i + 1);

      if (tip1 < ti)
        {
          GSL_ERROR("knots vector is not increasing", GSL_EINVAL);
        }

      if (ti <= x && x < tip1)
        break;
    }

  if (i == w->k + w->l - 1)
    *flag = 1;
  else
    *flag = 0;

  return i;
} /* bspline_find_interval() */


#ifndef NumericalApproximation_h
#define NumericalApproximation_h
/** This is free and unencumbered software released into the public domain.
The authors of ISIS do not claim copyright on the contents of this file.
For more details about the LICENSE terms and the AUTHORS, you will
find files of those names at the top level of this repository. **/

/* SPDX-License-Identifier: CC0-1.0 */

#include <string>
#include <vector>
#include <map>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>
#include "IException.h"

using namespace std;

namespace Isis {
  /**
   * @brief NumericalApproximation provides various numerical
   *        analysis methods of interpolation, extrapolation and
   *        approximation of a tabulated set of @a x, @a y data.
   *
   * This class contains a merged version of the Isis classes
   * @b DataInterp and @b NumericalMethods.  In addition,
   * some methods from @b AtmosModel were moved to this class, the
   * @a CubicNeighborhood interpolation type was
   * adapted from the IDL routine called @b interpol (using the
   * "/spline" keyword), and several differentiation and
   * integration approximation methods were created.
   *
   * NumericalApproximation contains 9 types of data interpolation
   * routines.  These routines act on x, y pairs of data points.
   * The following forms of data interpolation are supported:
   *
   *

   *

   *     Name
   *     Type (enum)
   *     MinPoints
   *     Description
   *

   *

   *     Linear
   *     @a Linear
   *     2
   *
   *        Linear interpolation approximates a curve by
   *        concatinating line segments between known data
   *        points.  This results in a continuous curve with a
   *        discontinuities of the derivative at the known data
   *        points.  This interpolation type uses the GSL
   *        library routines.
   *
   *
   *

   *

   *     Polynomial
   *     @a Polynomial
   *     3
   *
   *        Computes a polynomial interpolation.  This method
   *        should only be used for interpolating small numbers
   *        of points because polynomial interpolation introduces
   *        large oscillations, even for well-behaved datasets.
   *        The number of terms in the interpolating polynomial
   *        is equal to the number of points.  This interpolation
   *        type uses the GSL library routines.
   *
   *

   *

   *     Neville's Algorithm for Polynomial Interpolation
   *     @a PolynomialNeville
   *     3
   *
   *     Polynomial interpolation using Neville's algorithm. This
   *     first fits a polynomial of degree 0 through each point.
   *     On the second iteration, the polynomial of adjoining
   *     indices are combined to fit through pairs of points.
   *     This process repeats until a pyramid of approximations is
   *     reached. This is based on the Newton form of the
   *     interpolating polynomial and the recursion relation of
   *     the divided differences.
   *
   *

   *

   *     Natural Cubic Spline
   *     @a CubicNatural
   *     3
   *
   *       Cubic spline with natural boundary conditions.  The
   *       resulting curve is piecewise cubic on each interval
   *       with matching first and second derivatives at the
   *       supplied data points.  The second derivative is chosen
   *       to be zero at the endpoints, i.e. the first and last
   *       points of the data set. This is also refered to as
   *       free boundary condtions.  This interpolation type uses
   *       the GSL
   *        library routines.
   *
   *

   *

   *     Clamped Cubic Spline
   *     @a CubicClamped
   *     3
   *
   *     Cubic Spline interpolation with clamped boundary
   *     conditions.  The resulting curve is piecewise cubic on
   *     each interval.  For this type of boundary condition to
   *     hold, it is necessary to have either the values of the
   *     derivative at the endpoints or an accurate approximation
   *     to those values. In general, clamped boundary conditions
   *     lead to more accurate approximations since they include
   *     more information about the function.
   *
   *

   *

   *     Periodic Natural Cubic Spline
   *     @a CubicNatPeriodic
   *     2
   *
   *       Cubic spline with periodic boundary conditions.  The
   *       resulting curve is piecewise cubic on each interval
   *       with matching first and second derivatives at the
   *       supplied data points.  The derivatives at the first
   *       and last points are also matched.  Note that the last
   *       point in the data must have the same y-value as the
   *       first point, otherwise the resulting periodic
   *       interpolation will have a discontinuity at the
   *       boundary.  This interpolation type uses the GSL
   *       library routines.
   *
   *

   *

   *     Cubic Spline about 4-point Neighborhood
   *     @a CubicNeighborhood
   *     4
   *
   *       Cubic Spline interpolation using 4-pt Neighborhoods
   *       with natural boundary conditions. This interpolation
   *       type is adapted from the IDL interpol.pro application
   *       using the "/spline" keyword on an irregular grid.
   *       This type of cubic spline fits a natural cubic spline
   *       to the 4-point neighborhood of known data points
   *       surrounding the @a x value at which we wish to
   *       evaluate. For example, suppose {@a x0,
   *       @a x1, ..., @a xn} is the
   *       array of known domain values in the data set and @f$
   *       x_i \leq u < x_{i+1} @f$ for some @e i such that
   *       @f$ 0 \leq i \leq n @f$, then to evaluate the
   *       @a y value at @e u, this method of interpolation
   *       evaluates the ordinary cubic spline consisting of the
   *       data set {@a xi-1, @a xi,
   *       @a xi+1, @a xi+2} at
   *       @e u.
   *
   *

   *

   *     Cubic Spline using Hermite cubic polynomial
   *     @a CubicHermite
   *     2
   *
   *       Cubic Spline interpolation using the Hermite cubic
   *       polynomial (also called the unique Hermite
   *       interpolating fundamental polynomial of degree 3).
   *       This method requires the input of the slopes of the
   *       tangent lines of the curve (i.e. velocities, or first
   *       derivatives) at each of the data points.  This ensures
   *       that the approximation has the same "shape" as the
   *       curve at the known data points. This interpolation type
   *       uses piecewise Hermite cubic polynomials for each
   *       interval, (@a x0, @a x1), using
   *       the known data points, (@a
   *       x0,@a f(@a x0)) and
   *       (@a x1,@a f(@a x1)), and their
   *       derivatives,
   *       @a f '(@a x0) and @a f '(@a
   *       x1). The Hermite cubic polynomial is defined
   *       @f[ H_3(x) =
   *       f(x_0)h_0(x)+f(x_1)h_1(x)
   *       +f\prime(x_0)\hat{h}_0(x)+f\prime(x_1)\hat{h}_1(x)
   *       @f]
   *       where
   *       @f$ h_k(x) =
   *       [1-2(x-x_k)L\prime_k(x_k)](L_k(x))^2
   *       @f$
   *       and
   *       @f$ \hat{h}_k(x) =
   *       (x-x_k)(L_k(x))^2
   *       @f$
   *       for the kth-Lagrange coefficient polynomials of degree
   *       n = 1,
   *       @f$ L_0(x) = \frac{x- x_1}{x_0-x_1}@f$ and
   *       @f$ L_1(x) = \frac{x- x_0}{x_1-x_0}@f$.
   *
   *
   *

   *

   *     Akima
   *     @a Akima
   *     5
   *
   *       Non-rounded Akima spline with natural boundary
   *       conditions.  This method uses non-rounded corner
   *       algorithm of Wodicka. This interpolation type uses the
   *       GSL library routines.
   *
   *

   *

   *     Periodic Akima
   *     @a AkimaPeriodic
   *     5
   *
   *       Non-rounded Akima spline with periodic boundary
   *       conditions.  This method uses non-rounded corner
   *       algorithm of Wodicka.  This interpolation type uses
   *       the GSL library routines.
   *
   *

   *

   *
   *
   * Numerical analysis approximation methods for differentiating
   * and integrating unknown functions represented by a data set
   * are implemented by using the interpolations on the data set
   * created with a NumericalApproximation object. The Lagrange polynomial,
   *       @f[ L(x) =
   *       \sum_{k=0}^{n} \left(f(x_k) \prod_{i=0,i \neq k}^{n}
   *            \frac{x- x_i}{x_k-x_i}\right)
   *       @f]
   * is used to determine formulas for numerical differentiation
   * and integration.
   * @n
   * @b Numerical Differentiation
   * @n
   * The differentiation methods use difference formulas to
   * approximate first and second derivatives at a domain value
   * for a given a set of known (@a x,@a y) data points. Once all
   * of the data points are added, a Isis::NumericalApproximation
   * interpolation is used on the dataset to estimate the
   * function, @f$f:x \to y@f$, mapping @a x values to
   * corresponding @a y values. Then, the derivative of
   * @e f evaluated at @a x0 is approximated.
   * To do so, a uniform step size of @a h is chosen to
   * determine the distance between @a xi and @a
   * xi+1.
   *

   *
 Differentiation methods require the parameters
   *     @a a (domain value at which the derivative will be
   *     evaluated), @a n (number of points used in the difference
   *     formula), and @a h (step size of points). Note: @a a
   *     is denoted @a x0 in the above formulas.
   *

   *

   *     Numerical Differentiation Type
   *     Difference Formulas
   *     Methods Available
   *     Description
   *

   *

   *     Backward Difference
   *
   *

   *
 2-point backward difference.
   *           @f[
   *                f\prime(x_0) \approx \frac{1}{h}[f(x_0) - f(x_0 - h)]
   *           @f]
   *
 3-point backward difference.
   *           @f[
   *                f\prime(x_0) \approx \frac{1}{2h}[3f(x_0) - 4f(x_0 - h) +
   *                f(x_0 - 2h)]
   *           @f]
   *
 3-point backward second difference.
   *           @f[
   *                f\prime\prime(x_0) \approx \frac{1}{h^2}[f(x_0)
   *                - 2f(x_0 - h) + f(x_0 - 2h)]
   *           @f]
   *

   *
   *
   *

   *
BackwardFirstDifference()
   *
BackwardSecondDifference()
   *

   *
   *
   *       Backward difference formulas use a uniform step-size
   *       moving in the negative direction from the
   *       given @a x0. These formulas are derived
   *       by differentiating the Lagrange polynomials for
   *       @a xi = @a x0 - @a ih
   *       and evaluating them at@a x0.
   *
   *

   *

   *     Forward Difference
   *
   *

   *
 2-point forward difference.
   *           @f[
   *                f\prime(x_0) \approx \frac{1}{h}[f(x_0 + h) - f(x_0)]
   *           @f]
   *
 3-point forward difference.
   *           @f[
   *                f\prime(x_0) \approx \frac{1}{2h}[-f(x_0 + 2h) +
   *                4f(x_0 + h) - 3f(x_0)]
   *           @f]
   *
 3-point forward second difference.
   *           @f[
   *                f\prime\prime(x_0) \approx \frac{1}{h^2}[f(x_0 +
   *                2h) - 2f(x_0 + h) + f(x_0)]
   *           @f]
   *

   *
   *
   *

   *
ForwardFirstDifference()
   *
ForwardSecondDifference()
   *

   *
   *
   *       Forward difference formulas use a uniform step-size
   *       moving in the positive direction from
   *       @a x0. These formulas are derived by
   *       differentiating the Lagrange polynomials for
   *       @a xi = @a x0 + @a ih
   *       and evaluating them at@a x0.
   *
   *

   *

   *     Center Difference
   *
   *

   *
 3-point center difference.
   *           @f[
   *                f\prime(x_0) \approx \frac{1}{2h}[f(x_0 + h) -
   *                f(x_0 - h)]
   *           @f]
   *
 5-point center difference.
   *           @f[
   *                f\prime(x_0) \approx \frac{1}{12h}[-f(x_0 + 2h) +
   *                8f(x_0 +h) - 8f(x_0 - h) + f(x_0 - 2h)]
   *           @f]
   *
 3-point center second difference.
   *           @f[
   *                f\prime\prime(x_0) \approx \frac{1}{h^2}[f(x_0 + h)
   *                - 2f(x_0) + f(x_0 - h)]
   *           @f]
   *
 5-point center second difference.
   *           @f[
   *                f\prime\prime(x_0) \approx \frac{1}{12h^2}[-f(x_0 +
   *                2h) + 16f(x_0 +h) - 30f(x_0) + 16f(x_0 - h) - f(x_0
   *                - 2h)]
   *           @f]
   *

   *
   *
   *

   *
CenterFirstDifference()
   *
CenterSecondDifference()
   *

   *
   *
   *       Center difference formulas use a uniform step-size
   *       moving in both directions from @a x0 so this
   *       point stays centered. These formulas are derived by
   *       differentiating the Lagrange polynomials for
   *       @a xi = @a x0
   *       + (@e i - (@a n - 1)/2)@a h and evaluating them
   *       at @a x0.
   *
   *

   *

   *     GSL Differentiation
   *      Unknown
   *
   *

   *
GslFirstDerivative()
   *
GslSecondDerivative()
   *

   *
   *
   *       No documentation was found about the algorithms used
   *       for these methods.  They may only be called when
   *       interpolation type is one of the following: @a Linear,
   *       @a Polynomial, @a CubicNatural, @a CubicNatPeriodic, @a Akima, or
   *       @a AkimaPeriodic.
   *
   *

   *

   * @n
   * @b Numerical Integration
   * @n
   * The integration methods were derived using
   * @b Newton-Cotes, or @a quadrature, formulas for
   * approximating integrals given a set of known (@a x,@a y)
   * data points.  The @a x values may be irregularly spaced, but
   * must be unique and added to the dataset in ascending order.
   * Once all of the data points are added, a
   * Isis::NumericalApproximation interpolation is used on
   * the dataset to estimate the function, @f$f:x \to y@f$,
   * mapping @a x values to corresponding @a y values.
   * Then, the integral of @e f on the interval from @a a
   * to @a b is approximated. To do so, an algorithm for creating
   * a composite formula by applying the original formula to
   * segments that share a boundary point is used. The
   * NumericalApproximation::InterpType chosen for
   * interpolation computation seems to have little affect on the
   * error between the actual integral and the return values of
   * the integration methods. The BoolesRule() method varies the
   * most in error. Although errors are not high for any of the
   * interpolation types,
   * @a CubicNatural spline interpolations seem to
   * have the smallest error most often with BoolesRule(). For any
   * other numerical integration method, the errors appear to be
   * identical for any NumericalApproximation::InterpType except
   * @a Polynomial, which usually has a slightly larger
   * error than the other interpolation types. Note: A portion of
   * this algorithm is derived from the IDL function int_tabulated
   * for Boole's Method.
   *

   *
 Integration methods require the parameters @a a,
   *     @a b (interval of domain over which to integrate).
   *

   *

   *     Numerical Integration Type
   *     Integration Formulas
   *     Methods Available
   *     Description
   *

   *

   *     Trapezoidal Rule
   *
   *

   *
 2-point Newton-Cotes trapezoidal rule:
   *            @f[
   *           \int_{a}^b f(x)dx \approx \frac{h}{2}[f(a) + f(b)]
   *            @f]
   *         where @e h = @a b - @a a.
   *

   *
   *
   *

   *
TrapezoidalRule()
   *

   *
   *
   *       The 2-point closed rule, known as the trapezoidal
   *       rule, uses straight line segments between known data
   *       points to estimate the area under the curve (integral).
   *       This Newton-Cotes formula uses a uniform step-size of
   *       @e h = @a b - @a a and is derived by integrating the
   *       Lagrange polynomials over the closed interval [@a a,
   *       @a a+h] for @a xi =
   *       @a x0 + @e ih.
   *
   *

   *

   *     Simpson's Rule, or Simpson's 3-Point Rule
   *
   *

   *
 3-point Newton-Cotes, Simpson's Rule
   *           @f[
   *             \int_{a}^b f(x)dx \approx \frac{h}{3}[f(a) +
   *               4f(a+h) + f(a+2h)]
   *           @f]
   *           where @e h = (@a b - @a a)/2.
   *

   *
   *
   *

   *
Simpsons3PointRule()
   *

   *
   *
   *       The 3-point closed rule, known as Simpson's Rule or
   *       Simpson's 3-Point Rule, uses parabolic arcs between
   *       known data points to estimate the area under the curve
   *       (integral). This Newton-Cotes formula uses a uniform
   *       step-size of @e h = (@a b - @a a)/2 and is derived by
   *       integrating the Lagrange polynomials over the closed
   *       interval [@a a, @a a+2h] for @a xi =
   *       @a x0 + @e ih.
   *
   *

   *

   *     Simpson's 3/8 Rule, or Simpson's 4-Point Rule
   *
   *

   *
 4-point Newton-Cotes, Simpson's 3/8 Rule
   *           @f[
   *             \int_{a}^b f(x)dx \approx \frac{3h}{8}[f(a) +
   *                 3f(a+h) + 3f(a+2h) + f(a+3h)]
   *           @f]
   *           where @e h = (@a b - @a a)/3.
   *

   *
   *
   *

   *
Simpsons4PointRule()
   *

   *
   *
   *       The 4-point closed rule, known as Simpson's 3/8 Rule or
   *       Simpson's 4-Point Rule, uses cubic curves between
   *       known data points to estimate the area under the curve
   *       (integral). This Newton-Cotes formula uses a uniform
   *       step-size of @e h = (@a b - @a a)/3 and is derived by
   *       integrating the Lagrange polynomials over the closed
   *       interval [@a a, @a a+3h] for @a xi =
   *       @a x0 + @e ih.
   *
   *

   *

   *     Boole's Rule
   *
   *

   *
 5-point Newton-Cotes, Boole's Rule
   *           @f[
   *             \int_{a}^b f(x)dx \approx \frac{2h}{45}[7f(a) +
   *                 32f(a+h) + 12f(a+2h) + 32f(a+3h) + 7f(a+4h)]
   *           @f]
   *           where @e h = (@a b - @a a)/4.
   *

   *
   *
   *

   *
BoolesRule()
   *

   *
   *
   *       The 5-point closed rule, known as Boole's Rule, uses
   *       quartic curves between known data points to estimate
   *       the area under the curve (integral). This Newton-Cotes
   *       formula uses a uniform step-size of @e h = (@a b -
   *       @a a)/4 and is derived by integrating the Lagrange
   *       polynomials over the closed interval [@a a, @a a+4h]
   *       for @a xi = @a x0 +
   *       @a ih.
   *
   *

   *

   *     Refinements of Extended Trapezoidal Rule
   *
   *

   *
 Extended closed trapezoidal rule
   *           @f[
   *             \int_{a}^b f(x)dx \approx h[\frac12 f(x_0) +
   *                 f(x_1) + ... + f(x_{n-2} + \frac12
   *                 f(x_{n-1})]
   *           @f]
   *           where @e h = (@a b - @a a)/4, @a x0 =
   *           @a a, and @a xn-1 = @a b.
   *

   *
   *
   *

   *
RefineExtendedTrap()
   *

   *
   *
   *       The extended (or composite) trapezoidal rule can be
   *       used to with a series of refinements to approximate the
   *       integral.  The first stage of refinement returns the
   *       ordinary trapezoidal estimate of the integral.
   *       Subsequent stages will improve the accuracy, where, for
   *       the @a nth stage 2@a n-2  interior
   *       points are added.
   *
   *

   *

   *     Romberg's Method
   *
   *

   *
 Romberg's Method for Integration
   *

   *
   *

   *
RombergsMethod()
   *

   *
   *
   *       Romberg's Method is a potent numerical integration
   *       tool. It uses the extended trapezoidal rule and
   *       Neville's algorithm for polynomial interpolation.
   *
   *

   *

   *     GSL Integration
   *      Unknown
   *
   *

   *
GslIntegral()
   *

   *
   *
   *       No documentation was found about the algorithm used for
   *       this methods.  They may only be called when
   *       interpolation type is one of the following: @a Linear,
   *       @a Polynomial, @a CubicNatural, @a CubicNatPeriodic, @a Akima, or
   *       @a AkimaPeriodic.
   *
   *

   *

   *
   *
   * Below is an example demonstating the use of this class:
   * @code
   *    inline double f(double x) { return (x + cos ((x * x))); }
   *
   *    NumericalApproximation
   *    spline(NumericalApproximation::CubicClamped);
   *    for (int x = 0; x < 200 ; x++) {
   *      spline.addPoint(x, f(x));
   *    }
   *    spline.SetCubicClampedEndptDeriv(0,0);
   *    double yinterp = spline.Evaluate(50.7);
   *    double yextrap =
   *        spline.Evaluate(201,NumericalApproximation::Extrapolate);
   *    double deriv = spline.CenterFirstDifference(10);
   *    double integ =
   *        spline.RombergsMethod(spline.DomainMinimum(),spline.DomainMaximum());
   * @endcode
   *
   * To compute the same data set using the Akima spline, just use the following:
   * @code
   *    spline.Reset(NumericalApproximation::Akima);
   *    double yinterp = spline.Evaluate(50.7);
   *    double yextrap =
   *        spline.Evaluate(201,NumericalApproximation::NearestEndpoint);
   *    double deriv = spline.CenterFirstDifference(10);
   *    double integ =
   *        spline.RombergsMethod(spline.DomainMinimum(),spline.DomainMaximum());
   * @endcode
   *
   *
Caveats

   * When using this class, there are some important details that
   * require consideration.  Several interpolation types of this
   * class use the GSL library. In addition, the GSL library
   * default error handling scheme has implications when used in
   * C++ objects.  The default behavior is to terminate the
   * application when an error occurs.  Options for developers are
   * to turn off error trapping within the GSL, which means users
   * of the library must do their own error checking, or a
   * specific handler can be specified to replace the default
   * mode.  Neither option is well suited in object-oriented
   * implementations because they apply globally, to all users of
   * the library. The approach used here is to turn off error
   * handling and require users to handle their own error
   * checking.  This is not entirely safe as any user that does
   * not follow this strategy could change this behavior at
   * runtime.  Other options should be explored. An additional
   * option is that the default behavior can be altered prior to
   * building GSL but this is also problematic since this behavior
   * cannot be guaranteed universally.
   *
   * For these types, interpolation is strictly adhered to and
   * extrapolation is not handled well. If the input to be
   * evaluated is outside of the
   * minimum and maximum @a x values of the original data
   * points, then the @a y value corresponding to the
   * @a x value that is nearest to that input is returned.
   * However, for sufficiently close values, the clamped cubic and
   * polynomial Neville's types are fairly accurate in their
   * extrapolation techniques.  All differentiation and
   * integration methods throw an error if the value passed is
   * outside of the domain.
   *
   * @ingroup Math
   * @author 2008-11-05 Janet Barrett, Kris Becker, K Teal
   *         Thompson, Jeannie Walldren
   * @internal
   *   @history 1999-08-11 K Teal Thompson - Original version of
   *            NumericalMethods subroutines in Isis2.
   *   @history 2006-06-14 Kris Becker - Created DataInterp class
   *   @history 2007-02-20 Janet Barrett - Created NumericalMethods
   *            class from Isis2 subroutines
   *   @history 2007-02-20 Janet Barrett - Created AtmosModel class
   *            from Isis2 subroutines
   *   @history 2008-06-18 Christopher Austin - Fixed documentation
   *            for DataInterp
   *   @history 2008-06-18 Steven Koechle - Updated NumericalMethods
   *            unitTest.
   *   @history 2008-11-05 Jeannie Walldren - Merged DataInterp
   *            class, NumericalMethods class, and methods from
   *            AtmosModel.  Modified methods from various classes
   *            to be able to function similarly. Added InterpType
   *            @a CubicNeighborhood, difference formula methods, and
   *            integration methods. Created new unitTest.
   *   @history 2008-11-12 Jeannie Walldren - Fixed documentation.
   *   @history 2008-12-18 Jeannie Walldren - Added address
   *            operator (&) to input variables of type vector,
   *            NumericalApproximation::InterpType, and
   *            NumericalApproximation::ExtrapType in
   *            Constructors, AddData(), Evaluate(),
   *            ValueToExtrapolate(), EvaluateCubicNeighborhood(),
   *   @history 2009-01-26 Jeannie Walldren - Fixed error
   *            documentation.
   *   @history 2009-02-12 Jeannie Walldren - Fixed documentation
   *            to include weblinks.
   *   @history 2009-07-17 Steven Lambright - Added algorithm include,
   *            removed macro MAX
   *   @history 2009-06-10 Jeannie Walldren - Added interpolation
   *            type CubicHermite.  This involved modifying some
   *            methods, as well as adding variable p_fprimeOfx
   *            and methods AddCubicHermiteDeriv() and
   *            EvaluateCubicHermite().  Added Contains() method.
   *   @history 2009-07-02 Jeannie Walldren - Replaced Hermite
   *            interpolation algorithm with simpler version and
   *            added methods EvaluateCubicHermiteFirstDeriv and
   *            EvaluateCubicHermiteSecDeriv
   *   @history 2009-07-09 Debbie Cook - Finished Jeannie's
   *            modifications.
   *   @history 2009-08-03 Jeannie Walldren - Clean up code,
   *            documentation and check in changes from
   *            2009-06-10, 2009-07-02, 2009-07-09
   *   @history 2010-07-21 Sharmila Prasad - Remove doxygen documentation
   *            warnings
   *   @history 2010-12-06 Steven Lambright - Optimized AddData(vector, vector),
   *            AddCubicHermiteDeriv(vector), and Init() which causes a
   *            very significant increase in performance when constructing a lot
   *            or adding a lot of data.
   */

  class NumericalApproximation {
    public:
      /**
       * This enum defines the types of interpolation supported in this class
       */
      enum InterpType { Linear,               //!< Linear interpolation.
                        Polynomial,           //!< Polynomial interpolation.
                        PolynomialNeville,    //!< Polynomial interpolation using Neville's algorithm.
                        CubicNatural,         //!< Cubic Spline interpolation with natural boundary conditions.
                        CubicClamped,         //!< Cubic Spline interpolation with clamped boundary conditions.
                        CubicNatPeriodic,     //!< Cubic Spline interpolation with periodic boundary conditions.
                        CubicNeighborhood,    //!< Cubic Spline interpolation using 4-pt Neighborhoods with natural boundary conditions.
                        CubicHermite,         //!< Cubic Spline interpolation using the Hermite cubic polynomial.
                        Akima,                //!< Non-rounded Akima Spline interpolation with natural boundary conditions.
                        AkimaPeriodic         //!< Non-rounded Akima Spline interpolation with periodic boundary conditions.
                      };

      // CONSTRUCTORS
      NumericalApproximation(const NumericalApproximation::InterpType &itype = CubicNatural);
      NumericalApproximation(unsigned int n, double *x, double *y,
                             const NumericalApproximation::InterpType &itype = CubicNatural);
      NumericalApproximation(const vector <double> &x, const vector <double> &y,
                             const NumericalApproximation::InterpType &itype = CubicNatural);
      NumericalApproximation(const NumericalApproximation &dint);
      // ASSIGNMENT OPERATOR
      NumericalApproximation &operator=(const NumericalApproximation &numApMeth);
      // DESTRUCTOR
      virtual ~NumericalApproximation();

      // ACCESSOR METHODS FOR OBJECT PROPERTIES
      string Name() const;
      /**
       * @brief Returns the enumerated type of interpolation chosen
       *
       * This method can be selected after all the points are added in the
       * interpolation by using Compute() method.
       * Note that this prints out as an integer representaion of the
       * enumerated type:
       *

       *
 Linear             = 0
       *
 Polynomial         = 1
       *
 PolynomialNeville  = 2
       *
 CubicNatural       = 3
       *
 CubicClamped       = 4
       *
 CubicNatPeriodic   = 5
       *
 CubicNeighborhood  = 6
       *
 CubicHermite       = 7
       *
 Akima              = 8
       *
 AkimaPeriodic      = 9
       *

       *
       * @return NumericalApproximation::InterpType Currently assigned
       *         interpolation type
       */
      inline InterpType   InterpolationType() {
        return (p_itype);
      }
      int MinPoints();
      int MinPoints(NumericalApproximation::InterpType itype);

      // ADD DATA TO OBJECT
      void AddData(const double x, const double y);
      void AddData(unsigned int n, double *x, double *y);
      void AddData(const vector <double> &x, const vector <double> &y);
      void SetCubicClampedEndptDeriv(const double yp1, const double ypn);
      void AddCubicHermiteDeriv(unsigned int n, double *fprimeOfx);
      void AddCubicHermiteDeriv(const vector <double> &fprimeOfx);
      void AddCubicHermiteDeriv(const double fprimeOfx);

      //ACCESSOR METHODS AFTER DATA IS ENTERED
      double DomainMinimum();
      double DomainMaximum();
      bool Contains(double x);
      /**
       * Returns the number of the coordinates added to the data set.
       *
       * @return @b unsigned @b int Size of data set.
       */
      inline unsigned int Size() {
        return(p_x.size());
      }

      /**
      * This enum defines the manner in which a value outside of the
      * domain should be handled if passed to the Evaluate() method.
      */
      enum ExtrapType { ThrowError,     //!< Evaluate() throws an error if @a a is outside of the domain.
                        Extrapolate,    //!< Evaluate() attempts to extrapolate if @a a is outside of the domain.
                        //!< This is only valid for NumericalApproximation::InterpType @a CubicClamped or @a PolynomialNeville
                        //!< and the result will be accurate only if sufficiently close to the domain boundary.
                        NearestEndpoint //!< Evaluate() returns the y-value of the nearest endpoint if @a a is outside of the domain.
                      };
      // INTERPOLATION AND EXTRAPOLATION RESULTS
      double          Evaluate(const double          a, const ExtrapType &etype = ThrowError);
      vector <double> Evaluate(const vector <double> &a, const ExtrapType &etype = ThrowError);
      vector <double> PolynomialNevilleErrorEstimate();
      vector <double> CubicClampedSecondDerivatives();
      double EvaluateCubicHermiteFirstDeriv(const double a);
      double EvaluateCubicHermiteSecDeriv(const double a);

      // DIFFERENTIATION METHODS
      double GslFirstDerivative(const double a);
      double BackwardFirstDifference(const double a, const unsigned int n = 3,
                                     const double h = 0.1);
      double ForwardFirstDifference(const double a, const unsigned int n = 3,
                                    const double h = 0.1);
      double CenterFirstDifference(const double a, const unsigned int n = 5,
                                   const double h = 0.1);

      double GslSecondDerivative(const double a);
      double BackwardSecondDifference(const double a, const unsigned int n = 3,
                                      const double h = 0.1);
      double ForwardSecondDifference(const double a, const unsigned int n = 3,
                                     const double h = 0.1);
      double CenterSecondDifference(const double a, const unsigned int n = 5,
                                    const double h = 0.1);

      // INTERGRATION METHODS
      double GslIntegral(const double a, const double b);
      double TrapezoidalRule(const double a, const double b);
      double Simpsons3PointRule(const double a, const double b);
      double Simpsons4PointRule(const double a, const double b);
      double BoolesRule(const double a, const double b);
      double RefineExtendedTrap(double a, double b, double s, unsigned int n);
      double RombergsMethod(double a, double b);

      // ASSIGNMENT OPERATORS
      void Reset();
      void Reset(NumericalApproximation::InterpType itype);
      void SetInterpType(NumericalApproximation::InterpType itype);

    protected:
      // == CLASS VARIABLES
      // VARIABLES FOR ALL INTERP TYPES
      InterpType        p_itype;                            //!< Interpolation type
      vector<double>    p_x;                                //!< List of X values
      vector<double>    p_y;                                //!< List of Y values
      bool              p_dataValidated;                    //!< Flag variable to determine whether ValidateDataSet() has been called
      // GSL INTERP VARIABLES
      typedef const gsl_interp_type *InterpFunctor;         //!< GSL Interpolation specs
      typedef map<InterpType, InterpFunctor> FunctorList;   //!< Set up a std::map of GSL interpolator functors.  List of function types
      typedef FunctorList::const_iterator FunctorConstIter; //!< GSL Iterator
      gsl_interp_accel *p_acc;                              //!< Lookup accelorator
      gsl_spline       *p_interp;                           //!< Currently active interpolator
      static FunctorList p_interpFunctors;                   //!< Maintains list of interpolator options
      // CUBIC CLAMPED VARIABLES
      bool              p_clampedEndptsSet;                 //!< Flag variable to determine whether SetCubicClampedEndptDeriv() has been called after all data was added for @a CubicClamped interpolation.
      bool              p_clampedComputed;                  //!< Flag variable to determine whether ComputeCubicClamped() has been called.
      double            p_clampedDerivFirstPt;              //!< First derivative of first x-value, p_x[0].  This is only used for the @a CubicClamped interpolation type.
      double            p_clampedDerivLastPt;               //!< First derivative of last x-value, p_x[n-1].  This is only used for the @a CubicClamped interpolation type.
      vector<double>    p_clampedSecondDerivs;              //!< List of second derivatives evaluated at p_x values.  This is only used for the @a CubicClamped interpolation type.
      // POLYNOMIAL NEVILLE VARIABLES
      vector <double>   p_polyNevError;                     //!< Estimate of error for interpolation evaluated at x.  This is only used for the @a PolynomialNeville interpolation type. 91 taken from AtmosModel.
      // CUBIC HERMITE VARIABLES
      vector<double>    p_fprimeOfx;                        //!< List of first derivatives corresponding to each x value in the data set (i.e. each value in p_x)

      // == PROTECTED METHODS
      // CREATING, DESTROYING OBJECT
      void Init(NumericalApproximation::InterpType itype);
      bool GslInterpType(NumericalApproximation::InterpType itype) const;
      void GslAllocation(unsigned int npoints);
      void GslDeallocation();
      InterpFunctor GslFunctor(NumericalApproximation::InterpType itype) const;
      // VERIFICATION METHODS
      void GslIntegrityCheck(int gsl_status, const char *src,
                             int line);
      void ValidateDataSet();
      bool InsideDomain(const double a);
      // COMPUTATION AND EVALUATION METHODS
      bool   GslComputed() const;
      void   ComputeGsl();
      void   ComputeCubicClamped();
      double ValueToExtrapolate(const double a, const ExtrapType &etype);
      double          EvaluateCubicNeighborhood(const double a);
      vector <double> EvaluateCubicNeighborhood(const vector <double> &a, const ExtrapType &etype);
      double          EvaluateCubicClamped(const double a);
      double          EvaluateCubicHermite(const double a);
      double          EvaluatePolynomialNeville(const double a);
      vector <double> EvaluateForIntegration(const double a, const double b,
                                             const unsigned int n);
      int FindIntervalLowerIndex(const double a);
      // STANDARDIZE ERRORS
      void ReportException(IException::ErrorType type, const string &method,
                           const string &message, const char *filesrc,
                           int lineno) const;
  };
};

#endif


/* robust_wfun.c
 *
 * Copyright (C) 2013 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 <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_multifit.h>

/* default tuning constants */
#define TUNING_BISQUARE       (4.685)
#define TUNING_CAUCHY         (2.385)
#define TUNING_FAIR           (1.4)
#define TUNING_HUBER          (1.345)
#define TUNING_OLS            (1.0)
#define TUNING_WELSCH         (2.985)

/*
 * Note: for each of the weighting functions below, it
 * is safe to call them with in-place parameters, so that
 * input/output vectors are the same
 */

static int
bisquare(const gsl_vector *r, gsl_vector *w)
{
  size_t i;
  size_t n = r->size;

  for (i = 0; i < n; ++i)
    {
      double ri = gsl_vector_get(r, i);

      if (fabs(ri) < 1.0)
        gsl_vector_set(w, i, (1.0 - ri*ri)*(1.0 - ri*ri));
      else
        gsl_vector_set(w, i, 0.0);
    }

  return GSL_SUCCESS;
} /* bisquare() */

static int
bisquare_dpsi(const gsl_vector *r, gsl_vector *dpsi)
{
  size_t i;
  size_t n = r->size;

  for (i = 0; i < n; ++i)
    {
      double ri = gsl_vector_get(r, i);

      if (fabs(ri) < 1.0)
        gsl_vector_set(dpsi, i, (1.0 - ri*ri)*(1.0 - 5.0*ri*ri));
      else
        gsl_vector_set(dpsi, i, 0.0);
    }

  return GSL_SUCCESS;
} /* bisquare_dpsi() */

static const gsl_multifit_robust_type bisquare_type = {
  "bisquare",
  &bisquare,
  &bisquare_dpsi,
  TUNING_BISQUARE
};

static int
cauchy(const gsl_vector *r, gsl_vector *w)
{
  size_t i;
  size_t n = r->size;

  for (i = 0; i < n; ++i)
    {
      double ri = gsl_vector_get(r, i);

      gsl_vector_set(w, i, 1.0 / (1.0 + ri*ri));
    }

  return GSL_SUCCESS;
} /* cauchy() */

static int
cauchy_dpsi(const gsl_vector *r, gsl_vector *dpsi)
{
  size_t i;
  size_t n = r->size;

  for (i = 0; i < n; ++i)
    {
      double ri = gsl_vector_get(r, i);
      double rsq = ri * ri;

      gsl_vector_set(dpsi, i, (1 - rsq) / (1.0 + rsq) / (1.0 + rsq));
    }

  return GSL_SUCCESS;
} /* cauchy_dpsi() */

static const gsl_multifit_robust_type cauchy_type = {
  "cauchy",
  &cauchy,
  &cauchy_dpsi,
  TUNING_CAUCHY
};

static int
fair(const gsl_vector *r, gsl_vector *w)
{
  size_t i;
  size_t n = r->size;

  for (i = 0; i < n; ++i)
    {
      double ri = gsl_vector_get(r, i);

      gsl_vector_set(w, i, 1.0 / (1.0 + fabs(ri)));
    }

  return GSL_SUCCESS;
} /* fair() */

static int
fair_dpsi(const gsl_vector *r, gsl_vector *dpsi)
{
  size_t i;
  size_t n = r->size;

  for (i = 0; i < n; ++i)
    {
      double ri = gsl_vector_get(r, i);

      gsl_vector_set(dpsi, i, 1.0 / (1.0 + fabs(ri)) / (1.0 + fabs(ri)));
    }

  return GSL_SUCCESS;
} /* fair_dpsi() */

static const gsl_multifit_robust_type fair_type = {
  "fair",
  &fair,
  &fair_dpsi,
  TUNING_FAIR
};

static int
huber(const gsl_vector *r, gsl_vector *w)
{
  size_t i;
  size_t n = r->size;

  for (i = 0; i < n; ++i)
    {
      double absri = fabs(gsl_vector_get(r, i));

      if (absri <= 1.0)
        gsl_vector_set(w, i, 1.0);
      else
        gsl_vector_set(w, i, 1.0 / absri);
    }

  return GSL_SUCCESS;
} /* huber() */

static int
huber_dpsi(const gsl_vector *r, gsl_vector *dpsi)
{
  size_t i;
  size_t n = r->size;

  for (i = 0; i < n; ++i)
    {
      double ri = gsl_vector_get(r, i);

      if (fabs(ri) <= 1.0)
        gsl_vector_set(dpsi, i, 1.0);
      else
        gsl_vector_set(dpsi, i, 0.0);
    }

  return GSL_SUCCESS;
} /* huber_dpsi() */

static const gsl_multifit_robust_type huber_type = {
  "huber",
  &huber,
  &huber_dpsi,
  TUNING_HUBER
};

static int
ols(const gsl_vector *r, gsl_vector *w)
{
  gsl_vector_set_all(w, 1.0);

  return GSL_SUCCESS;
}

static int
ols_dpsi(const gsl_vector *r, gsl_vector *dpsi)
{
  gsl_vector_set_all(dpsi, 1.0);

  return GSL_SUCCESS;
}

static const gsl_multifit_robust_type ols_type = {
  "ols",
  &ols,
  &ols_dpsi,
  TUNING_OLS
};

static int
welsch(const gsl_vector *r, gsl_vector *w)
{
  size_t i;
  size_t n = r->size;

  for (i = 0; i < n; ++i)
    {
      double ri = gsl_vector_get(r, i);

      gsl_vector_set(w, i, exp(-ri*ri));
    }

  return GSL_SUCCESS;
} /* welsch() */

static int
welsch_dpsi(const gsl_vector *r, gsl_vector *dpsi)
{
  size_t i;
  size_t n = r->size;

  for (i = 0; i < n; ++i)
    {
      double ri = gsl_vector_get(r, i);

      gsl_vector_set(dpsi, i, (1.0 - 2.0*ri*ri) * exp(-ri*ri));
    }

  return GSL_SUCCESS;
} /* welsch_dpsi() */

static const gsl_multifit_robust_type welsch_type = {
  "welsch",
  &welsch,
  &welsch_dpsi,
  TUNING_WELSCH
};

const gsl_multifit_robust_type *gsl_multifit_robust_default = &bisquare_type;
const gsl_multifit_robust_type *gsl_multifit_robust_bisquare = &bisquare_type;
const gsl_multifit_robust_type *gsl_multifit_robust_cauchy = &cauchy_type;
const gsl_multifit_robust_type *gsl_multifit_robust_fair = &fair_type;
const gsl_multifit_robust_type *gsl_multifit_robust_huber = &huber_type;
const gsl_multifit_robust_type *gsl_multifit_robust_ols = &ols_type;
const gsl_multifit_robust_type *gsl_multifit_robust_welsch = &welsch_type;


/* fft/gsl_fft_real_float.h
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Brian Gough
 *
 * 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 2 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., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

#ifndef __GSL_FFT_REAL_FLOAT_H__
#define __GSL_FFT_REAL_FLOAT_H__

#include <stddef.h>

#include <gsl/gsl_math.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_fft.h>

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
# define __BEGIN_DECLS extern "C" {
# define __END_DECLS }
#else
# define __BEGIN_DECLS /* empty */
# define __END_DECLS /* empty */
#endif

__BEGIN_DECLS

int gsl_fft_real_float_radix2_transform (float data[], const size_t stride, const size_t n) ;

typedef struct
  {
    size_t n;
    size_t nf;
    size_t factor[64];
    gsl_complex_float *twiddle[64];
    gsl_complex_float *trig;
  }
gsl_fft_real_wavetable_float;

typedef struct
  {
    size_t n;
    float *scratch;
  }
gsl_fft_real_workspace_float;

gsl_fft_real_wavetable_float * gsl_fft_real_wavetable_float_alloc (size_t n);

void  gsl_fft_real_wavetable_float_free (gsl_fft_real_wavetable_float * wavetable);

gsl_fft_real_workspace_float * gsl_fft_real_workspace_float_alloc (size_t n);

void  gsl_fft_real_workspace_float_free (gsl_fft_real_workspace_float * workspace);

int gsl_fft_real_float_transform (float data[], const size_t stride, const size_t n,
                                  const gsl_fft_real_wavetable_float * wavetable,
                                  gsl_fft_real_workspace_float * work);

int gsl_fft_real_float_unpack (const float real_float_coefficient[],
			       float complex_coefficient[],
			       const size_t stride, const size_t n);

__END_DECLS

#endif /* __GSL_FFT_REAL_FLOAT_H__ */


#include <stdio.h>
#include <gsl/gsl_cdf.h>

int
main (void)
{
  double P, Q;
  double x = 2.0;

  P = gsl_cdf_ugaussian_P (x);
  printf ("prob(x < %f) = %f\n", x, P);

  Q = gsl_cdf_ugaussian_Q (x);
  printf ("prob(x > %f) = %f\n", x, Q);

  x = gsl_cdf_ugaussian_Pinv (P);
  printf ("Pinv(%f) = %f\n", P, x);

  x = gsl_cdf_ugaussian_Qinv (Q);
  printf ("Qinv(%f) = %f\n", Q, x);

  return 0;
}


/* movstat/movSn.c
 *
 * Compute moving "S_n" statistic from Croux and Rousseeuw, 1992
 *
 * 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_errno.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_movstat.h>
#include <gsl/gsl_sort.h>
#include <gsl/gsl_statistics.h>

/*
gsl_movstat_Sn()
  Calculate moving S_n statistic for input vector

Inputs: endtype - how to handle end points
        x       - input vector, size n
        xscale  - (output) vector of "S_n" statistics, size n
                  xscale_i = (S_n)_i for i-th window:
        w       - workspace
*/

int
gsl_movstat_Sn(const gsl_movstat_end_t endtype, const gsl_vector * x,
               gsl_vector * xscale, gsl_movstat_workspace * w)
{
  int status = gsl_movstat_apply_accum(endtype, x, gsl_movstat_accum_Sn, NULL, xscale, NULL, w);
  return status;
}


/* hmm class header */
// Andrew Kern 8/24/05
//

#include "adkGSL.h"
#include <math.h>
#include <string.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_rng.h>

typedef struct {
  int nstates;
  gsl_matrix *transition_matrix;
  gsl_matrix *transition_score_matrix; /* entries are logs of entries
                                        * in transition matrix */
  gsl_vector *piStart, *piEnd, *piStart_scores, *piEnd_scores;
  gsl_vector *otherData; //this is meant to hold distances between obs
} HMM;

HMM* newHMM(gsl_matrix *mat, gsl_vector *piStart, gsl_vector *piEnd, gsl_vector *otherData);
void freeHMM(HMM *hmm);
void printHMM(HMM *hmm);
void initHMM(HMM *hmm);
double forwardSumPrevious(HMM *hmm, gsl_matrix *alphas, gsl_matrix *transition_score_matrix_i, int state, int obsIndex);
double viterbiMaxPrevious(HMM *hmm, gsl_matrix *deltas, gsl_matrix *transition_score_matrix_i, int state, int obsIndex, int *point);
double backwardSumNext(HMM *hmm, gsl_matrix *alphas,  gsl_matrix *transition_score_matrix_i, gsl_matrix *emissions, int state, int obsIndex);
double forwardAlg(HMM *hmm, gsl_matrix *alphas, gsl_matrix *emissions, int length, int transitionFlag);
double backwardAlg(HMM *hmm, gsl_matrix *betas, gsl_matrix *emissions, int length,int transitionPowerFlag);
void posteriorProbs(HMM *hmm, gsl_matrix *postProbMat, gsl_matrix *emissions, int length, int transitionPowerFlag);
void posteriorProbsReduced(HMM *hmm, gsl_matrix *postProbMat, double pFor, gsl_matrix *alphas, gsl_matrix *betas, gsl_matrix *emissions, int length, int transitionPowerFlag);
void reestimateTransitions(HMM *hmm, gsl_matrix *postProbMat, gsl_matrix **jpMatArray, double pFor, gsl_matrix *alphas, \
			   gsl_matrix *betas, gsl_matrix *emisLogs, int length, int transitionPowerFlag);
void reestimateTransitions_vanilla(HMM *hmm, double pFor, gsl_matrix *alphas, \
	gsl_matrix *betas, gsl_matrix *emisLogs, int length);

double viterbi(HMM *hmm, gsl_vector *viterbiPath, gsl_matrix *emissions,  int length, int transitionPowerFlag);
void hmm_logify_transitions(HMM *hmm);
void hmm_normalize_transitions(HMM *hmm);
double forwardAlgOffset(HMM *hmm, gsl_matrix *alphas, gsl_matrix *emissions, int length, int offset, int transitionPowerFlag);
double hmm_subpath_score(HMM *hmm, gsl_matrix *emisLogs, gsl_vector *stateInclusion, int begin, int length,  int transitionPowerFlag);
double hmm_subpath_log_odds(HMM *hmm, gsl_matrix *emisLogs, gsl_vector *stateInclusion1,gsl_vector *stateInclusion2,int begin, int length, int powerTransitionFlag);
gsl_vector *simulatePath(HMM *h, void *r, int obsNumber);
gsl_vector *simulatePathSpaced(HMM *h, void *r, int obsNumber, gsl_vector *locs, double mean);
void posteriorDecode(HMM *hmm, gsl_vector *posteriorPath, gsl_matrix *posts, int length);
void printTransitions(HMM *h, char *outfileName);


#include "allelefreq.h"
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <gsl/gsl_sf_psi.h>
#include <gsl/gsl_errno.h>

void P_update_simple(const uint8_t* G, const double* zetabeta, const double* zetagamma, const double* xi, const double* beta, const double* gamma, double* var_beta, double* var_gamma, long N, long L, long K)
{
    uint8_t genotype;
    long idx, n, l, k;
    double theta_beta_sum, theta_gamma_sum;
    double *var_beta_tmp, *var_gamma_tmp;

    var_beta_tmp = (double*) malloc(K * sizeof(double));
    var_gamma_tmp = (double*) malloc(K * sizeof(double));

    //printf("\n----------- Beginning P_update_simple.\n");
    // loop over loci
    for (l=0; l<L; l++) {
        for (k=0; k<K; k++) {

            var_beta_tmp[k] = 0.0;
            var_gamma_tmp[k] = 0.0;
        }

        // loop over samples
        //printf("\nl,n,g,beta_sum,gamma_sum,zb1,zg1,zb2,zg2\n");
        for (n=0; n<N; n++) {
	  //printf("%ld,", l);
	  //printf("%ld,", n);
            genotype = G[n * L + l];
            //printf("%d,", genotype);
            // missing data do not contribute
            if (genotype!=3) {

	        // Add across the components to compute the normalizing constant
                // for the indicator variables.
                // These are already exponentiated, so they are terms from exp(log likelihood).
                //   xi: exp(E(log(probability of population k for individual n))
                //   zetabeta: exp(E(log(probability of allele l for population k))
                //
                // compute xi*zeta_{beta,gamma}
                theta_beta_sum = 0.0;
                theta_gamma_sum = 0.0;
                for (k=0; k<K; k++) {
		    // In my notation:
		    // theta_beta is the indicator that allele A is in population k.
		    // theta_gamma is the indicator that allele A is in population k.

                    theta_beta_sum += xi[n * K + k] * zetabeta[l * K + k];
                    theta_gamma_sum += xi[n * K + k] * zetagamma[l * K + k];
                }
                //printf("%f,", theta_beta_sum);
                //printf("%f,", theta_gamma_sum);

                // increment var_{beta,gamma}_tmp
                for (k=0; k<K; k++) {
		  // genotype is either 0, 1, or 2.
                  // If it is 2, both alleles count towards beta.
                  // If it is 0, both alleles count towards gamma.
                  // If it is 1, each allele gets one.
                  // Note that this is all multiplied by zetabeta and zetagamma below.
                    var_beta_tmp[k] += (double) genotype * xi[n * K + k] / theta_beta_sum;
                    var_gamma_tmp[k] += (double) (2 - genotype) * xi[n * K + k] / theta_gamma_sum;

                    idx = l * K + k;
                    //printf("%f,", zetabeta[idx] * xi[n * K + k] / theta_beta_sum);
                    //printf("%f,", zetagamma[idx] * xi[n * K + k] / theta_gamma_sum);
                }
            }
            //printf("\n");
        }

        // compute var_{beta,gamma}
        for (k=0; k<K; k++) {
	  // The variables <beta> and <gamma> are the priors.
            idx = l * K + k;
            var_beta[idx] = beta[idx] + zetabeta[idx] * var_beta_tmp[k];
            var_gamma[idx] = gamma[idx] + zetagamma[idx] * var_gamma_tmp[k];
        }
    }

    free( var_beta_tmp );
    free( var_gamma_tmp );
}

void P_update_logistic(const double* Dvarbeta, const double* Dvargamma, const double* mu, const double* Lambda, double* var_beta, double* var_gamma, double mintol, long L, long K)
{
    /*
    `Dvarbeta` and `Dvargamma` are a function of the values of `var_beta` and `var_gamma`. This
    dependence, however, is explicit on a set of latent populations assignments which in-turn
    depend on the variational parameters estimated at the previous step. So, the variables
    `Dvarbeta` and `Dvargamma` do not have to be updated.
    */

    long l, k, idx, numvar, update;
    long iter = 0;
    double tol = 10.0;
    double tmptol;
    double beta, gamma;
    double A, pbetagamma, pbeta, pgamma, ppbeta, ppgamma;
    double A_1, B_1, C_1, A_2, B_2, C_2;

    /*
    Iterate until succesive estimates are sufficiently similar
    or the number of iterations exceeds 1000.
    */
    while (tol>mintol && iter<1000) {

        numvar = 0;
        tol = 0.0;

        // loop over loci
        for (l=0; l<L; l++) {

            // only update a locus, if all its variational parameters
            // satisfy positivity constraints.
            update = 1;
            for (k=0; k<K; k++) {
                if (var_beta[l*K+k]<=0 || var_gamma[l*K+k]<=0) {
                    update = 0;
                }
            }

            if (update==1) {

                // loop over populations
                for (k=0; k<K; k++) {

                    idx = l*K+k;
                    // compute pseudo-hyperparameters
                    pbetagamma = gsl_sf_psi_1(var_beta[idx]+var_gamma[idx]);
                    pbeta = gsl_sf_psi_1(var_beta[idx]);
                    pgamma = gsl_sf_psi_1(var_gamma[idx]);
                    ppbeta = gsl_sf_psi_n(2, var_beta[idx]);
                    ppgamma = gsl_sf_psi_n(2, var_gamma[idx]);

                    A_1 = pbeta-pbetagamma;
                    B_1 = -1.*pbetagamma;
                    A_2 = -1.*pbetagamma;
                    B_2 = pgamma-pbetagamma;
                    A = (gsl_sf_psi(var_beta[idx]) - gsl_sf_psi(var_gamma[idx]) - mu[l])*Lambda[k];
                    C_1 = -1.*A*pbeta - 0.5*Lambda[k]*ppbeta;
                    C_2 = A*pgamma - 0.5*Lambda[k]*ppgamma;

                    beta = (C_1*B_2-C_2*B_1)/(A_1*B_2-A_2*B_1);
                    gamma = (C_1*A_2-C_2*A_1)/(B_1*A_2-B_2*A_1);

                    // compute var_{beta,gamma}
                    tmptol = var_beta[idx];
                    var_beta[idx] = beta + Dvarbeta[idx];
                    tol += fabs(var_beta[idx]-tmptol);

                    tmptol = var_gamma[idx];
                    var_gamma[idx] = gamma + Dvargamma[idx];
                    tol += fabs(var_gamma[idx]-tmptol);

                    numvar += 1;

                }
            }
        }

        // compute convergence tolerance
        tol = 0.5*tol/numvar;
        iter += 1;
    }

    // printf("tol = %.8f, iter = %lu\n",tol,iter);
}


/*
** ECM - eigenvector centrality mapping using full matrix
**
** G.Lohmann, MPI-KYB, Nov 2018
*/

#include <viaio/Vlib.h>
#include <viaio/VImage.h>
#include <viaio/mu.h>
#include <viaio/option.h>

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_cblas.h>
#include <gsl/gsl_blas.h>

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#ifdef _OPENMP
#include <omp.h>
#endif /*_OPENMP*/

#define SQR(x) ((x) * (x))

/* check if matrix is irreducible.
** If it is not, then Perron-Frobenius theorem does not apply
*/
void CheckReducibility(float *A,size_t nvox)
{
  size_t i,j,k,n;
  float umin = 99999;

  n=0;
  for (i=0; i<nvox; i++) {
    for (j=0; j<i; j++) {
      k=j+i*(i+1)/2;
      if (A[k] > 0) n++;
      if (A[k] > 0 && A[k] < umin) umin = A[k];
    }
    for (j=i+1; j<nvox; j++) {
      k=i+j*(j+1)/2;
      if (A[k] > 0) n++;
    }
  }

  if (n < nvox) {
    VWarning(" matrix is not irreducible, correction term is applied.");
    for (i=0; i<nvox; i++) {
      for (j=0; j<i; j++) {
	k=j+i*(i+1)/2;
	if (A[k] < umin) A[k] = umin*0.5;
      }
    }
  }
}

/* re-implementation of cblas_sspmv,  cblas_sspmv causes problems */
void my_sspmv(float *A,float *x,float *y,size_t n)
{
  size_t i,j,k,kk;
  float tmp1=0,tmp2=0;

  kk = k = 0;
  for (j=0; j<n; j++) {
    y[j] = 0;
    tmp1 = x[j];
    tmp2 = 0;
    k = kk;
    for (i=0; i<j; i++) {
      y[i] += tmp1 * A[k];
      tmp2 += A[k] * x[i];
      k++;
    }
    y[j] += tmp1*A[kk+j] + tmp2;
    kk += (j+1);
  }
}

void MatrixPowerIteration(float *A,float *ev,size_t nvox,int maxiter)
{
  int i,iter;
  float sum,d,nx;

  fprintf(stderr," power iteration...\n");
  float *y = (float *) VCalloc(nvox,sizeof(float));
  nx = (double)nvox;
  for (i=0; i<nvox; i++) ev[i] = y[i] = 1.0/nx;

  for (iter=0; iter < maxiter; iter++) {

    /* y = Ax,  A symmetric and lower-triangular */
    /* cblas_sspmv(CblasRowMajor,CblasLower,(int)n,1.0f,A,ev,1,1.0f,y,1); */
    my_sspmv(A,ev,y,nvox);

    /* normalize */
    sum = 0;
    for (i=0; i<nvox; i++) sum += y[i]*y[i];
    sum = sqrt(sum);

    /* check convergence */
    d = 0;
    for (i=0; i<nvox; i++) {
      y[i] /= sum;
      d += SQR(ev[i] - y[i]);
      ev[i] = y[i];
    }
    fprintf(stderr," %5d   %f\n",(int)iter,d);

    if (iter > 2 && d < 1.0e-6) break;
  }
  VFree(y);
}

float CorrMetric(float z,int type)
{
  switch (type) {
  case 1:  /* add */
    z += 1.0;
    break;
  case 2:  /* pos  */
    if (z < 0) z = 0;
    break;
  case 3:  /* abs */
    z = fabs(z);
    break;
  case 4:  /* neg */
    if (z < 0) z = -z;
    else z = 0;
    break;
  default:
    VError("illegal type");
  }
  return z;
}

double ECMcorrelation(const float *arr1,const float *arr2,size_t nt,int type)
{
  size_t i;
  double sum=0,z=0,kx=(double)nt;

  if ((type > 0 && type < 5) || (type==6)) {
    sum=0;
    for (i=0; i<nt; i++) {
      const double u = (double)arr1[i];
      const double v = (double)arr2[i];
      sum += u*v;
    }
    z = sum/kx;
  }

  switch (type) {

  case 0:  /* RLC positive */
    sum=0;
    for (i=0; i<nt; i++) {
      const double u = (double)arr1[i];
      const double v = (double)arr2[i];
      sum += u*v + fabs(u*v);
    }
    z = sum/(2.0*kx);
    break;

  case 1:  /* add */
     z += 1.0;
     break;

  case 2:  /* pos  */
    if (z < 0) z = 0;
    break;

  case 3:  /* abs */
    z = fabs(z);
    break;

  case 4:  /* neg */
    if (z < 0) z = -z;
    else z = 0;
    break;

  case 5:   /* Gaussian of Euclidean distance */
    sum=0;
    for (i=0; i<nt; i++) {
      const double u = (double)arr1[i];
      const double v = (double)arr2[i];
      const double d = u-v;
      sum += d*d;
    }
    z = sum/kx;
    z = exp(-0.5*z*z);
    break;

  case 7:  /* RLC negative */
    sum=0;
    for (i=0; i<nt; i++) {
      const double u = (double)arr1[i];
      const double v = (double)arr2[i];
      sum += (fabs(u*v) - u*v);
    }
    z = sum/(2.0*kx);
    break;

  default:
    ;
  }
  return z;
}

void VMatrixECM(gsl_matrix_float *X,float *ev,int type,int maxiter,VImage map)
{
  size_t i,j;
  size_t nvox = X->size1;
  size_t nt = X->size2;

  /* compute similarity matrix */
  size_t m = (nvox*(nvox+1))/2;
  float *A = (float *) calloc(m,sizeof(float));
  if (!A) VError(" err allocating correlation matrix (too big)");
  memset(A,0,m*sizeof(float));
  size_t progress=0;

#pragma omp parallel for shared(progress) private(j) schedule(guided) firstprivate(X,A)
  for (i=0; i<nvox; i++) {
    if (i%1000 == 0) fprintf(stderr," %d000  of  %lu\r",(int)(++progress),nvox);

    const float *arr1 = gsl_matrix_float_const_ptr(X,i,0);
    for (j=0; j<=i; j++) {
      if (i == j) continue;
      const float *arr2 = gsl_matrix_float_const_ptr(X,j,0);

      const double v = ECMcorrelation(arr1,arr2,nt,type);
      const size_t k=j+i*(i+1)/2;
      A[k] = v;
    }
  }
  fprintf(stderr,"\n");

  /* CheckReducibility(A,nvox); */
  /* DMN(map,A,nvox); */

  /* power iteration */
  MatrixPowerIteration(A,ev,nvox,maxiter);
  VFree(A);
}