/////////////////////////////////////////////////////////////////////////////////
//
//  Solution of linear systems involved in the Levenberg - Marquardt
//  minimization algorithm
//  Copyright (C) 2004  Manolis Lourakis (lourakis at ics forth gr)
//  Institute of Computer Science, Foundation for Research & Technology - Hellas
//  Heraklion, Crete, Greece.
//
//  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.
//
/////////////////////////////////////////////////////////////////////////////////

/* Solvers for the linear systems Ax=b. Solvers should NOT modify their A & B arguments! */

#ifndef LM_REAL // not included by Axb.c
#error This file should not be compiled directly!
#endif

#ifdef LINSOLVERS_RETAIN_MEMORY
#define __STATIC__ static
#else
#define __STATIC__ // empty
#endif /* LINSOLVERS_RETAIN_MEMORY */

#ifdef HAVE_LAPACK

/* prototypes of LAPACK routines */

#define GEQRF LM_MK_LAPACK_NAME(geqrf)
#define ORGQR LM_MK_LAPACK_NAME(orgqr)
#define TRTRS LM_MK_LAPACK_NAME(trtrs)
#define POTF2 LM_MK_LAPACK_NAME(potf2)
#define POTRF LM_MK_LAPACK_NAME(potrf)
#define POTRS LM_MK_LAPACK_NAME(potrs)
#define GETRF LM_MK_LAPACK_NAME(getrf)
#define GETRS LM_MK_LAPACK_NAME(getrs)
#define GESVD LM_MK_LAPACK_NAME(gesvd)
#define GESDD LM_MK_LAPACK_NAME(gesdd)
#define SYTRF LM_MK_LAPACK_NAME(sytrf)
#define SYTRS LM_MK_LAPACK_NAME(sytrs)
#define PLASMA_POSV LM_CAT_(PLASMA_, LM_ADD_PREFIX(posv))

#ifdef __cplusplus
extern "C" {
#endif
/* QR decomposition */
extern int GEQRF(int *m, int *n, LM_REAL *a, int *lda, LM_REAL *tau, LM_REAL *work, int *lwork, int *info);
extern int ORGQR(int *m, int *n, int *k, LM_REAL *a, int *lda, LM_REAL *tau, LM_REAL *work, int *lwork, int *info);

/* solution of triangular systems */
extern int TRTRS(char *uplo, char *trans, char *diag, int *n, int *nrhs, LM_REAL *a, int *lda, LM_REAL *b, int *ldb, int *info);

/* Cholesky decomposition and systems solution */
extern int POTF2(char *uplo, int *n, LM_REAL *a, int *lda, int *info);
extern int POTRF(char *uplo, int *n, LM_REAL *a, int *lda, int *info); /* block version of dpotf2 */
extern int POTRS(char *uplo, int *n, int *nrhs, LM_REAL *a, int *lda, LM_REAL *b, int *ldb, int *info);

/* LU decomposition and systems solution */
extern int GETRF(int *m, int *n, LM_REAL *a, int *lda, int *ipiv, int *info);
extern int GETRS(char *trans, int *n, int *nrhs, LM_REAL *a, int *lda, int *ipiv, LM_REAL *b, int *ldb, int *info);

/* Singular Value Decomposition (SVD) */
extern int GESVD(char *jobu, char *jobvt, int *m, int *n, LM_REAL *a, int *lda, LM_REAL *s, LM_REAL *u, int *ldu,
                   LM_REAL *vt, int *ldvt, LM_REAL *work, int *lwork, int *info);

/* lapack 3.0 new SVD routine, faster than xgesvd().
 * In case that your version of LAPACK does not include them, use the above two older routines
 */
extern int GESDD(char *jobz, int *m, int *n, LM_REAL *a, int *lda, LM_REAL *s, LM_REAL *u, int *ldu, LM_REAL *vt, int *ldvt,
                   LM_REAL *work, int *lwork, int *iwork, int *info);

/* LDLt/UDUt factorization and systems solution */
extern int SYTRF(char *uplo, int *n, LM_REAL *a, int *lda, int *ipiv, LM_REAL *work, int *lwork, int *info);
extern int SYTRS(char *uplo, int *n, int *nrhs, LM_REAL *a, int *lda, int *ipiv, LM_REAL *b, int *ldb, int *info);
#ifdef __cplusplus
}
#endif

/* precision-specific definitions */
#define AX_EQ_B_QR LM_ADD_PREFIX(Ax_eq_b_QR)
#define AX_EQ_B_QRLS LM_ADD_PREFIX(Ax_eq_b_QRLS)
#define AX_EQ_B_CHOL LM_ADD_PREFIX(Ax_eq_b_Chol)
#define AX_EQ_B_LU LM_ADD_PREFIX(Ax_eq_b_LU)
#define AX_EQ_B_SVD LM_ADD_PREFIX(Ax_eq_b_SVD)
#define AX_EQ_B_BK LM_ADD_PREFIX(Ax_eq_b_BK)
#define AX_EQ_B_PLASMA_CHOL LM_ADD_PREFIX(Ax_eq_b_PLASMA_Chol)

/*
 * This function returns the solution of Ax = b
 *
 * The function is based on QR decomposition with explicit computation of Q:
 * If A=Q R with Q orthogonal and R upper triangular, the linear system becomes
 * Q R x = b or R x = Q^T b.
 * The last equation can be solved directly.
 *
 * A is mxm, b is mx1
 *
 * The function returns 0 in case of error, 1 if successful
 *
 * This function is often called repetitively to solve problems of identical
 * dimensions. To avoid repetitive malloc's and free's, allocated memory is
 * retained between calls and free'd-malloc'ed when not of the appropriate size.
 * A call with NULL as the first argument forces this memory to be released.
 */
int AX_EQ_B_QR(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)
{
__STATIC__ LM_REAL *buf=NULL;
__STATIC__ int buf_sz=0;

static int nb=0; /* no __STATIC__ decl. here! */

LM_REAL *a, *tau, *r, *work;
int a_sz, tau_sz, r_sz, tot_sz;
register int i, j;
int info, worksz, nrhs=1;
register LM_REAL sum;

    if(!A)
#ifdef LINSOLVERS_RETAIN_MEMORY
    {
      if(buf) free(buf);
      buf=NULL;
      buf_sz=0;

      return 1;
    }
#else
      return 1; /* NOP */
#endif /* LINSOLVERS_RETAIN_MEMORY */

    /* calculate required memory size */
    a_sz=m*m;
    tau_sz=m;
    r_sz=m*m; /* only the upper triangular part really needed */
    if(!nb){
      LM_REAL tmp;

      worksz=-1; // workspace query; optimal size is returned in tmp
      GEQRF((int *)&m, (int *)&m, NULL, (int *)&m, NULL, (LM_REAL *)&tmp, (int *)&worksz, (int *)&info);
      nb=((int)tmp)/m; // optimal worksize is m*nb
    }
    worksz=nb*m;
    tot_sz=a_sz + tau_sz + r_sz + worksz;

#ifdef LINSOLVERS_RETAIN_MEMORY
    if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */
      if(buf) free(buf); /* free previously allocated memory */

      buf_sz=tot_sz;
      buf=(LM_REAL *)malloc(buf_sz*sizeof(LM_REAL));
      if(!buf){
        printf(RCAT("memory allocation in ", AX_EQ_B_QR) "() failed!\n");
        exit(1);
      }
    }
#else
      buf_sz=tot_sz;
      buf=(LM_REAL *)malloc(buf_sz*sizeof(LM_REAL));
      if(!buf){
        printf(RCAT("memory allocation in ", AX_EQ_B_QR) "() failed!\n");
        exit(1);
      }
#endif /* LINSOLVERS_RETAIN_MEMORY */

    a=buf;
    tau=a+a_sz;
    r=tau+tau_sz;
    work=r+r_sz;

  /* store A (column major!) into a */
	for(i=0; i<m; i++)
		for(j=0; j<m; j++)
			a[i+j*m]=A[i*m+j];

  /* QR decomposition of A */
  GEQRF((int *)&m, (int *)&m, a, (int *)&m, tau, work, (int *)&worksz, (int *)&info);
  /* error treatment */
  if(info!=0){
    if(info<0){
      printf(RCAT(RCAT("LAPACK error: illegal value for argument %d of ", GEQRF) " in ", AX_EQ_B_QR) "()\n", -info);
      exit(1);
    }
    else{
      printf(RCAT(RCAT("Unknown LAPACK error %d for ", GEQRF) " in ", AX_EQ_B_QR) "()\n", info);
#ifndef LINSOLVERS_RETAIN_MEMORY
      free(buf);
#endif

      return 0;
    }
  }

  /* R is stored in the upper triangular part of a; copy it in r so that ORGQR() below won't destroy it */
  memcpy(r, a, r_sz*sizeof(LM_REAL));

  /* compute Q using the elementary reflectors computed by the above decomposition */
  ORGQR((int *)&m, (int *)&m, (int *)&m, a, (int *)&m, tau, work, (int *)&worksz, (int *)&info);
  if(info!=0){
    if(info<0){
      printf(RCAT(RCAT("LAPACK error: illegal value for argument %d of ", ORGQR) " in ", AX_EQ_B_QR) "()\n", -info);
      exit(1);
    }
    else{
      printf(RCAT("Unknown LAPACK error (%d) in ", AX_EQ_B_QR) "()\n", info);
#ifndef LINSOLVERS_RETAIN_MEMORY
      free(buf);
#endif

      return 0;
    }
  }

  /* Q is now in a; compute Q^T b in x */
  for(i=0; i<m; i++){
    for(j=0, sum=0.0; j<m; j++)
      sum+=a[i*m+j]*B[j];
    x[i]=sum;
  }

  /* solve the linear system R x = Q^t b */
  TRTRS("U", "N", "N", (int *)&m, (int *)&nrhs, r, (int *)&m, x, (int *)&m, &info);
  /* error treatment */
  if(info!=0){
    if(info<0){
      printf(RCAT(RCAT("LAPACK error: illegal value for argument %d of ", TRTRS) " in ", AX_EQ_B_QR) "()\n", -info);
      exit(1);
    }
    else{
      printf(RCAT("LAPACK error: the %d-th diagonal element of A is zero (singular matrix) in ", AX_EQ_B_QR) "()\n", info);
#ifndef LINSOLVERS_RETAIN_MEMORY
      free(buf);
#endif

      return 0;
    }
  }

