/*
 *  SpecialFunctions.h
 *  PSCAcoustic
 *
 *  Special functions and more.
 *
 *
 * Copyright 2014 Pierre-David Letourneau
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to
 * deal in the Software without restriction, including without limitation the
 * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
 * sell copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
*/

#ifndef SPECIALFUNCTIONS_H
#define SPECIALFUNCTIONS_H

#include "General.h"

// GSL library
//#include "../gsl-1.16/include/gsl/gsl_sf_bessel.h" //spherical bessel functions
#include <gsl/gsl_sf_bessel.h> //spherical bessel functions
#include "./gsl-1.16/include/gsl/gsl_sf_legendre.h" //associated Legendre polynomials (for spherical harmonics)
#include "./gsl-1.16/include/gsl/gsl_complex_math.h"
#include "./gsl-1.16/include/gsl/gsl_sf_gamma.h"
#include "./gsl-1.16/include/gsl/gsl_sf_hyperg.h"
#include "./gsl-1.16/include/gsl/gsl_sf_coupling.h"
#include "./gsl-1.16/include/gsl/gsl_integration.h"
#include "./gsl-1.16/include/gsl/gsl_complex.h"
#include "./gsl-1.16/include/gsl/gsl_sf_coupling.h"
#include "./gsl-1.16/include/gsl/gsl_math.h"
#include "./gsl-1.16/include/gsl/gsl_ieee_utils.h"
#include "./gsl-1.16/include/gsl/gsl_errno.h"
#include "./gsl-1.16/include/gsl/gsl_integration.h"

// Complex Bessel functions
//#include "../AmosBessel/libAmosBessel.h"

//GSL wrappers (Cannot handle complex  arguments)

//Hankel function of the 1st kind (gsl)
inline complex  gsl_sf_hankel_1( int l, double x){
  return gsl_sf_bessel_jl(l,x) + gsl_sf_bessel_yl(l,x) * CI;
}

inline double gsl_sf_legendre( int l, int m, double x){
  double C = 1;
  if( m < 0 )
    C *= pow(-1, std::abs(m));

  return (C * gsl_sf_legendre_sphPlm ( l, std::abs(m), x) );
}

// Y_lm (spherical harmonics)
//\sqrt{(2l+1)/(4\pi)} \sqrt{(l-m)!/(l+m)!} P_l^m(x) exp(I*m*phi) )
inline complex  gsl_sf_harmonic( int l, int m, double theta, double phi){
  if( l >= std::abs(m) ){
    return gsl_sf_legendre(l, m, cos(theta)) * exp(m*phi*CI);
  } else {
    return 0.;
  }
}

inline complex  gsl_sf_clebsch(int l1, int l2, int l3, int m1, int m2, int m3){
  return pow(-1., (double) l1-l2+m3) * sqrt(2.*l3+1.) * gsl_sf_coupling_3j(2*l1, 2*l2, 2*l3, 2*m1, 2*m2, -2*m3);
}

inline double gsl_sf_gaunt(int n, int m, int nu, int mu, int q){
  return pow(-1., (double) (m+mu) ) * sqrt((2*n+1)*(2*nu+1)*(2*q+1)/(4*PI)) *
    gsl_sf_coupling_3j(2.*n,2.*nu,2.*q,0,0,0) * gsl_sf_coupling_3j(2.*n,2.*nu,2.*q,2.*m,2.*mu,-2.*(m+mu));
}

// TODO: FIX AMOS
//Amos wrappers for Bessel functions (these can handle complex  arguments)
inline complex  Amos_sf_bessel_jl(int l, complex  z){
  	complex  value;
	//AmosBessel('j', z, (double) l, 1, 0, &value);
	value = gsl_sf_bessel_jl(l,std::real(z));
	return value;
}

inline complex  Amos_sf_bessel_yl(int l, complex  z){
  	complex  value;
	//AmosBessel('y', z, (double) l, 1, 0, &value);
	value = gsl_sf_bessel_yl(l,std::real(z));
	return value;
}

inline complex  Amos_sf_hankel_1(int l, complex  z){
  //complex  valueR;
	//AmosBessel('j', z, (double) l, 1, 0, &valueR);
  	//complex  valueI;
	//AmosBessel('y', z, (double) l, 1, 0, &valueI);
	//return (valueR + valueI*CI);

	complex value = gsl_sf_hankel_1(l, std::real(z));
	return value;
}

// Derivatives

inline double gsl_sf_bessel_jl_prime(int l, double z){
  return l/z * gsl_sf_bessel_jl(l,z) - gsl_sf_bessel_jl(l+1,z);
}

inline complex  gsl_sf_hankel_l_prime(int l, double z){
  return l/z * gsl_sf_hankel_1( l, z) - gsl_sf_hankel_1(l+1, z);
}

inline complex  Amos_sf_bessel_jl_prime(double l, complex  z){
  return l/z * Amos_sf_bessel_jl(l,z) - Amos_sf_bessel_jl(l+1,z);
}

inline complex  Amos_sf_hankel_l_prime(double l, complex  z){
  return l/z * Amos_sf_hankel_1( l, z) - Amos_sf_hankel_1(l+1, z);
}

// Chebyshev polynomial evaluation
inline double Cheb(int n, double x){
  return cos(n*acos(x));
}

// Gauss-Legendre quadrature
inline void getGaussLegendreQuad(int N, std::vector<double>& x, std::vector<double>& w)
{
  x.resize(N);
  w.resize(N);

  gsl_integration_glfixed_table* GLTable = gsl_integration_glfixed_table_alloc(N);

  for( int n = 0; n < (N+1)/2; ++n ) {
    x[n + N/2] = GLTable->x[n];
    w[n + N/2] = GLTable->w[n];
  }
  for( int n = 0; n < N/2; ++n ) {
    x[n] = -x[N-1-n];
    w[n] =  w[N-1-n];
  }

  gsl_integration_glfixed_table_free( GLTable );
}

// Gauss-Chebyshev quadrature
inline void getGaussChebyshevQuad(int N, std::vector<double>& x, std::vector<double>& w)
{
  x.resize(N);
  w.resize(N);

  for( int n = 0; n < N; n++ ) {
    x[n] = cos( (2.*n+1.)/(2.*N)*PI  );
    w[n] = 2./N;
  }
}

//---------General Functions--------//

#define ISODD(x) ((x) & 1)
#define ISNAN(x) ((x) != (x))

//TODO : still needed?
inline int choose( int n, int r ){

  if( (n < r) || (r < 0) || (n < 0) ){
    return 0;
  } else {
    return gsl_sf_gamma(n+1) / (gsl_sf_gamma(r+1) * gsl_sf_gamma(n-r+1));
  }

}

// Implementation os Sparse Matrix class
// TODO:  this takes a lot of memory?
class SpMatrix
{
  map<std::pair<int,int>, complex > M;

 public:

  // Empty constructor
  SpMatrix(){}

  ~SpMatrix(){}

  void add(int i, int j, complex  val){
    assert( i >= 0 );
    assert( j >= 0 );

    std::pair<int,int> coord = std::make_pair(i,j);
    if( M.count(coord) ){
      M.insert(std::make_pair(coord, val));
    } else {
      M[coord] = val;
    }

  }

  complex  operator () (int i, int j){
    std::pair<int,int> coord = std::make_pair(i,j);
    complex  val = 0.;

    ( M.count(coord) ) ? val = M.find(coord)->second : val = 0.;

    return val;
  }

  inline int size() { return M.size(); }

};

#endif


/* linalg/sytd.c
 *
 * Copyright (C) 2001, 2007 Brian Gough
 * Copyright (C) 2019 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.
 */

/* Factorise a symmetric matrix A into
 *
 * A = Q T Q'
 *
 * where Q is orthogonal and T is symmetric tridiagonal.  Only the
 * diagonal and lower triangular part of A is referenced and modified.
 *
 * On exit, T is stored in the diagonal and first subdiagonal of
 * A. Since T is symmetric the upper diagonal is not stored.
 *
 * Q is stored as a packed set of Householder transformations in the
 * lower triangular part of the input matrix below the first subdiagonal.
 *
 * The full matrix for Q can be obtained as the product
 *
 *       Q = Q_1 Q_2 ... Q_(N-2)
 *
 * where
 *
 *       Q_i = (I - tau_i * v_i * v_i')
 *
 * and where v_i is a Householder vector
 *
 *       v_i = [0, ... , 0, 1, A(i+1,i), A(i+2,i), ... , A(N,i)]
 *
 * This storage scheme is the same as in LAPACK.  See LAPACK's
 * ssytd2.f for details.
 *
 * See Golub & Van Loan, "Matrix Computations" (3rd ed), Section 8.3
 *
 * Note: this description uses 1-based indices. The code below uses
 * 0-based indices
 */

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>

#include <gsl/gsl_linalg.h>

int
gsl_linalg_symmtd_decomp (gsl_matrix * A, gsl_vector * tau)
{
  if (A->size1 != A->size2)
    {
      GSL_ERROR ("symmetric tridiagonal decomposition requires square matrix",
                 GSL_ENOTSQR);
    }
  else if (tau->size + 1 != A->size1)
    {
      GSL_ERROR ("size of tau must be N-1", GSL_EBADLEN);
    }
  else
    {
      const size_t N = A->size1;
      size_t i;

      for (i = 0 ; i < N - 2; i++)
        {
          gsl_vector_view v = gsl_matrix_subcolumn (A, i, i + 1, N - i - 1);
          double tau_i = gsl_linalg_householder_transform (&v.vector);

          /* Apply the transformation H^T A H to the remaining columns */

          if (tau_i != 0.0)
            {
              gsl_matrix_view m = gsl_matrix_submatrix (A, i + 1, i + 1, N - i - 1, N - i - 1);
              double ei = gsl_vector_get(&v.vector, 0);
              gsl_vector_view x = gsl_vector_subvector (tau, i, N - i - 1);

              gsl_vector_set (&v.vector, 0, 1.0);

              /* x = tau * A * v */
              gsl_blas_dsymv (CblasLower, tau_i, &m.matrix, &v.vector, 0.0, &x.vector);

              /* w = x - (1/2) tau * (x' * v) * v  */
              {
                double xv, alpha;
                gsl_blas_ddot(&x.vector, &v.vector, &xv);
                alpha = -0.5 * tau_i * xv;
                gsl_blas_daxpy(alpha, &v.vector, &x.vector);
              }

              /* apply the transformation A = A - v w' - w v' */
              gsl_blas_dsyr2(CblasLower, -1.0, &v.vector, &x.vector, &m.matrix);

              gsl_vector_set (&v.vector, 0, ei);
            }

          gsl_vector_set (tau, i, tau_i);
        }

      return GSL_SUCCESS;
    }
}

/*  Form the orthogonal matrix Q from the packed QR matrix */

