/* linalg/ldlt_band.c
 *
 * 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.
 */

/* L D L^T decomposition of a symmetric banded positive semi-definite matrix */

#include <config.h>

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

static double symband_norm1(const gsl_matrix * A);
static int ldlt_band_Ainv(CBLAS_TRANSPOSE_t TransA, gsl_vector * x, void * params);

/*
gsl_linalg_ldlt_band_decomp()
  L D L^T decomposition of a square symmetric positive semi-definite banded
matrix

Inputs: A - matrix in symmetric banded format, N-by-ndiag where N is the size of
            the matrix and ndiag is the number of nonzero diagonals.

Notes:
1) The matrix D is stored in the first column of A;
the first subdiagonal of L in the second column and so on.

2) If ndiag > 1, the 1-norm of A is stored in A(N,ndiag) on output

3) At each diagonal element, the matrix is factored as

A(j:end,j:end) = [ A11 A21^T ] = [ 1 0 ] [ alpha 0 ] [ 1 v^T ]
                 [ A21 A22   ]   [ v L ] [   0   D ] [ 0 L^T ]

where:

alpha = A(j,j)
v = A(j+1:end, j) / alpha
A22 = L D L^T + alpha v v^T

So we start at A(1,1) and work right. Pseudo-code is:

loop j = 1, ..., N
  alpha = A(j,j)
  A(j+1:end, j) := A(j+1:end, j) / alpha     (DSCAL)
  A(j+1:end, j+1:end) -= alpha v v^T         (DSYR)

Due to the banded structure, v has at most p non-zero elements, where
p is the lower bandwidth
*/