#ifndef LINSOLVERS_RETAIN_MEMORY
  free(buf);
#endif

	return 1;
}

/*
 * This function returns the solution of min_x ||Ax - b||
 *
 * || . || is the second order (i.e. L2) norm. This is a least squares technique that
 * is based on QR decomposition:
 * If A=Q R with Q orthogonal and R upper triangular, the normal equations become
 * (A^T A) x = A^T b  or (R^T Q^T Q R) x = A^T b or (R^T R) x = A^T b.
 * This amounts to solving R^T y = A^T b for y and then R x = y for x
 * Note that Q does not need to be explicitly computed
 *
 * A is mxn, b is mx1
 *
 * The function returns 0 in case of error, 1 if successful
 *
 * This function is often called repetitively to solve problems of identical
 * dimensions. To avoid repetitive malloc's and free's, allocated memory is
 * retained between calls and free'd-malloc'ed when not of the appropriate size.
 * A call with NULL as the first argument forces this memory to be released.
 */
int AX_EQ_B_QRLS(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m, int n)
{
__STATIC__ LM_REAL *buf=NULL;
__STATIC__ int buf_sz=0;

static int nb=0; /* no __STATIC__ decl. here! */

LM_REAL *a, *tau, *r, *work;
int a_sz, tau_sz, r_sz, tot_sz;
register int i, j;
int info, worksz, nrhs=1;
register LM_REAL sum;

    if(!A)
#ifdef LINSOLVERS_RETAIN_MEMORY
    {
      if(buf) free(buf);
      buf=NULL;
      buf_sz=0;

      return 1;
    }
#else
      return 1; /* NOP */
#endif /* LINSOLVERS_RETAIN_MEMORY */

    if(m<n){
		  printf(RCAT("Normal equations require that the number of rows is greater than number of columns in ", AX_EQ_B_QRLS) "() [%d x %d]! -- try transposing\n", m, n);
		  exit(1);
	  }

    /* calculate required memory size */
    a_sz=m*n;
    tau_sz=n;
    r_sz=n*n;
    if(!nb){
      LM_REAL tmp;

      worksz=-1; // workspace query; optimal size is returned in tmp
      GEQRF((int *)&m, (int *)&m, NULL, (int *)&m, NULL, (LM_REAL *)&tmp, (int *)&worksz, (int *)&info);
      nb=((int)tmp)/m; // optimal worksize is m*nb
    }
    worksz=nb*m;
    tot_sz=a_sz + tau_sz + r_sz + worksz;

#ifdef LINSOLVERS_RETAIN_MEMORY
    if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */
      if(buf) free(buf); /* free previously allocated memory */

      buf_sz=tot_sz;
      buf=(LM_REAL *)malloc(buf_sz*sizeof(LM_REAL));
      if(!buf){
        printf(RCAT("memory allocation in ", AX_EQ_B_QRLS) "() failed!\n");
        exit(1);
      }
    }
#else
      buf_sz=tot_sz;
      buf=(LM_REAL *)malloc(buf_sz*sizeof(LM_REAL));
      if(!buf){
        printf(RCAT("memory allocation in ", AX_EQ_B_QRLS) "() failed!\n");
        exit(1);
      }
#endif /* LINSOLVERS_RETAIN_MEMORY */

    a=buf;
    tau=a+a_sz;
    r=tau+tau_sz;
    work=r+r_sz;

  /* store A (column major!) into a */
	for(i=0; i<m; i++)
		for(j=0; j<n; j++)
			a[i+j*m]=A[i*n+j];

  /* compute A^T b in x */
  for(i=0; i<n; i++){
    for(j=0, sum=0.0; j<m; j++)
      sum+=A[j*n+i]*B[j];
    x[i]=sum;
  }

  /* QR decomposition of A */
  GEQRF((int *)&m, (int *)&n, a, (int *)&m, tau, work, (int *)&worksz, (int *)&info);
  /* error treatment */
  if(info!=0){
    if(info<0){
      printf(RCAT(RCAT("LAPACK error: illegal value for argument %d of ", GEQRF) " in ", AX_EQ_B_QRLS) "()\n", -info);
      exit(1);
    }
    else{
      printf(RCAT(RCAT("Unknown LAPACK error %d for ", GEQRF) " in ", AX_EQ_B_QRLS) "()\n", info);
#ifndef LINSOLVERS_RETAIN_MEMORY
      free(buf);
#endif

      return 0;
    }
  }

  /* R is stored in the upper triangular part of a. Note that a is mxn while r nxn */
  for(j=0; j<n; j++){
    for(i=0; i<=j; i++)
      r[i+j*n]=a[i+j*m];

    /* lower part is zero */
    for(i=j+1; i<n; i++)
      r[i+j*n]=0.0;
  }

  /* solve the linear system R^T y = A^t b */
  TRTRS("U", "T", "N", (int *)&n, (int *)&nrhs, r, (int *)&n, x, (int *)&n, &info);
  /* error treatment */
  if(info!=0){
    if(info<0){
      printf(RCAT(RCAT("LAPACK error: illegal value for argument %d of ", TRTRS) " in ", AX_EQ_B_QRLS) "()\n", -info);
      exit(1);
    }
    else{
      printf(RCAT("LAPACK error: the %d-th diagonal element of A is zero (singular matrix) in ", AX_EQ_B_QRLS) "()\n", info);
#ifndef LINSOLVERS_RETAIN_MEMORY
      free(buf);
#endif

      return 0;
    }
  }

  /* solve the linear system R x = y */
  TRTRS("U", "N", "N", (int *)&n, (int *)&nrhs, r, (int *)&n, x, (int *)&n, &info);
  /* error treatment */
  if(info!=0){
    if(info<0){
      printf(RCAT(RCAT("LAPACK error: illegal value for argument %d of ", TRTRS) " in ", AX_EQ_B_QRLS) "()\n", -info);
      exit(1);
    }
    else{
      printf(RCAT("LAPACK error: the %d-th diagonal element of A is zero (singular matrix) in ", AX_EQ_B_QRLS) "()\n", info);
#ifndef LINSOLVERS_RETAIN_MEMORY
      free(buf);
#endif

      return 0;
    }
  }

#ifndef LINSOLVERS_RETAIN_MEMORY
  free(buf);
#endif

	return 1;
}

/*
 * This function returns the solution of Ax=b
 *
 * The function assumes that A is symmetric & postive definite and employs
 * the Cholesky decomposition:
 * If A=L L^T with L lower triangular, the system to be solved becomes
 * (L L^T) x = b
 * This amounts to solving L y = b for y and then L^T x = y for x
 *
 * A is mxm, b is mx1
 *
 * The function returns 0 in case of error, 1 if successful
 *
 * This function is often called repetitively to solve problems of identical
 * dimensions. To avoid repetitive malloc's and free's, allocated memory is
 * retained between calls and free'd-malloc'ed when not of the appropriate size.
 * A call with NULL as the first argument forces this memory to be released.
 */
int AX_EQ_B_CHOL(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)
{
__STATIC__ LM_REAL *buf=NULL;
__STATIC__ int buf_sz=0;

LM_REAL *a;
int a_sz, tot_sz;
int info, nrhs=1;

    if(!A)
#ifdef LINSOLVERS_RETAIN_MEMORY
    {
      if(buf) free(buf);
      buf=NULL;
      buf_sz=0;

      return 1;
    }
#else
      return 1; /* NOP */
#endif /* LINSOLVERS_RETAIN_MEMORY */

    /* calculate required memory size */
    a_sz=m*m;
    tot_sz=a_sz;

#ifdef LINSOLVERS_RETAIN_MEMORY
    if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */
      if(buf) free(buf); /* free previously allocated memory */

      buf_sz=tot_sz;
      buf=(LM_REAL *)malloc(buf_sz*sizeof(LM_REAL));
      if(!buf){
        printf(RCAT("memory allocation in ", AX_EQ_B_CHOL) "() failed!\n");
        exit(1);
      }
    }
#else
      buf_sz=tot_sz;
      buf=(LM_REAL *)malloc(buf_sz*sizeof(LM_REAL));
      if(!buf){
        printf(RCAT("memory allocation in ", AX_EQ_B_CHOL) "() failed!\n");
        exit(1);
      }
#endif /* LINSOLVERS_RETAIN_MEMORY */

  a=buf;

  /* store A into a and B into x. A is assumed symmetric,
   * hence no transposition is needed
   */
  memcpy(a, A, a_sz*sizeof(LM_REAL));
  memcpy(x, B, m*sizeof(LM_REAL));

  /* Cholesky decomposition of A */
  //POTF2("L", (int *)&m, a, (int *)&m, (int *)&info);
  POTRF("L", (int *)&m, a, (int *)&m, (int *)&info);
  /* error treatment */
  if(info!=0){
    if(info<0){
      printf(RCAT(RCAT(RCAT("LAPACK error: illegal value for argument %d of ", POTF2) "/", POTRF) " in ",
                      AX_EQ_B_CHOL) "()\n", -info);
      exit(1);
    }
    else{
      printf(RCAT(RCAT(RCAT("LAPACK error: the leading minor of order %d is not positive definite,\nthe factorization could not be completed for ", POTF2) "/", POTRF) " in ", AX_EQ_B_CHOL) "()\n", info);
#ifndef LINSOLVERS_RETAIN_MEMORY
      free(buf);
#endif

      return 0;
    }
  }

  /* solve using the computed Cholesky in one lapack call */
  POTRS("L", (int *)&m, (int *)&nrhs, a, (int *)&m, x, (int *)&m, &info);
  if(info<0){
    printf(RCAT(RCAT("LAPACK error: illegal value for argument %d of ", POTRS) " in ", AX_EQ_B_CHOL) "()\n", -info);
    exit(1);
  }