int
gsl_linalg_symmtd_unpack (const gsl_matrix * A,
                          const gsl_vector * tau,
                          gsl_matrix * Q,
                          gsl_vector * diag,
                          gsl_vector * sdiag)
{
  if (A->size1 !=  A->size2)
    {
      GSL_ERROR ("matrix A must be square", GSL_ENOTSQR);
    }
  else if (tau->size + 1 != A->size1)
    {
      GSL_ERROR ("size of tau must be (matrix size - 1)", GSL_EBADLEN);
    }
  else if (Q->size1 != A->size1 || Q->size2 != A->size1)
    {
      GSL_ERROR ("size of Q must match size of A", GSL_EBADLEN);
    }
  else if (diag->size != A->size1)
    {
      GSL_ERROR ("size of diagonal must match size of A", GSL_EBADLEN);
    }
  else if (sdiag->size + 1 != A->size1)
    {
      GSL_ERROR ("size of subdiagonal must be (matrix size - 1)", GSL_EBADLEN);
    }
  else
    {
      const size_t N = A->size1;
      gsl_vector_const_view d = gsl_matrix_const_diagonal(A);;
      gsl_vector_const_view sd = gsl_matrix_const_subdiagonal(A, 1);;
      size_t i;

      /* Initialize Q to the identity */

      gsl_matrix_set_identity (Q);

      for (i = N - 2; i-- > 0;)
        {
          gsl_vector_const_view h = gsl_matrix_const_subcolumn (A, i, i + 1, N - i - 1);
          double ti = gsl_vector_get (tau, i);
          gsl_matrix_view m = gsl_matrix_submatrix (Q, i + 1, i + 1, N - i - 1, N - i - 1);
          gsl_vector_view work = gsl_vector_subvector(diag, 0, N - i - 1);

          gsl_linalg_householder_left (ti, &h.vector, &m.matrix, &work.vector);
        }

      /* copy diagonal into diag */
      gsl_vector_memcpy(diag, &d.vector);

      /* copy subdiagonal into sd */
      gsl_vector_memcpy(sdiag, &sd.vector);

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_symmtd_unpack_T (const gsl_matrix * A,
                            gsl_vector * diag,
                            gsl_vector * sdiag)
{
  if (A->size1 !=  A->size2)
    {
      GSL_ERROR ("matrix A must be square", GSL_ENOTSQR);
    }
  else if (diag->size != A->size1)
    {
      GSL_ERROR ("size of diagonal must match size of A", GSL_EBADLEN);
    }
  else if (sdiag->size + 1 != A->size1)
    {
      GSL_ERROR ("size of subdiagonal must be (matrix size - 1)", GSL_EBADLEN);
    }
  else
    {
      const size_t N = A->size1;
      gsl_vector_const_view d = gsl_matrix_const_diagonal(A);;
      gsl_vector_const_view sd = gsl_matrix_const_subdiagonal(A, 1);;

      /* copy diagonal into diag */
      gsl_vector_memcpy(diag, &d.vector);

      /* copy subdiagonal into sd */
      gsl_vector_memcpy(sdiag, &sd.vector);

      return GSL_SUCCESS;
    }
}


/* multimin/diff.c
 *
 * Copyright (C) 2000 David Morrison
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <config.h>
#include <gsl/gsl_multimin.h>

int
gsl_multimin_diff (const gsl_multimin_function * f,
                   const gsl_vector * x, gsl_vector * g)
{
  size_t i, n = f->n;

  double h = GSL_SQRT_DBL_EPSILON;

  gsl_vector * x1 = gsl_vector_alloc (n);  /* FIXME: pass as argument */

  gsl_vector_memcpy (x1, x);

  for (i = 0; i < n; i++)
    {
      double fl, fh;

      double xi = gsl_vector_get (x, i);
      double dx = fabs(xi) * h;

      if (dx == 0.0) dx = h;

      gsl_vector_set (x1, i, xi + dx);
      fh = GSL_MULTIMIN_FN_EVAL(f, x1);

      gsl_vector_set (x1, i, xi - dx);
      fl = GSL_MULTIMIN_FN_EVAL(f, x1);

      gsl_vector_set (x1, i, xi);
      gsl_vector_set (g, i, (fh - fl) / (2.0 * dx));
    }

  gsl_vector_free (x1);

  return GSL_SUCCESS;
}


/*
 * Kabsch's algorithm: compute least-square-best transformation
 * Copyright (C) 2003, Arno Formella (formella@ei.uvigo.es)
 *
 * 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.
 *
 * This is a gsl-based implementation of Kabsch's algorithm presented in:
 *
 *   W. Kabsch, A solution for the best rotation to relate two sets of vectors,
 *   Acta Cryst. (1976), A32, 922-923
 *
 *   W. Kabsch, A discussion of the solution for the best rotation to relate
 *   two sets of vectors, Acta Cryst. (1978), A34, 827-828
 *
 * The code is C++-robust.
 *
 * More information about GSL
 * ==========================
 *
 * The project homepage is http://www.gnu.org/software/gsl/
 * The development site is http://sources.redhat.com/gsl/
 */

#ifndef KABSCH_H
#define KABSCH_H

#include <gsl/gsl_vector_double.h>
#include <gsl/gsl_matrix_double.h>

#if defined(__cplusplus)
extern "C" {
#endif

/*
   preconditions:
     size > 0
     X and Y point to (size x 3)-matrices specifying 3D points x_i and y_i
     U points to a (3 x 3)-matrix
     t points to a (3)-vector
     s points to double
   postconditions if return 1:
     X and Y will be centralized at their respective origins
     U will hold the rotation
     t will hold the translation
     s will hold the scaling value
     such that:
       sum_i (U * s * x_i + t - y_i)^2  is minimal
   postconditions if return 0:
     X and Y will be centralized at their respective origins
     U will hold the identity
     t will hold the difference vector of the centroids t = cy -cx
     s will hold 1.0
   note:
     if s == NULL, no scaling is computed
*/
int kabsch(
  unsigned int size,
  gsl_matrix *X,
  gsl_matrix *Y,
  gsl_matrix *U,
  gsl_vector *t,
  double *s
);

#if defined(__cplusplus)
}
#endif

#endif


/* bspline/bspline.c
 *
 * Copyright (C) 2006, 2007, 2008, 2009 Patrick Alken
 * Copyright (C) 2008 Rhys Ulerich
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <config.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_bspline.h>

/*
 * This module contains routines related to calculating B-splines.
 * The algorithms used are described in
 *
 * [1] Carl de Boor, "A Practical Guide to Splines", Springer
 *     Verlag, 1978.
 *
 * The bspline_pppack_* internal routines contain code adapted from
 *
 * [2] "PPPACK - Piecewise Polynomial Package",
 *     http://www.netlib.org/pppack/
 *
 */

#include "bspline.h"

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

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

Return: pointer to workspace
*/

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

      w = (gsl_bspline_workspace *) malloc (sizeof (gsl_bspline_workspace));

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

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

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

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

      w->deltar = gsl_vector_alloc (k);
      if (w->deltar == 0)
	{
	  gsl_vector_free (w->deltal);
	  gsl_vector_free (w->knots);
	  free (w);
	  GSL_ERROR_NULL ("failed to allocate space for deltar vector",
			  GSL_ENOMEM);
	}

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

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

/*
gsl_bspline_deriv_alloc()
  Allocate space for a bspline derivative workspace. The size of the
workspace is O(2k^2)

Inputs: k      - spline order (cubic = 4)

Return: pointer to workspace
*/

gsl_bspline_deriv_workspace *
gsl_bspline_deriv_alloc (const size_t k)
{
  if (k == 0)
    {
      GSL_ERROR_NULL ("k must be at least 1", GSL_EINVAL);
    }
  else
    {
      gsl_bspline_deriv_workspace *dw;

      dw =
	(gsl_bspline_deriv_workspace *)
	malloc (sizeof (gsl_bspline_deriv_workspace));

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

      dw->A = gsl_matrix_alloc (k, k);
      if (dw->A == 0)
	{
	  free (dw);
	  GSL_ERROR_NULL
	    ("failed to allocate space for derivative work matrix",
	     GSL_ENOMEM);
	}

      dw->dB = gsl_matrix_alloc (k, k + 1);
      if (dw->dB == 0)
	{
	  gsl_matrix_free (dw->A);
	  free (dw);
	  GSL_ERROR_NULL
	    ("failed to allocate space for temporary derivative matrix",
	     GSL_ENOMEM);
	}

      dw->k = k;

      return dw;
    }
}				/* gsl_bspline_deriv_alloc() */

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

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

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

/* Return the location of the i-th breakpoint*/
double
gsl_bspline_breakpoint (size_t i, gsl_bspline_workspace * w)
{
  size_t j = i + w->k - 1;
  return gsl_vector_get (w->knots, j);
}

/*
gsl_bspline_free()
  Free a gsl_bspline_workspace.

Inputs: w - workspace to free

Return: none
*/

void
gsl_bspline_free (gsl_bspline_workspace * w)
{
  RETURN_IF_NULL (w);
  gsl_vector_free (w->knots);
  gsl_vector_free (w->deltal);
  gsl_vector_free (w->deltar);
  gsl_vector_free (w->B);
  free (w);
}				/* gsl_bspline_free() */

/*
gsl_bspline_deriv_free()
  Free a gsl_bspline_deriv_workspace.

Inputs: dw - workspace to free

Return: none
*/

void
gsl_bspline_deriv_free (gsl_bspline_deriv_workspace * dw)
{
  RETURN_IF_NULL (dw);
  gsl_matrix_free (dw->A);
  gsl_matrix_free (dw->dB);
  free (dw);
}				/* gsl_bspline_deriv_free() */

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

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

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

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

Inputs: breakpts - breakpoints
        w        - bspline workspace

Return: success or error
*/

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

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

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

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

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

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

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

Return: success or error

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

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

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

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

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

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

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

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

  return GSL_SUCCESS;
}				/* gsl_bspline_knots_uniform() */

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

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

Return: success or error

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