int
gsl_linalg_ldlt_band_decomp(gsl_matrix * A)
{
  const size_t N = A->size1;     /* size of matrix */
  const size_t ndiag = A->size2; /* number of diagonals in band, including main diagonal */

  if (ndiag > N)
    {
      GSL_ERROR ("invalid matrix dimensions", GSL_EBADLEN);
    }
  else
    {
      const size_t p = ndiag - 1; /* lower bandwidth */
      const int kld = (int) GSL_MAX(1, p);
      double Anorm;
      size_t j;

      /* check for quick return */
      if (ndiag == 1)
        return GSL_SUCCESS;

      /*
       * calculate 1-norm of A and store in lower right of matrix, which is not accessed
       * by rest of routine. gsl_linalg_ldlt_band_rcond() will use this later. If
       * A is diagonal, there is no empty slot to store the 1-norm, so the rcond routine
       * will have to compute it.
       */
      Anorm = symband_norm1(A);
      gsl_matrix_set(A, N - 1, p, Anorm);

      for (j = 0; j < N - 1; ++j)
        {
          double ajj = gsl_matrix_get(A, j, 0);
          size_t lenv;

          if (ajj == 0.0)
            {
              GSL_ERROR("matrix is singular", GSL_EDOM);
            }

          /* number of elements in v, which will normally be p, unless we
           * are in lower right corner of matrix */
          lenv = GSL_MIN(p, N - j - 1);

          if (lenv > 0)
            {
              gsl_vector_view v = gsl_matrix_subrow(A, j, 1, lenv);
              gsl_matrix_view m = gsl_matrix_submatrix(A, j + 1, 0, lenv, lenv);

              gsl_blas_dscal(1.0 / ajj, &v.vector);

              m.matrix.tda = kld;
              gsl_blas_dsyr(CblasUpper, -ajj, &v.vector, &m.matrix);
            }
        }

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_ldlt_band_solve (const gsl_matrix * LDLT,
                            const gsl_vector * b,
                            gsl_vector * x)
{
  if (LDLT->size1 != b->size)
    {
      GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
    }
  else if (LDLT->size1 != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else
    {
      int status;

      /* copy x <- b */
      gsl_vector_memcpy (x, b);

      status = gsl_linalg_ldlt_band_svx(LDLT, x);

      return status;
    }
}

int
gsl_linalg_ldlt_band_svx (const gsl_matrix * LDLT, gsl_vector * x)
{
  if (LDLT->size1 != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else
    {
      gsl_vector_const_view diag = gsl_matrix_const_column(LDLT, 0);

      /* solve for z using forward-substitution, L z = b */
      cblas_dtbsv(CblasColMajor, CblasLower, CblasNoTrans, CblasUnit,
                  (int) LDLT->size1, (int) (LDLT->size2 - 1), LDLT->data, LDLT->tda,
                  x->data, x->stride);

      /* solve for y, D y = z */
      gsl_vector_div(x, &diag.vector);

      /* perform back-substitution, L^T x = y */
      cblas_dtbsv(CblasColMajor, CblasLower, CblasTrans, CblasUnit,
                  (int) LDLT->size1, (int) (LDLT->size2 - 1), LDLT->data, LDLT->tda,
                  x->data, x->stride);

      return GSL_SUCCESS;
    }
}

/*
gsl_linalg_ldlt_band_unpack()
  Unpack symmetric banded format matrix LDLT into
larger matrix L and diagonal vector D
*/

int
gsl_linalg_ldlt_band_unpack (const gsl_matrix * LDLT, gsl_matrix * L, gsl_vector * D)
{
  const size_t N = LDLT->size1;

  if (N != L->size1)
    {
      GSL_ERROR("L matrix does not match LDLT dimensions", GSL_EBADLEN);
    }
  else if (L->size1 != L->size2)
    {
      GSL_ERROR("L matrix is not square", GSL_ENOTSQR);
    }
  else if (N != D->size)
    {
      GSL_ERROR("D vector does not match LDLT dimensions", GSL_EBADLEN);
    }
  else
    {
      const size_t p = LDLT->size2 - 1; /* lower bandwidth */
      gsl_vector_const_view diag = gsl_matrix_const_column(LDLT, 0);
      gsl_vector_view diagL = gsl_matrix_diagonal(L);
      size_t i;

      /* copy diagonal entries */
      gsl_vector_memcpy(D, &diag.vector);

      /* copy subdiagonals into L */
      for (i = 1; i <= p; ++i)
        {
          gsl_vector_const_view v = gsl_matrix_const_subcolumn(LDLT, i, 0, N - i);
          gsl_vector_view w = gsl_matrix_subdiagonal(L, i);
          gsl_vector_memcpy(&w.vector, &v.vector);
        }

      /* set main diagonal of L */
      gsl_vector_set_all(&diagL.vector, 1.0);

      /* zero out remaining subdiagonals */
      for (i = p + 1; i < N; ++i)
        {
          gsl_vector_view w = gsl_matrix_subdiagonal(L, i);
          gsl_vector_set_zero(&w.vector);
        }

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_ldlt_band_rcond (const gsl_matrix * LDLT, double * rcond, gsl_vector * work)
{
  const size_t N = LDLT->size1;

  if (work->size != 3 * N)
    {
      GSL_ERROR ("work vector must have length 3*N", GSL_EBADLEN);
    }
  else
    {
      int status;
      const size_t ndiag = LDLT->size2;
      double Anorm;    /* ||A||_1 */
      double Ainvnorm; /* ||A^{-1}||_1 */

      if (ndiag == 1)
        {
          /* diagonal matrix, compute 1-norm since it has not been stored */
          Anorm = symband_norm1(LDLT);
        }
      else
        {
          /* 1-norm is stored in A(N, ndiag) by gsl_linalg_ldlt_band_decomp() */
          Anorm = gsl_matrix_get(LDLT, N - 1, ndiag - 1);
        }

      *rcond = 0.0;

      /* return if matrix is singular */
      if (Anorm == 0.0)
        return GSL_SUCCESS;

      status = gsl_linalg_invnorm1(N, ldlt_band_Ainv, (void *) LDLT, &Ainvnorm, work);
      if (status)
        return status;

      if (Ainvnorm != 0.0)
        *rcond = (1.0 / Anorm) / Ainvnorm;

      return GSL_SUCCESS;
    }
}

/* compute 1-norm of symmetric banded matrix */
static double
symband_norm1(const gsl_matrix * A)
{
  const size_t N = A->size1;
  const size_t ndiag = A->size2; /* number of diagonals in band, including main diagonal */
  double value;

  if (ndiag == 1)
    {
      /* diagonal matrix */
      gsl_vector_const_view v = gsl_matrix_const_column(A, 0);
      CBLAS_INDEX_t idx = gsl_blas_idamax(&v.vector);
      value = gsl_vector_get(&v.vector, idx);
    }
  else
    {
      size_t j;

      value = 0.0;
      for (j = 0; j < N; ++j)
        {
          size_t ncol = GSL_MIN(ndiag, N - j); /* number of elements in column j below and including main diagonal */
          gsl_vector_const_view v = gsl_matrix_const_subrow(A, j, 0, ncol);
          double sum = gsl_blas_dasum(&v.vector);
          size_t k, l;

          /* sum now contains the absolute sum of elements below and including main diagonal for column j; we
           * have to add the symmetric elements above the diagonal */
          k = j;
          l = 1;
          while (k > 0 && l < ndiag)
            {
              double Akl = gsl_matrix_get(A, --k, l++);
              sum += fabs(Akl);
            }

          value = GSL_MAX(value, sum);
        }
    }

  return value;
}

/* x := A^{-1} x = A^{-t} x, A = L D L^T */
static int
ldlt_band_Ainv(CBLAS_TRANSPOSE_t TransA, gsl_vector * x, void * params)
{
  gsl_matrix * LDLT = (gsl_matrix * ) params;
  gsl_vector_const_view diag = gsl_matrix_const_column(LDLT, 0);

  (void) TransA; /* unused parameter warning */

  /* compute x := L^{-1} x */
  cblas_dtbsv(CblasColMajor, CblasLower, CblasNoTrans, CblasUnit,
              (int) LDLT->size1, (int) (LDLT->size2 - 1), LDLT->data, LDLT->tda,
              x->data, x->stride);

  /* compute x := D^{-1} x */
  gsl_vector_div(x, &diag.vector);

  /* compute x := L^{-T} x */
  cblas_dtbsv(CblasColMajor, CblasLower, CblasTrans, CblasUnit,
              (int) LDLT->size1, (int) (LDLT->size2 - 1), LDLT->data, LDLT->tda,
              x->data, x->stride);

  return GSL_SUCCESS;
}


/* ode-initval2/msbdf.c
 *
 * Copyright (C) 2009, 2010 Tuomo Keskitalo
 *
 * 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.
 */

/* A variable-coefficient linear multistep backward differentiation
   formula (BDF) method in Nordsieck form. This stepper uses the
   explicit BDF formula as predictor and implicit BDF formula as
   corrector. A modified Newton iteration method is used to
   solve the system of non-linear equations. Method order varies
   dynamically between 1 and 5.

   References:

   Byrne, G. D., and Hindmarsh, A. C., A Polyalgorithm for the
   Numerical Solution of Ordinary Differential Equations,
   ACM Trans. Math. Software, 1 (1975), pp. 71-96.

   Brown, P. N., Byrne, G. D., and Hindmarsh, A. C., VODE: A
   Variable-coefficient ODE Solver, SIAM J. Sci. Stat. Comput. 10,
   (1989), pp. 1038-1051.

   Hindmarsh, A. C., Brown, P. N., Grant, K. E., Lee, S. L., Serban,
   R., Shumaker, D. E., and Woodward, C. S., SUNDIALS: Suite of
   Nonlinear and Differential/Algebraic Equation Solvers, ACM
   Trans. Math. Software 31 (2005), pp. 363-396.

   Note: The algorithms have been adapted for GSL ode-initval2
   framework.
*/

#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv2.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>

#include "odeiv_util.h"

/* Maximum order of BDF methods */
#define MSBDF_MAX_ORD 5

/* Steps until Jacobian evaluation is forced */
#define MSBDF_JAC_WAIT 50

/* Steps until iteration matrix M evaluation is forced */
#define MSBDF_M_WAIT 20

typedef struct
{
  /* Nordsieck history matrix. Includes concatenated
     Nordsieck vectors [y_n, h*y_n', (h^2/2!)*y_n'', ...,
     (h^ord/ord!)*d^(ord)(y_n)]. Nordsieck vector number i is located
     at z[i*dim] (i=0..ord).
   */
  double *z;

  double *zbackup;              /* backup of Nordsieck matrix */
  double *ytmp;                 /* work area */
  double *ytmp2;                /* work area */
  double *l;                    /* polynomial coefficients */
  double *hprev;                /* previous step sizes */
  double *hprevbackup;          /* backup of hprev */
  size_t *ordprev;              /* orders of previous calls */
  size_t *ordprevbackup;        /* backup of ordprev */
  double *errlev;               /* desired error level of y */
  gsl_vector *abscor;           /* absolute y values for correction */
  gsl_vector *relcor;           /* relative y values for correction */
  gsl_vector *svec;             /* saved abscor & work area */
  gsl_vector *tempvec;          /* work area */
  const gsl_odeiv2_driver *driver;      /* pointer to gsl_odeiv2_driver object */
  gsl_matrix *dfdy;             /* Jacobian */
  double *dfdt;                 /* storage for time derivative of f */
  gsl_matrix *M;                /* Newton iteration matrix */
  gsl_permutation *p;           /* permutation for LU decomposition of M */
  gsl_vector *rhs;              /* right hand side equations (-G) */

  long int ni;                  /* stepper call counter */
  size_t ord;                   /* current order of method */
  double tprev;                 /* t point of previous call */
  size_t ordwait;               /* counter for order change */
  size_t ordwaitbackup;         /* backup of ordwait */
  size_t failord;               /* order of convergence failure */
  double failt;                 /* t point of convergence failure */
  double ordp1coeffprev;        /* saved order coefficient */
  size_t nJ;                    /* step counter for Jacobian evaluation */
  size_t nM;                    /* step counter for update of M */
  double gammaprev;             /* gamma of previous call */
  double gammaprevbackup;       /* backup of gammaprev */
  size_t failcount;             /* counter for rejected steps */
}
msbdf_state_t;

/* Introduce msbdf_reset for use in msbdf_alloc and _apply */

static int msbdf_reset (void *, size_t);

static void *
msbdf_alloc (size_t dim)
{
  msbdf_state_t *state = (msbdf_state_t *) malloc (sizeof (msbdf_state_t));

  if (state == 0)
    {
      GSL_ERROR_NULL ("failed to allocate space for msbdf_state", GSL_ENOMEM);
    }

  state->z = (double *) malloc ((MSBDF_MAX_ORD + 1) * dim * sizeof (double));

  if (state->z == 0)
    {
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for z", GSL_ENOMEM);
    }

  state->zbackup =
    (double *) malloc ((MSBDF_MAX_ORD + 1) * dim * sizeof (double));

  if (state->zbackup == 0)
    {
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for zbackup", GSL_ENOMEM);
    }

  state->ytmp = (double *) malloc (dim * sizeof (double));

  if (state->ytmp == 0)
    {
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for ytmp", GSL_ENOMEM);
    }

  state->ytmp2 = (double *) malloc (dim * sizeof (double));

  if (state->ytmp2 == 0)
    {
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for ytmp2", GSL_ENOMEM);
    }

  state->l = (double *) malloc ((MSBDF_MAX_ORD + 1) * sizeof (double));

  if (state->l == 0)
    {
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for l", GSL_ENOMEM);
    }

  state->hprev = (double *) malloc (MSBDF_MAX_ORD * sizeof (double));

  if (state->hprev == 0)
    {
      free (state->l);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for hprev", GSL_ENOMEM);
    }

  state->hprevbackup = (double *) malloc (MSBDF_MAX_ORD * sizeof (double));

  if (state->hprevbackup == 0)
    {
      free (state->hprev);
      free (state->l);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for hprevbackup", GSL_ENOMEM);
    }

  state->ordprev = (size_t *) malloc (MSBDF_MAX_ORD * sizeof (size_t));

  if (state->ordprev == 0)
    {
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for ordprev", GSL_ENOMEM);
    }

  state->ordprevbackup = (size_t *) malloc (MSBDF_MAX_ORD * sizeof (size_t));

  if (state->ordprevbackup == 0)
    {
      free (state->ordprev);
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for ordprevbackup",
                      GSL_ENOMEM);
    }

  state->errlev = (double *) malloc (dim * sizeof (double));

  if (state->errlev == 0)
    {
      free (state->ordprevbackup);
      free (state->ordprev);
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for errlev", GSL_ENOMEM);
    }

  state->abscor = gsl_vector_alloc (dim);

  if (state->abscor == 0)
    {
      free (state->errlev);
      free (state->ordprevbackup);
      free (state->ordprev);
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for abscor", GSL_ENOMEM);
    }

  state->relcor = gsl_vector_alloc (dim);

  if (state->relcor == 0)
    {
      gsl_vector_free (state->abscor);
      free (state->errlev);
      free (state->ordprevbackup);
      free (state->ordprev);
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for relcor", GSL_ENOMEM);
    }

  state->svec = gsl_vector_alloc (dim);

  if (state->svec == 0)
    {
      gsl_vector_free (state->relcor);
      gsl_vector_free (state->abscor);
      free (state->errlev);
      free (state->ordprevbackup);
      free (state->ordprev);
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for svec", GSL_ENOMEM);
    }

  state->tempvec = gsl_vector_alloc (dim);

  if (state->tempvec == 0)
    {
      gsl_vector_free (state->svec);
      gsl_vector_free (state->relcor);
      gsl_vector_free (state->abscor);
      free (state->errlev);
      free (state->ordprevbackup);
      free (state->ordprev);
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for tempvec", GSL_ENOMEM);
    }

  state->dfdy = gsl_matrix_alloc (dim, dim);

  if (state->dfdy == 0)
    {
      gsl_vector_free (state->tempvec);
      gsl_vector_free (state->svec);
      gsl_vector_free (state->relcor);
      gsl_vector_free (state->abscor);
      free (state->errlev);
      free (state->ordprevbackup);
      free (state->ordprev);
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for dfdy", GSL_ENOMEM);
    }

  state->dfdt = (double *) malloc (dim * sizeof (double));

  if (state->dfdt == 0)
    {
      gsl_matrix_free (state->dfdy);
      gsl_vector_free (state->tempvec);
      gsl_vector_free (state->svec);
      gsl_vector_free (state->relcor);
      gsl_vector_free (state->abscor);
      free (state->errlev);
      free (state->ordprevbackup);
      free (state->ordprev);
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for dfdt", GSL_ENOMEM);
    }

  state->M = gsl_matrix_alloc (dim, dim);

  if (state->M == 0)
    {
      free (state->dfdt);
      gsl_matrix_free (state->dfdy);
      gsl_vector_free (state->tempvec);
      gsl_vector_free (state->svec);
      gsl_vector_free (state->relcor);
      gsl_vector_free (state->abscor);
      free (state->errlev);
      free (state->ordprevbackup);
      free (state->ordprev);
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for M", GSL_ENOMEM);
    }

  state->p = gsl_permutation_alloc (dim);

  if (state->p == 0)
    {
      gsl_matrix_free (state->M);
      free (state->dfdt);
      gsl_matrix_free (state->dfdy);
      gsl_vector_free (state->tempvec);
      gsl_vector_free (state->svec);
      gsl_vector_free (state->relcor);
      gsl_vector_free (state->abscor);
      free (state->errlev);
      free (state->ordprevbackup);
      free (state->ordprev);
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for p", GSL_ENOMEM);
    }

  state->rhs = gsl_vector_alloc (dim);

  if (state->rhs == 0)
    {
      gsl_permutation_free (state->p);
      gsl_matrix_free (state->M);
      free (state->dfdt);
      gsl_matrix_free (state->dfdy);
      gsl_vector_free (state->tempvec);
      gsl_vector_free (state->svec);
      gsl_vector_free (state->relcor);
      gsl_vector_free (state->abscor);
      free (state->errlev);
      free (state->ordprevbackup);
      free (state->ordprev);
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for rhs", GSL_ENOMEM);
    }

  msbdf_reset ((void *) state, dim);

  state->driver = NULL;

  return state;
}

static int
msbdf_failurehandler (void *vstate, const size_t dim, const double t)
{
  /* Internal failure handler routine for msbdf. Adjusted strategy
     for GSL: Decrease order if this is the second time a failure
     has occurred at this order and point.
   */

  msbdf_state_t *state = (msbdf_state_t *) vstate;

  const size_t ord = state->ord;

  if (ord > 1 && (ord - state->ordprev[0] == 0) &&
      ord == state->failord && t == state->failt)
    {
      state->ord--;
    }

  /* Save information about failure */

  state->failord = ord;
  state->failt = t;
  state->ni++;

  /* Force reinitialization if failure took place at lowest
     order
   */

  if (ord == 1)
    {
      msbdf_reset (vstate, dim);
    }

  return GSL_SUCCESS;
}

static int
msbdf_calccoeffs (const size_t ord, const size_t ordwait,
                  const double h, const double hprev[],
                  double l[],
                  double *errcoeff, double *ordm1coeff,
                  double *ordp1coeff, double *ordp2coeff, double *gamma)
{
  /* Calculates coefficients (l) of polynomial Lambda, error and
     auxiliary order change evaluation coefficients.
   */

  if (ord == 1)
    {
      l[0] = 1.0;
      l[1] = 1.0;
      *errcoeff = 0.5;
      *ordp1coeff = 2.0;

      {
        const double hsum = h + hprev[0];

        const double a5 = -1.5;
        const double a6 = -1.0 - h / hsum;
        const double c2 = 2.0 / (1.0 - a6 + a5);

        *ordp2coeff = fabs (c2 * (h / hsum) * 3.0 * a5);
      }
    }
  else
    {
      size_t i, j;
      double hsum = h;
      double coeff1 = -1.0;
      double x;

      /* Calculate the actual polynomial coefficients (l) */

      DBL_ZERO_MEMSET (l, MSBDF_MAX_ORD + 1);

      l[0] = 1.0;
      l[1] = 1.0;

      for (i = 2; i < ord; i++)
        {
          hsum += hprev[i - 2];
          coeff1 += -1.0 / i;

          for (j = i; j > 0; j--)
            {
              l[j] += h / hsum * l[j - 1];
            }
        }

      coeff1 += -1.0 / ord;

      x = -l[1] - coeff1;

      for (i = ord; i > 0; i--)
        {
          l[i] += l[i - 1] * x;
        }

#ifdef DEBUG
      {
        size_t di;

        printf ("-- calccoeffs l: ");
        for (di = 0; di < ord + 1; di++)
          {
            printf ("%.5e ", l[di]);
          }
        printf ("\n");
      }
#endif

      hsum += hprev[ord - 2];

      {
        const double coeff2 = -l[1] - h / hsum;
        const double a1 = 1.0 - coeff2 + coeff1;
        const double a2 = 1.0 + ord * a1;

        /* Calculate error coefficient */

        *errcoeff = fabs (a1 / (coeff1 * a2));

        /* Calculate auxiliary coefficients used in evaluation of change
           of order
        */

        if (ordwait < 2)
          {
            const double a3 = coeff1 + 1.0 / ord;
            const double a4 = coeff2 + h / hsum;
            const double c1 = a3 / (1.0 - a4 + a3);

            *ordm1coeff = fabs (c1 / (x / l[ord]));

            *ordp1coeff = fabs (a2 / (l[ord] * (h / hsum) / x));

            hsum += hprev[ord - 1];

            {
              const double a5 = coeff1 - 1.0 / (ord + 1.0);
              const double a6 = coeff2 - h / hsum;
              const double c2 = a2 / (1.0 - a6 + a5);

              *ordp2coeff = fabs (c2 * (h / hsum) * (ord + 2) * a5);
            }
          }
      }
    }

  *gamma = h / l[1];

#ifdef DEBUG
  printf ("-- calccoeffs ordm1coeff=%.5e ", *ordm1coeff);
  printf ("ordp1coeff=%.5e ", *ordp1coeff);
  printf ("ordp2coeff=%.5e ", *ordp2coeff);
  printf ("errcoeff=%.5e\n", *errcoeff);
#endif

  return GSL_SUCCESS;
}

static int
msbdf_update (void *vstate, const size_t dim, gsl_matrix * dfdy, double *dfdt,
              const double t, const double *y, const gsl_odeiv2_system * sys,
              gsl_matrix * M, gsl_permutation * p,
              const size_t iter, size_t * nJ, size_t * nM,
              const double tprev, const double failt,
              const double gamma, const double gammaprev, const double hratio)
{
  /* Evaluates Jacobian dfdy and updates iteration matrix M
     if criteria for update is met.
   */

  /* Jacobian is evaluated
     - at first step
     - if MSBDF_JAC_WAIT steps have been made without re-evaluation
     - in case of a convergence failure if
     --- change in gamma is small, or
     --- convergence failure resulted in step size decrease
   */

  const double c = 0.2;
  const double gammarel = fabs (gamma / gammaprev - 1.0);

  if (*nJ == 0 || *nJ > MSBDF_JAC_WAIT ||
      (t == failt && (gammarel < c || hratio < 1.0)))
    {
#ifdef DEBUG
      printf ("-- evaluate jacobian\n");
#endif
      int s = GSL_ODEIV_JA_EVAL (sys, t, y, dfdy->data, dfdt);

      if (s == GSL_EBADFUNC)
        {
          return s;
        }

      if (s != GSL_SUCCESS)
        {
          msbdf_failurehandler (vstate, dim, t);
#ifdef DEBUG
          printf ("-- FAIL at jacobian function evaluation\n");
#endif
          return s;
        }

      /* Reset counter */

      *nJ = 0;
    }

  /* Iteration matrix M (and it's LU decomposition) is generated
     - at first step
     - if MSBDF_M_WAIT steps have been made without an update
     - if change in gamma is significant (e.g. change in step size)
     - if previous step was rejected
   */

  if (*nM == 0 || *nM > MSBDF_M_WAIT || gammarel >= c ||
      t == tprev || t == failt)
    {
#ifdef DEBUG
      printf ("-- update M, gamma=%.5e\n", gamma);
#endif
      size_t i;
      gsl_matrix_memcpy (M, dfdy);
      gsl_matrix_scale (M, -gamma);

      for (i = 0; i < dim; i++)
        {
          gsl_matrix_set (M, i, i, gsl_matrix_get (M, i, i) + 1.0);
        }

      {
        int signum;
        int s = gsl_linalg_LU_decomp (M, p, &signum);

        if (s != GSL_SUCCESS)
          {
            return GSL_FAILURE;
          }
      }

      /* Reset counter */

      *nM = 0;
    }

  return GSL_SUCCESS;
}

static int
msbdf_corrector (void *vstate, const gsl_odeiv2_system * sys,
                 const double t, const double h, const size_t dim,
                 const double z[], const double errlev[],
                 const double l[], const double errcoeff,
                 gsl_vector * abscor, gsl_vector * relcor,
                 double ytmp[], double ytmp2[],
                 gsl_matrix * dfdy, double dfdt[], gsl_matrix * M,
                 gsl_permutation * p, gsl_vector * rhs,
                 size_t * nJ, size_t * nM,
                 const double tprev, const double failt,
                 const double gamma, const double gammaprev,
                 const double hprev0)
{
  /* Calculates the correction step (abscor). Equation
     system M = I - gamma * dfdy = -G is solved by Newton iteration.
   */

  size_t mi, i;
  const size_t max_iter = 3;    /* Maximum number of iterations */
  double convrate = 1.0;        /* convergence rate */
  double stepnorm = 0.0;        /* norm of correction step */
  double stepnormprev = 0.0;    /* previous norm value */

  /* Evaluate at predicted values */

  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + h, z, ytmp);

    if (s == GSL_EBADFUNC)
      {
        return s;
      }

    if (s != GSL_SUCCESS)
      {
        msbdf_failurehandler (vstate, dim, t);

#ifdef DEBUG
        printf ("-- FAIL at user function evaluation\n");
#endif
        return s;
      }
  }

  /* Calculate correction step (abscor) */

  gsl_vector_set_zero (abscor);

  for (mi = 0; mi < max_iter; mi++)
    {
      const double safety = 0.3;
      const double safety2 = 0.1;

      /* Generate or update Jacobian and/or iteration matrix M if needed */

      if (mi == 0)
        {
          int s = msbdf_update (vstate, dim, dfdy, dfdt, t + h, z,
                                sys, M, p, mi,
                                nJ, nM, tprev, failt,
                                gamma, gammaprev,
                                h / hprev0);

          if (s != GSL_SUCCESS)
            {
              return s;
            }
        }

      /* Evaluate the right hand side (-G) */

      for (i = 0; i < dim; i++)
        {
          const double r = -1.0 * gsl_vector_get (abscor, i) -
            z[1 * dim + i] / l[1] + gamma * ytmp[i];

          gsl_vector_set (rhs, i, r);
        }

      /* Solve system of equations */

      {
        int s = gsl_linalg_LU_solve (M, p, rhs, relcor);

        if (s != GSL_SUCCESS)
          {
            msbdf_failurehandler (vstate, dim, t);

#ifdef DEBUG
            printf ("-- FAIL at LU_solve\n");
#endif
            return GSL_FAILURE;
          }
      }

#ifdef DEBUG
      {
        size_t di;
        printf ("-- dstep: ");
        for (di = 0; di < dim; di++)
          {
            printf ("%.5e ", gsl_vector_get (relcor, di));
          }
        printf ("\n");
      }
#endif

      /* Add iteration results */

      for (i = 0; i < dim; i++)
        {
          const double r =
            gsl_vector_get (abscor, i) + gsl_vector_get (relcor, i);

          gsl_vector_set (abscor, i, r);

          ytmp2[i] = z[i] + r;

          gsl_vector_set (relcor, i, gsl_vector_get (relcor, i) / errlev[i]);
        }

#ifdef DEBUG
      {
        size_t di;
        printf ("-- abscor: ");
        for (di = 0; di < dim; di++)
          {
            printf ("%.5e ", gsl_vector_get (abscor, di));
          }
        printf ("\n");
      }
#endif

      /* Convergence test. Norms used are root-mean-square norms. */

      stepnorm = gsl_blas_dnrm2 (relcor) / sqrt (dim);

      if (mi > 0)
        {
          convrate = GSL_MAX_DBL (safety * convrate, stepnorm / stepnormprev);
        }
      else
        {
          convrate = 1.0;
        }

      {
        const double convtest =
          GSL_MIN_DBL (convrate, 1.0) * stepnorm * errcoeff / safety2;

#ifdef DEBUG
        printf
          ("-- newt iter loop %d, errcoeff=%.5e, stepnorm =%.5e, convrate = %.5e, convtest = %.5e\n",
           (int) mi, errcoeff, stepnorm, convrate, convtest);
#endif
        if (convtest <= 1.0)
          {
            break;
          }
      }

      /* Check for divergence during iteration */

      {
        const double div_const = 2.0;

        if (mi > 1 && stepnorm > div_const * stepnormprev)
          {
            msbdf_failurehandler (vstate, dim, t);

#ifdef DEBUG
            printf ("-- FAIL, diverging Newton iteration\n");
#endif
            return GSL_FAILURE;
          }
      }

      /* Evaluate at new y */

      {
        int s = GSL_ODEIV_FN_EVAL (sys, t + h, ytmp2, ytmp);

        if (s == GSL_EBADFUNC)
          {
            return s;
          }

        if (s != GSL_SUCCESS)
          {
            msbdf_failurehandler (vstate, dim, t);

#ifdef DEBUG
            printf ("-- FAIL at user function evaluation\n");
#endif
            return s;
          }
      }

      stepnormprev = stepnorm;
    }

#ifdef DEBUG
  printf ("-- Newton iteration exit at mi=%d\n", (int) mi);
#endif

  /* Handle convergence failure */

  if (mi == max_iter)
    {
      msbdf_failurehandler (vstate, dim, t);

#ifdef DEBUG
      printf ("-- FAIL, max_iter reached\n");
#endif
      return GSL_FAILURE;
    }

  return GSL_SUCCESS;
}

static int
msbdf_eval_order (gsl_vector * abscor, gsl_vector * tempvec,
                  gsl_vector * svec, const double errcoeff,
                  const size_t dim, const double errlev[],
                  const double ordm1coeff, const double ordp1coeff,
                  const double ordp1coeffprev, const double ordp2coeff,
                  const double hprev[],
                  const double h, const double z[],
                  size_t * ord, size_t * ordwait)
{
  /* Evaluates and executes change in method order (current, current-1
     or current+1). Order which maximizes the step length is selected.
   */

  size_t i;

  /* step size estimates at current order, order-1 and order+1 */
  double ordest = 0.0;
  double ordm1est = 0.0;
  double ordp1est = 0.0;

  const double safety = 1e-6;
  const double bias = 6.0;
  const double bias2 = 10.0;
  const double min_incr = 1.5;

  /* Relative step length estimate for current order */

  ordest = 1.0 / (pow (bias * gsl_blas_dnrm2 (abscor) / sqrt (dim)
                       * errcoeff, 1.0 / (*ord + 1)) + safety);

  /* Relative step length estimate for order ord - 1 */

  if (*ord > 1)
    {
      for (i = 0; i < dim; i++)
        {
          gsl_vector_set (tempvec, i, z[*ord * dim + i] / errlev[i]);
        }

      ordm1est = 1.0 / (pow (bias * gsl_blas_dnrm2 (tempvec) / sqrt (dim)
                             / ordm1coeff, 1.0 / (*ord)) + safety);
    }
  else
    {
      ordm1est = 0.0;
    }

  /* Relative step length estimate for order ord + 1 */

  if (*ord < MSBDF_MAX_ORD)
    {
      const double c = -ordp1coeff / ordp1coeffprev *
        pow (h / hprev[1], *ord + 1);

      for (i = 0; i < dim; i++)
        {
          gsl_vector_set (svec, i, gsl_vector_get (svec, i) * c +
                          gsl_vector_get (abscor, i));
        }

      ordp1est = 1.0 / (pow (bias2 * gsl_blas_dnrm2 (svec) / sqrt (dim)
                             / ordp2coeff, 1.0 / (*ord + 2)) + safety);
    }
  else
    {
      ordp1est = 0.0;
    }

#ifdef DEBUG
  printf
    ("-- eval_order ord=%d, ordest=%.5e, ordm1est=%.5e, ordp1est=%.5e\n",
     (int) *ord, ordest, ordm1est, ordp1est);
#endif

  /* Choose order that maximises step size and increases step
     size markedly compared to current step
   */

  if (ordm1est > ordest && ordm1est > ordp1est && ordm1est > min_incr)
    {
      *ord -= 1;
#ifdef DEBUG
      printf ("-- eval_order order DECREASED to %d\n", (int) *ord);
#endif
    }

  else if (ordp1est > ordest && ordp1est > ordm1est && ordp1est > min_incr)
    {
      *ord += 1;
#ifdef DEBUG
      printf ("-- eval_order order INCREASED to %d\n", (int) *ord);
#endif
    }

  *ordwait = *ord + 2;

  return GSL_SUCCESS;
}

static int
msbdf_check_no_order_decrease (size_t const ordprev[])
{
  /* Checks if order has not been decreased according to order history
     array. Used in stability enhancement.
   */

  size_t i;

  for (i = 0; i < MSBDF_MAX_ORD - 1; i++)
    {
      if (ordprev[i + 1] > ordprev[i])
        {
          return 0;
        }
    }

  return 1;
}

static int
msbdf_check_step_size_decrease (double const hprev[])
{
  /* Checks if step size has decreased markedly according to
     step size history array. Used in stability enhancement.
   */

  size_t i;
  double max = fabs (hprev[0]);
  const double min = fabs (hprev[0]);
  const double decrease_limit = 0.5;

  for (i = 1; i < MSBDF_MAX_ORD; i++)
    {
      const double h = fabs (hprev[i]);

      if (h > min && h > max)
        {
          max = h;
        }
    }

  if (min / max < decrease_limit)
    {
      return 1;
    }

  return 0;
}

static int
msbdf_apply (void *vstate, size_t dim, double t, double h,
             double y[], double yerr[],
             const double dydt_in[], double dydt_out[],
             const gsl_odeiv2_system * sys)
{
  /* Carries out a step by BDF linear multistep methods. */

  msbdf_state_t *state = (msbdf_state_t *) vstate;

  double *const z = state->z;
  double *const zbackup = state->zbackup;
  double *const ytmp = state->ytmp;
  double *const ytmp2 = state->ytmp2;
  double *const l = state->l;
  double *const hprev = state->hprev;
  double *const hprevbackup = state->hprevbackup;
  size_t *const ordprev = state->ordprev;
  size_t *const ordprevbackup = state->ordprevbackup;
  double *const errlev = state->errlev;
  gsl_vector *const abscor = state->abscor;
  gsl_vector *const relcor = state->relcor;
  gsl_vector *const svec = state->svec;
  gsl_vector *const tempvec = state->tempvec;

  size_t ord = state->ord;      /* order for this step */
  double ordm1coeff = 0.0;
  double ordp1coeff = 0.0;
  double ordp2coeff = 0.0;
  double errcoeff = 0.0;        /* error coefficient */
  double gamma = 0.0;           /* gamma coefficient */

  const size_t max_failcount = 3;
  size_t i;

#ifdef DEBUG
  {
    size_t di;

    printf ("msbdf_apply: t=%.5e, ord=%d, h=%.5e, y:", t, (int) ord, h);

    for (di = 0; di < dim; di++)
      {
        printf ("%.5e ", y[di]);
      }
    printf ("\n");
  }
#endif

  /* Check if t is the same as on previous stepper call (or last
     failed call). This means that calculation of previous step failed
     or the step was rejected, and therefore previous state will be
     restored or the method will be reset.
   */

  if (state->ni > 0 && (t == state->tprev || t == state->failt))
    {
      if (state->ni == 1)
        {
          /* No step has been accepted yet, reset method */

          msbdf_reset (vstate, dim);
#ifdef DEBUG
          printf ("-- first step was REJECTED, msbdf_reset called\n");
#endif
        }
      else
        {
          /* A succesful step has been saved, restore previous state. */

          /* If previous step suggests order increase, but the step was
             rejected, then do not increase order.
           */

          if (ord > ordprev[0])
            {
              state->ord = ordprev[0];
              ord = state->ord;
            }

          /* Restore previous state */

          DBL_MEMCPY (z, zbackup, (MSBDF_MAX_ORD + 1) * dim);
          DBL_MEMCPY (hprev, hprevbackup, MSBDF_MAX_ORD);

          for (i = 0; i < MSBDF_MAX_ORD; i++)
            {
              ordprev[i] = ordprevbackup[i];
            }

          state->ordwait = state->ordwaitbackup;
          state->gammaprev = state->gammaprevbackup;

#ifdef DEBUG
          printf ("-- previous step was REJECTED, state restored\n");
#endif
        }

      /* If step is repeatedly rejected, then reset method */

      state->failcount++;

      if (state->failcount > max_failcount && state->ni > 1)
        {
          msbdf_reset (vstate, dim);
          ord = state->ord;

#ifdef DEBUG
          printf ("-- max_failcount reached, msbdf_reset called\n");
#endif
        }
    }
  else
    {
      /* The previous step was accepted. Backup current state. */

      DBL_MEMCPY (zbackup, z, (MSBDF_MAX_ORD + 1) * dim);
      DBL_MEMCPY (hprevbackup, hprev, MSBDF_MAX_ORD);

      for (i = 0; i < MSBDF_MAX_ORD; i++)
        {
          ordprevbackup[i] = ordprev[i];
        }

      state->ordwaitbackup = state->ordwait;
      state->gammaprevbackup = state->gammaprev;

      state->failcount = 0;

#ifdef DEBUG
      if (state->ni > 0)
        {
          printf ("-- previous step was ACCEPTED, state saved\n");
        }
#endif
    }

#ifdef DEBUG
  printf ("-- ord=%d, ni=%ld, ordwait=%d\n", (int) ord, state->ni,
          (int) state->ordwait);

  size_t di;
  printf ("-- ordprev: ");

  for (di = 0; di < MSBDF_MAX_ORD; di++)
    {
      printf ("%d ", (int) ordprev[di]);
    }

  printf ("\n");
#endif

  /* Get desired error levels via gsl_odeiv2_control object through driver
     object, which is a requirement for this stepper.
   */

  if (state->driver == NULL)
    {
      return GSL_EFAULT;
    }
  else
    {
      size_t i;

      for (i = 0; i < dim; i++)
        {
          if (dydt_in != NULL)
            {
              gsl_odeiv2_control_errlevel (state->driver->c, y[i],
                                           dydt_in[i], h, i, &errlev[i]);
            }
          else
            {
              gsl_odeiv2_control_errlevel (state->driver->c, y[i],
                                           0.0, h, i, &errlev[i]);
            }
        }
    }

#ifdef DEBUG
  {
    size_t di;
    printf ("-- errlev: ");
    for (di = 0; di < dim; di++)
      {
        printf ("%.5e ", errlev[di]);
      }
    printf ("\n");
  }
#endif

  /* On first call initialize Nordsieck matrix */

  if (state->ni == 0)
    {
      size_t i;

      DBL_ZERO_MEMSET (z, (MSBDF_MAX_ORD + 1) * dim);

      if (dydt_in != NULL)
        {
          DBL_MEMCPY (ytmp, dydt_in, dim);
        }
      else
        {
          int s = GSL_ODEIV_FN_EVAL (sys, t, y, ytmp);

          if (s != GSL_SUCCESS)
            {
              return s;
            }
        }

      DBL_MEMCPY (&z[0 * dim], y, dim);
      DBL_MEMCPY (&z[1 * dim], ytmp, dim);

      for (i = 0; i < dim; i++)
        {
          z[1 * dim + i] *= h;
        }
    }

  /* Stability enhancement heuristic for msbdf: If order > 1 and order
     has not been changed, check for decrease in step size, that is
     not accompanied by a decrease in method order. This condition may
     be indication of BDF method stability problems, a change in ODE
     system, or convergence problems in Newton iteration. In all
     cases, the strategy is to decrease method order.
   */

#ifdef DEBUG
  printf ("-- check_no_order_decrease %d, check_step_size_decrease %d\n",
          msbdf_check_no_order_decrease (ordprev),
          msbdf_check_step_size_decrease (hprev));
#endif

  if (ord > 1 &&
      ord - ordprev[0] == 0 &&
      msbdf_check_no_order_decrease (ordprev) &&
      msbdf_check_step_size_decrease (hprev))
    {
      state->ord--;
      state->ordwait = ord + 2;
      ord = state->ord;

#ifdef DEBUG
      printf ("-- stability enhancement decreased order to %d\n", (int) ord);
#endif
    }

  /* Sanity check */

  {
    const int deltaord = ord - ordprev[0];

  if (deltaord > 1 || deltaord < -1)
    {
      printf ("-- order change %d\n", deltaord);
      GSL_ERROR_NULL ("msbdf_apply too large order change", GSL_ESANITY);
    }

  /* Modify Nordsieck matrix if order or step length has been changed */

  /* If order increased by 1, adjust Nordsieck matrix */

  if (deltaord == 1)
    {
      if (ord > 2)
        {
          size_t i, j;
          double hsum = h;
          double coeff1 = -1.0;
          double coeff2 = 1.0;
          double hrelprev = 1.0;
          double hrelprod = 1.0;
          double hrel = 0.0;

          /* Calculate coefficients used in adjustment to l */

          DBL_ZERO_MEMSET (l, MSBDF_MAX_ORD + 1);

          l[2] = 1.0;

          for (i = 1; i < ord - 1; i++)
            {
              hsum += hprev[i];
              hrel = hsum / h;
              hrelprod *= hrel;
              coeff1 -= 1.0 / (i + 1);
              coeff2 += 1.0 / hrel;

              for (j = i + 2; j > 1; j--)
                {
                  l[j] *= hrelprev;
                  l[j] += l[j - 1];
                }

              hrelprev = hrel;
            }

          /* Scale Nordsieck matrix */

          {
            const double c = (-coeff1 - coeff2) / hrelprod;

            for (i = 0; i < dim; i++)
              {
                z[ord * dim + i] = c * gsl_vector_get (abscor, i);
              }
          }
          for (i = 2; i < ord; i++)
            for (j = 0; j < dim; j++)
              {
                z[i * dim + j] += l[i] * z[ord * dim + j];
              }
        }
      else
        {
          /* zero new vector for order incease from 1 to 2 */

          DBL_ZERO_MEMSET (&z[ord * dim], dim);
        }

#ifdef DEBUG
      printf ("-- order increase detected, Nordsieck modified\n");
#endif
    }

  /* If order decreased by 1, adjust Nordsieck matrix */

  if (deltaord == -1)
    {
      size_t i, j;
      double hsum = 0.0;

      /* Calculate coefficients used in adjustment to l */

      DBL_ZERO_MEMSET (l, MSBDF_MAX_ORD + 1);

      l[2] = 1.0;

      for (i = 1; i < ord; i++)
        {
          hsum += hprev[i - 1];

          for (j = i + 2; j > 1; j--)
            {
              l[j] *= hsum / h;
              l[j] += l[j - 1];
            }
        }

      /* Scale Nordsieck matrix */

      for (i = 2; i < ord + 1; i++)
        for (j = 0; j < dim; j++)
          {
            z[i * dim + j] += -l[i] * z[(ord + 1) * dim + j];
          }

#ifdef DEBUG
      printf ("-- order decrease detected, Nordsieck modified\n");
#endif
    }

  /* Scale Nordsieck vectors if step size has been changed */

  if (state->ni > 0 && h != hprev[0])
    {
      size_t i, j;
      const double hrel = h / hprev[0];
      double coeff = hrel;

      for (i = 1; i < ord + 1; i++)
        {
          for (j = 0; j < dim; j++)
            {
              z[i * dim + j] *= coeff;
            }

          coeff *= hrel;
        }

#ifdef DEBUG
      printf ("-- h != hprev, Nordsieck modified\n");
#endif
    }

  /* Calculate polynomial coefficients (l), error coefficient and
     auxiliary coefficients
   */

  msbdf_calccoeffs (ord, state->ordwait, h, hprev, l, &errcoeff,
                    &ordm1coeff, &ordp1coeff, &ordp2coeff, &gamma);

  /* Carry out the prediction step */

  {
    size_t i, j, k;

    for (i = 1; i < ord + 1; i++)
      for (j = ord; j > i - 1; j--)
        for (k = 0; k < dim; k++)
          {
            z[(j - 1) * dim + k] += z[j * dim + k];
          }

#ifdef DEBUG
    {
      size_t di;
      printf ("-- predicted y: ");
      for (di = 0; di < dim; di++)
        {
          printf ("%.5e ", z[di]);
        }
      printf ("\n");
    }
#endif
  }

  /* Calculate correction step to abscor */
  {
    int s;
    s = msbdf_corrector (vstate, sys, t, h, dim, z, errlev, l, errcoeff,
                         abscor, relcor, ytmp, ytmp2,
                         state->dfdy, state->dfdt, state->M,
                         state->p, state->rhs,
                         &(state->nJ), &(state->nM),
                         state->tprev, state->failt, gamma,
                         state->gammaprev, hprev[0]);

    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  {
    /* Add accepted final correction step to Nordsieck matrix */

    size_t i, j;

    for (i = 0; i < ord + 1; i++)
      for (j = 0; j < dim; j++)
        {
          z[i * dim + j] += l[i] * gsl_vector_get (abscor, j);
        }

#ifdef DEBUG
    {
      size_t di;
      printf ("---- l: ");
      for (di = 0; di < ord + 1; di++)
        {
          printf ("%.5e ", l[di]);
        }
      printf ("\n");

      printf ("-- corrected y: ");
      for (di = 0; di < dim; di++)
        {
          printf ("%.5e ", z[di]);
        }
      printf ("\n");
    }
#endif

    /* Derivatives at output */

    if (dydt_out != NULL)
      {
        int s = GSL_ODEIV_FN_EVAL (sys, t + h, z, dydt_out);

        if (s == GSL_EBADFUNC)
          {
            return s;
          }

        if (s != GSL_SUCCESS)
          {
            msbdf_failurehandler (vstate, dim, t);

#ifdef DEBUG
            printf ("-- FAIL at user function evaluation\n");
#endif
            return s;
          }
      }

    /* Calculate error estimate */

    for (i = 0; i < dim; i++)
      {
        yerr[i] = fabs (gsl_vector_get (abscor, i)) * errcoeff;
      }

#ifdef DEBUG
    {
      size_t di;
      printf ("-- yerr: ");
      for (di = 0; di < dim; di++)
        {
          printf ("%.5e ", yerr[di]);
        }
      printf ("\n");
    }
#endif

    /* Save y values */

    for (i = 0; i < dim; i++)
      {
        y[i] = z[0 * dim + i];
      }
  }

  /* Scale abscor with errlev for later use in norm calculations */
  {
    size_t i;

    for (i = 0; i < dim; i++)
      {
        gsl_vector_set (abscor, i, gsl_vector_get (abscor, i) / errlev[i]);
      }
  }

  /* Save items needed for evaluation of order increase on next
     call, if needed
   */

  if (state->ordwait == 1 && ord < MSBDF_MAX_ORD)
    {
      size_t i;

      state->ordp1coeffprev = ordp1coeff;

      for (i = 0; i < dim; i++)
        {
          gsl_vector_set (svec, i, gsl_vector_get (abscor, i));
        }
    }

  /* Consider and execute order change for next step */

  if (state->ordwait == 0)
    {
      msbdf_eval_order (abscor, tempvec, svec, errcoeff, dim, errlev,
                        ordm1coeff, ordp1coeff,
                        state->ordp1coeffprev, ordp2coeff,
                        hprev, h, z, &(state->ord), &(state->ordwait));
    }

  /* Undo scaling of abscor for possible order increase on next step */
  {
    size_t i;

    for (i = 0; i < dim; i++)
      {
        gsl_vector_set (abscor, i, gsl_vector_get (abscor, i) * errlev[i]);
      }
  }

  /* Save information about current step in state and update counters */
  {
    size_t i;

    for (i = MSBDF_MAX_ORD - 1; i > 0; i--)
      {
        hprev[i] = hprev[i - 1];
        ordprev[i] = ordprev[i - 1];
      }
  }

  hprev[0] = h;
  ordprev[0] = ord;

#ifdef DEBUG
  {
    size_t di;
    printf ("-- hprev: ");
    for (di = 0; di < MSBDF_MAX_ORD; di++)
      {
        printf ("%.5e ", hprev[di]);
      }
    printf ("\n");
  }
#endif

  state->tprev = t;
  state->ordwait--;
  state->ni++;
  state->gammaprev = gamma;

  state->nJ++;
  state->nM++;

#ifdef DEBUG
  printf ("-- nJ=%d, nM=%d\n", (int) state->nJ, (int) state->nM);
#endif
  }

  return GSL_SUCCESS;
}

static int
msbdf_set_driver (void *vstate, const gsl_odeiv2_driver * d)
{
  msbdf_state_t *state = (msbdf_state_t *) vstate;

  state->driver = d;

  return GSL_SUCCESS;
}

static int
msbdf_reset (void *vstate, size_t dim)
{
  msbdf_state_t *state = (msbdf_state_t *) vstate;
  size_t i;

  state->ni = 0;
  state->ord = 1;
  state->ordwait = 2;
  state->ordwaitbackup = 2;
  state->failord = 0;
  state->failt = GSL_NAN;
  state->gammaprev = 1.0;
  state->nJ = 0;
  state->nM = 0;
  state->failcount = 0;

  DBL_ZERO_MEMSET (state->hprev, MSBDF_MAX_ORD);
  DBL_ZERO_MEMSET (state->hprevbackup, MSBDF_MAX_ORD);
  DBL_ZERO_MEMSET (state->z, (MSBDF_MAX_ORD + 1) * dim);
  DBL_ZERO_MEMSET (state->zbackup, (MSBDF_MAX_ORD + 1) * dim);

  for (i = 0; i < MSBDF_MAX_ORD; i++)
    {
      state->ordprev[i] = 1;
      state->ordprevbackup[i] = 1;
    }

#ifdef DEBUG
  printf ("-- msbdf_reset called\n");
#endif

  return GSL_SUCCESS;
}

static unsigned int
msbdf_order (void *vstate)
{
  msbdf_state_t *state = (msbdf_state_t *) vstate;

  return state->ord;
}

static void
msbdf_free (void *vstate)
{
  msbdf_state_t *state = (msbdf_state_t *) vstate;

  gsl_vector_free (state->rhs);
  gsl_permutation_free (state->p);
  gsl_matrix_free (state->M);
  free (state->dfdt);
  gsl_matrix_free (state->dfdy);
  gsl_vector_free (state->tempvec);
  gsl_vector_free (state->svec);
  gsl_vector_free (state->relcor);
  gsl_vector_free (state->abscor);
  free (state->errlev);
  free (state->ordprevbackup);
  free (state->ordprev);
  free (state->hprevbackup);
  free (state->hprev);
  free (state->l);
  free (state->ytmp2);
  free (state->ytmp);
  free (state->zbackup);
  free (state->z);
  free (state);
}

static const gsl_odeiv2_step_type msbdf_type = {
  "msbdf",                      /* name */
  1,                            /* can use dydt_in? */
  1,                            /* gives exact dydt_out? */
  &msbdf_alloc,
  &msbdf_apply,
  &msbdf_set_driver,
  &msbdf_reset,
  &msbdf_order,
  &msbdf_free
};

const gsl_odeiv2_step_type *gsl_odeiv2_step_msbdf = &msbdf_type;


/*
** Copyright (C) 2004 Jonathan G. Underwood <j.underwood@open.ac.uk>
**
** 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.
*/

#ifndef __GSL_SF_WIGNER_H__
#define  __GSL_SF_WIGNER_H__ 1
#include <gsl/gsl_sf_result.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
  double gsl_sf_wigner_3j (const int two_j1, const int two_j2,
                           const int two_j3, const int two_m1,
                           const int two_m2, const int two_m3);
int gsl_sf_wigner_3j_e (const int two_j1, const int two_j2, const int two_j3,
                        const int two_m1, const int two_m2, const int two_m3,
                        gsl_sf_result * result);

double gsl_sf_wigner_6j (const int two_j1, const int two_j2, const int two_j3,
                         const int two_j4, const int two_j5,
                         const int two_j6);
int gsl_sf_wigner_6j_e (const int two_j1, const int two_j2, const int two_j3,
                        const int two_j4, const int two_j5, const int two_j6,
                        gsl_sf_result * result);

double gsl_sf_wigner_9j (const int two_j1, const int two_j2, const int two_j3,
                         const int two_j4, const int two_j5, const int two_j6,
                         const int two_j7, const int two_j8,
                         const int two_j9);
int gsl_sf_wigner_9j_e (const int two_j1, const int two_j2, const int two_j3,
                        const int two_j4, const int two_j5, const int two_j6,
                        const int two_j7, const int two_j8, const int two_j9,
                        gsl_sf_result * result);

double gsl_sf_wigner_drot (const int two_j, const int two_m1,
                           const int two_m2, const double theta);
int gsl_sf_wigner_drot_e (const int two_j, const int two_m1, const int two_m2,
                          const double theta, gsl_sf_result * result);

__END_DECLS
#endif /* __GSL_SF_WIGNER_H__ */


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

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_blas.h>

#include <lapacke.h>
#include "kjg_gsl.h"

void
kjg_gsl_matrix_fprintf (FILE * stream, gsl_matrix * m, const char *template)
{
  size_t i, j;
  for (i = 0; i < m->size1; i++)
    {
      fprintf (stream, template, gsl_matrix_get (m, i, 0));
      for (j = 1; j < m->size2; j++)
	{
	  fprintf (stream, "\t");
	  fprintf (stream, template, gsl_matrix_get (m, i, j));
	}
      fprintf (stream, "\n");
    }
}

void
kjg_gsl_matrix_fscanf (FILE * stream, gsl_matrix * m)
{
  size_t i, j;
  double x;
  for (i = 0; i < m->size1; i++)
    {
      for (j = 0; j < m->size2; j++)
	{
	  fscanf (stream, "%lg", &x);
	  gsl_matrix_set (m, i, j, x);
	}
    }
}

void
kjg_gsl_evec_fprintf (FILE * stream,
		      gsl_vector * eval,
		      gsl_matrix * evec, const char *template)
{
  size_t i, j;
  fprintf (stream, "#");
  fprintf (stream, template, gsl_vector_get (eval, 0));
  for (i = 1; i < eval->size; i++)
    {
      fprintf (stream, "\t");
      fprintf (stream, template, gsl_vector_get (eval, i));
    }
  fprintf (stream, "\n");
  kjg_gsl_matrix_fprintf (stream, evec, template);
}

int
kjg_gsl_evec_fscanf (FILE * stream, gsl_vector * eval, gsl_matrix * evec)
{
  size_t i, j;
  int r;
  double x;

  r = fscanf (stream, "#%lg", &x);
  if (r != 1)
    return (r);
  gsl_vector_set (eval, 0, x);

  for (i = 1; i < eval->size; i++)
    {
      r = fscanf (stream, "%lg", &x);
      if (r != 1)
	return (r);
      gsl_vector_set (eval, i, x);
    }

  for (i = 0; i < evec->size1; i++)
    {
      for (j = 0; j < evec->size2; j++)
	{
	  r = fscanf (stream, "%lg", &x);
	  if (r != 1)
	    return (r);
	  gsl_matrix_set (evec, i, j, x);
	}
    }

  return (0);
}

gsl_rng *
kjg_gsl_rng_init ()
{
  const gsl_rng_type *T;
  gsl_rng *r;
  extern long seed;

  gsl_rng_env_setup ();

  gsl_rng_default_seed = seed;

  T = gsl_rng_default;
  r = gsl_rng_alloc (T);

//  fprintf (stderr, "generator type: %s\n", gsl_rng_name (r));
//  fprintf (stderr, "seed = %lu\n", gsl_rng_default_seed);

  return (r);
}

int
kjg_gsl_matrix_frobenius_normalize (gsl_matrix * m)
{
  double s = kjg_gsl_dlange ('F', m);
  double d = m->size1 * m->size2;
  return (gsl_matrix_scale (m, d / s));
}

double
kjg_gsl_dlange (const char norm, const gsl_matrix * m)
{
  return (LAPACKE_dlange (LAPACK_ROW_MAJOR, norm, m->size1, m->size2, m->data,
			  m->tda));
}

int
kjg_gsl_dgeqrf (gsl_matrix * m, gsl_vector * tau)
{
  return (LAPACKE_dgeqrf (LAPACK_ROW_MAJOR, m->size1, m->size2, m->data,
			  m->tda, tau->data));
}

int
kjg_gsl_dorgqr (gsl_matrix * m, gsl_vector * tau)
{
  return (LAPACKE_dorgqr (LAPACK_ROW_MAJOR, m->size2, m->size2, m->size2,
			  m->data, m->tda, tau->data));
}

void
kjg_gsl_ran_ugaussian_pair (const gsl_rng * r, double x[2])
{
  double r2;

  do
    {
      /* choose x,y in uniform square (-1,-1) to (+1,+1) */
      x[0] = -1 + 2 * gsl_rng_uniform_pos (r);
      x[1] = -1 + 2 * gsl_rng_uniform_pos (r);

      /* see if it is in the unit circle */
      r2 = x[0] * x[0] + x[1] * x[1];
    }
  while (r2 > 1.0 || r2 == 0);

  r2 = sqrt (-2.0 * log (r2) / r2);

  x[0] *= r2;
  x[1] *= r2;
}

void
kjg_gsl_ran_ugaussian_matrix (const gsl_rng * r, gsl_matrix * m)
{
  size_t i, j;
  double *data;
  double x, y, r2;

  for (i = 0; i < m->size1; i++)
    {
      data = gsl_matrix_ptr (m, i, 0);

      for (j = 0; j < m->size2 - 1; j += 2)
	{
	  kjg_gsl_ran_ugaussian_pair (r, data);
	  data += 2;
	}

      if (m->size2 % 2)
	*data = gsl_rng_uniform_pos (r);
    }
}

void
kjg_gsl_matrix_QR (gsl_matrix * m)
{
  gsl_vector *tau = gsl_vector_alloc (m->size2);
  kjg_gsl_dgeqrf (m, tau);
  kjg_gsl_dorgqr (m, tau);
  gsl_vector_free (tau);
}

int
kjg_gsl_SVD (gsl_matrix * M, gsl_matrix * V, gsl_vector * S)
{
  size_t big_enough = M->size1 + V->size2;
  double *superb = malloc (big_enough * sizeof (double));
  double *U;
  int info = LAPACKE_dgesvd (LAPACK_ROW_MAJOR,	// row major
			     'O', 'S', M->size1, M->size2, M->data, M->tda,
			     S->data, U,
			     big_enough, V->data, V->tda, superb);
  free (superb);
  return (info);
}


/* specfunc/gsl_sf_mathieu.h
 *
 * Copyright (C) 2002 Lowell Johnson
 *
 * 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., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/* Author:  L. Johnson */

#ifndef __GSL_SF_MATHIEU_H__
#define __GSL_SF_MATHIEU_H__

#if !defined( GSL_FUN )
#  if !defined( GSL_DLL )
#    define GSL_FUN extern
#  elif defined( BUILD_GSL_DLL )
#    define GSL_FUN extern __declspec(dllexport)
#  else
#    define GSL_FUN extern __declspec(dllimport)
#  endif
#endif

#include <gsl/gsl_sf_result.h>
#include <gsl/gsl_eigen.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

#define GSL_SF_MATHIEU_COEFF 100

typedef struct
{
  size_t size;
  size_t even_order;
  size_t odd_order;
  int extra_values;
  double qa;   /* allow for caching of results: not implemented yet */
  double qb;   /* allow for caching of results: not implemented yet */
  double *aa;
  double *bb;
  double *dd;
  double *ee;
  double *tt;
  double *e2;
  double *zz;
  gsl_vector *eval;
  gsl_matrix *evec;
  gsl_eigen_symmv_workspace *wmat;
} gsl_sf_mathieu_workspace;

/* Compute an array of characteristic (eigen) values from the recurrence
   matrices for the Mathieu equations. */
GSL_FUN int gsl_sf_mathieu_a_array(int order_min, int order_max, double qq, gsl_sf_mathieu_workspace *work, double result_array[]);
GSL_FUN int gsl_sf_mathieu_b_array(int order_min, int order_max, double qq,  gsl_sf_mathieu_workspace *work, double result_array[]);

/* Compute the characteristic value for a Mathieu function of order n and
   type ntype. */
GSL_FUN int gsl_sf_mathieu_a_e(int order, double qq, gsl_sf_result *result);
GSL_FUN double gsl_sf_mathieu_a(int order, double qq);
GSL_FUN int gsl_sf_mathieu_b_e(int order, double qq, gsl_sf_result *result);
GSL_FUN double gsl_sf_mathieu_b(int order, double qq);

/* Compute the Fourier coefficients for a Mathieu function. */
GSL_FUN int gsl_sf_mathieu_a_coeff(int order, double qq, double aa, double coeff[]);
GSL_FUN int gsl_sf_mathieu_b_coeff(int order, double qq, double aa, double coeff[]);

/* Allocate computational storage space for eigenvalue solution. */
GSL_FUN gsl_sf_mathieu_workspace *gsl_sf_mathieu_alloc(const size_t nn,
                                               const double qq);
GSL_FUN void gsl_sf_mathieu_free(gsl_sf_mathieu_workspace *workspace);

/* Compute an angular Mathieu function. */
GSL_FUN int gsl_sf_mathieu_ce_e(int order, double qq, double zz, gsl_sf_result *result);
GSL_FUN double gsl_sf_mathieu_ce(int order, double qq, double zz);
GSL_FUN int gsl_sf_mathieu_se_e(int order, double qq, double zz, gsl_sf_result *result);
GSL_FUN double gsl_sf_mathieu_se(int order, double qq, double zz);
GSL_FUN int gsl_sf_mathieu_ce_array(int nmin, int nmax, double qq, double zz,
                            gsl_sf_mathieu_workspace *work,
                            double result_array[]);
GSL_FUN int gsl_sf_mathieu_se_array(int nmin, int nmax, double qq, double zz,
                            gsl_sf_mathieu_workspace *work,
                            double result_array[]);

/* Compute a radial Mathieu function. */
GSL_FUN int gsl_sf_mathieu_Mc_e(int kind, int order, double qq, double zz,
                      gsl_sf_result *result);
GSL_FUN double gsl_sf_mathieu_Mc(int kind, int order, double qq, double zz);
GSL_FUN int gsl_sf_mathieu_Ms_e(int kind, int order, double qq, double zz,
                      gsl_sf_result *result);
GSL_FUN double gsl_sf_mathieu_Ms(int kind, int order, double qq, double zz);
GSL_FUN int gsl_sf_mathieu_Mc_array(int kind, int nmin, int nmax, double qq,
                            double zz, gsl_sf_mathieu_workspace *work,
                            double result_array[]);
GSL_FUN int gsl_sf_mathieu_Ms_array(int kind, int nmin, int nmax, double qq,
                            double zz, gsl_sf_mathieu_workspace *work,
                            double result_array[]);

__END_DECLS

#endif /* !__GSL_SF_MATHIEU_H__ */


/* specfunc/gsl_sf_airy.h
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 *
 * 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.
 */

/* Author:  G. Jungman */

#ifndef __GSL_SF_AIRY_H__
#define __GSL_SF_AIRY_H__

#include <gsl/gsl_mode.h>
#include <gsl/gsl_sf_result.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

/* Airy function Ai(x)
 *
 * exceptions: GSL_EUNDRFLW
 */
int gsl_sf_airy_Ai_e(const double x, const gsl_mode_t mode, gsl_sf_result * result);
double gsl_sf_airy_Ai(const double x, gsl_mode_t mode);

/* Airy function Bi(x)
 *
 * exceptions: GSL_EOVRFLW
 */
int gsl_sf_airy_Bi_e(const double x, gsl_mode_t mode, gsl_sf_result * result);
double gsl_sf_airy_Bi(const double x, gsl_mode_t mode);

/* scaled Ai(x):
 *                     Ai(x)   x < 0
 *   exp(+2/3 x^{3/2}) Ai(x)   x > 0
 *
 * exceptions: none
 */
int gsl_sf_airy_Ai_scaled_e(const double x, gsl_mode_t mode, gsl_sf_result * result);
double gsl_sf_airy_Ai_scaled(const double x, gsl_mode_t mode);

/* scaled Bi(x):
 *                     Bi(x)   x < 0
 *   exp(-2/3 x^{3/2}) Bi(x)   x > 0
 *
 * exceptions: none
 */
int gsl_sf_airy_Bi_scaled_e(const double x, gsl_mode_t mode, gsl_sf_result * result);
double gsl_sf_airy_Bi_scaled(const double x, gsl_mode_t mode);

/* derivative Ai'(x)
 *
 * exceptions: GSL_EUNDRFLW
 */
int gsl_sf_airy_Ai_deriv_e(const double x, gsl_mode_t mode, gsl_sf_result * result);
double gsl_sf_airy_Ai_deriv(const double x, gsl_mode_t mode);

/* derivative Bi'(x)
 *
 * exceptions: GSL_EOVRFLW
 */
int gsl_sf_airy_Bi_deriv_e(const double x, gsl_mode_t mode, gsl_sf_result * result);
double gsl_sf_airy_Bi_deriv(const double x, gsl_mode_t mode);

/* scaled derivative Ai'(x):
 *                     Ai'(x)   x < 0
 *   exp(+2/3 x^{3/2}) Ai'(x)   x > 0
 *
 * exceptions: none
 */
int gsl_sf_airy_Ai_deriv_scaled_e(const double x, gsl_mode_t mode, gsl_sf_result * result);
double gsl_sf_airy_Ai_deriv_scaled(const double x, gsl_mode_t mode);

/* scaled derivative:
 *                     Bi'(x)   x < 0
 *   exp(-2/3 x^{3/2}) Bi'(x)   x > 0
 *
 * exceptions: none
 */
int gsl_sf_airy_Bi_deriv_scaled_e(const double x, gsl_mode_t mode, gsl_sf_result * result);
double gsl_sf_airy_Bi_deriv_scaled(const double x, gsl_mode_t mode);

/* Zeros of Ai(x)
 */
int gsl_sf_airy_zero_Ai_e(unsigned int s, gsl_sf_result * result);
double gsl_sf_airy_zero_Ai(unsigned int s);

/* Zeros of Bi(x)
 */
int gsl_sf_airy_zero_Bi_e(unsigned int s, gsl_sf_result * result);
double gsl_sf_airy_zero_Bi(unsigned int s);

/* Zeros of Ai'(x)
 */
int gsl_sf_airy_zero_Ai_deriv_e(unsigned int s, gsl_sf_result * result);
double gsl_sf_airy_zero_Ai_deriv(unsigned int s);

/* Zeros of Bi'(x)
 */
int gsl_sf_airy_zero_Bi_deriv_e(unsigned int s, gsl_sf_result * result);
double gsl_sf_airy_zero_Bi_deriv(unsigned int s);

__END_DECLS

#endif /* __GSL_SF_AIRY_H__ */


#include <model.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_cdf.h>

#define P_inv_gamma gsl_cdf_gamma_Pinv // invverse cumulative gamma dist
#define P_inc_gamma gsl_sf_gamma_inc_P // regularised incomplete Gamma function
#define lngamma gsl_sf_lngamma       // ln Gamma function

#define V0 9
#define NUMBER_SHEEP 30
#define NUM_DONORS 10
#define SHEEP_FIRST_TEST_WEEK 1

// month defined as 365/12 days
// week defined as 7 days
# define WEEKS_PER_MONTH ((365/12)/7)

#include "Texel_clean_data.h"

typedef struct {
  double a, b;
  double *lambda;
  double *Lambda;
  double **W4;
  double *W2;
} parameters_t;

double u(int, double*);
double lngexpan(double, double, int*);
double ln_inc_gamma_star(double, double);
double w(int, int, int, parameters_t*);
double Prob_seroconvert(int, parameters_t*);
extern double R0(double, double, double, int, double*);

void Variables(struct variables *V)
{
  Var(V, 0, "week");
  Var(V, 1, "field infected");
  Var(V, 2, "new positives");
  Var(V, 3, "cumulative new positives");
  Var(V, 4, "seroconversion rate");
  Var(V, 5, "number infectious");
  Var(V, 6, "number infectious");
  Var(V, 6, "housed infected");
  Var(V, 7, "infected");
  Var(V, 8, "infected");
  Var(V, V0, "log_p_pos");
}

void Parameters(int trt, int ind, double *p, struct parameterset *Q)
{
  Par(Q,  0, Real, "beta_{field}", Exponential, 0.0008);
  Par(Q,  1, Real, "beta_{housed}", Gamma, 2.0, 0.1);
  Par(Q,  2, Real, "a", Exponential, 3.2);
  Par(Q,  3, Real, "k1", Gamma, 2.0, 4.0);
  // Par(Q,  0, Real, "beta_{field}", Exponential, 0.0082/WEEKS_PER_MONTH);
  // Par(Q,  1, Real, "beta_{housed}", Gamma, 2.0, 0.01);
  // Par(Q,  2, Real, "mu", Gamma, 2.0, 1.5);
  // Par(Q,  3, Real, "sigma", Gamma, 2.0, 1.5);

  // Par(Q,  8, Real, "a", Der);
  // Par(Q,  9, Real, "b", Der);
  // Par(Q, 10, Real, "beta_{housed}/beta_{field}", Der);
  // Par(Q, 12, Real, "E[inf_{field}]", Der);
  // Par(Q, 13, Real, "E[inf_{housed}]", Der);
  // Par(Q, 14, Real, "E[inf]", Der);
  // Par(Q, 15, Real, "s_{low}", Der);
  // Par(Q, 16, Real, "s_{high}", Der);
}

int Model(const uint trt, const uint ind, const uint nweeks, double *p, double **V, TimePoints *Data)
{
  int i, j, m, *num_infected;
  double **p_seropos = V, *Einf_field, *Einf_housed;
  parameters_t pars;

  double beta_field = p[0]/WEEKS_PER_MONTH;
  double beta_housed = p[1]/WEEKS_PER_MONTH;
  double S_a = p[2];
  double S_b = S_a/p[3]/WEEKS_PER_MONTH;
  int seroconversion = lrint(p[3]);

  for (m = 0; m < nweeks; m++)
    V[m][0] = m;

  int *num_infectious = integervector(nweeks);
  pars.Lambda = doublevector(nweeks);
  pars.lambda = doublevector(nweeks);
  pars.W4 = doublematrix(nweeks, nweeks);
  pars.W2 = doublevector(nweeks);
  pars.a = S_a;
  pars.b = S_b;

  // ewe to ewe transmission
  // the 10 donor sheep are infectious at start of experiment
  for (j = 0; j < NUM_DONORS; j++)
    for (m = 0; m <= sheep_last_week[j]; m++)
        num_infectious[m]++;

  for (m = 0; m < nweeks; m++)
    if (m == 20 || m == 21)
      pars.lambda[m] = num_infectious[m]*beta_housed;
    else
      pars.lambda[m] = num_infectious[m]*beta_field;

  // cumulative sum of lambda
  pars.Lambda[0] = 0;
  for (m = 1; m < nweeks; m++)
    pars.Lambda[m] = pars.Lambda[m-1]+pars.lambda[m-1];

  for (m = 0; m < nweeks; m++) {
    for (i = 0; i < m; i++)
      pars.W4[i][m] = w(i, i+1, m+1, &pars)
                    - w(i, i+1, m,   &pars)
                    - w(i, i,   m+1, &pars)
                    + w(i, i,   m,   &pars);
    pars.W2[m] = w(m, m, m+1, &pars);
  }

  // for each testing date calculate the probability that a sheep tests positive since the last test
  for (m = 0; m < nweeks; m++)
    p_seropos[m][V0] = Prob_seroconvert(m, &pars);

  /******************** OUTPUT ***********************/
  if (Data->mode == OUTPUT) {
    for (m = 0; m < nweeks; m++)
       V[m][0] = m;

    // expected number of new positives on each test date
    for (j = NUM_DONORS; j < NUMBER_SHEEP; j++)
      for (i = 0; i < sheep_last_test_week[j]-1; i++)
        V[i+1][2] += p_seropos[i][V0];

    // cumulative expected number of new positives
    V[0][3] = V[0][2];
    for (i = 0; i < nweeks-1; i++)
      V[i+1][3] = V[i][3]+V[i+1][2];

    // seroconversion rate (new positives per week)
    // V[0][4] = 0;
    // for (i = 1; i < Data->n; i++)
    //   V[i][4] = V[i][2]/(test_weeks[i]-test_weeks[i-1]);

    // // number of infectious animals
    // for (i = 0; i < Data->n; i++)
    //   V[i][5] = num_infectious[test_weeks[i]];

    // Einf_field = doublevector(nweeks);
    // Einf_housed = doublevector(nweeks);
    // for (j = NUM_DONORS; j < NUMBER_SHEEP; j++)
    //   for (m = 0; m <= SHEEP_LAST_WEEK; m++)
    //     if (test_weeks[m] == 19 || test_weeks[m] == 20)
    //       Einf_housed[m] += (1.-exp(-pars.lambda[m]))*u(m, pars.Lambda);
    //     else
    //       Einf_field[m] += (1.-exp(-pars.lambda[m]))*u(m, pars.Lambda);

    // for (m = 1; m < nweeks; m++) {
    //   Einf_field[m] += Einf_field[m-1];
    //   Einf_housed[m] += Einf_housed[m-1];
    // }
    // for (i = 0; i < Data->n; i++) {
    //   V[i][1] = Einf_field[test_weeks[i]];
    //   V[i][6] = Einf_housed[test_weeks[i]];
    //   V[i][7] = V[i][1]+V[i][6];
    // }
    // free(Einf_field);
    // free(Einf_housed);

    // number infected
    // num_infected = integervector(nweeks);
    // for (j = 0; j < NUMBER_SHEEP; j++)
    //   if (sheep_pos_week[j] != -1)
    //     for (m = max(0, sheep_pos_week[j]-seroconversion); m <= SHEEP_LAST_WEEK; m++)
    //       num_infected[m]++;

    // for (i = 0; i < Data->n; i++)
    //   V[i][8] = num_infected[test_weeks[i]];
    // free(num_infected);
  }

  free(num_infectious);
  free(pars.lambda);
  free(pars.Lambda);
  free(pars.W4[0]); free(pars.W4);
  free(pars.W2);
  return SUCCESS;
}

double P_pos(int s1, int s2, double **V)
{
  int m;
  double sum = 0;
  for (m = s1; m < s2; m++)
    sum += V[m][V0];
  return sum;
}

double logLikelihood(const uint trt, const uint ind, const double *p, double **V, const TimePoints *Data)
{
  int j, pos_idx;
  double lnL = 0;

  for (j = NUM_DONORS; j < NUMBER_SHEEP; j++)
    if (sheep_pos_index[j] != 0) {
      pos_idx = sheep_pos_index[j];
      if (pos_idx > 0) {
        do {
          pos_idx--;
        } while (Data->t[pos_idx][j] == 0);
        // ewe is first seropositive at start of month m, therefore it seroconverted
        // sometime in the months from the last test to the month prior to seroconverting
        lnL += log(P_pos(lrint(Data->t[pos_idx][0]), sheep_pos_week[j], V));
      }
      else
        // ewe never seroconverted
        lnL += log(1. - P_pos(SHEEP_FIRST_TEST_WEEK, sheep_last_test_week[j], V));
    }
  return lnL;
}

void OutputModel(int trt, int ind, double *Output, double *V)
{
  int i;
  for (i = 1; i < V0; i++)
    Output[i] = V[i];
}

void OutputData(int trt, int ind, double *Output, double *Var, double *Data, uint *value, int index)
{
  Output[2] = Data[31];
  Output[3] = Data[32];
  Output[4] = Data[33];
  Output[5] = Data[34];
}

int f0(int trt, int ind, double *x, double *y, double *p, TimePoints *TP)
{
  // R0 2 month housed
  int i, n = 2;
  if (x == NULL) return n;
  for (i = 0; i < n; i++) {
    x[i] = 100*i+1;
    y[i] = R0(x[i], 2., 0.1, 5, p);
  }
  return n;
}

int f1(int trt, int ind, double *x, double *y, double *p, TimePoints *TP)
{
  // R0 0 months housed
  int i, n = 2;
  if (x == NULL) return n;
  for (i = 0; i < n; i++) {
    x[i] = 100*i+1;
    y[i] = R0(x[i], 0., 0.1, 5, p);
  }
  return n;
}

int f2(int trt, int ind, double *x, double *y, double *p, TimePoints *TP)
{
  // R0 for 25 sheep housed for different durations
  int i, n = 14;
  if (x == NULL) return n;
  for (i = 0; i < n; i++) {
    x[i] = i;
    y[i] = R0(25., x[i]*12./365., 0.1, 5, p);
  }
  return n;
}

int f3(int trt, int ind, double *x, double *y, double *p, TimePoints *TP)
{
  // CDF of normal seroconversion period
  int i, n = 1000;
  if (x == NULL) return n;
  double S_a = p[2];
  double S_k1 = p[3];

  for (i = 0; i < n; i++) {
    x[i] = (double)i/20.;
    // y[i] = gsl_ran_gamma_pdf(x[i], a, 1./b);
    y[i] = gsl_cdf_gamma_P(x[i], S_a, S_k1);
  }
  return n;
}

void function_list(functions_t *F)
{
  F->n_func = 0;
  // F->function_list = (function_ptr*) malloc(F->n_func*sizeof(function_ptr));
  // F->function_list[0] = f0;
  // F->function_list[1] = f1;
  // F->function_list[2] = f2;
  // F->function_list[3] = f3;
}

double u(int m, double *Lambda)
{
  // The probability a sheep is uninfected at the start of week m
  return exp(-Lambda[m]);
}

double lngseries(double a, double x, int *ierr)
{
  double giant = HUGE_VAL/1000., eps = 1e-15;
  double t = 1./a, v = t;
  double p, q, lnigam;
  int k = 0;
  *ierr = 0;

  while ((fabs(t/v) > eps) && *ierr == 0) {
    p = (a+k)/(a+k+1);
    q = k+1;
    t *= -x*p/q;
    v += t;
    k += 1;
    if (t > giant)
      *ierr = 1;
  }
  if (*ierr == 0)
    if (lngamma(a) < log(giant))
      lnigam = log(v)-lngamma(a);
    else {
      lnigam = 0;
      *ierr = 1;
    }
  else {
    lnigam = 0;
    *ierr = 1;
  }
  return lnigam;
}

double lngexpan(double a, double x, int *ierr)
{
  double giant = HUGE_VAL/1000., eps = 1e-15;
  double t = 1, v = t;
  double p, lnigam;
  int k = 0;
  *ierr = 0;

  if (x > log(giant)) {
      *ierr = 1;
      return 0;
  }
  while ((fabs(t/v) > eps) && *ierr == 0) {
    p = 1-a+k;
    t *= p/x;
    v += t;
    k += 1;
    if (t > giant)
      *ierr = 1;
  }
  if (*ierr == 0)
    if (lngamma(a) < log(giant))
      lnigam = log(v)+x-log(x)-lngamma(a);
    else {
      lnigam = 0;
      *ierr = 1;
    }
  else {
    lnigam = 0;
    *ierr = 1;
  }
  return lnigam;
}

double ln_inc_gamma_star(double a, double z)
{
  int ierr;
  double lnigam;

  if (z < -50) {
    lnigam = lngexpan(a, -z, &ierr);
    if (ierr != 0)
      printf("error1\n");
  }
  else {
    lnigam = lngseries(a, z, &ierr);
    if (ierr != 0)
      printf("error2\n");
  }
  return lnigam;
}

double w(int n, int t, int s, parameters_t *p)
{
  double a = p->a;
  double l = p->lambda[n];
  double b = p->b;
  double r = (b-l)/b;
  double Z, z, v;

  Z = b*(s-t);
  if (Z == 0)
    return 0;

  z = r*Z;
  if (z <= 0)
    v = exp(a*log(Z) - l*Z/b + ln_inc_gamma_star(a, z));
  else
    v = exp(-a*log(r) - l*Z/b + log(P_inc_gamma(a, z)));

  return exp(-l*(t-n))*(v - P_inc_gamma(a, Z));
}

double Prob_seroconvert(int m, parameters_t *p)
{
  int n;
  double P_m = 0;

  for (n = 0; n < m; n++)
    P_m += u(n, p->Lambda)*p->W4[n][m];
  P_m -= u(m, p->Lambda)*p->W2[m];

  return min(max(0, P_m), 1);
}

void DerivedParameters(int trt, int ind, int nmonths, double *p, double **V, TimePoints *Data)
{
  // int i, j, m;
  // double a, b;
  // double beta_field = p[0];
  // double beta_housed = p[1];
  // double mu = p[2];
  // double sd = p[3];
  // int seroconversion = lrint(mu);
  // int latency = lrint(p[4]);
  // int nweeks = lrint(Data->t[Data->n-1][0])+1;
  // int *test_weeks = integervector(Data->n);
  // int *num_infectious = integervector(nweeks);
  // double *Lambda = doublevector(nweeks);
  // double *lambda = doublevector(nweeks);

  // p[8]  = a = pow(mu/sd, 2.);
  // p[9]  = b = a/mu;
  // p[10] = beta_housed/beta_field;
  // p[15] = P_inv_gamma(0.025, a, 1./b);
  // p[16] = P_inv_gamma(0.975, a, 1./b);

  // for (i = 0; i < Data->n; i++)
  //   test_weeks[i] = lrint(Data->t[i][0]);

  // for (j = 0; j < NUM_DONORS; j++)
  //   for (m = 0; m <= SHEEP_LAST_WEEK; m++)
  //       num_infectious[m]++;

  // for (j = NUM_DONORS; j < NUMBER_SHEEP; j++)
  //   if (sheep_pos_week[j] > 0)
  //     for (m = max(0, sheep_pos_week[j]-seroconversion+latency); m <= SHEEP_LAST_WEEK; m++)
  //       num_infectious[m]++;

  // for (m = 0; m < nweeks; m++)
  //   if (test_weeks[m] == 19 || test_weeks[m] == 20)
  //     lambda[m] = num_infectious[m]*beta_housed;
  //   else
  //     lambda[m] = num_infectious[m]*beta_field;

  // Lambda[0] = 0;
  // for (m = 1; m < nweeks; m++)
  //   Lambda[m] = Lambda[m-1]+lambda[m-1];

  // p[12] = 0;
  // p[13] = 0;
  // for (j = NUM_DONORS; j < NUMBER_SHEEP; j++)
  //   for (m = 0; m <= SHEEP_LAST_WEEK; m++)
  //     if (test_weeks[m] == 19 || test_weeks[m] == 20)
  //       p[13] += (1.-exp(-lambda[m]))*u(m, Lambda);
  //     else
  //       p[12] += (1.-exp(-lambda[m]))*u(m, Lambda);
  // p[14] = p[12]+p[13];

  // free(lambda);
  // free(Lambda);
  // free(num_infectious);
  // free(test_weeks);
}

void WAIC(int trt, int ind, double **lnL, double **V, double *p, const TimePoints *Data) {}
void PredictData(int trt, int ind, double *Output, double *V, double *p, gsl_rng *stream) {}
void SimulateData(int trt, int ind, double *Output, double *V, double *p, double *Data, uint *value, gsl_rng *stream) {}
double timestep(void) {return 1;}
void GlobalParameters() {}
double UserPDF(double x) {return 0;}
void HyperParameters(Treatments T, Hyperparameters *H) {}
void SaturatedModel(int trt, int ind, double **V, double *p, const TimePoints *Data) {}
void Residual(int trt, int ind, double *R, double *V, double *p, double *Data, uint *value) {}


/* specfunc/legendre_Qn.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 *
 * 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.
 */

/* Author:  G. Jungman */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include "gsl_sf_bessel.h"
#include "gsl_sf_elementary.h"
#include "gsl_sf_exp.h"
#include "gsl_sf_pow_int.h"
#include "gsl_sf_legendre.h"

#include "error.h"

/* Evaluate f_{ell+1}/f_ell
 * f_ell := Q^{b}_{a+ell}(x)
 * x > 1
 */
static
int
legendreQ_CF1_xgt1(int ell, double a, double b, double x, double * result)
{
  const double RECUR_BIG = GSL_SQRT_DBL_MAX;
  const int maxiter = 5000;
  int n = 1;
  double Anm2 = 1.0;
  double Bnm2 = 0.0;
  double Anm1 = 0.0;
  double Bnm1 = 1.0;
  double a1 = ell + 1.0 + a + b;
  double b1 = (2.0*(ell+1.0+a) + 1.0) * x;
  double An = b1*Anm1 + a1*Anm2;
  double Bn = b1*Bnm1 + a1*Bnm2;
  double an, bn;
  double fn = An/Bn;

  while(n < maxiter) {
    double old_fn;
    double del;
    double lna;
    n++;
    Anm2 = Anm1;
    Bnm2 = Bnm1;
    Anm1 = An;
    Bnm1 = Bn;
    lna = ell + n + a;
    an = b*b - lna*lna;
    bn = (2.0*lna + 1.0) * x;
    An = bn*Anm1 + an*Anm2;
    Bn = bn*Bnm1 + an*Bnm2;

    if(fabs(An) > RECUR_BIG || fabs(Bn) > RECUR_BIG) {
      An /= RECUR_BIG;
      Bn /= RECUR_BIG;
      Anm1 /= RECUR_BIG;
      Bnm1 /= RECUR_BIG;
      Anm2 /= RECUR_BIG;
      Bnm2 /= RECUR_BIG;
    }

    old_fn = fn;
    fn = An/Bn;
    del = old_fn/fn;

    if(fabs(del - 1.0) < 4.0*GSL_DBL_EPSILON) break;
  }

  *result = fn;

  if(n == maxiter)
    GSL_ERROR ("error", GSL_EMAXITER);
  else
    return GSL_SUCCESS;
}

/* Uniform asymptotic for Q_l(x).
 * Assumes x > -1.0 and x != 1.0.
 * Discards second order and higher terms.
 */
static
int
legendre_Ql_asymp_unif(const double ell, const double x, gsl_sf_result * result)
{
  if(x < 1.0) {
    double u   = ell + 0.5;
    double th  = acos(x);
    gsl_sf_result Y0, Y1;
    int stat_Y0, stat_Y1;
    int stat_m;
    double pre;
    double B00;
    double sum;

    /* B00 = 1/8 (1 - th cot(th) / th^2
     * pre = sqrt(th/sin(th))
     */
    if(th < GSL_ROOT4_DBL_EPSILON) {
      B00 = (1.0 + th*th/15.0)/24.0;
      pre = 1.0 + th*th/12.0;
    }
    else {
      double sin_th = sqrt(1.0 - x*x);
      double cot_th = x / sin_th;
      B00 = 1.0/8.0 * (1.0 - th * cot_th) / (th*th);
      pre = sqrt(th/sin_th);
    }

    stat_Y0 = gsl_sf_bessel_Y0_e(u*th, &Y0);
    stat_Y1 = gsl_sf_bessel_Y1_e(u*th, &Y1);

    sum = -0.5*M_PI * (Y0.val + th/u * Y1.val * B00);

    stat_m = gsl_sf_multiply_e(pre, sum, result);
    result->err += 0.5*M_PI * fabs(pre) * (Y0.err + fabs(th/u*B00)*Y1.err);
    result->err += GSL_DBL_EPSILON * fabs(result->val);

    return GSL_ERROR_SELECT_3(stat_m, stat_Y0, stat_Y1);
  }
  else {
    double u   = ell + 0.5;
    double xi  = acosh(x);
    gsl_sf_result K0_scaled, K1_scaled;
    int stat_K0, stat_K1;
    int stat_e;
    double pre;
    double B00;
    double sum;

    /* B00 = -1/8 (1 - xi coth(xi) / xi^2
     * pre = sqrt(xi/sinh(xi))
     */
    if(xi < GSL_ROOT4_DBL_EPSILON) {
      B00 = (1.0-xi*xi/15.0)/24.0;
      pre = 1.0 - xi*xi/12.0;
    }
    else {
      double sinh_xi = sqrt(x*x - 1.0);
      double coth_xi = x / sinh_xi;
      B00 = -1.0/8.0 * (1.0 - xi * coth_xi) / (xi*xi);
      pre = sqrt(xi/sinh_xi);
    }

    stat_K0 = gsl_sf_bessel_K0_scaled_e(u*xi, &K0_scaled);
    stat_K1 = gsl_sf_bessel_K1_scaled_e(u*xi, &K1_scaled);

    sum = K0_scaled.val - xi/u * K1_scaled.val * B00;

    stat_e = gsl_sf_exp_mult_e(-u*xi, pre * sum, result);
    result->err  = GSL_DBL_EPSILON * fabs(result->val) * fabs(u*xi);
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);

    return GSL_ERROR_SELECT_3(stat_e, stat_K0, stat_K1);
  }
}

/*-*-*-*-*-*-*-*-*-*-*-* Functions with Error Codes *-*-*-*-*-*-*-*-*-*-*-*/

int
gsl_sf_legendre_Q0_e(const double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(x <= -1.0 || x == 1.0) {
    DOMAIN_ERROR(result);
  }
  else if(x*x < GSL_ROOT6_DBL_EPSILON) { /* |x| <~ 0.05 */
    const double c3 = 1.0/3.0;
    const double c5 = 1.0/5.0;
    const double c7 = 1.0/7.0;
    const double c9 = 1.0/9.0;
    const double c11 = 1.0/11.0;
    const double y = x * x;
    const double series = 1.0 + y*(c3 + y*(c5 + y*(c7 + y*(c9 + y*c11))));
    result->val = x * series;
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(x);
    return GSL_SUCCESS;
  }
  else if(x < 1.0) {
    result->val = 0.5 * log((1.0+x)/(1.0-x));
    result->err  = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x < 10.0) {
    result->val = 0.5 * log((x+1.0)/(x-1.0));
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x*GSL_DBL_MIN < 2.0) {
    const double y = 1.0/(x*x);
    const double c1 = 1.0/3.0;
    const double c2 = 1.0/5.0;
    const double c3 = 1.0/7.0;
    const double c4 = 1.0/9.0;
    const double c5 = 1.0/11.0;
    const double c6 = 1.0/13.0;
    const double c7 = 1.0/15.0;
    result->val = (1.0/x) * (1.0 + y*(c1 + y*(c2 + y*(c3 + y*(c4 + y*(c5 + y*(c6 + y*c7)))))));
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    UNDERFLOW_ERROR(result);
  }
}

int
gsl_sf_legendre_Q1_e(const double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(x <= -1.0 || x == 1.0) {
    DOMAIN_ERROR(result);
  }
  else if(x*x < GSL_ROOT6_DBL_EPSILON) { /* |x| <~ 0.05 */
    const double c3 = 1.0/3.0;
    const double c5 = 1.0/5.0;
    const double c7 = 1.0/7.0;
    const double c9 = 1.0/9.0;
    const double c11 = 1.0/11.0;
    const double y = x * x;
    const double series = 1.0 + y*(c3 + y*(c5 + y*(c7 + y*(c9 + y*c11))));
    result->val = x * x * series - 1.0;
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x < 1.0){
    result->val = 0.5 * x * (log((1.0+x)/(1.0-x))) - 1.0;
    result->err  = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x < 6.0) {
    result->val = 0.5 * x * log((x+1.0)/(x-1.0)) - 1.0;
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x*GSL_SQRT_DBL_MIN < 0.99/M_SQRT3) {
    const double y = 1/(x*x);
    const double c1 = 3.0/5.0;
    const double c2 = 3.0/7.0;
    const double c3 = 3.0/9.0;
    const double c4 = 3.0/11.0;
    const double c5 = 3.0/13.0;
    const double c6 = 3.0/15.0;
    const double c7 = 3.0/17.0;
    const double c8 = 3.0/19.0;
    const double sum = 1.0 + y*(c1 + y*(c2 + y*(c3 + y*(c4 + y*(c5 + y*(c6 + y*(c7 + y*c8)))))));
    result->val = sum / (3.0*x*x);
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    UNDERFLOW_ERROR(result);
  }
}

int
gsl_sf_legendre_Ql_e(const int l, const double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(x <= -1.0 || x == 1.0 || l < 0) {
    DOMAIN_ERROR(result);
  }
  else if(l == 0) {
    return gsl_sf_legendre_Q0_e(x, result);
  }
  else if(l == 1) {
    return gsl_sf_legendre_Q1_e(x, result);
  }
  else if(l > 100000) {
    return legendre_Ql_asymp_unif(l, x, result);
  }
  else if(x < 1.0){
    /* Forward recurrence.
     */
    gsl_sf_result Q0, Q1;
    int stat_Q0 = gsl_sf_legendre_Q0_e(x, &Q0);
    int stat_Q1 = gsl_sf_legendre_Q1_e(x, &Q1);
    double Qellm1 = Q0.val;
    double Qell   = Q1.val;
    double Qellp1;
    int ell;
    for(ell=1; ell<l; ell++) {
      Qellp1 = (x*(2.0*ell + 1.0) * Qell - ell * Qellm1) / (ell + 1.0);
      Qellm1 = Qell;
      Qell   = Qellp1;
    }
    result->val = Qell;
    result->err = GSL_DBL_EPSILON * l * fabs(result->val);
    return GSL_ERROR_SELECT_2(stat_Q0, stat_Q1);
  }
  else {
    /* x > 1.0 */

    double rat;
    int stat_CF1  = legendreQ_CF1_xgt1(l, 0.0, 0.0, x, &rat);
    int stat_Q;
    double Qellp1 = rat * GSL_SQRT_DBL_MIN;
    double Qell   = GSL_SQRT_DBL_MIN;
    double Qellm1;
    int ell;
    for(ell=l; ell>0; ell--) {
      Qellm1 = (x * (2.0*ell + 1.0) * Qell - (ell+1.0) * Qellp1) / ell;
      Qellp1 = Qell;
      Qell   = Qellm1;
    }

    if(fabs(Qell) > fabs(Qellp1)) {
      gsl_sf_result Q0;
      stat_Q = gsl_sf_legendre_Q0_e(x, &Q0);
      result->val = GSL_SQRT_DBL_MIN * Q0.val / Qell;
      result->err = l * GSL_DBL_EPSILON * fabs(result->val);
    }
    else {
      gsl_sf_result Q1;
      stat_Q = gsl_sf_legendre_Q1_e(x, &Q1);
      result->val = GSL_SQRT_DBL_MIN * Q1.val / Qellp1;
      result->err = l * GSL_DBL_EPSILON * fabs(result->val);
    }

    return GSL_ERROR_SELECT_2(stat_Q, stat_CF1);
  }
}

/*-*-*-*-*-*-*-*-*-* Functions w/ Natural Prototypes *-*-*-*-*-*-*-*-*-*-*/

#include "eval.h"

double gsl_sf_legendre_Q0(const double x)
{
  EVAL_RESULT(gsl_sf_legendre_Q0_e(x, &result));
}

double gsl_sf_legendre_Q1(const double x)
{
  EVAL_RESULT(gsl_sf_legendre_Q1_e(x, &result));
}

double gsl_sf_legendre_Ql(const int l, const double x)
{
  EVAL_RESULT(gsl_sf_legendre_Ql_e(l, x, &result));
}