#if 0
  /* alternative: solve the linear system L y = b ... */
  TRTRS("L", "N", "N", (int *)&m, (int *)&nrhs, a, (int *)&m, x, (int *)&m, &info);
  /* error treatment */
  if(info!=0){
    if(info<0){
      printf(RCAT(RCAT("LAPACK error: illegal value for argument %d of ", TRTRS) " in ", AX_EQ_B_CHOL) "()\n", -info);
      exit(1);
    }
    else{
      printf(RCAT("LAPACK error: the %d-th diagonal element of A is zero (singular matrix) in ", AX_EQ_B_CHOL) "()\n", info);
#ifndef LINSOLVERS_RETAIN_MEMORY
      free(buf);
#endif

      return 0;
    }
  }

  /* ... solve the linear system L^T x = y */
  TRTRS("L", "T", "N", (int *)&m, (int *)&nrhs, a, (int *)&m, x, (int *)&m, &info);
  /* error treatment */
  if(info!=0){
    if(info<0){
      printf(RCAT(RCAT("LAPACK error: illegal value for argument %d of ", TRTRS) "in ", AX_EQ_B_CHOL) "()\n", -info);
      exit(1);
    }
    else{
      printf(RCAT("LAPACK error: the %d-th diagonal element of A is zero (singular matrix) in ", AX_EQ_B_CHOL) "()\n", info);
#ifndef LINSOLVERS_RETAIN_MEMORY
      free(buf);
#endif

      return 0;
    }
  }
#endif /* 0 */

#ifndef LINSOLVERS_RETAIN_MEMORY
  free(buf);
#endif

	return 1;
}

#ifdef HAVE_PLASMA

/* Linear algebra using PLASMA parallel library for multicore CPUs.
 * http://icl.cs.utk.edu/plasma/
 *
 * WARNING: BLAS multithreading should be disabled, e.g. setenv MKL_NUM_THREADS 1
 */

#ifndef _LM_PLASMA_MISC_
/* avoid multiple inclusion of helper code */
#define _LM_PLASMA_MISC_

#include <plasma.h>
#include <cblas.h>
#include <lapacke.h>
#include <plasma_tmg.h>
#include <core_blas.h>

/* programmatically determine the number of cores on the current machine */
#ifdef _WIN32
#include <windows.h>
#elif __linux
#include <unistd.h>
#endif
static int getnbcores()
{
#ifdef _WIN32
  SYSTEM_INFO sysinfo;
  GetSystemInfo(&sysinfo);
  return sysinfo.dwNumberOfProcessors;
#elif __linux
  return sysconf(_SC_NPROCESSORS_ONLN);
#else // unknown system
  return 2<<1; // will be halved by right shift below
#endif
}

static int PLASMA_ncores=-(getnbcores()>>1); // >0 if PLASMA initialized, <0 otherwise

/* user-specified number of cores */
void levmar_PLASMA_setnbcores(int cores)
{
  PLASMA_ncores=(cores>0)? -cores : ((cores)? cores : -2);
}
#endif /* _LM_PLASMA_MISC_ */

/*
 * This function returns the solution of Ax=b
 *
 * The function assumes that A is symmetric & positive definite and employs the
 * Cholesky decomposition implemented by PLASMA for homogeneous multicore processors.
 *
 * A is mxm, b is mx1
 *
 * The function returns 0 in case of error, 1 if successfull
 *
 * This function is often called repetitively to solve problems of identical
 * dimensions. To avoid repetitive malloc's and free's, allocated memory is
 * retained between calls and free'd-malloc'ed when not of the appropriate size.
 * A call with NULL as the first argument forces this memory to be released.
 */
int AX_EQ_B_PLASMA_CHOL(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)
{
__STATIC__ LM_REAL *buf=NULL;
__STATIC__ int buf_sz=0;

LM_REAL *a;
int a_sz, tot_sz;
int info, nrhs=1;

    if(A==NULL){
#ifdef LINSOLVERS_RETAIN_MEMORY
      if(buf) free(buf);
      buf=NULL;
      buf_sz=0;
#endif /* LINSOLVERS_RETAIN_MEMORY */

      PLASMA_Finalize();
      PLASMA_ncores=-PLASMA_ncores;

      return 1;
    }

    /* calculate required memory size */
    a_sz=m*m;
    tot_sz=a_sz;

#ifdef LINSOLVERS_RETAIN_MEMORY
    if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */
      if(buf) free(buf); /* free previously allocated memory */

      buf_sz=tot_sz;
      buf=(LM_REAL *)malloc(buf_sz*sizeof(LM_REAL));
      if(!buf){
        printf(RCAT("memory allocation in ", AX_EQ_B_PLASMA_CHOL) "() failed!\n");
        exit(1);
      }
    }
#else
    buf_sz=tot_sz;
    buf=(LM_REAL *)malloc(buf_sz*sizeof(LM_REAL));
    if(!buf){
      printf(RCAT("memory allocation in ", AX_EQ_B_PLASMA_CHOL) "() failed!\n");
      exit(1);
    }
#endif /* LINSOLVERS_RETAIN_MEMORY */

    a=buf;

    /* store A into a and B into x; A is assumed to be symmetric,
     * hence no transposition is needed
     */
    memcpy(a, A, a_sz*sizeof(LM_REAL));
    memcpy(x, B, m*sizeof(LM_REAL));

  /* initialize PLASMA */
  if(PLASMA_ncores<0){
    PLASMA_ncores=-PLASMA_ncores;
    PLASMA_Init(PLASMA_ncores);
    printf(RCAT("\n", AX_EQ_B_PLASMA_CHOL) "(): PLASMA is running on %d cores.\n\n", PLASMA_ncores);
  }

  /* Solve the linear system */
  info=PLASMA_POSV(PlasmaLower, m, 1, a, m, x, m);
  /* error treatment */
  if(info!=0){
    if(info<0){
      printf(RCAT(RCAT("LAPACK error: illegal value for argument %d of ", PLASMA_POSV) " in ",
                      AX_EQ_B_PLASMA_CHOL) "()\n", -info);
      exit(1);
    }
    else{
      printf(RCAT(RCAT("LAPACK error: the leading minor of order %d is not positive definite,\n"
                                "the factorization could not be completed for ", PLASMA_POSV) " in ", AX_EQ_B_CHOL) "()\n", info);
#ifndef LINSOLVERS_RETAIN_MEMORY
      free(buf);
#endif
      return 0;
    }
  }

#ifndef LINSOLVERS_RETAIN_MEMORY
  free(buf);
#endif

	return 1;
}
#endif /* HAVE_PLASMA */

/*
 * This function returns the solution of Ax = b
 *
 * The function employs LU decomposition:
 * If A=L U with L lower and U upper triangular, then the original system
 * amounts to solving
 * L y = b, U x = y
 *
 * A is mxm, b is mx1
 *
 * The function returns 0 in case of error, 1 if successful
 *
 * This function is often called repetitively to solve problems of identical
 * dimensions. To avoid repetitive malloc's and free's, allocated memory is
 * retained between calls and free'd-malloc'ed when not of the appropriate size.
 * A call with NULL as the first argument forces this memory to be released.
 */
int AX_EQ_B_LU(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)
{
__STATIC__ LM_REAL *buf=NULL;
__STATIC__ int buf_sz=0;

int a_sz, ipiv_sz, tot_sz;
register int i, j;
int info, *ipiv, nrhs=1;
LM_REAL *a;

    if(!A)
#ifdef LINSOLVERS_RETAIN_MEMORY
    {
      if(buf) free(buf);
      buf=NULL;
      buf_sz=0;

      return 1;
    }
#else
      return 1; /* NOP */
#endif /* LINSOLVERS_RETAIN_MEMORY */

    /* calculate required memory size */
    ipiv_sz=m;
    a_sz=m*m;
    tot_sz=a_sz*sizeof(LM_REAL) + ipiv_sz*sizeof(int); /* should be arranged in that order for proper doubles alignment */

#ifdef LINSOLVERS_RETAIN_MEMORY
    if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */
      if(buf) free(buf); /* free previously allocated memory */

      buf_sz=tot_sz;
      buf=(LM_REAL *)malloc(buf_sz);
      if(!buf){
        printf(RCAT("memory allocation in ", AX_EQ_B_LU) "() failed!\n");
        exit(1);
      }
    }
#else
      buf_sz=tot_sz;
      buf=(LM_REAL *)malloc(buf_sz);
      if(!buf){
        printf(RCAT("memory allocation in ", AX_EQ_B_LU) "() failed!\n");
        exit(1);
      }
#endif /* LINSOLVERS_RETAIN_MEMORY */

    a=buf;
    ipiv=(int *)(a+a_sz);

    /* store A (column major!) into a and B into x */
	  for(i=0; i<m; i++){
		  for(j=0; j<m; j++)
        a[i+j*m]=A[i*m+j];

      x[i]=B[i];
    }

  /* LU decomposition for A */
	GETRF((int *)&m, (int *)&m, a, (int *)&m, ipiv, (int *)&info);
	if(info!=0){
		if(info<0){
      printf(RCAT(RCAT("argument %d of ", GETRF) " illegal in ", AX_EQ_B_LU) "()\n", -info);
			exit(1);
		}
		else{
      printf(RCAT(RCAT("singular matrix A for ", GETRF) " in ", AX_EQ_B_LU) "()\n");
#ifndef LINSOLVERS_RETAIN_MEMORY
      free(buf);
#endif

			return 0;
		}
	}

  /* solve the system with the computed LU */
  GETRS("N", (int *)&m, (int *)&nrhs, a, (int *)&m, ipiv, x, (int *)&m, (int *)&info);
	if(info!=0){
		if(info<0){
			printf(RCAT(RCAT("argument %d of ", GETRS) " illegal in ", AX_EQ_B_LU) "()\n", -info);
			exit(1);
		}
		else{
			printf(RCAT(RCAT("unknown error for ", GETRS) " in ", AX_EQ_B_LU) "()\n");
#ifndef LINSOLVERS_RETAIN_MEMORY
      free(buf);
#endif

			return 0;
		}
	}