int
gsl_bspline_eval (const double x, gsl_vector * B, gsl_bspline_workspace * w)
{
  if (B->size != w->n)
    {
      GSL_ERROR ("vector B not of length n", GSL_EBADLEN);
    }
  else
    {
      size_t i;			/* looping */
      size_t istart;		/* first non-zero spline for x */
      size_t iend;		/* last non-zero spline for x, knot for x */
      int error;		/* error handling */

      /* find all non-zero B_i(x) values */
      error = gsl_bspline_eval_nonzero (x, w->B, &istart, &iend, w);
      if (error)
	{
	  return error;
	}

      /* store values in appropriate part of given vector */
      for (i = 0; i < istart; i++)
	gsl_vector_set (B, i, 0.0);

      for (i = istart; i <= iend; i++)
	gsl_vector_set (B, i, gsl_vector_get (w->B, i - istart));

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

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

/*
gsl_bspline_eval_nonzero()
  Evaluate all non-zero B-spline functions at point x.
These are the B_i(x) for i in [istart, iend].
Always B_i(x) = 0 for i < istart and for i > iend.

Inputs: x      - point at which to evaluate splines
        Bk     - (output) where to store B-spline values (length k)
        istart - (output) B-spline function index of
                 first non-zero basis for given x
        iend   - (output) B-spline function index of
                 last non-zero basis for given x.
                 This is also the knot index corresponding to x.
        w      - bspline workspace

Return: success or error

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

       2) On output, B contains

             [B_{istart,k}, B_{istart+1,k},
             ..., B_{iend-1,k}, B_{iend,k}]

          evaluated at the given x.
*/

int
gsl_bspline_eval_nonzero (const double x, gsl_vector * Bk, size_t * istart,
			  size_t * iend, gsl_bspline_workspace * w)
{
  if (Bk->size != w->k)
    {
      GSL_ERROR ("Bk vector length does not match order k", GSL_EBADLEN);
    }
  else
    {
      size_t i;			/* spline index */
      size_t j;			/* looping */
      int flag = 0;		/* interval search flag */
      int error = 0;		/* error flag */

      i = bspline_find_interval (x, &flag, w);
      error = bspline_process_interval_for_eval (x, &i, flag, w);
      if (error)
	{
	  return error;
	}

      *istart = i - w->k + 1;
      *iend = i;

      bspline_pppack_bsplvb (w->knots, w->k, 1, x, *iend, &j, w->deltal,
			     w->deltar, Bk);

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

/*
gsl_bspline_deriv_eval()
  Evaluate d^j/dx^j B_i(x) for all i, 0 <= j <= nderiv.
This is a wrapper function for gsl_bspline_deriv_eval_nonzero()
which formats the output in a nice way.

Inputs: x      - point for evaluation
        nderiv - number of derivatives to compute, inclusive.
        dB     - (output) where to store d^j/dx^j B_i(x)
                 values. the size of this matrix is
                 (n = nbreak + k - 2 = l + k - 1 = w->n)
                 by (nderiv + 1)
        w      - bspline derivative workspace

Return: success or error

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

       2) based on PPPACK's bsplvd
*/

int
gsl_bspline_deriv_eval (const double x, const size_t nderiv, gsl_matrix * dB,
			gsl_bspline_workspace * w,
			gsl_bspline_deriv_workspace * dw)
{
  if (dB->size1 != w->n)
    {
      GSL_ERROR ("dB matrix first dimension not of length n", GSL_EBADLEN);
    }
  else if (dB->size2 < nderiv + 1)
    {
      GSL_ERROR
	("dB matrix second dimension must be at least length nderiv+1",
	 GSL_EBADLEN);
    }
  else if (dw->k < w->k)
    {
      GSL_ERROR ("derivative workspace is too small", GSL_EBADLEN);
    }
  else
    {
      size_t i;			/* looping */
      size_t j;			/* looping */
      size_t istart;		/* first non-zero spline for x */
      size_t iend;		/* last non-zero spline for x, knot for x */
      int error;		/* error handling */

      /* find all non-zero d^j/dx^j B_i(x) values */
      error =
	gsl_bspline_deriv_eval_nonzero (x, nderiv, dw->dB, &istart, &iend, w,
					dw);
      if (error)
	{
	  return error;
	}

      /* store values in appropriate part of given matrix */
      for (j = 0; j <= nderiv; j++)
	{
	  for (i = 0; i < istart; i++)
	    gsl_matrix_set (dB, i, j, 0.0);

	  for (i = istart; i <= iend; i++)
	    gsl_matrix_set (dB, i, j, gsl_matrix_get (dw->dB, i - istart, j));

	  for (i = iend + 1; i < w->n; i++)
	    gsl_matrix_set (dB, i, j, 0.0);
	}

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

/*
gsl_bspline_deriv_eval_nonzero()
  At point x evaluate all requested, non-zero B-spline function
derivatives and store them in dB.  These are the
d^j/dx^j B_i(x) with i in [istart, iend] and j in [0, nderiv].
Always d^j/dx^j B_i(x) = 0 for i < istart and for i > iend.

Inputs: x      - point at which to evaluate splines
        nderiv - number of derivatives to request, inclusive
        dB     - (output) where to store dB-spline derivatives
                 (size k by nderiv + 1)
        istart - (output) B-spline function index of
                 first non-zero basis for given x
        iend   - (output) B-spline function index of
                 last non-zero basis for given x.
                 This is also the knot index corresponding to x.
        w      - bspline derivative workspace

Return: success or error

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

       2) On output, dB contains

            [[B_{istart,  k}, ..., d^nderiv/dx^nderiv B_{istart  ,k}],
             [B_{istart+1,k}, ..., d^nderiv/dx^nderiv B_{istart+1,k}],
             ...
             [B_{iend-1,  k}, ..., d^nderiv/dx^nderiv B_{iend-1,  k}],
             [B_{iend,    k}, ..., d^nderiv/dx^nderiv B_{iend,    k}]]

          evaluated at x.  B_{istart, k} is stored in dB(0,0).
          Each additional column contains an additional derivative.

       3) Note that the zero-th column of the result contains the
          0th derivative, which is simply a function evaluation.

       4) based on PPPACK's bsplvd
*/

int
gsl_bspline_deriv_eval_nonzero (const double x, const size_t nderiv,
				gsl_matrix * dB, size_t * istart,
				size_t * iend, gsl_bspline_workspace * w,
				gsl_bspline_deriv_workspace * dw)
{
  if (dB->size1 != w->k)
    {
      GSL_ERROR ("dB matrix first dimension not of length k", GSL_EBADLEN);
    }
  else if (dB->size2 < nderiv + 1)
    {
      GSL_ERROR
	("dB matrix second dimension must be at least length nderiv+1",
	 GSL_EBADLEN);
    }
  else if (dw->k < w->k)
    {
      GSL_ERROR ("derivative workspace is too small", GSL_EBADLEN);
    }
  else
    {
      size_t i;			/* spline index */
      size_t j;			/* looping */
      int flag = 0;		/* interval search flag */
      int error = 0;		/* error flag */
      size_t min_nderivk;

      i = bspline_find_interval (x, &flag, w);
      error = bspline_process_interval_for_eval (x, &i, flag, w);
      if (error)
	{
	  return error;
	}

      *istart = i - w->k + 1;
      *iend = i;

      bspline_pppack_bsplvd (w->knots, w->k, x, *iend,
			     w->deltal, w->deltar, dw->A, dB, nderiv);

      /* An order k b-spline has at most k-1 nonzero derivatives
         so we need to zero all requested higher order derivatives */
      min_nderivk = GSL_MIN_INT (nderiv, w->k - 1);
      for (j = min_nderivk + 1; j <= nderiv; j++)
	{
	  for (i = 0; i < w->k; i++)
	    {
	      gsl_matrix_set (dB, i, j, 0.0);
	    }
	}

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

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

/*
bspline_find_interval()
  Find knot interval such that t_i <= x < t_{i + 1}
where the t_i are knot values.

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

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

Notes: The error conditions are reported as follows:

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

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

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

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

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

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

      if (ti < x && x == tip1 && tip1 == gsl_vector_get (w->knots, w->k + w->l
							 - 1))
	break;
    }

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

  return i;
}				/* bspline_find_interval() */

/*
bspline_process_interval_for_eval()
  Consumes an x location, left knot from bspline_find_interval, flag
from bspline_find_interval, and a workspace.  Checks that x lies within
the splines' knots, enforces some endpoint continuity requirements, and
avoids divide by zero errors in the underlying bspline_pppack_* functions.
*/
static inline int
bspline_process_interval_for_eval (const double x, size_t * i, const int flag,
				   gsl_bspline_workspace * w)
{
  if (flag == -1)
    {
      GSL_ERROR ("x outside of knot interval", GSL_EINVAL);
    }
  else if (flag == 1)
    {
      if (x <= gsl_vector_get (w->knots, *i) + GSL_DBL_EPSILON)
	{
	  *i -= 1;
	}
      else
	{
	  GSL_ERROR ("x outside of knot interval", GSL_EINVAL);
	}
    }

  if (gsl_vector_get (w->knots, *i) == gsl_vector_get (w->knots, *i + 1))
    {
      GSL_ERROR ("knot(i) = knot(i+1) will result in division by zero",
		 GSL_EINVAL);
    }

  return GSL_SUCCESS;
}				/* bspline_process_interval_for_eval */

/********************************************************************
 * PPPACK ROUTINES
 *
 * The routines herein deliberately avoid using the bspline workspace,
 * choosing instead to pass all work areas explicitly.  This allows
 * others to more easily adapt these routines to low memory or
 * parallel scenarios.
 ********************************************************************/

/*
bspline_pppack_bsplvb()
  calculates the value of all possibly nonzero b-splines at x of order
jout = max( jhigh , (j+1)*(index-1) ) with knot sequence t.

Parameters:
   t      - knot sequence, of length left + jout , assumed to be
            nondecreasing.  assumption t(left).lt.t(left + 1).
            division by zero  will result if t(left) = t(left+1)
   jhigh  -
   index  - integers which determine the order jout = max(jhigh,
            (j+1)*(index-1))  of the b-splines whose values at x
            are to be returned.  index  is used to avoid
            recalculations when several columns of the triangular
            array of b-spline values are needed (e.g., in  bsplpp
            or in  bsplvd ).  precisely,

            if  index = 1 ,
               the calculation starts from scratch and the entire
               triangular array of b-spline values of orders
               1,2,...,jhigh  is generated order by order , i.e.,
               column by column .

            if  index = 2 ,
               only the b-spline values of order j+1, j+2, ..., jout
               are generated, the assumption being that biatx, j,
               deltal, deltar are, on entry, as they were on exit
               at the previous call.

            in particular, if jhigh = 0, then jout = j+1, i.e.,
            just the next column of b-spline values is generated.
   x      - the point at which the b-splines are to be evaluated.
   left   - an integer chosen (usually) so that
            t(left) .le. x .le. t(left+1).
   j      - (output) a working scalar for indexing
   deltal - (output) a working area which must be of length at least jout
   deltar - (output) a working area which must be of length at least jout
   biatx  - (output) array of length jout, with  biatx(i)
            containing the value at  x  of the polynomial of order
            jout which agrees with the b-spline b(left-jout+i,jout,t)
            on the interval (t(left), t(left+1)) .

Method:
   the recurrence relation

                      x - t(i)              t(i+j+1) - x
      b(i,j+1)(x) = -----------b(i,j)(x) + ---------------b(i+1,j)(x)
                    t(i+j)-t(i)            t(i+j+1)-t(i+1)

   is used (repeatedly) to generate the (j+1)-vector  b(left-j,j+1)(x),
   ...,b(left,j+1)(x)  from the j-vector  b(left-j+1,j)(x),...,
   b(left,j)(x), storing the new values in  biatx  over the old. the
   facts that

      b(i,1) = 1  if  t(i) .le. x .lt. t(i+1)

   and that

      b(i,j)(x) = 0  unless  t(i) .le. x .lt. t(i+j)

   are used. the particular organization of the calculations follows
   algorithm (8) in chapter x of [1].

Notes:

   (1) This is a direct translation of PPPACK's bsplvb routine with
       j, deltal, deltar rewritten as input parameters and
       utilizing zero-based indexing.

   (2) This routine contains no error checking.  Please use routines
       like gsl_bspline_eval().
*/

static void
bspline_pppack_bsplvb (const gsl_vector * t,
		       const size_t jhigh,
		       const size_t index,
		       const double x,
		       const size_t left,
		       size_t * j,
		       gsl_vector * deltal,
		       gsl_vector * deltar, gsl_vector * biatx)
{
  size_t i;			/* looping */
  double saved;
  double term;

  if (index == 1)
    {
      *j = 0;
      gsl_vector_set (biatx, 0, 1.0);
    }

  for ( /* NOP */ ; *j < jhigh - 1; *j += 1)
    {
      gsl_vector_set (deltar, *j, gsl_vector_get (t, left + *j + 1) - x);
      gsl_vector_set (deltal, *j, x - gsl_vector_get (t, left - *j));

      saved = 0.0;

      for (i = 0; i <= *j; i++)
	{
	  term = gsl_vector_get (biatx, i) / (gsl_vector_get (deltar, i)
					      + gsl_vector_get (deltal,
								*j - i));

	  gsl_vector_set (biatx, i,
			  saved + gsl_vector_get (deltar, i) * term);

	  saved = gsl_vector_get (deltal, *j - i) * term;
	}

      gsl_vector_set (biatx, *j + 1, saved);
    }

  return;
}				/* gsl_bspline_pppack_bsplvb */

/*
bspline_pppack_bsplvd()
  calculates value and derivs of all b-splines which do not vanish at x

Parameters:
   t      - the knot array, of length left+k (at least)
   k      - the order of the b-splines to be evaluated
   x      - the point at which these values are sought
   left   - an integer indicating the left endpoint of the interval
            of interest. the k b-splines whose support contains the
            interval (t(left), t(left+1)) are to be considered.
            it is assumed that t(left) .lt. t(left+1)
            division by zero will result otherwise (in  bsplvb).
            also, the output is as advertised only if
            t(left) .le. x .le. t(left+1) .
   deltal - a working area which must be of length at least k
   deltar - a working area which must be of length at least k
   a      - an array of order (k,k), to contain b-coeffs of the
            derivatives of a certain order of the k b-splines
            of interest.
   dbiatx - an array of order (k,nderiv). its entry (i,m) contains
            value of (m)th derivative of (left-k+i)-th b-spline
            of order k for knot sequence  t, i=1,...,k, m=0,...,nderiv.
   nderiv - an integer indicating that values of b-splines and
            their derivatives up to AND INCLUDING the nderiv-th
            are asked for. (nderiv is replaced internally by the
            integer mhigh in (1,k) closest to it.)

Method:
   values at x of all the relevant b-splines of order k,k-1,..., k+1-nderiv
   are generated via bsplvb and stored temporarily in dbiatx.  then, the
   b-coeffs of the required derivatives of the b-splines of interest are
   generated by differencing, each from the preceeding one of lower order,
   and combined with the values of b-splines of corresponding order in
   dbiatx  to produce the desired values .

Notes:

   (1) This is a direct translation of PPPACK's bsplvd routine with
       deltal, deltar rewritten as input parameters (to later feed them
       to bspline_pppack_bsplvb) and utilizing zero-based indexing.

   (2) This routine contains no error checking.
*/

static void
bspline_pppack_bsplvd (const gsl_vector * t,
		       const size_t k,
		       const double x,
		       const size_t left,
		       gsl_vector * deltal,
		       gsl_vector * deltar,
		       gsl_matrix * a,
		       gsl_matrix * dbiatx, const size_t nderiv)
{
  int i, ideriv, il, j, jlow, jp1mid, kmm, ldummy, m, mhigh;
  double factor, fkmm, sum;

  size_t bsplvb_j;
  gsl_vector_view dbcol = gsl_matrix_column (dbiatx, 0);

  mhigh = GSL_MIN_INT (nderiv, k - 1);
  bspline_pppack_bsplvb (t, k - mhigh, 1, x, left, &bsplvb_j, deltal, deltar,
			 &dbcol.vector);
  if (mhigh > 0)
    {
      /* the first column of dbiatx always contains the b-spline
         values for the current order. these are stored in column
         k-current order before bsplvb is called to put values
         for the next higher order on top of it.  */
      ideriv = mhigh;
      for (m = 1; m <= mhigh; m++)
	{
	  for (j = ideriv, jp1mid = 0; j < (int) k; j++, jp1mid++)
	    {
	      gsl_matrix_set (dbiatx, j, ideriv,
			      gsl_matrix_get (dbiatx, jp1mid, 0));
	    }
	  ideriv--;
	  bspline_pppack_bsplvb (t, k - ideriv, 2, x, left, &bsplvb_j, deltal,
				 deltar, &dbcol.vector);
	}

      /* at this point,  b(left-k+i, k+1-j)(x) is in  dbiatx(i,j)
         for i=j,...,k-1 and j=0,...,mhigh. in particular, the
         first column of dbiatx is already in final form. to obtain
         corresponding derivatives of b-splines in subsequent columns,
         generate their b-repr. by differencing, then evaluate at x. */
      jlow = 0;
      for (i = 0; i < (int) k; i++)
	{
	  for (j = jlow; j < (int) k; j++)
	    {
	      gsl_matrix_set (a, j, i, 0.0);
	    }
	  jlow = i;
	  gsl_matrix_set (a, i, i, 1.0);
	}

      /* at this point, a(.,j) contains the b-coeffs for the j-th of the
         k b-splines of interest here. */
      for (m = 1; m <= mhigh; m++)
	{
	  kmm = k - m;
	  fkmm = (float) kmm;
	  il = left;
	  i = k - 1;

	  /* for j=1,...,k, construct b-coeffs of (m)th  derivative
	     of b-splines from those for preceding derivative by
	     differencing and store again in  a(.,j) . the fact that
	     a(i,j) = 0  for i .lt. j  is used. */
	  for (ldummy = 0; ldummy < kmm; ldummy++)
	    {
	      factor =
		fkmm / (gsl_vector_get (t, il + kmm) -
			gsl_vector_get (t, il));
	      /* the assumption that t(left).lt.t(left+1) makes
	         denominator in factor nonzero. */
	      for (j = 0; j <= i; j++)
		{
		  gsl_matrix_set (a, i, j, factor * (gsl_matrix_get (a, i, j)
						     - gsl_matrix_get (a,
								       i - 1,
								       j)));
		}
	      il--;
	      i--;
	    }

	  /* for i=1,...,k, combine b-coeffs a(.,i) with b-spline values
	     stored in dbiatx(.,m) to get value of (m)th  derivative
	     of i-th b-spline (of interest here) at x, and store in
	     dbiatx(i,m). storage of this value over the value of a
	     b-spline of order m there is safe since the remaining
	     b-spline derivatives of the same order do not use this
	     value due to the fact that a(j,i) = 0 for j .lt. i . */
	  for (i = 0; i < (int) k; i++)
	    {
	      sum = 0;
	      jlow = GSL_MAX_INT (i, m);
	      for (j = jlow; j < (int) k; j++)
		{
		  sum +=
		    gsl_matrix_get (a, j, i) * gsl_matrix_get (dbiatx, j, m);
		}
	      gsl_matrix_set (dbiatx, i, m, sum);
	    }
	}
    }

  return;

}				/* bspline_pppack_bsplvd */


/*
**
** Lit:
**   Y.Yu, S.T. Acton:
**   Speckle reducing anisotropic diffusion.
**   IEEE Trans Image Proc., Vol. 11, No. 11, Nov 2002
**
**  G.Lohmann, MPI-CBS, 2008
*/

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

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

#include <gsl/gsl_math.h>

#define SQR(x) ((x) * (x))
#define ABS(x) ((x) > 0 ? (x) : -(x))

/*
** repair border
*/
void
Border(VImage src)
{
  int b=0,r,c,nrows,ncols;
  double u;

  nrows = VImageNRows(src);
  ncols = VImageNColumns(src);

  for (r=0; r<VImageNRows(src); r++) {
    u = VGetPixel(src,b,r,1);
    VSetPixel(src,b,r,0,u);
    u = VGetPixel(src,b,r,ncols-2);
    VSetPixel(src,b,r,ncols-1,u);
  }

  for (c=0; c<VImageNColumns(src); c++) {
    u = VGetPixel(src,b,1,c);
    VSetPixel(src,b,0,c,u);
    u = VGetPixel(src,b,nrows-2,c);
    VSetPixel(src,b,nrows-1,c,u);
  }
}

VImage
VSRAD(VImage src,VImage dest,VShort numiter,VShort type,VFloat rho)
{
  static VImage tmp1=NULL,tmp2=NULL,ctmp=NULL;
  int nbands,nrows,ncols;
  int b,r,c,iter;
  double u,v,w,x,y;
  double d,da,dd,dt,t,q,q0;
  double cx,c0,c1,c2,c3;
  double tiny=1.0e-6;

  nbands = VImageNBands(src);
  nrows  = VImageNRows(src);
  ncols  = VImageNColumns(src);

  if (tmp1 == NULL) {
    tmp1 = VCreateImage(1,nrows,ncols,VFloatRepn);
    tmp2 = VCreateImage(1,nrows,ncols,VFloatRepn);
    ctmp = VCreateImage(1,nrows,ncols,VFloatRepn);
  }

  dt = 0.05;

  dest = VCopyImage(src,dest,VAllBands);

  for (b=0; b<nbands; b++) {

    t = 0;
    for (r=0; r<nrows; r++) {
      for (c=0; c<ncols; c++) {
	VPixel(tmp1,0,r,c,VFloat) = VGetPixel(src,b,r,c);
      }
    }

    VFillImage(ctmp,VAllBands,0);
    VFillImage(tmp2,VAllBands,0);

    q0 = 1;
    for (iter=0; iter < numiter; iter++) {

      for (r=1; r<nrows-1; r++) {
	for (c=1; c<ncols-1; c++) {

	  u  = VPixel(tmp1,0,r,c,VFloat);
	  if (ABS(u) < tiny) continue;

	  v  = VPixel(tmp1,0,r+1,c,VFloat);
	  w  = VPixel(tmp1,0,r,c+1,VFloat);
	  x  = VPixel(tmp1,0,r-1,c,VFloat);
	  y  = VPixel(tmp1,0,r,c-1,VFloat);

	  da = (SQR(u-v) + SQR(u-w) + SQR(u-x) + SQR(u-y))/(u*u);
	  dd = (v+w+x+y - 4.0*u)/(u*u);
	  q = (0.5*da - 0.0625*dd) / (SQR(1.0 + 0.25*dd));
	  if (q > 0) {
	    q = sqrt(q);
	    u = (q*q - q0*q0) / ((q0*q0)*(1.0 + q0*q0));
	  }
	  else
	    u = 0;

	  cx = 0;
	  if (type == 0)
	    cx = 1.0 / (1.0 + u);
	  else
	    cx = exp(-u);

	  if (gsl_isnan(cx) || gsl_isinf(cx)) {
	    cx = 0;
	    VWarning(" cx, insnan, isinf");
	  }

	  VPixel(ctmp,0,r,c,VFloat) = cx;
	}
      }
      Border(ctmp);

      for (r=1; r<nrows-1; r++) {
	for (c=1; c<ncols-1; c++) {

	  u  = VPixel(tmp1,0,r,c,VFloat);
	  if (ABS(u) < tiny) continue;

	  v  = VPixel(tmp1,0,r+1,c,VFloat);
	  w  = VPixel(tmp1,0,r,c+1,VFloat);
	  x  = VPixel(tmp1,0,r-1,c,VFloat);
	  y  = VPixel(tmp1,0,r,c-1,VFloat);

	  c0  = VPixel(ctmp,0,r+1,c,VFloat);
	  c1  = VPixel(ctmp,0,r,c+1,VFloat);
	  c2  = VPixel(ctmp,0,r-1,c,VFloat);
	  c3  = VPixel(ctmp,0,r,c-1,VFloat);

          /*  ???
          cx =  VPixel(ctmp,0,r,c,VFloat);
	  d = c0*(v-u) + c1*(w-u) + cx*(x-u) + cx*(y-u);
          */

          d = c0*(v-u) + c1*(w-u) + c2*(x-u) + c3*(y-u);

	  u = u + 0.25*dt * d;
	  if (gsl_isnan(u) || gsl_isinf(u)) {
	    u = 0;
	    VWarning(" insnan, isinf");
	  }

	  if (u > VPixelMaxValue (tmp2)) u = VPixelMaxValue (tmp2);
	  if (u < VPixelMinValue (tmp2)) u = VPixelMinValue (tmp2);
	  VPixel(tmp2,0,r,c,VFloat) = u;

	}
      }
      q0 *= exp(-rho*t);

      Border(tmp2);
      tmp1 = VCopyImagePixels(tmp2,tmp1,VAllBands);
      t += dt;
    }

    /*
    ** output
    */
    for (r=1; r<nrows-1; r++) {
      for (c=1; c<ncols-1; c++) {
	v = VPixel(tmp2,0,r,c,VFloat);
	if (gsl_isnan(v) || gsl_isinf(v)) {
	  v = 0;
	  VWarning(" insnan, isinf");
	}

	if (v > VPixelMaxValue (dest)) v = VPixelMaxValue (dest);
	if (v < VPixelMinValue (dest)) v = VPixelMinValue (dest);
	VSetPixel(dest,b,r,c,(VDouble) v);
      }
    }
  }

  /*
  VDestroyImage(tmp1);
  VDestroyImage(tmp2);
  */
  return dest;
}


// build commmand:
// gcc -c test_fresnel.c -O2 -std=c99 -I../LISAsim -I/opt/local/include
// gcc -o test_fresnel test_fresnel.o struct.o ../LISAsim/LISAnoise.o -lgsl -lgslcblas

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

#include <gsl/gsl_errno.h>
#include <gsl/gsl_bspline.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_min.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_complex.h>

#include "constants.h"
#include "struct.h"
#include "splinecoeffs.h"

/* Implementation of the Thomas algorithm to solve a tridiagonal system */
/* Note: assumes numerical stability (e.g. diagonal-dominated matrix) */
static void SolveTridiagThomas(
  gsl_vector* vectx,        /* Output: solution vector, length n - already allocated */
  gsl_vector* vecta,        /* Diagonal of the matrix, length n */
  gsl_vector* vectb,        /* Lower diagonal, length n-1 */
  gsl_vector* vectc,        /* Upper diagonal, length n-1 */
  gsl_vector* vecty,        /* Right-hand-side vector, length n */
  int n)                    /* Length of vectx, vecty */
{
  /* Check lengths */
  if(!(vectx->size==n && vecty->size==n && vecta->size==n && vectb->size==n-1 && vectc->size==n-1)) {
    printf("Error: incompatible lengths in SolveTridiagThomas.\n");
    exit(1);
  }

  /* Note: we modify the vectors c, y in place */
  double* chat = vectc->data;
  double* yhat = vecty->data;
  double* x = vectx->data;
  double* a = vecta->data;
  double* b = vectb->data;

  /* Sweep forward, computing the chat and yhat values */
  chat[0] = chat[0] / a[0];
  yhat[0] = yhat[0] / a[0];
  double factor;
  for(int i=1; i<=n-2; i++) {
    factor = 1./(a[i] - b[i-1]*chat[i-1]);
    chat[i] = chat[i] * factor;
    yhat[i] = (yhat[i] - b[i-1]*yhat[i-1]) * factor;
  }
  factor = 1./(a[n-1] - b[n-2]*chat[n-2]);
  yhat[n-1] = (yhat[n-1] - b[n-2]*yhat[n-2]) * factor;

  /* Solve for x going backward */
  x[n-1] = yhat[n-1];
  for(int i=n-2; i>=0; i--) {
    x[i] = yhat[i] - chat[i] * x[i+1];
  }
}

void BuildNotAKnotSpline(
  gsl_matrix* splinecoeffs,   /* Output: matrix containing all the spline coeffs (already allocated) */
  gsl_vector* vectx,          /* Input: vector x*/
  gsl_vector* vecty,          /* Input: vector y */
  int n)                      /* Size of x, y, and of output matrix */
{
  /* Check lengths */
  if(!(vectx->size==n && vecty->size==n && splinecoeffs->size1==n && splinecoeffs->size2==5)) {
    printf("Error: incompatible lengths in NotAKnotSpline.\n");
    exit(1);
  }
  double* x = vectx->data;
  double* y = vecty->data;

  /* Computing vecth and vectDeltay */
  gsl_vector* vecth = gsl_vector_alloc(n-1);
  gsl_vector* vectDeltay = gsl_vector_alloc(n-1);
  gsl_vector* vectDeltayoverh = gsl_vector_alloc(n-1);
  double* h = vecth->data;
  double* Deltay = vectDeltay->data;
  double* Deltayoverh = vectDeltayoverh->data;
  for(int i=0; i<n-1; i++) {
    h[i] = x[i+1] - x[i];
    Deltay[i] = y[i+1] - y[i];
    Deltayoverh[i] = Deltay[i] / h[i];
  }

  /* Structures for the tridiagonal system */
  gsl_vector* vectY = gsl_vector_alloc(n-2);
  gsl_vector* vecta = gsl_vector_alloc(n-2);
  gsl_vector* vectb = gsl_vector_alloc(n-3);
  gsl_vector* vectc = gsl_vector_alloc(n-3);
  double* Y = vectY->data;
  double* a = vecta->data;
  double* b = vectb->data;
  double* c = vectc->data;
  for(int i=0; i<=n-3; i++) {
    Y[i] = 3.*(Deltayoverh[i+1] - Deltayoverh[i]);
    a[i] = 2.*(h[i+1] + h[i]);
  }
  for(int i=0; i<=n-4; i++) {
    b[i] = h[i+1];
    c[i] = h[i+1];
  }
  /* Adjusting for the not-a-knot condition */
  a[0] += h[0] + h[0]*h[0]/h[1];
  c[0] += -h[0]*h[0]/h[1];
  a[n-3] += h[n-2] + h[n-2]*h[n-2]/h[n-3];
  b[n-4] += -h[n-2]*h[n-2]/h[n-3];

  /* Solving the tridiagonal system */
  gsl_vector* vectp2 = gsl_vector_alloc(n);
  gsl_vector_view viewp2trunc = gsl_vector_subvector(vectp2, 1, n-2);
  SolveTridiagThomas(&viewp2trunc.vector, vecta, vectb, vectc, vectY, n-2);
  double* p2 = vectp2->data;
  p2[0] = p2[1] - h[0]/h[1] * (p2[2] - p2[1]);
  p2[n-1] = p2[n-2] + h[n-2]/h[n-3] * (p2[n-2] - p2[n-3]);

  /* Deducing the p1's and the p3's */
  gsl_vector* vectp1 = gsl_vector_alloc(n);
  gsl_vector* vectp3 = gsl_vector_alloc(n);
  double* p1 = vectp1->data;
  double* p3 = vectp3->data;
  for(int i=0; i<=n-2; i++) {
    p1[i] = Deltayoverh[i] - h[i]/3. * (p2[i+1] + 2.*p2[i]);
    p3[i] = (p2[i+1] - p2[i]) / (3*h[i]);
  }
  /* Note: p1[n-1], p2[n-1], p3[n-1] are set to values coherent with the derivatives of the spline at the last point, but they are not stricly speaking coefficients of the spline. */
  p1[n-1] = p1[n-2] + 2.*p2[n-2]*h[n-2] + 3.*p3[n-2]*h[n-2]*h[n-2];
  p3[n-1] = p3[n-2];

  /* Copying the results in the output matrix */
  gsl_vector_view viewx = gsl_matrix_column(splinecoeffs, 0);
  gsl_vector_view viewp0 = gsl_matrix_column(splinecoeffs, 1);
  gsl_vector_view viewp1 = gsl_matrix_column(splinecoeffs, 2);
  gsl_vector_view viewp2 = gsl_matrix_column(splinecoeffs, 3);
  gsl_vector_view viewp3 = gsl_matrix_column(splinecoeffs, 4);
  gsl_vector_memcpy(&viewx.vector, vectx);
  gsl_vector_memcpy(&viewp0.vector, vecty);
  gsl_vector_memcpy(&viewp1.vector, vectp1);
  gsl_vector_memcpy(&viewp2.vector, vectp2);
  gsl_vector_memcpy(&viewp3.vector, vectp3);

  /* Cleanup*/
  gsl_vector_free(vecth);
  gsl_vector_free(vectDeltay);
  gsl_vector_free(vectDeltayoverh);
  gsl_vector_free(vectY);
  gsl_vector_free(vecta);
  gsl_vector_free(vectb);
  gsl_vector_free(vectc);
  gsl_vector_free(vectp1);
  gsl_vector_free(vectp2);
  gsl_vector_free(vectp3);
}

void BuildQuadSpline(
  gsl_matrix* splinecoeffs,   /* Output: matrix containing all the spline coeffs (already allocated) */
  gsl_vector* vectx,          /* Input: vector x*/
  gsl_vector* vecty,          /* Input: vector y */
  int n)                      /* Size of x, y, and of output matrix */
{
  /* Check lengths */
  if(!(vectx->size==n && vecty->size==n && splinecoeffs->size1==n && splinecoeffs->size2==4)) {
    printf("Error: incompatible lengths in NotAKnotSpline.\n");
    exit(1);
  }
  double* x = vectx->data;
  double* y = vecty->data;

  /* Computing vecth and vectDeltay */
  gsl_vector* vecth = gsl_vector_alloc(n-1);
  gsl_vector* vectDeltay = gsl_vector_alloc(n-1);
  gsl_vector* vectDeltayoverh = gsl_vector_alloc(n-1);
  double* h = vecth->data;
  double* Deltay = vectDeltay->data;
  double* Deltayoverh = vectDeltayoverh->data;
  for(int i=0; i<n-1; i++) {
    h[i] = x[i+1] - x[i];
    Deltay[i] = y[i+1] - y[i];
    Deltayoverh[i] = Deltay[i] / h[i];
  }

  /* Solving for p1 */
  gsl_vector* vectp1 = gsl_vector_alloc(n);
  double* p1 = vectp1->data;
  double ratio = h[n-2] / h[n-3];
  p1[n-3] = ((2. + ratio)*Deltayoverh[n-3] - Deltayoverh[n-2]) / (1. + ratio);
  p1[n-2] = -p1[n-3] + 2.*Deltayoverh[n-3];
  for(int i=n-4; i>=0; i--) {
    p1[i] = -p1[i+1] + 2.*Deltayoverh[i];
  }
  p1[n-1] = (1. + ratio)*p1[n-2] - ratio*p1[n-3];

  /* Deducing the p2's */
  gsl_vector* vectp2 = gsl_vector_alloc(n);
  double* p2 = vectp2->data;
  for(int i=0; i<=n-2; i++) {
    p2[i] = (p1[i+1] - p1[i]) / (2.*h[i]);
  }
  /* Note: p2[n-1] is set to values coherent with the derivatives of the spline at the last point, but not stricly speaking a coefficient of the spline. */
  p2[n-1] = p2[n-2];

  /* Copying the results in the output matrix */
  gsl_vector_view viewx = gsl_matrix_column(splinecoeffs, 0);
  gsl_vector_view viewp0 = gsl_matrix_column(splinecoeffs, 1);
  gsl_vector_view viewp1 = gsl_matrix_column(splinecoeffs, 2);
  gsl_vector_view viewp2 = gsl_matrix_column(splinecoeffs, 3);
  gsl_vector_memcpy(&viewx.vector, vectx);
  gsl_vector_memcpy(&viewp0.vector, vecty);
  gsl_vector_memcpy(&viewp1.vector, vectp1);
  gsl_vector_memcpy(&viewp2.vector, vectp2);

  /* Cleanup*/
  gsl_vector_free(vecth);
  gsl_vector_free(vectDeltay);
  gsl_vector_free(vectDeltayoverh);
  gsl_vector_free(vectp1);
  gsl_vector_free(vectp2);
}

void BuildSplineCoeffs(
  CAmpPhaseSpline** splines,                  /*  */
  CAmpPhaseFrequencySeries* freqseries)       /*  */
{
  /* Initialize output structure */
  int n = (int) freqseries->freq->size;
  CAmpPhaseSpline_Init(splines, n);

  /* Build the splines */
  BuildNotAKnotSpline((*splines)->spline_amp_real, freqseries->freq, freqseries->amp_real, n);
  BuildNotAKnotSpline((*splines)->spline_amp_imag, freqseries->freq, freqseries->amp_imag, n);
  BuildQuadSpline((*splines)->quadspline_phase, freqseries->freq, freqseries->phase, n);
}

void BuildListmodesCAmpPhaseSpline(
  ListmodesCAmpPhaseSpline** listspline,              /* Output: list of modes of splines in matrix form */
  ListmodesCAmpPhaseFrequencySeries* listh)           /* Input: list of modes in amplitude/phase form */
{
    if(*listspline){ /* We don't allow for the case where listspline already points to something */
      printf("Error: Tried to add a mode to an already existing ListmodesCAmpPhaseSpline ");
      exit(1);
    }
    else {
      ListmodesCAmpPhaseFrequencySeries* listelementh = listh;
      while(listelementh) {
	int l = listelementh->l;
	int m = listelementh->m;
	CAmpPhaseSpline* splines = NULL;
	BuildSplineCoeffs(&splines, listelementh->freqseries);
	*listspline = ListmodesCAmpPhaseSpline_AddModeNoCopy(*listspline, splines, l, m);
	listelementh = listelementh->next;
      }
    }
}

/* Note: for the spines in matrix form, the first column contains the x values, so the coeffs start at 1 */
double EvalCubic(
  gsl_vector* coeffs,  /**/
  double eps,          /**/
  double eps2,         /**/
  double eps3)         /**/
{
  double p0 = gsl_vector_get(coeffs, 1);
  double p1 = gsl_vector_get(coeffs, 2);
  double p2 = gsl_vector_get(coeffs, 3);
  double p3 = gsl_vector_get(coeffs, 4);
  return p0 + p1*eps + p2*eps2 + p3*eps3;
}

double EvalQuad(
  gsl_vector* coeffs,  /**/
  double eps,          /**/
  double eps2)         /**/
{
  double p0 = gsl_vector_get(coeffs, 1);
  double p1 = gsl_vector_get(coeffs, 2);
  double p2 = gsl_vector_get(coeffs, 3);
  return p0 + p1*eps + p2*eps2;
}

void EvalCAmpPhaseSpline(
  CAmpPhaseSpline* splines,                    //input
  CAmpPhaseFrequencySeries* freqseries)  //in/out defines CAmpPhase from defined freqs
{
  int ispline=0;
  //printf("Enter: n=%i\n",freqseries->freq->size);
  for(int i=0;i<freqseries->freq->size;i++){
    double f=gsl_vector_get(freqseries->freq,i);
    //printf(" f:%g < %g < %g\n",gsl_matrix_get(splines->quadspline_phase, 0, 0),f,gsl_matrix_get(splines->quadspline_phase, splines->quadspline_phase->size1-1, 0));

    /* Adjust the index in the spline if necessary and compute */
    while(gsl_matrix_get(splines->quadspline_phase, ispline+1, 0)<f)ispline++;

    double eps = f - gsl_matrix_get(splines->quadspline_phase, ispline, 0);
    double eps2 = eps*eps;
    double eps3 = eps2*eps;
    gsl_vector_view coeffsampreal = gsl_matrix_row(splines->spline_amp_real, ispline);
    gsl_vector_view coeffsampimag = gsl_matrix_row(splines->spline_amp_imag, ispline);
    gsl_vector_view coeffsphase = gsl_matrix_row(splines->quadspline_phase, ispline);
    double Ar = EvalCubic(&coeffsampreal.vector, eps, eps2, eps3);
    double Ai = EvalCubic(&coeffsampimag.vector, eps, eps2, eps3);
    double Ph = EvalQuad(&coeffsphase.vector, eps, eps2);

    gsl_vector_set(freqseries->amp_real,i,Ar);
    gsl_vector_set(freqseries->amp_imag,i,Ai);
    gsl_vector_set(freqseries->phase,i,Ph);
  }
};


#define USE_GSL_INTEGRATION 0

#if USE_GSL_INTEGRATION
	#include <gsl/gsl_integration.h>
#endif

struct integrand_params {
	u32 n[2];
	u32 component_count;
	u32 coeff_count;
	f64* coeff;

	nlse_operator_func* op;
	void* op_userdata;

	struct basis basis;
};

static void sbmf_log_integration_result(struct quadgk_result* res) {
	sbmf_log_info("integral: %.10e", res->integral);
	sbmf_log_info("error: %.10e", res->error);
	//sbmf_log_info("performed evals: %d", res->performed_evals);
	sbmf_log_info("converged: %s", (res->converged) ? "yes" : "no");
}

/* functions to handle spatial guesses */

struct guess_integrand_params {
    u32 n;
	nlse_spatial_guess_func* func;
	struct basis b;
};

void guess_integrand(f64* out, f64* in, u32 len, void* data) {
    struct guess_integrand_params* params = data;

    f64 eig[len];
	params->b.eigenfunc(params->n, len, eig, in);

	f64 sample[len];
	params->func(sample, in, len, NULL);

    for (u32 i = 0; i < len; ++i) {
        out[i] = eig[i] * sample[i];
    }
}

/* function to sample linear and non-linear matrix elements */
static void linear_me_integrand(f64* out, f64* in, u32 len, void* data) {
	struct integrand_params* params = data;

	f64 eig1[len];
	f64 eig2[len];
	params->basis.eigenfunc(params->n[0], len, eig1, in);
	params->basis.eigenfunc(params->n[1], len, eig2, in);

	f64 op[len];
	params->op(len, op, in, 0, NULL, params->op_userdata);

	for (u32 i = 0; i < len; ++i) {
		out[i] = eig1[i]*eig2[i]*op[i];
	}
}

static void nonlinear_me_integrand(f64* out, f64* in, u32 len, void* data) {
	struct integrand_params* params = data;

	f64 eig1[len];
	f64 eig2[len];
	params->basis.eigenfunc(params->n[0], len, eig1, in);
	params->basis.eigenfunc(params->n[1], len, eig2, in);

	f64 sample[len*params->component_count];
	for (u32 i = 0; i < params->component_count; ++i) {
		params->basis.sample(params->coeff_count, &params->coeff[i*params->coeff_count], len, &sample[i*len], in);
	}

	f64 op[len];
	params->op(len, op, in, params->component_count, sample, params->op_userdata);

	for (u32 i = 0; i < len; ++i) {
		out[i] = eig1[i]*eig2[i]*op[i];
	}
}

#if USE_GSL_INTEGRATION
static f64 nonlinear_me_integrand_gsl(f64 in, void* data) {
	struct integrand_params* params = data;

	f64 eig1;
	f64 eig2;
	params->basis.eigenfunc(params->n[0], 1, &eig1, &in);
	params->basis.eigenfunc(params->n[1], 1, &eig2, &in);

	f64 sample[params->component_count];
	for (u32 i = 0; i < params->component_count; ++i) {
		params->basis.sample(params->coeff_count, &params->coeff[i*params->coeff_count], 1, &sample[i], &in);
	}

	f64 op;
	params->op(1, &op, &in, params->component_count, sample, params->op_userdata);

	return eig1*eig2*op;
}
#endif

struct nlse_result nlse_solver(struct nlse_settings settings, const u32 component_count, struct nlse_component* component) {
	/* Lazy */
	const u32 N = settings.num_basis_funcs;

	/* Setup results struct */
	struct nlse_result res = {
		.iterations = 0,
		.component_count = component_count,
		.coeff_count = N,
		.coeff 		 = sbmf_stack_push(component_count*(N*sizeof(f64))),
		.abs_error 	 = sbmf_stack_push(component_count*sizeof(f64)),
		.energy 	 = sbmf_stack_push(component_count*sizeof(f64)),
		.residual 	 = sbmf_stack_push(component_count*sizeof(f64)),
		.hamiltonian = sbmf_stack_push(component_count * sizeof(struct symmetric_bandmat)),
		.converged = false,
	};
	for (u32 i = 0; i < component_count; ++i) {
		res.hamiltonian[i] = symmetric_bandmat_new(N,N);
	}

	/* Place to store coeffs from previous iterations */
	f64* old_coeff = sbmf_stack_push(component_count*(N*sizeof(f64)));
	f64* old_energy = sbmf_stack_push(component_count * sizeof(f64));

	struct quadgk_settings int_settings = {
		.gk = (settings.gk.gauss_size > 0) ? settings.gk : gk15,
		.abs_error_tol = 1e-15,
		.rel_error_tol = 1e-8,
		.max_iters = settings.max_quadgk_iters,
	};

	/* Setup intial guess values for coeffs */
	for (u32 i = 0; i < component_count; ++i) {
		switch (component[i].guess.type) {
			case DEFAULT_GUESS: {
				for (u32 j = 0; j < res.coeff_count; ++j) {
					res.coeff[i*res.coeff_count + j] = 0;
				}
				/* Initialize the i:th component to the i:th eigenfunction */
				res.coeff[i*res.coeff_count + i] = 1;
				break;
			}
			case SPATIAL_GUESS: {
				struct guess_integrand_params p = {
					.func = component[i].guess.data.spatial_guess,
					.b = settings.basis,
				};
				int_settings.userdata = &p;
				f64* out = &res.coeff[i*res.coeff_count];
				for (u32 j = 0; j < res.coeff_count; ++j) {
					p.n = j;
					struct quadgk_result r;

					u8 quadgk_memory[quadgk_required_memory_size(&int_settings)];
					quadgk_infinite_interval(guess_integrand, &int_settings, quadgk_memory, &r);
					out[j] = r.integral;
				}
				f64_normalize(out, out, res.coeff_count);

				break;
			}
			case COEFF_GUESS: {
				component[i].guess.data.coeff_guess(&res.coeff[i*res.coeff_count], res.coeff_count, i);
				break;
			}
			case RANDOM_GUESS: {
				struct symmetric_bandmat bm = symmetric_bandmat_new_zero(N,N);
				SYMMETRIC_BANDMAT_FOREACH(bm, r,c) {
					u32 index = symmetric_bandmat_index(bm, r,c);
					bm.data[index] = 2.0 * ((f64)rand()/(f64)RAND_MAX) - 1.0;
				}
				struct eigen_result_real eigres = find_eigenpairs_sparse_real(bm, 1, EV_SMALLEST);
				for (u32 j = 0; j < res.coeff_count; ++j) {
					res.coeff[i*res.coeff_count + j] = eigres.eigenvectors[j];
				}
				break;
			}
			default:
				sbmf_log_error("nlse_solver: component %u has invalid guess!", i);
				return res;
		};
	}

	struct integrand_params params = {
		.component_count = res.component_count,
		.coeff_count = res.coeff_count,
		.coeff = res.coeff,
		.op = NULL,
		.basis = settings.basis,
	};

	/*
	 * Unique number of me's that need to be calculated in
	 * a symmetric matrix
	 */
	const u32 matrix_element_count = symmetric_bandmat_element_count(N);

	/* Precompute indices for matrix elements
	 * 	This way we get a single array looping
	 * 	over the matrix which is easier to
	 * 	parallelize well.
	 * 		Picking the first hamiltonian as a
	 * 	dummny matrix just to get the correct
	 * 	iteration.
	 * */
	u32 matrix_element_rows[matrix_element_count];
	u32 matrix_element_cols[matrix_element_count];
	{
		u32 matrix_element_index = 0;
		SYMMETRIC_BANDMAT_FOREACH(res.hamiltonian[0], r,c) {
			matrix_element_rows[matrix_element_index] = r;
			matrix_element_cols[matrix_element_index] = c;
			matrix_element_index++;
		}
	}

	/* Construct linear hamiltonian */
	struct symmetric_bandmat linear_hamiltonian = symmetric_bandmat_new_zero(N,N);
	{
		if (settings.spatial_pot_perturbation) {
			params.op = settings.spatial_pot_perturbation;
#pragma omp parallel for firstprivate(params, int_settings) shared(res)
			for (u32 i = 0; i < matrix_element_count; ++i) {
				u32 r = matrix_element_rows[i];
				u32 c = matrix_element_cols[i];

				int_settings.userdata = &params;
				params.n[0] = r;
				params.n[1] = c;

				u8 quadgk_memory[quadgk_required_memory_size(&int_settings)];
				struct quadgk_result int_res;
				quadgk_infinite_interval(linear_me_integrand, &int_settings, quadgk_memory, &int_res);

				if (fabs(int_res.integral) <= settings.zero_threshold)
					int_res.integral = 0.0;

				if (!int_res.converged) {
					sbmf_log_error("In construction of linear hamiltonian:");
					sbmf_log_error("\tIntegration failed for %d,%d", r,c);
					sbmf_log_integration_result(&int_res);
				}
				assert(int_res.converged);

				u32 me_index = symmetric_bandmat_index(linear_hamiltonian, r,c);
				linear_hamiltonian.data[me_index] = int_res.integral;
			}
		}

		for (u32 i = 0; i < res.coeff_count; ++i) {
			u32 me_index = symmetric_bandmat_index(linear_hamiltonian, i,i);
			linear_hamiltonian.data[me_index] += settings.basis.eigenval(i);
		}

		assert(symmetric_bandmat_is_valid(linear_hamiltonian));
	}

#if USE_GSL_INTEGRATION
	const u32 num_threads = omp_get_max_threads();
	gsl_integration_workspace* ws[num_threads];
	/* Create gsl workspaces if necessary */
	for (u32 i = 0; i < num_threads; ++i) {
		ws[i] = gsl_integration_workspace_alloc(settings.max_iterations);
	}
#endif

	struct symmetric_bandmat old_Hs[res.component_count];
	struct symmetric_bandmat premix_Hs[res.component_count];
	for (u32 i = 0; i < res.component_count; ++i) {
		old_Hs[i] = symmetric_bandmat_new(N,N);
		premix_Hs[i] = symmetric_bandmat_new(N,N);
	}

	/* Do the actual iterations */
	for (; res.iterations < settings.max_iterations; ++res.iterations) {
		sbmf_log_info("nlse starting iteration %u", res.iterations);
		memcpy(old_energy, res.energy, res.component_count * sizeof(f64));

		/* Call debug callback if requested by user */
		if (settings.measure_every > 0 &&
				settings.debug_callback &&
				res.iterations % settings.measure_every == 0) {
			settings.debug_callback(settings, res);
		}

		/* Should we still apply mixing? */
		if (settings.orbital_mixing > 0 && settings.mix_until_iteration > 0 && res.iterations == settings.mix_until_iteration) {
			settings.orbital_mixing = 0;
		}
		if (settings.hamiltonian_mixing > 0 && settings.mix_until_iteration > 0 && res.iterations == settings.mix_until_iteration) {
			settings.hamiltonian_mixing = 0;
		}

		if (settings.orbital_choice != NLSE_ORBITAL_MAXIMUM_OVERLAP && settings.mom_enable_at_iteration > 0 && res.iterations == settings.mom_enable_at_iteration) {
			settings.orbital_choice = NLSE_ORBITAL_MAXIMUM_OVERLAP;
		}

		/* Apply orbital mixing */
		for (u32 i = 0; i < component_count; ++i) {
			if (settings.orbital_mixing > 0.0) {
				//res.energy[i] = (1.0 - settings.orbital_mixing) * res.energy[i] + settings.orbital_mixing*old_energy[i];

				for (u32 j = 0; j < res.coeff_count; ++j) {
					res.coeff[i*res.coeff_count + j] =
						(1.0 - settings.orbital_mixing) * res.coeff[i*res.coeff_count + j] + settings.orbital_mixing * old_coeff[i*res.coeff_count + j];
				}
				f64_normalize(&res.coeff[i*res.coeff_count], &res.coeff[i*res.coeff_count], res.coeff_count);
			}
		}
		memcpy(old_coeff, res.coeff, res.component_count * N * sizeof(f64));

		/*
		 * Construct all the Hamiltonians
		 */
		for (u32 i = 0; i < component_count; ++i) {
			params.op = component[i].op;
			params.op_userdata = component[i].userdata;

			memcpy(old_Hs[i].data, res.hamiltonian[i].data, N*N*sizeof(f64));

			/* Construct the i:th component's hamiltonian */
#pragma omp parallel for firstprivate(params, int_settings) shared(res, linear_hamiltonian)
			for (u32 j = 0; j < matrix_element_count; ++j) {
				u32 r = matrix_element_rows[j];
				u32 c = matrix_element_cols[j];
				int_settings.userdata = &params;
				params.n[0] = r;
				params.n[1] = c;

#if USE_GSL_INTEGRATION
				integration_result int_res;
				gsl_function F;
				F.function = nonlinear_me_integrand_gsl;
				F.params = &params;
				gsl_integration_qagi(&F, 1e-10, 1e-7, settings.max_iterations, ws[omp_get_thread_num()], &int_res.integral, &int_res.error);
				int_res.converged = true;
#else
				u8 quadgk_memory[quadgk_required_memory_size(&int_settings)];

				struct quadgk_result int_res;
				quadgk_infinite_interval(nonlinear_me_integrand, &int_settings, quadgk_memory, &int_res);
#endif

				/* Check if the resultant integral is less than what we can resolve,
				 * if so, zero it.
				 */
				if (fabs(int_res.integral) <= settings.zero_threshold)
					int_res.integral = 0.0;

				if (!int_res.converged) {
					sbmf_log_error("In construction of component %u's hamiltonian:", i);
					sbmf_log_error("\tIntegration failed for %d,%d", r,c);
					sbmf_log_integration_result(&int_res);
				}
				assert(int_res.converged);

				u32 me_index = symmetric_bandmat_index(res.hamiltonian[i], r,c);
				res.hamiltonian[i].data[me_index] = linear_hamiltonian.data[me_index] + int_res.integral;
				premix_Hs[i].data[me_index] = res.hamiltonian[i].data[me_index];

				if (settings.hamiltonian_mixing > 0) {
					res.hamiltonian[i].data[me_index] = (1.0 - settings.hamiltonian_mixing)*res.hamiltonian[i].data[me_index] + settings.hamiltonian_mixing*old_Hs[i].data[me_index];
				}
			}

			assert(symmetric_bandmat_is_valid(res.hamiltonian[i]));
		}

		/*
		 * Solve and normalize all the Hamiltonian
		 * eigenvalue problems
		 */
		for (u32 i = 0; i < component_count; ++i) {
			struct eigen_result_real eigres;
			u32 new_orbital_index = 0;
			switch (settings.orbital_choice) {
				case NLSE_ORBITAL_LOWEST_ENERGY: {
					eigres = find_eigenpairs_sparse_real(res.hamiltonian[i], 1, EV_SMALLEST);
					f64_normalize(&eigres.eigenvectors[0], &eigres.eigenvectors[0], res.coeff_count);
					break;
				}
				case NLSE_ORBITAL_MAXIMUM_OVERLAP: {
					eigres = find_eigenpairs_sparse_real(res.hamiltonian[i], settings.mom_orbitals_to_consider, EV_SMALLEST);
					f64 maximum_overlap = -INFINITY;
					for (u32 j = 0; j < settings.mom_orbitals_to_consider; ++j) {
						f64_normalize(&eigres.eigenvectors[j*res.coeff_count], &eigres.eigenvectors[j*res.coeff_count], res.coeff_count);
						f64 overlap = 0.0;
						for (u32 k = 0; k < res.coeff_count; ++k) {
							f64 a = eigres.eigenvectors[j*res.coeff_count + k];
							f64 b = old_coeff[i*res.coeff_count + k];
							overlap += a*b;
						}
						overlap = fabs(overlap);

						if (overlap > maximum_overlap) {
							new_orbital_index = j;
							maximum_overlap = overlap;
						}
					}
					break;
				}
				default: {
					sbmf_log_panic("nlse_solver(...): Unspecified energy selection method!");
					assert(0);
				}
			};

			/* Copy energies and coeffs */
			res.energy[i] = eigres.eigenvalues[new_orbital_index];
			for (u32 j = 0; j < res.coeff_count; ++j) {
				res.coeff[i*res.coeff_count + j] = eigres.eigenvectors[res.coeff_count*new_orbital_index + j];
			}
			f64_normalize(&res.coeff[i*res.coeff_count], &res.coeff[i*res.coeff_count], res.coeff_count);

			///* Apply orbital mixing */
			//if (settings.orbital_mixing > 0.0) {
			//	res.energy[i] = (1.0 - settings.orbital_mixing) * res.energy[i] + settings.orbital_mixing*old_energy[i];

			//	for (u32 j = 0; j < res.coeff_count; ++j) {
			//		res.coeff[i*res.coeff_count + j] =
			//			(1.0 - settings.orbital_mixing) * res.coeff[i*res.coeff_count + j] + settings.orbital_mixing * old_coeff[i*res.coeff_count + j];
			//	}
			//	f64_normalize(&res.coeff[i*res.coeff_count], &res.coeff[i*res.coeff_count], res.coeff_count);
			//}

		}

		/* Calculate error */
		for (u32 i = 0; i < component_count; ++i) {
			f64 sum_diff = 0.0;
			f64 sum_prev = 0.0;
			for (u32 j = 0; j < res.coeff_count; ++j) {
				f64 diff = fabs(res.coeff[i*res.coeff_count + j]) - fabs(old_coeff[i*res.coeff_count + j]);
				sum_diff += diff*diff;
				sum_prev += old_coeff[i*res.coeff_count + j]*old_coeff[i*res.coeff_count + j];
			}
			res.abs_error[i] = sqrt(sum_diff);
		}

		/* Break condition */
		bool should_exit = true;
		for (u32 i = 0; i < component_count; ++i) {
			if (res.abs_error[i] > settings.abs_error_tol)
				should_exit = false;
			sbmf_log_info("\t[%u] -- abs error: %.10e energy: %.10e", i, res.abs_error[i], res.energy[i]);
		}

		if (should_exit) {
			res.converged = true;
			break;
		}
	}

#if USE_GSL_INTEGRATION
	/* free gsl workspaces if necessary */
	for (u32 i = 0; i < num_threads; ++i) {
		gsl_integration_workspace_free(ws[i]);
	}
#endif

	/* Compute residuals */
	for (u32 i = 0; i < component_count; ++i) {
		f64 ans1[N], ans2[N];
		symmetric_bandmat_mulv(ans1, premix_Hs[i], &res.coeff[i*N]);

		for (u32 j = 0; j < N; ++j) {
			ans2[j] = res.energy[i] * res.coeff[i*N + j];
		}

		f64 sum = 0.0;
		for (u32 j = 0; j < N; ++j) {
			sum += fabs(ans1[j] - ans2[j]);
		}
		sum = sqrt(sum);

		res.residual[i] = sum;
		sbmf_log_info("\t[%u] residual %e", i, sum);
	}

	return res;
}

/*
 * basic serialization
 */

void nlse_write_to_binary_file(const char* file, struct nlse_result res) {
	FILE* fd = fopen(file, "w");
	fwrite(&res.iterations, 	 sizeof(u32), 1, fd);
	fwrite(&res.component_count, sizeof(u32), 1, fd);
	fwrite(&res.coeff_count, 	 sizeof(u32), 1, fd);

	fwrite(res.coeff,  sizeof(f64), res.coeff_count*res.component_count, fd);
	fwrite(res.abs_error,  sizeof(f64), res.component_count, fd);
	fwrite(res.energy, sizeof(f64), res.component_count, fd);

	for (u32 i = 0; i < res.component_count; ++i) {
		fwrite(&res.hamiltonian[i].size, 	  sizeof(u32), 1, fd);
		fwrite(&res.hamiltonian[i].bandcount, sizeof(u32), 1, fd);
		u32 elements_written = fwrite(res.hamiltonian[i].data, sizeof(f64),
				res.hamiltonian[i].bandcount*res.hamiltonian[i].size,
				fd);
		assert(elements_written == res.hamiltonian[i].bandcount*res.hamiltonian[i].size);
	}
	fwrite(&res.converged, sizeof(bool), 1, fd);
	fclose(fd);
}

struct nlse_result nlse_read_from_binary_file(const char* file) {
	struct nlse_result res;

	FILE* fd = fopen(file, "r");
	fread(&res.iterations, 	 	sizeof(u32), 1, fd);
	fread(&res.component_count, sizeof(u32), 1, fd);
	fread(&res.coeff_count,  	sizeof(u32), 1, fd);

	res.coeff = sbmf_stack_push(sizeof(f64)*res.coeff_count*res.component_count);
	fread(res.coeff,  sizeof(f64), res.coeff_count*res.component_count, fd);

	res.abs_error = sbmf_stack_push(sizeof(f64)*res.component_count);
	fread(res.abs_error,  sizeof(f64), res.component_count, fd);

	res.energy = sbmf_stack_push(sizeof(f64)*res.component_count);
	fread(res.energy, sizeof(f64), res.component_count, fd);

	res.hamiltonian = sbmf_stack_push(res.component_count * sizeof(struct symmetric_bandmat));
	for (u32 i = 0; i < res.component_count; ++i) {
		u32 size, bandcount;
		fread(&size, 	  sizeof(u32), 1, fd);
		fread(&bandcount, sizeof(u32), 1, fd);
		res.hamiltonian[i] = symmetric_bandmat_new(bandcount, size);
		u32 bytes_written = fread(res.hamiltonian[i].data, sizeof(f64),
				bandcount*size,
				fd);
		assert(bytes_written == bandcount*size);
	}
	fread(&res.converged, sizeof(bool), 1, fd);
	fclose(fd);

	sbmf_log_info("loaded:");
	sbmf_log_info("    iterations: %u", res.iterations);
	sbmf_log_info("    component_count: %u", res.component_count);
	sbmf_log_info("    coeff_count: %u", res.coeff_count);
	for (u32 i = 0; i < res.component_count; ++i) {
		sbmf_log_info("    [%u]: size: %u", i, res.hamiltonian[i].size);
		sbmf_log_info("    [%u]: bandcount: %u", i, res.hamiltonian[i].bandcount);
	}

	return res;
}


//CELL operation: does output side of Elman RNN layer.
//There is no strict definition of Elman RNN in the literature.
//For example, the Wikipedia article doesn't specify the dims of the hidden or other vectors.
//Here I implement an interpretation where there are N neurons and thus N inputs/outputs.
//The inputs here are from the IN stage, so reduced to N driving input time-series in X.

//Again, this is not an "Elman network", rather a layer of N neurons
//that I have named "Elman" neurons due to their great similarity to an Elman RNN.

//To do: should I allow other output activations other than logistic?

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

#ifdef __cplusplus
namespace codee {
extern "C" {
#endif

int elman_s (float *Y, const float *X, const float *U, float *H, const float *W, const float *B, const size_t N, const size_t T, const char iscolmajor, const size_t dim);
int elman_d (double *Y, const double *X, const double *U, double *H, const double *W, const double *B, const size_t N, const size_t T, const char iscolmajor, const size_t dim);

int elman_s (float *Y, const float *X, const float *U, float *H, const float *W, const float *B, const size_t N, const size_t T, const char iscolmajor, const size_t dim)
{
    const float o = 1.0f;

    if (N==1u)
    {
        for (size_t t=0u; t<T; ++t)
        {
            H[0] = 1.0f / (1.0f+expf(-X[t]-U[0]*H[0]));
            Y[t] = 1.0f / (1.0f+expf(-B[0]-W[0]*H[0]));
        }
    }
    else
    {
        float *tmp;
        if (!(tmp=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in elman_s: problem with malloc. "); perror("malloc"); return 1; }

        if (dim==0u)
        {
            if (iscolmajor)
            {
                for (size_t t=0u; t<T; ++t)
                {
                    cblas_scopy((int)N,&X[t*N],1,tmp,1);
                    cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,H,1,o,tmp,1);
                    for (size_t n=0u; n<N; ++n) { H[n] = 1.0f/(1.0f+expf(-tmp[n])); }
                    //cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&X[t*N],1);
                    //for (size_t n=0u; n<N; ++n) { H[n] = 1.0f/(1.0f+expf(-X[t*N+n])); }
                    //for (size_t n=0u; n<N; ++n) { Y[t*N+n] = 1.0f/(1.0f+expf(-cblas_sdot((int)N,&W[n],(int)N,H,1)-B[n])); }
                    cblas_scopy((int)N,B,1,tmp,1);
                    cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,W,(int)N,H,1,o,tmp,1);
                    for (size_t n=0u; n<N; ++n) { Y[t*N+n] = 1.0f/(1.0f+expf(-tmp[n])); }
                }
            }
            else
            {
                for (size_t t=0u; t<T; ++t)
                {
                    cblas_scopy((int)N,&X[t],(int)T,tmp,1);
                    cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,H,1,o,tmp,1);
                    for (size_t n=0u; n<N; ++n) { H[n] = 1.0f/(1.0f+expf(-tmp[n])); }
                    cblas_scopy((int)N,B,1,tmp,1);
                    cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,W,(int)N,H,1,o,tmp,1);
                    for (size_t n=0u; n<N; ++n) { Y[t+n*T] = 1.0f/(1.0f+expf(-tmp[n])); }
                }
            }
        }
        else if (dim==1u)
        {
            if (iscolmajor)
            {
                for (size_t t=0u; t<T; ++t)
                {
                    cblas_scopy((int)N,&X[t],(int)T,tmp,1);
                    cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,H,1,o,tmp,1);
                    for (size_t n=0u; n<N; ++n) { H[n] = 1.0f/(1.0f+expf(-tmp[n])); }
                    cblas_scopy((int)N,B,1,tmp,1);
                    cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,W,(int)N,H,1,o,tmp,1);
                    for (size_t n=0u; n<N; ++n) { Y[t+n*T] = 1.0f/(1.0f+expf(-tmp[n])); }
                }
            }
            else
            {
                for (size_t t=0u; t<T; ++t)
                {
                    cblas_scopy((int)N,&X[t*N],1,tmp,1);
                    cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,H,1,o,tmp,1);
                    for (size_t n=0u; n<N; ++n) { H[n] = 1.0f/(1.0f+expf(-tmp[n])); }
                    cblas_scopy((int)N,B,1,tmp,1);
                    cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,W,(int)N,H,1,o,tmp,1);
                    for (size_t n=0u; n<N; ++n) { Y[t*N+n] = 1.0f/(1.0f+expf(-tmp[n])); }
                }
            }
        }
        else
        {
            fprintf(stderr,"error in elman_s: dim must be 0 or 1.\n"); return 1;
        }
    }

    return 0;
}

int elman_d (double *Y, const double *X, const double *U, double *H, const double *W, const double *B, const size_t N, const size_t T, const char iscolmajor, const size_t dim)
{
    const double o = 1.0;

    if (N==1u)
    {
        for (size_t t=0u; t<T; ++t)
        {
            H[0] = 1.0 / (1.0+exp(-X[t]-U[0]*H[0]));
            Y[t] = 1.0 / (1.0+exp(-B[0]-W[0]*H[0]));
        }
    }
    else
    {
        double *tmp;
        if (!(tmp=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in elman_d: problem with malloc. "); perror("malloc"); return 1; }

        if (dim==0u)
        {
            if (iscolmajor)
            {
                for (size_t t=0u; t<T; ++t)
                {
                    cblas_dcopy((int)N,&X[t*N],1,tmp,1);
                    cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,H,1,o,tmp,1);
                    for (size_t n=0u; n<N; ++n) { H[n] = 1.0/(1.0+exp(-tmp[n])); }
                    cblas_dcopy((int)N,B,1,tmp,1);
                    cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,W,(int)N,H,1,o,tmp,1);
                    for (size_t n=0u; n<N; ++n) { Y[t*N+n] = 1.0/(1.0+exp(-tmp[n])); }
                }
            }
            else
            {
                for (size_t t=0u; t<T; ++t)
                {
                    cblas_dcopy((int)N,&X[t],(int)T,tmp,1);
                    cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,H,1,o,tmp,1);
                    for (size_t n=0u; n<N; ++n) { H[n] = 1.0/(1.0+exp(-tmp[n])); }
                    cblas_dcopy((int)N,B,1,tmp,1);
                    cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,W,(int)N,H,1,o,tmp,1);
                    for (size_t n=0u; n<N; ++n) { Y[t+n*T] = 1.0/(1.0+exp(-tmp[n])); }
                }
            }
        }
        else if (dim==1u)
        {
            if (iscolmajor)
            {
                for (size_t t=0u; t<T; ++t)
                {
                    cblas_dcopy((int)N,&X[t],(int)T,tmp,1);
                    cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,H,1,o,tmp,1);
                    for (size_t n=0u; n<N; ++n) { H[n] = 1.0/(1.0+exp(-tmp[n])); }
                    cblas_dcopy((int)N,B,1,tmp,1);
                    cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,W,(int)N,H,1,o,tmp,1);
                    for (size_t n=0u; n<N; ++n) { Y[t+n*T] = 1.0/(1.0+exp(-tmp[n])); }
                }
            }
            else
            {
                for (size_t t=0u; t<T; ++t)
                {
                    cblas_dcopy((int)N,&X[t*N],1,tmp,1);
                    cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,H,1,o,tmp,1);
                    for (size_t n=0u; n<N; ++n) { H[n] = 1.0/(1.0+exp(-tmp[n])); }
                    cblas_dcopy((int)N,B,1,tmp,1);
                    cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,W,(int)N,H,1,o,tmp,1);
                    for (size_t n=0u; n<N; ++n) { Y[t*N+n] = 1.0/(1.0+exp(-tmp[n])); }
                }
            }
        }
        else
        {
            fprintf(stderr,"error in elman_d: dim must be 0 or 1.\n"); return 1;
        }
    }

    return 0;
}