#ifndef LINSOLVERS_RETAIN_MEMORY
  free(buf);
#endif

	return 1;
}

/*
 * This function returns the solution of Ax = b
 *
 * The function is based on SVD decomposition:
 * If A=U D V^T with U, V orthogonal and D diagonal, the linear system becomes
 * (U D V^T) x = b or x=V D^{-1} U^T b
 * Note that V D^{-1} U^T is the pseudoinverse A^+
 *
 * A is mxm, b is mx1.
 *
 * The function returns 0 in case of error, 1 if successful
 *
 * This function is often called repetitively to solve problems of identical
 * dimensions. To avoid repetitive malloc's and free's, allocated memory is
 * retained between calls and free'd-malloc'ed when not of the appropriate size.
 * A call with NULL as the first argument forces this memory to be released.
 */
int AX_EQ_B_SVD(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)
{
__STATIC__ LM_REAL *buf=NULL;
__STATIC__ int buf_sz=0;
static LM_REAL eps=LM_CNST(-1.0);

register int i, j;
LM_REAL *a, *u, *s, *vt, *work;
int a_sz, u_sz, s_sz, vt_sz, tot_sz;
LM_REAL thresh, one_over_denom;
register LM_REAL sum;
int info, rank, worksz, *iwork, iworksz;

    if(!A)
#ifdef LINSOLVERS_RETAIN_MEMORY
    {
      if(buf) free(buf);
      buf=NULL;
      buf_sz=0;

      return 1;
    }
#else
      return 1; /* NOP */
#endif /* LINSOLVERS_RETAIN_MEMORY */

  /* calculate required memory size */
#if 1 /* use optimal size */
  worksz=-1; // workspace query. Keep in mind that GESDD requires more memory than GESVD
  /* note that optimal work size is returned in thresh */
  GESVD("A", "A", (int *)&m, (int *)&m, NULL, (int *)&m, NULL, NULL, (int *)&m, NULL, (int *)&m, (LM_REAL *)&thresh, (int *)&worksz, &info);
  //GESDD("A", (int *)&m, (int *)&m, NULL, (int *)&m, NULL, NULL, (int *)&m, NULL, (int *)&m, (LM_REAL *)&thresh, (int *)&worksz, NULL, &info);
  worksz=(int)thresh;
#else /* use minimum size */
  worksz=5*m; // min worksize for GESVD
  //worksz=m*(7*m+4); // min worksize for GESDD
#endif
  iworksz=8*m;
  a_sz=m*m;
  u_sz=m*m; s_sz=m; vt_sz=m*m;

  tot_sz=(a_sz + u_sz + s_sz + vt_sz + worksz)*sizeof(LM_REAL) + iworksz*sizeof(int); /* should be arranged in that order for proper doubles alignment */

#ifdef LINSOLVERS_RETAIN_MEMORY
  if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */
    if(buf) free(buf); /* free previously allocated memory */

    buf_sz=tot_sz;
    buf=(LM_REAL *)malloc(buf_sz);
    if(!buf){
      printf(RCAT("memory allocation in ", AX_EQ_B_SVD) "() failed!\n");
      exit(1);
    }
  }
#else
    buf_sz=tot_sz;
    buf=(LM_REAL *)malloc(buf_sz);
    if(!buf){
      printf(RCAT("memory allocation in ", AX_EQ_B_SVD) "() failed!\n");
      exit(1);
    }
#endif /* LINSOLVERS_RETAIN_MEMORY */

  a=buf;
  u=a+a_sz;
  s=u+u_sz;
  vt=s+s_sz;
  work=vt+vt_sz;
  iwork=(int *)(work+worksz);

  /* store A (column major!) into a */
  for(i=0; i<m; i++)
    for(j=0; j<m; j++)
      a[i+j*m]=A[i*m+j];

  /* SVD decomposition of A */
  GESVD("A", "A", (int *)&m, (int *)&m, a, (int *)&m, s, u, (int *)&m, vt, (int *)&m, work, (int *)&worksz, &info);
  //GESDD("A", (int *)&m, (int *)&m, a, (int *)&m, s, u, (int *)&m, vt, (int *)&m, work, (int *)&worksz, iwork, &info);

  /* error treatment */
  if(info!=0){
    if(info<0){
      printf(RCAT(RCAT(RCAT("LAPACK error: illegal value for argument %d of ", GESVD), "/" GESDD) " in ", AX_EQ_B_SVD) "()\n", -info);
      exit(1);
    }
    else{
      printf(RCAT("LAPACK error: dgesdd (dbdsdc)/dgesvd (dbdsqr) failed to converge in ", AX_EQ_B_SVD) "() [info=%d]\n", info);
#ifndef LINSOLVERS_RETAIN_MEMORY
      free(buf);
#endif

      return 0;
    }
  }

  if(eps<0.0){
    LM_REAL aux;

    /* compute machine epsilon */
    for(eps=LM_CNST(1.0); aux=eps+LM_CNST(1.0), aux-LM_CNST(1.0)>0.0; eps*=LM_CNST(0.5))
                                          ;
    eps*=LM_CNST(2.0);
  }

  /* compute the pseudoinverse in a */
	for(i=0; i<a_sz; i++) a[i]=0.0; /* initialize to zero */
  for(rank=0, thresh=eps*s[0]; rank<m && s[rank]>thresh; rank++){
    one_over_denom=LM_CNST(1.0)/s[rank];

    for(j=0; j<m; j++)
      for(i=0; i<m; i++)
        a[i*m+j]+=vt[rank+i*m]*u[j+rank*m]*one_over_denom;
  }

	/* compute A^+ b in x */
	for(i=0; i<m; i++){
	  for(j=0, sum=0.0; j<m; j++)
      sum+=a[i*m+j]*B[j];
    x[i]=sum;
  }

#ifndef LINSOLVERS_RETAIN_MEMORY
  free(buf);
#endif

	return 1;
}

/*
 * This function returns the solution of Ax = b for a real symmetric matrix A
 *
 * The function is based on LDLT factorization with the pivoting
 * strategy of Bunch and Kaufman:
 * A is factored as L*D*L^T where L is lower triangular and
 * D symmetric and block diagonal (aka spectral decomposition,
 * Banachiewicz factorization, modified Cholesky factorization)
 *
 * A is mxm, b is mx1.
 *
 * The function returns 0 in case of error, 1 if successfull
 *
 * This function is often called repetitively to solve problems of identical
 * dimensions. To avoid repetitive malloc's and free's, allocated memory is
 * retained between calls and free'd-malloc'ed when not of the appropriate size.
 * A call with NULL as the first argument forces this memory to be released.
 */
int AX_EQ_B_BK(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)
{
__STATIC__ LM_REAL *buf=NULL;
__STATIC__ int buf_sz=0, nb=0;

LM_REAL *a, *work;
int a_sz, ipiv_sz, work_sz, tot_sz;
int info, *ipiv, nrhs=1;

  if(!A)
#ifdef LINSOLVERS_RETAIN_MEMORY
  {
    if(buf) free(buf);
    buf=NULL;
    buf_sz=0;

    return 1;
  }
#else
  return 1; /* NOP */
#endif /* LINSOLVERS_RETAIN_MEMORY */

  /* calculate required memory size */
  ipiv_sz=m;
  a_sz=m*m;
  if(!nb){
    LM_REAL tmp;

    work_sz=-1; // workspace query; optimal size is returned in tmp
    SYTRF("L", (int *)&m, NULL, (int *)&m, NULL, (LM_REAL *)&tmp, (int *)&work_sz, (int *)&info);
    nb=((int)tmp)/m; // optimal worksize is m*nb
  }
  work_sz=(nb!=-1)? nb*m : 1;
  tot_sz=(a_sz + work_sz)*sizeof(LM_REAL) + ipiv_sz*sizeof(int); /* should be arranged in that order for proper doubles alignment */

#ifdef LINSOLVERS_RETAIN_MEMORY
  if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */
    if(buf) free(buf); /* free previously allocated memory */

    buf_sz=tot_sz;
    buf=(LM_REAL *)malloc(buf_sz);
    if(!buf){
      printf(RCAT("memory allocation in ", AX_EQ_B_BK) "() failed!\n");
      exit(1);
    }
  }
#else
  buf_sz=tot_sz;
  buf=(LM_REAL *)malloc(buf_sz);
  if(!buf){
    printf(RCAT("memory allocation in ", AX_EQ_B_BK) "() failed!\n");
    exit(1);
  }
#endif /* LINSOLVERS_RETAIN_MEMORY */

  a=buf;
  work=a+a_sz;
  ipiv=(int *)(work+work_sz);

  /* store A into a and B into x; A is assumed to be symmetric, hence
   * the column and row major order representations are the same
   */
  memcpy(a, A, a_sz*sizeof(LM_REAL));
  memcpy(x, B, m*sizeof(LM_REAL));

  /* LDLt factorization for A */
	SYTRF("L", (int *)&m, a, (int *)&m, ipiv, work, (int *)&work_sz, (int *)&info);
	if(info!=0){
		if(info<0){
      printf(RCAT(RCAT("LAPACK error: illegal value for argument %d of ", SYTRF) " in ", AX_EQ_B_BK) "()\n", -info);
			exit(1);
		}
		else{
      printf(RCAT(RCAT("LAPACK error: singular block diagonal matrix D for", SYTRF) " in ", AX_EQ_B_BK)"() [D(%d, %d) is zero]\n", info, info);
#ifndef LINSOLVERS_RETAIN_MEMORY
      free(buf);
#endif

			return 0;
		}
	}

  /* solve the system with the computed factorization */
  SYTRS("L", (int *)&m, (int *)&nrhs, a, (int *)&m, ipiv, x, (int *)&m, (int *)&info);
  if(info<0){
    printf(RCAT(RCAT("LAPACK error: illegal value for argument %d of ", SYTRS) " in ", AX_EQ_B_BK) "()\n", -info);
    exit(1);
	}