#ifdef __cplusplus
}
}
#endif


/* This file is auto-generated from weight_functions.h.src */
#ifndef DOUBLE_PREC
#define DOUBLE_PREC
#endif
// # -*- mode: c -*-
#pragma once

#include "defs.h"

#include <stdint.h>

#include <gsl/gsl_matrix_double.h>
#include <gsl/gsl_linalg.h>

// Info about a particle pair that we will pass to the weight function
typedef struct
{
    int64_t nprojbins;
    int64_t nsbins;
    double sqr_s;
    double *supp_sqr; //proper way to have array in struct?

} proj_struct_double;

//typedef double (*weight_func_t_double)(const pair_struct_double*);

//////////////////////////////////
// Projection functions
//////////////////////////////////

static inline void tophat_double(const proj_struct_double *proj, double *u){

    int ins = -1;
    for(int p=0;p<proj->nsbins;p++){
        u[p] = 0;
        if (proj->sqr_s >= proj->supp_sqr[p] && proj->sqr_s < proj->supp_sqr[p+1]){
            ins = p;
        }
    }
    if (ins>=0 && ins<proj->nprojbins){
        u[ins] = 1.0;
    }
}

//////////////////////////////////
// Utility functions
//////////////////////////////////

static inline void compute_amplitudes(int nprojbins, int nd1, int nd2, int nr1, int nr2,
            double *dd, double *dr, double *rd, double *rr, double *qq, double *amps){

    printf("Computing amps\n");
    printf("qq:\n");
    for(int i=0;i<nprojbins;i++){
        for (int j=0; j<nprojbins; j++){
            printf(" %f", qq[i*nprojbins+j]);
        }
        printf("\n");
    }

    // Computer numerator of estimator
    double numerator[nprojbins];
    double qqnorm[nprojbins*nprojbins];
    for (int i=0; i<nprojbins; i++){
        double ddnorm = dd[i]/((double)nd1*(double)nd2);
        double drnorm = dr[i]/((double)nd1*(double)nr2);
        double rdnorm = rd[i]/((double)nr1*(double)nd2);
        double rrnorm = rr[i]/((double)nr1*(double)nr2);
        numerator[i] = ddnorm - drnorm - rdnorm + rrnorm;
        for (int j=0; j<nprojbins; j++){
            printf("%f", qq[i*nprojbins+j]);
            qqnorm[i*nprojbins+j] = qq[i*nprojbins+j]/((double)nr1*(double)nr2);
        }
    }

    printf("qqnorm:\n");
    for(int i=0;i<nprojbins;i++){
        for (int j=0; j<nprojbins; j++){
            printf(" %f", qqnorm[i*nprojbins+j]);
        }
        printf("\n");
    }

	int s;
	// Define all the used matrices
	gsl_matrix *qq_mat = gsl_matrix_alloc(nprojbins, nprojbins);
	gsl_matrix *qq_mat_inv = gsl_matrix_alloc(nprojbins, nprojbins);
	gsl_permutation *perm = gsl_permutation_alloc(nprojbins);
	// Fill the matrix m
	for (int i=0; i<nprojbins; i++){
        for (int j=0; j<nprojbins; j++){
            gsl_matrix_set(qq_mat, i, j, qqnorm[i*nprojbins+j]);
        }
    }
	// Make LU decomposition of matrix m
	gsl_linalg_LU_decomp(qq_mat, perm, &s);
	// Invert the matrix m
	gsl_linalg_LU_invert(qq_mat, perm, qq_mat_inv);

    printf("qqinv:\n");
    for(int i=0;i<nprojbins;i++){
        for (int j=0; j<nprojbins; j++){
            printf(" %f", gsl_matrix_get(qq_mat_inv, i, j));
        }
        printf("\n");
    }

    // Take inner product of qqinv * numerator, get amplitude vector
    // TODO: double check
    for (int i=0; i<nprojbins; i++){
        double aval = 0;
        for (int j=0; j<nprojbins; j++){
            aval += gsl_matrix_get(qq_mat_inv, i, j) * numerator[j];
        }
        amps[i] = aval;
    }
}