#ifndef LINSOLVERS_RETAIN_MEMORY
  free(buf);
#endif

	return 1;
}

/* undefine all. IT MUST REMAIN IN THIS POSITION IN FILE */
#undef AX_EQ_B_QR
#undef AX_EQ_B_QRLS
#undef AX_EQ_B_CHOL
#undef AX_EQ_B_LU
#undef AX_EQ_B_SVD
#undef AX_EQ_B_BK
#undef AX_EQ_B_PLASMA_CHOL

#undef GEQRF
#undef ORGQR
#undef TRTRS
#undef POTF2
#undef POTRF
#undef POTRS
#undef GETRF
#undef GETRS
#undef GESVD
#undef GESDD
#undef SYTRF
#undef SYTRS
#undef PLASMA_POSV

#else // no LAPACK

/* precision-specific definitions */
#define AX_EQ_B_LU LM_ADD_PREFIX(Ax_eq_b_LU_noLapack)

/*
 * This function returns the solution of Ax = b
 *
 * The function employs LU decomposition followed by forward/back substitution (see
 * also the LAPACK-based LU solver above)
 *
 * A is mxm, b is mx1
 *
 * The function returns 0 in case of error, 1 if successful
 *
 * This function is often called repetitively to solve problems of identical
 * dimensions. To avoid repetitive malloc's and free's, allocated memory is
 * retained between calls and free'd-malloc'ed when not of the appropriate size.
 * A call with NULL as the first argument forces this memory to be released.
 */
int AX_EQ_B_LU(LM_REAL *A, LM_REAL *B, LM_REAL *x, int m)
{
__STATIC__ void *buf=NULL;
__STATIC__ int buf_sz=0;

register int i, j, k;
int *idx, maxi=-1, idx_sz, a_sz, work_sz, tot_sz;
LM_REAL *a, *work, max, sum, tmp;

    if(!A)
#ifdef LINSOLVERS_RETAIN_MEMORY
    {
      if(buf) free(buf);
      buf=NULL;
      buf_sz=0;

      return 1;
    }
#else
    return 1; /* NOP */
#endif /* LINSOLVERS_RETAIN_MEMORY */

  /* calculate required memory size */
  idx_sz=m;
  a_sz=m*m;
  work_sz=m;
  tot_sz=(a_sz+work_sz)*sizeof(LM_REAL) + idx_sz*sizeof(int); /* should be arranged in that order for proper doubles alignment */

#ifdef LINSOLVERS_RETAIN_MEMORY
  if(tot_sz>buf_sz){ /* insufficient memory, allocate a "big" memory chunk at once */
    if(buf) free(buf); /* free previously allocated memory */

    buf_sz=tot_sz;
    buf=(void *)malloc(tot_sz);
    if(!buf){
      printf(RCAT("memory allocation in ", AX_EQ_B_LU) "() failed!\n");
      exit(1);
    }
  }
#else
    buf_sz=tot_sz;
    buf=(void *)malloc(tot_sz);
    if(!buf){
      printf(RCAT("memory allocation in ", AX_EQ_B_LU) "() failed!\n");
      exit(1);
    }
#endif /* LINSOLVERS_RETAIN_MEMORY */

  a=buf;
  work=a+a_sz;
  idx=(int *)(work+work_sz);

  /* avoid destroying A, B by copying them to a, x resp. */
  memcpy(a, A, a_sz*sizeof(LM_REAL));
  memcpy(x, B, m*sizeof(LM_REAL));

  /* compute the LU decomposition of a row permutation of matrix a; the permutation itself is saved in idx[] */
	for(i=0; i<m; ++i){
		max=0.0;
		for(j=0; j<m; ++j)
			if((tmp=FABS(a[i*m+j]))>max)
        max=tmp;
		  if(max==0.0){
        printf(RCAT("Singular matrix A in ", AX_EQ_B_LU) "()!\n");
#ifndef LINSOLVERS_RETAIN_MEMORY
        free(buf);
#endif

        return 0;
      }
		  work[i]=LM_CNST(1.0)/max;
	}

	for(j=0; j<m; ++j){
		for(i=0; i<j; ++i){
			sum=a[i*m+j];
			for(k=0; k<i; ++k)
        sum-=a[i*m+k]*a[k*m+j];
			a[i*m+j]=sum;
		}
		max=0.0;
		for(i=j; i<m; ++i){
			sum=a[i*m+j];
			for(k=0; k<j; ++k)
        sum-=a[i*m+k]*a[k*m+j];
			a[i*m+j]=sum;
			if((tmp=work[i]*FABS(sum))>=max){
				max=tmp;
				maxi=i;
			}
		}
		if(j!=maxi){
			for(k=0; k<m; ++k){
				tmp=a[maxi*m+k];
				a[maxi*m+k]=a[j*m+k];
				a[j*m+k]=tmp;
			}
			work[maxi]=work[j];
		}
		idx[j]=maxi;
		if(a[j*m+j]==0.0)
      a[j*m+j]=LM_REAL_EPSILON;
		if(j!=m-1){
			tmp=LM_CNST(1.0)/(a[j*m+j]);
			for(i=j+1; i<m; ++i)
        a[i*m+j]*=tmp;
		}
	}

  /* The decomposition has now replaced a. Solve the linear system using
   * forward and back substitution
   */
	for(i=k=0; i<m; ++i){
		j=idx[i];
		sum=x[j];
		x[j]=x[i];
		if(k!=0)
			for(j=k-1; j<i; ++j)
        sum-=a[i*m+j]*x[j];
		else
      if(sum!=0.0)
			  k=i+1;
		x[i]=sum;
	}

	for(i=m-1; i>=0; --i){
		sum=x[i];
		for(j=i+1; j<m; ++j)
      sum-=a[i*m+j]*x[j];
		x[i]=sum/a[i*m+i];
	}

#ifndef LINSOLVERS_RETAIN_MEMORY
  free(buf);
#endif

  return 1;
}

/* undefine all. IT MUST REMAIN IN THIS POSITION IN FILE */
#undef AX_EQ_B_LU

#endif /* HAVE_LAPACK */


/* interpolation/interp.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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/* Author:  G. Jungman
 */
#include <config.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_interp.h>

#define DISCARD_STATUS(s) if ((s) != GSL_SUCCESS) { GSL_ERROR_VAL("interpolation error", (s),  GSL_NAN); }

gsl_interp *
gsl_interp_alloc (const gsl_interp_type * T, size_t size)
{
  gsl_interp * interp;

  if (size < T->min_size)
    {
      GSL_ERROR_NULL ("insufficient number of points for interpolation type",
                      GSL_EINVAL);
    }

  interp = (gsl_interp *) malloc (sizeof(gsl_interp));

  if (interp == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for interp struct",
                      GSL_ENOMEM);
    }

  interp->type = T;
  interp->size = size;

  if (interp->type->alloc == NULL)
    {
      interp->state = NULL;
      return interp;
    }

  interp->state = interp->type->alloc(size);

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

  return interp;
}

int
gsl_interp_init (gsl_interp * interp, const double x_array[], const double y_array[], size_t size)
{
  if (size != interp->size)
    {
      GSL_ERROR ("data must match size of interpolation object", GSL_EINVAL);
    }

  interp->xmin = x_array[0];
  interp->xmax = x_array[size - 1];

  {
    int status = interp->type->init(interp->state, x_array, y_array, size);
    return status;
  }
}

const char *
gsl_interp_name(const gsl_interp * interp)
{
  return interp->type->name;
}

unsigned int
gsl_interp_min_size(const gsl_interp * interp)
{
  return interp->type->min_size;
}

void
gsl_interp_free (gsl_interp * interp)
{
  if (interp->type->free)
    interp->type->free (interp->state);
  free (interp);
}

int
gsl_interp_eval_e (const gsl_interp * interp,
                   const double xa[], const double ya[], double x,
                   gsl_interp_accel * a, double *y)
{
  if (x < interp->xmin)
    {
      *y = ya[0];
      return GSL_EDOM;
    }
  else if (x > interp->xmax)
    {
      *y = ya[interp->size - 1];
      return GSL_EDOM;
    }

  return interp->type->eval (interp->state, xa, ya, interp->size, x, a, y);
}

double
gsl_interp_eval (const gsl_interp * interp,
                 const double xa[], const double ya[], double x,
                 gsl_interp_accel * a)
{
  double y;
  int status = interp->type->eval (interp->state, xa, ya, interp->size, x, a, &y);

  DISCARD_STATUS(status);

  return y;
}

int
gsl_interp_eval_deriv_e (const gsl_interp * interp,
                         const double xa[], const double ya[], double x,
                         gsl_interp_accel * a,
                         double *dydx)
{
  if (x < interp->xmin)
    {
      *dydx = 0.0;
      return GSL_EDOM;
    }
  else if (x > interp->xmax)
    {
      *dydx = 0.0;
      return GSL_EDOM;
    }

  return interp->type->eval_deriv (interp->state, xa, ya, interp->size, x, a, dydx);
}

double
gsl_interp_eval_deriv (const gsl_interp * interp,
                       const double xa[], const double ya[], double x,
                       gsl_interp_accel * a)
{
  double dydx;
  int status = interp->type->eval_deriv (interp->state, xa, ya, interp->size, x, a, &dydx);

  DISCARD_STATUS(status);

  return dydx;
}

int
gsl_interp_eval_deriv2_e (const gsl_interp * interp,
                          const double xa[], const double ya[], double x,
                          gsl_interp_accel * a,
                          double * d2)
{
  if (x < interp->xmin)
    {
      *d2 = 0.0;
      return GSL_EDOM;
    }
  else if (x > interp->xmax)
    {
      *d2 = 0.0;
      return GSL_EDOM;
    }

  return interp->type->eval_deriv2 (interp->state, xa, ya, interp->size, x, a, d2);
}