static inline void evaluate_xi(int nprojbins, double *amps, int nsvals, double *svals,
                                                int nsbins, double *sbins, double *xi){

    // will need to generalize, projbins won't always be rela   ted to sbins
    double supp_sqr[nsbins];
    //plus 1 because one more edge than number of bins
    for (int i=0; i<nsbins+1; i++){
        supp_sqr[i] = pow(sbins[i], 2);
    }

    // ns: number of s values at which to evaluate xi
    for (int i=0; i<nsvals; i++){
        //get basis function u for given value of s
        double u[nprojbins];
        double sqr_s = pow(svals[i], 2);
        proj_struct_double projdata = {.nprojbins=nprojbins, .nsbins=nsbins, .sqr_s=sqr_s, .supp_sqr=supp_sqr};
        tophat_double(&projdata, u);

        //multiply u by the amplitudes to get xi in that s bin (xi is vector of length ns_toeval)
        double xival = 0;
        for (int j=0; j<nprojbins; j++){
            xival += amps[j]*u[j];

        }
        xi[i] = xival;
    }

}

/* Gives a pointer to the weight function for the given weighting method
 * and instruction set.
 */
//static inline weight_func_t_double get_weight_func_by_method_double(const weight_method_t method){
//    switch(method){
//        case PAIR_PRODUCT:
//            return &pair_product_double;
//        default:
//        case NONE:
//            return NULL;
//    }
//}


/* ode-initval/rkck.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.
 */

/* Runge-Kutta 4(5), Cash-Karp */

/* Author:  G. Jungman
 */
#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv.h>

#include "odeiv_util.h"

/* Cash-Karp constants */
static const double ah[] = { 1.0 / 5.0, 0.3, 3.0 / 5.0, 1.0, 7.0 / 8.0 };
static const double b21 = 1.0 / 5.0;
static const double b3[] = { 3.0 / 40.0, 9.0 / 40.0 };
static const double b4[] = { 0.3, -0.9, 1.2 };
static const double b5[] = { -11.0 / 54.0, 2.5, -70.0 / 27.0, 35.0 / 27.0 };
static const double b6[] =
  { 1631.0 / 55296.0, 175.0 / 512.0, 575.0 / 13824.0, 44275.0 / 110592.0,
    253.0 / 4096.0 };
static const double c1 = 37.0 / 378.0;
static const double c3 = 250.0 / 621.0;
static const double c4 = 125.0 / 594.0;
static const double c6 = 512.0 / 1771.0;
static const double ec[] = { 0.0,
  /* the first value is the same as c1, above */
  37.0 / 378.0 - 2825.0 / 27648.0,
  0.0,
  /* the first value is the same as c3, above */
  250.0 / 621.0 - 18575.0 / 48384.0,
  /* the first value is the same as c4, above */
  125.0 / 594.0 - 13525.0 / 55296.0,
  -277.00 / 14336.0,
  /* the first value is the same as c6, above */
  512.0 / 1771.0 - 0.25
};