double
gsl_interp_eval_deriv2 (const gsl_interp * interp,
                        const double xa[], const double ya[], double x,
                        gsl_interp_accel * a)
{
  double d2;
  int status = interp->type->eval_deriv2 (interp->state, xa, ya, interp->size, x, a, &d2);

  DISCARD_STATUS(status);

  return d2;
}

int
gsl_interp_eval_integ_e (const gsl_interp * interp,
                         const double xa[], const double ya[],
                         double a, double b,
                         gsl_interp_accel * acc,
                         double * result)
{
  if (a > b || a < interp->xmin || b > interp->xmax)
    {
      *result = 0.0;
      return GSL_EDOM;
    }
  else if(a == b)
    {
      *result = 0.0;
      return GSL_SUCCESS;
    }

  return interp->type->eval_integ (interp->state, xa, ya, interp->size, acc, a, b, result);
}

double
gsl_interp_eval_integ (const gsl_interp * interp,
                       const double xa[], const double ya[],
                       double a, double b,
                       gsl_interp_accel * acc)
{
  double result;
  int status = interp->type->eval_integ (interp->state, xa, ya, interp->size, acc, a, b, &result);

  DISCARD_STATUS(status);

  return result;
}


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

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sum.h>

int
gsl_sum_levin_u_accel (const double *array, const size_t array_size,
                       gsl_sum_levin_u_workspace * w,
                       double *sum_accel, double *abserr)
{
  return gsl_sum_levin_u_minmax (array, array_size,
                                 0, array_size - 1, w, sum_accel, abserr);
}

int
gsl_sum_levin_u_minmax (const double *array, const size_t array_size,
                        const size_t min_terms, const size_t max_terms,
                        gsl_sum_levin_u_workspace * w,
                        double *sum_accel, double *abserr)
{
  /* Ignore any trailing zeros in the array */
  size_t size = array_size;

  while (size > 0 && array[size - 1] == 0) {
    size--;
  }

  if (size == 0)
    {
      *sum_accel = 0.0;
      *abserr = 0.0;
      w->sum_plain = 0.0;
      w->terms_used = 0;
      return GSL_SUCCESS;
    }
  else if (size == 1)
    {
      *sum_accel = array[0];
      *abserr = 0.0;
      w->sum_plain = array[0];
      w->terms_used = 1;
      return GSL_SUCCESS;
    }
  else
    {
      const double SMALL = 0.01;
      const size_t nmax = GSL_MAX (max_terms, array_size) - 1;
      double noise_n = 0.0, noise_nm1 = 0.0;
      double trunc_n = 0.0, trunc_nm1 = 0.0;
      double actual_trunc_n = 0.0, actual_trunc_nm1 = 0.0;
      double result_n = 0.0, result_nm1 = 0.0;
      double variance = 0;
      size_t n;
      unsigned int i;
      int better = 0;
      int before = 0;
      int converging = 0;
      double least_trunc = GSL_DBL_MAX;
      double least_trunc_noise = GSL_DBL_MAX;
      double least_trunc_result;

      /* Calculate specified minimum number of terms.  No convergence
         tests are made, and no truncation information is stored.  */

      for (n = 0; n < min_terms; n++)
        {
          const double t = array[n];
          result_nm1 = result_n;
          gsl_sum_levin_u_step (t, n, nmax, w, &result_n);
        }

      least_trunc_result = result_n;

      variance = 0;
      for (i = 0; i < n; i++)
        {
          double dn = w->dsum[i] * GSL_MACH_EPS * array[i];
          variance += dn * dn;
        }
      noise_n = sqrt (variance);

      /* Calculate up to maximum number of terms.  Check truncation
         condition.  */

      for (; n <= nmax; n++)
        {
          const double t = array[n];

          result_nm1 = result_n;
          gsl_sum_levin_u_step (t, n, nmax, w, &result_n);

          /* Compute the truncation error directly */

          actual_trunc_nm1 = actual_trunc_n;
          actual_trunc_n = fabs (result_n - result_nm1);

          /* Average results to make a more reliable estimate of the
             real truncation error */

          trunc_nm1 = trunc_n;
          trunc_n = 0.5 * (actual_trunc_n + actual_trunc_nm1);

          noise_nm1 = noise_n;
          variance = 0;

          for (i = 0; i <= n; i++)
            {
              double dn = w->dsum[i] * GSL_MACH_EPS * array[i];
              variance += dn * dn;
            }

          noise_n = sqrt (variance);

          /* Determine if we are in the convergence region.  */

          better = (trunc_n < trunc_nm1 || trunc_n < SMALL * fabs (result_n));
          converging = converging || (better && before);
          before = better;

          if (converging)
            {
              if (trunc_n < least_trunc)
                {
                  /* Found a low truncation point in the convergence
                     region. Save it. */

                  least_trunc_result = result_n;
                  least_trunc = trunc_n;
                  least_trunc_noise = noise_n;
                }

              if (noise_n > trunc_n / 3.0)
                break;

              if (trunc_n < 10.0 * GSL_MACH_EPS * fabs (result_n))
                break;
            }

        }

      if (converging)
        {
          /* Stopped in the convergence region.  Return result and
             error estimate.  */

          *sum_accel = least_trunc_result;
          *abserr = GSL_MAX_DBL (least_trunc, least_trunc_noise);
          w->terms_used = n;
          return GSL_SUCCESS;
        }
      else
        {
          /* Never reached the convergence region.  Use the last
             calculated values.  */

          *sum_accel = result_n;
          *abserr = GSL_MAX_DBL (trunc_n, noise_n);
          w->terms_used = n;
          return GSL_SUCCESS;
        }
    }
}

int
gsl_sum_levin_u_step (const double term, const size_t n, const size_t nmax,
                      gsl_sum_levin_u_workspace * w, double *sum_accel)
{

#define I(i,j) ((i)*(nmax+1) + (j))

  if (n == 0)
    {
      *sum_accel = term;
      w->sum_plain = term;

      w->q_den[0] = 1.0 / term;
      w->q_num[0] = 1.0;

      w->dq_den[I (0, 0)] = -1.0 / (term * term);
      w->dq_num[I (0, 0)] = 0.0;

      w->dsum[0] = 1.0;

      return GSL_SUCCESS;
    }
  else
    {
      double result;
      double factor = 1.0;
      double ratio = (double) n / (n + 1.0);
      unsigned int i;
      int j;

      w->sum_plain += term;

      w->q_den[n] = 1.0 / (term * (n + 1.0) * (n + 1.0));
      w->q_num[n] = w->sum_plain * w->q_den[n];

      for (i = 0; i < n; i++)
        {
          w->dq_den[I (i, n)] = 0;
          w->dq_num[I (i, n)] = w->q_den[n];
        }

      w->dq_den[I (n, n)] = -w->q_den[n] / term;
      w->dq_num[I (n, n)] =
        w->q_den[n] + w->sum_plain * (w->dq_den[I (n, n)]);

      for (j = n - 1; j >= 0; j--)
        {
          double c = factor * (j + 1) / (n + 1);
          factor *= ratio;
          w->q_den[j] = w->q_den[j + 1] - c * w->q_den[j];
          w->q_num[j] = w->q_num[j + 1] - c * w->q_num[j];

          for (i = 0; i < n; i++)
            {
              w->dq_den[I (i, j)] =
                w->dq_den[I (i, j + 1)] - c * w->dq_den[I (i, j)];
              w->dq_num[I (i, j)] =
                w->dq_num[I (i, j + 1)] - c * w->dq_num[I (i, j)];
            }

          w->dq_den[I (n, j)] = w->dq_den[I (n, j + 1)];
          w->dq_num[I (n, j)] = w->dq_num[I (n, j + 1)];
        }

      result = w->q_num[0] / w->q_den[0];

      *sum_accel = result;

      for (i = 0; i <= n; i++)
        {
          w->dsum[i] =
            (w->dq_num[I (i, 0)] -
             result * w->dq_den[I (i, 0)]) / w->q_den[0];
        }

      return GSL_SUCCESS;
    }
}


/*
Copyright (c) 2015, Patrick Weltevrede
All rights reserved.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.

2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.

3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_min.h>
#include "gsl/gsl_roots.h"
#include "psrsalsa.h"
int minimize_1D_double_refine_borders(int findroot, double (*funk)(double, void *), void *params, int gridsearch, int investigateLocalMinima, double *x_lower, double *x_upper, int debug_verbose)
{
  double x, y, ymin[3], ymin_new[3], dy[3], ymax, x_lower_new, x_upper_new, sign_old, gridsearch_margin, imin_new[3], i_originalgrid, imin1, imin2;
  int imin[3], unset[3], i, minima;
  unset[0] = unset[1] = unset[2] = 1;
  for(i = 0; i < gridsearch; i++) {
    x = *x_lower + i*(*x_upper - *x_lower)/(double)(gridsearch-1);
    y = funk(x, params);
    if(debug_verbose) {
      fflush(stdout); fprintf(stderr, "x=%f y=%f\n", x, y);
    }
    if(findroot) {
      if(i == 0) {
 sign_old = y;
 imin[0] = 0;
      }else {
 if(y < 0 && sign_old > 0) {
   imin[0] = i;
 }else if(y > 0 && sign_old < 0) {
   imin[0] = i;
 }
 if(imin[0] != 0)
   break;
      }
    }else {
      if(unset[0]) {
 imin[0] = i;
 ymin[0] = y;
 unset[0] = 0;
      }else if(unset[1]) {
 imin[1] = i;
 ymin[1] = y;
 unset[1] = 0;
      }else if(unset[2]) {
 imin[2] = i;
 ymin[2] = y;
 unset[2] = 0;
      }else {
 for(minima = 0; minima < 3; minima++) {
   dy[minima] = ymin[minima] - y;
 }
 if(dy[0] > dy[1] && dy[0] > dy[2]) {
   if(dy[0] > 0) {
     imin[0] = i;
     ymin[0] = y;
   }
 }else if(dy[1] > dy[0] && dy[1] > dy[2]) {
   if(dy[1] > 0) {
     imin[1] = i;
     ymin[1] = y;
   }
 }else {
   if(dy[2] > 0) {
     imin[2] = i;
     ymin[2] = y;
   }
 }
      }
      if(i == 0 || y > ymax) {
 ymax = y;
      }
    }
  }
  if(findroot == 0) {
    imin1 = imin[0];
    imin2 = imin[0];
    if(imin[1] < imin1)
      imin1 = imin[1];
    if(imin[2] < imin1)
      imin1 = imin[2];
    if(imin[1] > imin2)
      imin2 = imin[1];
    if(imin[2] > imin2)
      imin2 = imin[2];
    if(imin2-imin1 != 2 && debug_verbose) {
      fflush(stdout); fprintf(stderr, "Found more than one possible minima (range = %.2f .. %.2f)\n", imin1, imin2);
    }
    if(imin2-imin1 > 0.3*gridsearch) {
      if(investigateLocalMinima == 0) {
 imin1 = imin[0];
 imin2 = imin[0];
 if(ymin[1] < ymin[0]) {
   imin1 = imin[1];
   imin2 = imin[1];
 }else if(ymin[2] < ymin[0] && ymin[2] < ymin[1]) {
   imin1 = imin[2];
   imin2 = imin[2];
 }
      }else {
 if(debug_verbose) {
   fflush(stdout); fprintf(stderr, "Refining gridsearch around local minima\n");
 }
 unset[0] = unset[1] = unset[2] = 1;
 for(minima = 0; minima < 3; minima++) {
   for(x = *x_lower + (imin[minima]-1.5)*(*x_upper - *x_lower)/(double)(gridsearch-1); x < *x_lower + (imin[minima]+1.5)*(*x_upper - *x_lower)/(double)(gridsearch-1); x += 0.09*(*x_upper - *x_lower)/(double)(gridsearch-1)) {
     y = funk(x, params);
     if(debug_verbose) {
       fflush(stdout); fprintf(stderr, "x=%f y=%f\n", x, y);
     }
     i_originalgrid = (x - *x_lower)*(double)(gridsearch-1)/(*x_upper - *x_lower);
     if(unset[0] || y < ymin_new[0]) {
       imin_new[0] = i_originalgrid;
       ymin_new[0] = y;
       unset[0] = 0;
     }else if(unset[1] || y < ymin_new[1]) {
       imin_new[1] = i_originalgrid;
       ymin_new[1] = y;
       unset[1] = 0;
     }else if(unset[2] || y < ymin_new[2]) {
       imin_new[2] = i_originalgrid;
       ymin_new[2] = y;
       unset[2] = 0;
     }
   }
 }
 imin1 = imin_new[0];
 imin2 = imin_new[0];
 if(imin_new[1] < imin1)
   imin1 = imin_new[1];
 if(imin_new[2] < imin1)
   imin1 = imin_new[2];
 if(imin_new[1] > imin2)
   imin2 = imin_new[1];
 if(imin_new[2] > imin2)
   imin2 = imin_new[2];
 if(imin2-imin1 != 2 && debug_verbose) {
   fflush(stdout); fprintf(stderr, "Found more than one possible minima (range = %.2f .. %.2f)\n", imin1, imin2);
 }
 if(imin2-imin1 > 0.3*gridsearch) {
   fflush(stdout); fprintf(stderr, "minimize_1D_double_refine_borders: Refining gridsearch around local minima failed\n");
   exit(0);
 }
      }
    }
  }
  gridsearch_margin = 0.1*(double)gridsearch;
  if(gridsearch_margin < 1)
    gridsearch_margin = 1;
  if(findroot) {
    if(imin[0] == 0) {
      return 2;
    }
    x_lower_new = *x_lower + (imin[0]-gridsearch_margin)*(*x_upper - *x_lower)/(double)(gridsearch-1);
    x_upper_new = *x_lower + (imin[0]+gridsearch_margin-1)*(*x_upper - *x_lower)/(double)(gridsearch-1);
    *x_lower = x_lower_new;
    *x_upper = x_upper_new;
  }else {
    x_lower_new = *x_lower + (imin1-gridsearch_margin)*(*x_upper - *x_lower)/(double)(gridsearch-1);
    x_upper_new = *x_lower + (imin2+gridsearch_margin)*(*x_upper - *x_lower)/(double)(gridsearch-1);
    *x_lower = x_lower_new;
    *x_upper = x_upper_new;
  }
  return 0;
}
int minimize_1D_double(int findroot, double (*funk)(double, void *), void *params, double x_lower, double x_upper, int gridsearch, int investigateLocalMinima, int nested, double *x_minimum, int max_iter, double epsabs, double epsrel, int verbose, int debug_verbose)
{
  const gsl_min_fminimizer_type *T_minimizer;
  gsl_min_fminimizer *s_minimizer;
  const gsl_root_fsolver_type *T_root;
  gsl_root_fsolver *s_root;
  int status;
  int iter, i, ret, nest;
  gsl_function F;
  F.function = funk;
  F.params = params;
  iter = 0;
#if GSL_VERSION_NUMBER < 102
  printerror(0, "ERROR minimize_1D_double: Not supported for GSL < 1.2");
  exit(0);
#endif
  if(verbose) {
    for(i = 0; i < verbose - 1; i++)
      printf(" ");
    if(findroot == 0)
      printf("Minimising function between %f and %f\n", x_lower, x_upper);
    else
      printf("Finding root of function between %f and %f\n", x_lower, x_upper);
  }
  if(gridsearch > 1) {
    if(verbose) {
      for(i = 0; i < verbose - 1; i++)
 printf(" ");
      printf("  Refining boundaries with a %d point grid search\n", gridsearch);
    }
    for(nest = 0; nest < 1+nested; nest++) {
      ret = minimize_1D_double_refine_borders(findroot, funk, params, gridsearch, investigateLocalMinima, &x_lower, &x_upper, debug_verbose);
      if(ret != 0) {
 if(ret == 1) {
   if(verbose) {
     for(i = 0; i < verbose - 1; i++)
       printf(" ");
     printf("  Maximum itterations (%d) exceeded while refining borders\n", max_iter);
   }
 }
 return ret;
      }
      if(verbose) {
 for(i = 0; i < verbose - 1; i++)
   printf(" ");
 printf("  Refined boundaries are %f and %f\n", x_lower, x_upper);
      }
    }
  }
  *x_minimum = 0.5*(x_upper+x_lower);
  if(findroot == 0) {
    T_minimizer = gsl_min_fminimizer_brent;
    s_minimizer = gsl_min_fminimizer_alloc(T_minimizer);
    fflush(stdout);
    gsl_min_fminimizer_set(s_minimizer, &F, *x_minimum, x_lower, x_upper);
  }else {
    T_root = gsl_root_fsolver_brent;
    s_root = gsl_root_fsolver_alloc(T_root);
    if(gridsearch == 0) {
      double val1, val2;
      val1 = funk(x_lower, params);
      val2 = funk(x_upper, params);
      if((val1 > 0 && val2 > 0) || (val1 < 0 && val2 < 0)) {
 return 3;
      }
    }
    gsl_root_fsolver_set(s_root, &F, x_lower, x_upper);
  }
  do {
    iter++;
    if(findroot == 0) {
      status = gsl_min_fminimizer_iterate(s_minimizer);
#if GSL_VERSION_NUMBER >= 102
      *x_minimum = gsl_min_fminimizer_x_minimum (s_minimizer);
#endif
      x_lower = gsl_min_fminimizer_x_lower(s_minimizer);
      x_upper = gsl_min_fminimizer_x_upper(s_minimizer);
    }else {
      status = gsl_root_fsolver_iterate(s_root);
      *x_minimum = gsl_root_fsolver_root(s_root);
      x_lower = gsl_root_fsolver_x_lower(s_root);
      x_upper = gsl_root_fsolver_x_upper(s_root);
    }
    status = gsl_min_test_interval (x_lower, x_upper, epsabs, epsrel);
  }while(status == GSL_CONTINUE && iter < max_iter);
  if(findroot)
    gsl_root_fsolver_free(s_root);
  else
    gsl_min_fminimizer_free(s_minimizer);
  if(status == GSL_SUCCESS) {
    if(verbose) {
      for(i = 0; i < verbose - 1; i++)
 printf(" ");
      if(findroot)
 printf("  Root found at %f with error %f\n", *x_minimum, fabs(x_upper - x_lower));
      else
 printf("  Minimum found at %f with error %f\n", *x_minimum, fabs(x_upper - x_lower));
    }
    return 0;
  }else if(iter == max_iter) {
    if(verbose) {
      for(i = 0; i < verbose - 1; i++)
 printf(" ");
      printf("  Maximum itterations (%d) exceeded. Value confined to [%f, %f]\n", max_iter, x_lower, x_upper);
    }
    return 1;
  }else {
    if(verbose) {
      for(i = 0; i < verbose - 1; i++)
 printf(" ");
      printf("  Unknown error during minimization\n");
    }
    return 5;
  }
  return status;
}
double (*internal_funk_provided_by_user)(double *, void *);
int internal_paramnr;
double *internal_xminimum;
double internal_desired_chi2;
double internal_find_1D_error_funk(double x, void *params)
{
  double chi2;
  internal_xminimum[internal_paramnr] = x;
  chi2 = internal_funk_provided_by_user(internal_xminimum, params);
  chi2 -= internal_desired_chi2;
  return chi2;
}
int find_1D_error(double (*funk)(double *, void *), double *xminimum, int paramnr, int nrparameters, double dx, double dxmax, void *params, double sigma, double chi2min, int max_itr, double epsabs, double epsrel, double *errorbar, int verbose)
{
  int ittr, i, ret, debug_verbose;
  double x_lower, x_upper, diff, xval, *xminimum_fiddle;
  debug_verbose = 0;
  dx = fabs(dx);
  if(verbose) {
    for(i = 0; i < verbose - 1; i++)
      printf(" ");
    printf("Finding error at value %f with stepsize %f\n", xminimum[paramnr], dx);
  }
  xminimum_fiddle = malloc(nrparameters*sizeof(double));
  if(xminimum_fiddle == NULL) {
    fflush(stdout);
    fprintf(stderr, "ERROR find_1D_error: Memory allocation error\n");
    return 4;
  }
  memcpy(xminimum_fiddle, xminimum, nrparameters*sizeof(double));
  internal_paramnr = paramnr;
  internal_xminimum = xminimum_fiddle;
  internal_funk_provided_by_user = funk;
  internal_desired_chi2 = chi2min*(1+fabs(sigma));
  if(verbose) {
    for(i = 0; i < verbose - 1; i++)
      printf(" ");
    printf("  Mimimum chi2 = %f, so %f sigma point corresponds to %f\n", chi2min, fabs(sigma), internal_desired_chi2);
  }
  x_lower = xminimum[paramnr];
  x_upper = xminimum[paramnr];
  ittr = 0;
  diff = internal_find_1D_error_funk(x_upper, params);
  if(diff > 0) {
    fflush(stdout);
    fprintf(stderr, "ERROR find_1D_error: Function called with initial parameters outside specified sigma limit: chi2 = %f higher than sigma border (%f).\n", diff, internal_desired_chi2);
    exit(0);
  }
  do {
    if(sigma >= 0) {
      x_upper += dx;
      if(dxmax >= 0) {
 if(fabs(x_upper - xminimum[paramnr]) > dxmax) {
   *errorbar = dxmax;
   free(xminimum_fiddle);
   return 0;
 }
      }
      diff = internal_find_1D_error_funk(x_upper, params);
    }else {
      x_lower -= dx;
      if(dxmax >= 0) {
 if(fabs(x_lower - xminimum[paramnr]) > dxmax) {
   *errorbar = dxmax;
   free(xminimum_fiddle);
   return 0;
 }
      }
      diff = internal_find_1D_error_funk(x_lower, params);
    }
    ittr++;
    if(ittr == max_itr) {
      free(xminimum_fiddle);
      return 1;
    }
  }while(diff < 0);
  if(verbose) {
    for(i = 0; i < verbose - 1; i++)
      printf(" ");
    printf("  Found brackets: [%f %f]\n", x_lower, x_upper);
    verbose += 2;
  }
  ret = minimize_1D_double(1, internal_find_1D_error_funk, params, x_lower, x_upper, 0, 0, 0, &xval, max_itr, epsabs, epsrel, verbose, debug_verbose);
  *errorbar = fabs(xval - xminimum[paramnr]);
  if(verbose) {
    verbose -= 2;
    for(i = 0; i < verbose - 1; i++)
      printf(" ");
    if(ret == 0)
      printf("  Found errorbar: %f\n", *errorbar);
    else if(ret == 1)
      printf("  Maximum nr of itterations reached, did not converge fully\n");
    else if(ret == 2)
      printf("  Did not find root in specified range\n");
    else if(ret == 3)
      printf("  Lower and upper limit do not bracket a root\n");
    else {
      ret = 5;
      printf("  Other unspecified error\n");
    }
  }
  free(xminimum_fiddle);
  return ret;
}


#include <stdio.h>
#include <stdlib.h>
#include <gsl/gsl_sf_expint.h>
#include <math.h>

/* Calculate the exponential integral function by invoking the GNU scientific
   library */