typedef struct
{
  double *k1;
  double *k2;
  double *k3;
  double *k4;
  double *k5;
  double *k6;
  double *y0;
  double *ytmp;
}
rkck_state_t;

static void *
rkck_alloc (size_t dim)
{
  rkck_state_t *state = (rkck_state_t *) malloc (sizeof (rkck_state_t));

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

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

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

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

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

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

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

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

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

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

  if (state->k5 == 0)
    {
      free (state->k4);
      free (state->k3);
      free (state->k2);
      free (state->k1);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for k5", GSL_ENOMEM);
    }

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

  if (state->k6 == 0)
    {
      free (state->k5);
      free (state->k4);
      free (state->k3);
      free (state->k2);
      free (state->k1);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for k6", GSL_ENOMEM);
    }

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

  if (state->y0 == 0)
    {
      free (state->k6);
      free (state->k5);
      free (state->k4);
      free (state->k3);
      free (state->k2);
      free (state->k1);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for y0", GSL_ENOMEM);
    }

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

  if (state->ytmp == 0)
    {
      free (state->y0);
      free (state->k6);
      free (state->k5);
      free (state->k4);
      free (state->k3);
      free (state->k2);
      free (state->k1);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for ytmp", GSL_ENOMEM);
    }

  return state;
}

static int
rkck_apply (void *vstate,
            size_t dim,
            double t,
            double h,
            double y[],
            double yerr[],
            const double dydt_in[],
            double dydt_out[], const gsl_odeiv_system * sys)
{
  rkck_state_t *state = (rkck_state_t *) vstate;

  size_t i;
  int status = 0;

  double *const k1 = state->k1;
  double *const k2 = state->k2;
  double *const k3 = state->k3;
  double *const k4 = state->k4;
  double *const k5 = state->k5;
  double *const k6 = state->k6;
  double *const ytmp = state->ytmp;

  /* k1 step */
  if (dydt_in != NULL)
    {
      DBL_MEMCPY (k1, dydt_in, dim);
    }
  else
    {
      int s = GSL_ODEIV_FN_EVAL (sys, t, y, k1);
      GSL_STATUS_UPDATE (&status, s);
    }

  for (i = 0; i < dim; i++)
    ytmp[i] = y[i] + b21 * h * k1[i];

  /* k2 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + ah[0] * h, ytmp, k2);
    GSL_STATUS_UPDATE (&status, s);
  }
  for (i = 0; i < dim; i++)
    ytmp[i] = y[i] + h * (b3[0] * k1[i] + b3[1] * k2[i]);

  /* k3 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + ah[1] * h, ytmp, k3);
    GSL_STATUS_UPDATE (&status, s);
  }
  for (i = 0; i < dim; i++)
    ytmp[i] = y[i] + h * (b4[0] * k1[i] + b4[1] * k2[i] + b4[2] * k3[i]);

  /* k4 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + ah[2] * h, ytmp, k4);
    GSL_STATUS_UPDATE (&status, s);
  }
  for (i = 0; i < dim; i++)
    ytmp[i] =
      y[i] + h * (b5[0] * k1[i] + b5[1] * k2[i] + b5[2] * k3[i] +
                  b5[3] * k4[i]);

  /* k5 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + ah[3] * h, ytmp, k5);
    GSL_STATUS_UPDATE (&status, s);
  }
  for (i = 0; i < dim; i++)
    ytmp[i] =
      y[i] + h * (b6[0] * k1[i] + b6[1] * k2[i] + b6[2] * k3[i] +
                  b6[3] * k4[i] + b6[4] * k5[i]);

  /* k6 step and final sum */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + ah[4] * h, ytmp, k6);
    GSL_STATUS_UPDATE (&status, s);
  }

  for (i = 0; i < dim; i++)
    {
      const double d_i = c1 * k1[i] + c3 * k3[i] + c4 * k4[i] + c6 * k6[i];
      y[i] += h * d_i;
      if (dydt_out != NULL)
        dydt_out[i] = d_i;
    }

  /* difference between 4th and 5th order */
  for (i = 0; i < dim; i++)
    yerr[i] =
      h * (ec[1] * k1[i] + ec[3] * k3[i] + ec[4] * k4[i] + ec[5] * k5[i] +
           ec[6] * k6[i]);

  return status;
}