/* Only intended to be used with R; not the most elegant integration but it
   may do for now */

/* One argument, the value (double) at which to evaluate Ei(x) */

double x, ei; /* argument to exponential integral, value */
char *program_name; /* name program is invoked under, for errors */

int main(int argc, char* argv[]) {
  void usage(void);	/* Warn users about proper usage */

  program_name = argv[0];
  if (argc != 2) {
    usage();
  }
  x = atof(&argv[1][0]);
  ei = gsl_sf_expint_E1(x);
  printf("%.18e\n",ei);
  return(0);
}

void usage(void) {
  (void) fprintf(stderr, "Usage is %s [floating-point argument]\n", program_name);
  exit(8);
}


/* Copyright (c) 2011-2012, Jérémy Fix. All rights reserved. */

/* Redistribution and use in source and binary forms, with or without */
/* modification, are permitted provided that the following conditions are met: */

/* * Redistributions of source code must retain the above copyright notice, */
/* this list of conditions and the following disclaimer. */
/* * Redistributions in binary form must reproduce the above copyright notice, */
/* this list of conditions and the following disclaimer in the documentation */
/* and/or other materials provided with the distribution. */
/* * None of the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. */

/* THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS "AS IS" AND */
/* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED */
/* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE */
/* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE */
/* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL */
/* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR */
/* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER */
/* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, */
/* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE */
/* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */

#ifndef UKF_MATH_H
#define UKF_MATH_H

#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>

#include <iostream>
#include <cassert>
#include <cmath>

namespace ukf
{
    namespace math
    {

        inline double min(double a, double b)
        {
            return a < b ? a : b;
        }

        inline double max(double a, double b)
        {
            return a < b ? b : a;
        }

        inline double signof(double x)
        {
            return x <= 0.0 ? -1.0 : 1.0;
        }

        inline bool cmp_equal(double x, double y)
        {
            return fabs(x - y) <= 1e-10;
        }
        inline bool cmp_diff(double x, double y)
        {
            return fabs(x - y) >= 1e-10;
        }

        /**
         * @brief This function performs a cholesky update according to Strange et al.(2007)
         * @author Mathieu.Geist@Supelec.fr
         *
         */
        void choleskyUpdate(gsl_matrix * sigmaTheta, double alpha, gsl_vector *x) {

            /*
                This function performs a cholesky update of the
                cholesky factorization sigmaTheta, that is it replaces
                the Cholesky factorization sigmaTheta by the cholesky factorization of

                            sigmaTheta*sigmaTheta^T + alpha * x * x^T

                The algorithm is an adaptation of a LU factorization rank one update.

                Reference is :

                Peter Strange, Andreas Griewank and Matthias Bollhöfer.
                On the Efficient Update of Rectangular LU Factorizations subject to Low Rank Modifications.
                Electronic Transactions on Numerical Analysis, 26:161-177, 2007.
                alg. is given in left part of fig.2.1.

                Perhaps a more efficient algorithm exists, however it should do the work for now. And the code is probably not optimal...
            */

            if (sigmaTheta->size1 != sigmaTheta->size2)
                std::cout<<"ERROR ukf::math::choleskyUpdate Cannot use CholeskyUpdate on non-squared matrices"<<std::endl ;

            int i,j,n= sigmaTheta->size1 ;
            double tmp ;
            gsl_matrix *U = gsl_matrix_alloc(n,n) ;
            gsl_vector *D = gsl_vector_alloc(n) ;
            gsl_vector *y = gsl_vector_alloc(n) ;

            // A first thing is to set SS' (chol factor) in a LU form, L being unitriangular
            // Compute U = L^T and D = diag(L)
            gsl_matrix_set_zero(U) ;
            for (i=0; i<n; i++){
                gsl_vector_set(D,i,gsl_matrix_get(sigmaTheta,i,i));
                for (j=0; j<=i; j++){
                    gsl_matrix_set(U,j,i,gsl_matrix_get(sigmaTheta,i,j));
                }
            }
            // Replace L by L*D^{-1} and U by D*U
            for (i=0; i<n; i++){
                for (j=0; j<=i; j++){
                    tmp = gsl_matrix_get(sigmaTheta,i,j);
                    tmp /= gsl_vector_get(D,j);
                    gsl_matrix_set(sigmaTheta,i,j,tmp);
                    tmp = gsl_matrix_get(U,j,i);
                    tmp *= gsl_vector_get(D,j);
                    gsl_matrix_set(U,j,i,tmp);
                }
            }

            // compute the y = alpha x vector
            gsl_vector_memcpy(y,x) ;
            gsl_vector_scale(y,alpha) ;

            // perform the rank 1 LU modification
            for (i=0; i<n; i++){

                // diagonal update
                tmp = gsl_matrix_get(U,i,i) +
                      gsl_vector_get(x,i)*gsl_vector_get(y,i);
                gsl_matrix_set(U,i,i,tmp);
                tmp = gsl_vector_get(y,i);
                tmp /= gsl_matrix_get(U,i,i);
                gsl_vector_set(y,i,tmp);

                for (j=i+1; j<n; j++){
                    // L (that is sigmaTheta) update
                    tmp = gsl_vector_get(x,j) -
                          gsl_vector_get(x,i)*gsl_matrix_get(sigmaTheta,j,i);
                    gsl_vector_set(x,j,tmp) ;
                    tmp = gsl_matrix_get(sigmaTheta,j,i) + gsl_vector_get(y,i) * gsl_vector_get(x,j);
                    gsl_matrix_set(sigmaTheta,j,i,tmp);
                }

                for (j=i+1; j<n; j++) {
                    // U update
                    tmp = gsl_matrix_get(U,i,j) +
                          gsl_vector_get(x,i)*gsl_vector_get(y,j);
                    gsl_matrix_set(U,i,j,tmp) ;
                    tmp = gsl_vector_get(y,j) -
                          gsl_vector_get(y,i) * gsl_matrix_get(U,i,j);
                    gsl_vector_set(y,j,tmp) ;
                }
            }

            // Now we want the chol decomposition
            // first D = sqrt(diag(U)) ;
            for (i=0; i<n; i++){
                tmp =  gsl_matrix_get(U,i,i) ;

                if (tmp<=0){std::cout<< "WARNING ukf::math::choleskyUpdate::matrix not positive definite  : " << tmp << std::endl;}
                gsl_vector_set(D,i,sqrt(tmp)) ;
            }
            // then L = L*D ;
            for (i=0; i<n; i++) {
                for (j=0; j<n; j++) {
                    tmp = gsl_matrix_get(sigmaTheta,i,j) * gsl_vector_get(D,j);
                    gsl_matrix_set(sigmaTheta,i,j,tmp);
                }
            }
            // that's all folks !

            //free memory
            gsl_matrix_free(U);
            gsl_vector_free(y);
            gsl_vector_free(D);
        }
    }  // math
}  // ukf

#endif  // UKF_MATH_H