static int
rkck_reset (void *vstate, size_t dim)
{
  rkck_state_t *state = (rkck_state_t *) vstate;

  DBL_ZERO_MEMSET (state->k1, dim);
  DBL_ZERO_MEMSET (state->k2, dim);
  DBL_ZERO_MEMSET (state->k3, dim);
  DBL_ZERO_MEMSET (state->k4, dim);
  DBL_ZERO_MEMSET (state->k5, dim);
  DBL_ZERO_MEMSET (state->k6, dim);
  DBL_ZERO_MEMSET (state->ytmp, dim);

  return GSL_SUCCESS;
}

static unsigned int
rkck_order (void *vstate)
{
  rkck_state_t *state = (rkck_state_t *) vstate;
  state = 0; /* prevent warnings about unused parameters */
  return 5;
}

static void
rkck_free (void *vstate)
{
  rkck_state_t *state = (rkck_state_t *) vstate;

  free (state->ytmp);
  free (state->y0);
  free (state->k6);
  free (state->k5);
  free (state->k4);
  free (state->k3);
  free (state->k2);
  free (state->k1);
  free (state);
}

static const gsl_odeiv_step_type rkck_type = { "rkck",  /* name */
  1,                            /* can use dydt_in */
  0,                            /* gives exact dydt_out */
  &rkck_alloc,
  &rkck_apply,
  &rkck_reset,
  &rkck_order,
  &rkck_free
};

const gsl_odeiv_step_type *gsl_odeiv_step_rkck = &rkck_type;