/*
* -----------------------------------------------------------------
*  ODE Solver Library --- ode_lib.c
*  Version: 1.6180
*  Date: Jan 4, 2011
* -----------------------------------------------------------------
*  Programmer: Americo Barbosa da Cunha Junior
*              americo.cunhajr@gmail.com
* -----------------------------------------------------------------
*  Copyright (c) 2010 by Americo Barbosa da Cunha Junior
*
*  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.
*
*  A copy of the GNU General Public License is available in
*  LICENSE.txt or http://www.gnu.org/licenses/.
* -----------------------------------------------------------------
*  This is the implementation file of a library
*  with ODE solution tools.
* -----------------------------------------------------------------
*/

#include <stdlib.h>
#include <math.h>
#include <float.h>
#include <nvector/nvector_serial.h>
#include <cvode/cvode_dense.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>

#include "../include/ode_lib.h"

/*
*------------------------------------------------------------
*   uround
*
*   This function computes the machine unit roundoff
*   defined as the smallest u such that 1.0 + u > 1.0
*
*   Output:
*   uround - machine unit roundoff
*
*   last update: May 20, 2009
*------------------------------------------------------------
*/

double uround(void)
{
    double u = 1.0;
    double comp = 0.0;

    while(comp != 1.0)
    {
	u *= 0.5;
	comp = 1.0 + u;
    }

    return 2.0*u;
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   wnorm
*
*   This function computes v1 and v2 vector weight norm
*   defined as
*   wnorm = sqrt( (1/n)*(sum (v1[i]*v2[i])^2) )
*
*   Input:
*   n     - vectors dimension
*   v     - vector 1
*   v     - vector 2
*
*   Output:
*   wnorm - weight norm
*
*   last update: May 20, 2009
*------------------------------------------------------------
*/

double wnorm(int n,double *v1,double *v2)
{
    int i;
    double wnorm = 0.0;

    for ( i = 0; i < n; i++ )
	wnorm += v1[i]*v1[i]*v2[i]*v2[i];

    return sqrt( wnorm / (double)n );
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   ewset
*
*   This function creates a error weight vector
*   defined as ewv[i] = rtol*abs(v[i]) + atol
*
*   Input:
*   n    - vector dimension
*   v    - vector
*   atol - absolute tolerance
*   rtol - relative tolerance
*
*   Output:
*   ewv  - error weight vector
*
*   last update: May 22, 2009
*------------------------------------------------------------
*/

void ewtset(int n,double *v,double atol,double rtol,double *ewt)
{
    int i;

    for ( i = 0; i < n; i++ )
        ewt[i] =  rtol*fabs(v[i]) + atol;

    return;
}
/*------------------------------------------------------------*/

/*------------------------------------------------------------
*   jacobian
*
*   This function computes the jacobian matrix of ydot = f(y,t)
*   defiend as  Jij = dfi/dyj
*
*   Input:
*   n      - vector dimension
*   f_data - pointer to external data
*   Fy     - reaction mapping
*   y      - composition vector
*   t      - time step
*   atol   - absolute tolerance
*   rtol   - relative tolerance
*
*   Output:
*   J       - jacobian matrix
*   success or error
*
*   last update: Oct 7, 2009
------------------------------------------------------------*/

int jacobian(CVRhsFn f,void *f_data,gsl_vector *Fy,gsl_vector *y,
                    double t,double atol,double rtol,gsl_matrix *J)
{
    unsigned int i, j, N1, N2;
    double roundoff, min_inc_mult;
    double fnorm, minInc, inc, inc_inv, yjsaved, srur;
    double *yd_data  = NULL;
    double *Fyd_data = NULL;
    N_Vector yd      = NULL;
    N_Vector Fyd     = NULL;
    gsl_vector *ewt  = NULL;

    min_inc_mult = 1.0e3;

    /* vectors dimensions */
    N1 = y->size;
    N2 = Fy->size;

    /* error weight vector */
    ewt = gsl_vector_calloc(N1);

    /* computing the unit roundoff */
    roundoff = uround();

    /* memory allocation and startup of yd and Fyd */
    yd  = N_VNew_Serial(N1);
    Fyd = N_VNew_Serial(N2);
    if ( yd == NULL || Fyd == NULL )
        return GSL_ENOMEM;

    /* obtaining yd and Fyd compoments  */
    yd_data  = NV_DATA_S(yd);
    Fyd_data = NV_DATA_S(Fyd);

    /* setting yd equal y vector */
    for ( i = 0; i < N1; i++ )
        yd_data[i] = y->data[i];

    /* setting Fyd equal the null vector */
    for ( i = 0; i < N2; i++ )
        Fyd_data[i] = 0.0;

    /* defing error weight vector */
    ewtset(N1,y->data,atol,rtol,ewt->data);

    /* computing weight norm */
    fnorm = wnorm(N2,Fy->data,ewt->data);

    /* square root of the machine roundoff */
    srur = sqrt(roundoff);

    /* computing disturbance parameter */
    minInc = (fnorm != 0.0) ?
	    (min_inc_mult*fabs(t)*roundoff*((double)N1)*fnorm) : 1.0;

    for ( j = 0; j < N1; j++ )
    {
        /* saving y[j] value */
        yjsaved = yd_data[j];

        /* disturbance */
        inc = GSL_MAX(srur*fabs(yjsaved),minInc/ewt->data[j]);

    	/* disturbing y */
        yd_data[j] += inc;

        /* computing Fy disturbed */
        f(t,yd,Fyd,f_data);

	/* restoring yd[j] original value */
        yd_data[j] = yjsaved;

	/* computing the step */
        inc_inv = 1.0/inc;

        /* computing jacobian matrix column j */
    	for ( i = 0; i < N2; i++ )
            J->data[i*J->tda+j] = inc_inv*(Fyd_data[i] - Fy->data[i]);
    }

    /* releasing allocated memory */
    N_VDestroy_Serial(yd);
    N_VDestroy_Serial(Fyd);
    gsl_vector_free(ewt);
    yd       = NULL;
    Fyd      = NULL;
    yd_data  = NULL;
    Fyd_data = NULL;
    ewt      = NULL;

    return GSL_SUCCESS;
}
/*------------------------------------------------------------*/

/*------------------------------------------------------------
*   gradient
*
*   This function computes the gradient matrix of R(phi)
*   defiend as: Aij = d R_i/d phi_j
*
*   Input:
*   f       - right hand side function
*   f_data  - pointer to external data
*   t0      - initial time
*   delta_t - time step
*   atol    - absolute tolerance
*   rtol    - relative tolerance
*   phi     - composition vector
*   Rphi    - reac tion mapping
*
*   Output:
*   A       - gradient matrix
*   success or error
*
*   last update: Feb 19, 2010
------------------------------------------------------------*/

int gradient(CVRhsFn f,void *f_data,void *cvode_mem,double t0,double delta_t,
    double atol,double rtol,gsl_vector *phi,gsl_vector *Rphi,gsl_matrix *A)
{
    unsigned int i, j, flag;
    double inc, inc_inv, phijsaved, srur;
    gsl_vector *Rphid = NULL;

    /* memory allocation for Rphid */
    Rphid = gsl_vector_calloc(Rphi->size);

    /* square root of 1.0e6 times the machine roundoff */
    srur = sqrt(1.0e6*DBL_EPSILON);

    for ( j = 0; j < phi->size; j++ )
    {
        /* saving phi_j value */
        phijsaved = phi->data[j];

        /* disturbance */
        inc = phijsaved*srur + srur;

    	/* disturbing phi_j */
        phi->data[j] += inc;

        /* computing R(phi) disturbed */
	flag = odesolver_reinit(f,f_data,t0,atol,rtol,phi,cvode_mem);
        if ( flag != GSL_SUCCESS )
            return flag;

	flag = odesolver(cvode_mem,delta_t,Rphid);
        if ( flag != GSL_SUCCESS )
            return flag;

	/* restoring phi_j original value */
        phi->data[j] = phijsaved;

	/* computing the step */
        inc_inv = 1.0/inc;

        /* computing gradient matrix column j */
    	for ( i = 0; i < Rphi->size; i++ )
            A->data[i*A->tda+j] = inc_inv*(Rphid->data[i] - Rphi->data[i]);
    }

    /* releasing allocated memory */
    gsl_vector_free(Rphid);
    Rphid = NULL;

    return GSL_SUCCESS;
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   linear_approx
*
*   This function computes the linear approximation
*   for a vector function F(x) near the point x0.
*
*   F(x) = F(x0) + DF(x0)*(x-x0) + O(|x-x0|^2)
*
*   Input:
*   x    - vector x
*   x0   - vector x0
*   Fx0  - function at x0
*   DFx0 - jacobian matrix at x0
*
*   Output:
*   Fx   - linear approximation for F(x)
*
*   last update: Jun 9, 2009
*------------------------------------------------------------
*/

void linear_approx(gsl_vector *x,gsl_vector *x0,
                gsl_vector *Fx0,gsl_matrix *DFx0,gsl_vector *Fx)
{
    gsl_vector *aux = NULL;

    /* memory allocation */
    aux = gsl_vector_alloc(x0->size);

    /* aux := x */
    gsl_vector_memcpy(aux,x);

    /* aux := x - x0 */
    gsl_vector_sub(aux,x0);

    /* Fx := F(x0) */
    gsl_vector_memcpy(Fx,Fx0);

    /* Fx := F(x0) + DF(x0)*(x-x0) */
    gsl_blas_dgemv(CblasNoTrans,1.0,DFx0,aux,1.0,Fx);

    /* releasing allocated memory */
    gsl_vector_free(aux);
    aux = NULL;

    return;
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   odesolver_init
*
*   This function initiates the ODE solver workspace.
*
*   Input:
*   f         - right hand side function
*   f_data    - right hand side function data
*   t0        - initial time
*   mxsteps   - max # of solver iterations
*   atol      - absolute tolerance
*   rtol      - relative tolerance
*   x         - initial condition
*   cvode_mem - ODE solver workspace
*
*   Output:
*   success or error
*
*   last update: Jan 4, 2011
*------------------------------------------------------------
*/

int odesolver_init(CVRhsFn f,void *f_data,double t0,int mxsteps,
		   double atol,double rtol,gsl_vector *x,void *cvode_mem)
{
    int maxnef  = 100;
    int flag    = CV_SUCCESS;
    N_Vector y0 = NULL;

    /* memory allocation and startup of y0 */
    y0 = N_VMake_Serial(x->size,x->data);
    if ( y0 == NULL )
	return GSL_ENOMEM;

    /* memory allocation for CVODE */
    flag = CVodeMalloc(cvode_mem,f,t0,y0,CV_SS,rtol,&atol);
    if( flag != CV_SUCCESS )
    	return GSL_ENOMEM;

    /* setting user data for right hand side function */
    flag = CVodeSetFdata(cvode_mem,f_data);
    if( flag != CV_SUCCESS )
        return flag;

    /* setting max # of steps to be taken by the solver */
    flag = CVodeSetMaxNumSteps(cvode_mem,mxsteps);
    if( flag != CV_SUCCESS )
        return flag;

    /* setting max # of error test failures permitted */
    flag = CVodeSetMaxErrTestFails(cvode_mem,maxnef);
    if( flag != CV_SUCCESS )
        return flag;

    /* setting dense linear solver */
    flag = CVDense(cvode_mem,x->size);
    if( flag != CV_SUCCESS )
        return flag;

    /* releasing allocated memory */
    N_VDestroy_Serial(y0);
    y0 = NULL;

    return GSL_SUCCESS;
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   odesolver_reinit
*
*   This function reinitiates the ODE solver workspace.
*
*   Input:
*   f         - right hand side function
*   f_data    - right hand side function data
*   t0        - initial time
*   atol      - absolute tolerance
*   rtol      - relative tolerance
*   x         - initial condition
*   cvode_mem - ODE solver workspace
*
*   Output:
*   success or error
*
*   last update: Oct 31, 2009
*------------------------------------------------------------
*/

int odesolver_reinit(CVRhsFn f,void *f_data,double t0,
		   double atol,double rtol,gsl_vector *x,void *cvode_mem)
{
    int flag    = CV_SUCCESS;
    N_Vector y0 = NULL;

    /* memory allocation and startup of y0 */
    y0 = N_VMake_Serial(x->size,x->data);
    if ( y0 == NULL )
	return GSL_ENOMEM;

    /* restarting CVODE workspace */
    flag = CVodeReInit(cvode_mem,f,t0,y0,CV_SS,rtol,&atol);
    if( flag != CV_SUCCESS )
    	return flag;

    /* setting user data for right hand side function */
    flag = CVodeSetFdata(cvode_mem,f_data);
    if( flag != CV_SUCCESS )
        return flag;

    /* releasing allocated memory */
    N_VDestroy_Serial(y0);
    y0 = NULL;

    return GSL_SUCCESS;
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   odesolver
*
*   This function performs the direct integration of the
*   governing equations.
*
*   Input:
*   cvode_mem - ODE solver workspace
*   tf   - final time
*   Fx   - solution vector
*
*   Output:
*   success or error
*
*   last update: Oct 31, 2009
*------------------------------------------------------------
*/

int odesolver(void *cvode_mem,double tf,gsl_vector *Fx)
{
    double t;
    int flag   = CV_SUCCESS;
    N_Vector y = NULL;

    /* memory allocation and startup of y */
    y = N_VMake_Serial(Fx->size,Fx->data);
    if ( y == NULL )
	return GSL_ENOMEM;

    /* calling CVode solver */
    flag = CVode(cvode_mem,tf,y,&t,CV_NORMAL);
    if( flag != CV_SUCCESS )
        return flag;

    /* releasing allocated memory */
    N_VDestroy_Serial(y);
    y = NULL;

    return GSL_SUCCESS;
}
/*------------------------------------------------------------*/


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

#include <stdlib.h>

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

/*
 * This module computes the eigenvalues of a real generalized
 * symmetric-definite eigensystem A x = \lambda B x, where A and
 * B are symmetric, and B is positive-definite.
 */

/*
gsl_eigen_gensymm_alloc()

Allocate a workspace for solving the generalized symmetric-definite
eigenvalue problem. The size of this workspace is O(2n).

Inputs: n - size of matrices

Return: pointer to workspace
*/

gsl_eigen_gensymm_workspace *
gsl_eigen_gensymm_alloc(const size_t n)
{
  gsl_eigen_gensymm_workspace *w;

  if (n == 0)
    {
      GSL_ERROR_NULL ("matrix dimension must be positive integer",
                      GSL_EINVAL);
    }

  w = (gsl_eigen_gensymm_workspace *) calloc (1, sizeof (gsl_eigen_gensymm_workspace));

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

  w->size = n;

  w->symm_workspace_p = gsl_eigen_symm_alloc(n);
  if (!w->symm_workspace_p)
    {
      gsl_eigen_gensymm_free(w);
      GSL_ERROR_NULL("failed to allocate space for symm workspace", GSL_ENOMEM);
    }

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

/*
gsl_eigen_gensymm_free()
  Free workspace w
*/

void
gsl_eigen_gensymm_free (gsl_eigen_gensymm_workspace * w)
{
  RETURN_IF_NULL (w);

  if (w->symm_workspace_p)
    gsl_eigen_symm_free(w->symm_workspace_p);

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

/*
gsl_eigen_gensymm()

Solve the generalized symmetric-definite eigenvalue problem

A x = \lambda B x

for the eigenvalues \lambda.

Inputs: A    - real symmetric matrix
        B    - real symmetric and positive definite matrix
        eval - where to store eigenvalues
        w    - workspace

Return: success or error
*/

int
gsl_eigen_gensymm (gsl_matrix * A, gsl_matrix * B, gsl_vector * eval,
                   gsl_eigen_gensymm_workspace * w)
{
  const size_t N = A->size1;

  /* check matrix and vector sizes */

  if (N != A->size2)
    {
      GSL_ERROR ("matrix must be square to compute eigenvalues", GSL_ENOTSQR);
    }
  else if ((N != B->size1) || (N != B->size2))
    {
      GSL_ERROR ("B matrix dimensions must match A", GSL_EBADLEN);
    }
  else if (eval->size != N)
    {
      GSL_ERROR ("eigenvalue vector must match matrix size", GSL_EBADLEN);
    }
  else if (w->size != N)
    {
      GSL_ERROR ("matrix size does not match workspace", GSL_EBADLEN);
    }
  else
    {
      int s;

      /* compute Cholesky factorization of B */
      s = gsl_linalg_cholesky_decomp(B);
      if (s != GSL_SUCCESS)
        return s; /* B is not positive definite */

      /* transform to standard symmetric eigenvalue problem */
      gsl_eigen_gensymm_standardize(A, B);

      s = gsl_eigen_symm(A, eval, w->symm_workspace_p);

      return s;
    }
} /* gsl_eigen_gensymm() */

/*
gsl_eigen_gensymm_standardize()
  Reduce the generalized symmetric-definite eigenproblem to
the standard symmetric eigenproblem by computing

C = L^{-1} A L^{-t}

where L L^t is the Cholesky decomposition of B

Inputs: A - (input/output) real symmetric matrix
        B - real symmetric, positive definite matrix in Cholesky form

Return: success

Notes: A is overwritten by L^{-1} A L^{-t}
*/

int
gsl_eigen_gensymm_standardize(gsl_matrix *A, const gsl_matrix *B)
{
  const size_t N = A->size1;
  size_t i;
  double a, b, c;

  for (i = 0; i < N; ++i)
    {
      /* update lower triangle of A(i:n, i:n) */

      a = gsl_matrix_get(A, i, i);
      b = gsl_matrix_get(B, i, i);
      a /= b * b;
      gsl_matrix_set(A, i, i, a);

      if (i < N - 1)
        {
          gsl_vector_view ai = gsl_matrix_subcolumn(A, i, i + 1, N - i - 1);
          gsl_matrix_view ma =
            gsl_matrix_submatrix(A, i + 1, i + 1, N - i - 1, N - i - 1);
          gsl_vector_const_view bi =
            gsl_matrix_const_subcolumn(B, i, i + 1, N - i - 1);
          gsl_matrix_const_view mb =
            gsl_matrix_const_submatrix(B, i + 1, i + 1, N - i - 1, N - i - 1);

          gsl_blas_dscal(1.0 / b, &ai.vector);

          c = -0.5 * a;
          gsl_blas_daxpy(c, &bi.vector, &ai.vector);

          gsl_blas_dsyr2(CblasLower, -1.0, &ai.vector, &bi.vector, &ma.matrix);

          gsl_blas_daxpy(c, &bi.vector, &ai.vector);

          gsl_blas_dtrsv(CblasLower,
                         CblasNoTrans,
                         CblasNonUnit,
                         &mb.matrix,
                         &ai.vector);
        }
    }

  return GSL_SUCCESS;
} /* gsl_eigen_gensymm_standardize() */


/*****************************************************************\
           __
          / /
		 / /                     __  __
		/ /______    _______    / / / / ________   __       __
	   / ______  \  /_____  \  / / / / / _____  | / /      / /
	  / /      | / _______| / / / / / / /____/ / / /      / /
	 / /      / / / _____  / / / / / / _______/ / /      / /
	/ /      / / / /____/ / / / / / / |______  / |______/ /
   /_/      /_/ |________/ / / / /  \_______/  \_______  /
                          /_/ /_/                     / /
			                                         / /
		       High Level Game Framework            /_/

  ---------------------------------------------------------------

  Copyright (c) 2007-2011 - Rodrigo Braz Monteiro.
  This file is subject to the terms of halley_license.txt.

\*****************************************************************/

#pragma once

#include <algorithm>
#include <limits>
#include <list>
#include <cmath>

#include <memory>
#include <array>
#include <functional>
#include <halley/data_structures/vector.h>
#include <gsl/gsl_assert>

#ifdef _MSC_VER
#include <xmmintrin.h>
#endif

namespace Halley {
	// General aliases
	using Byte = unsigned char;
	using Bytes = Vector<Byte>;

	template <typename T>
	constexpr inline T clamp(T value, T minValue, T maxValue)
	{
		return std::min(std::max(minValue, value), maxValue);
	}

	template <typename T>
	constexpr inline T clamp2(T value, T minValue, T maxValue)
	{
		return std::max(minValue, std::min(value, maxValue));
	}

	template <typename T>
	constexpr inline T maxAbs(T a, T b)
	{
		return abs(a) > abs(b) ? a : b;
	}

	template <typename T>
	constexpr inline T minAbs(T a, T b)
	{
		return abs(a) < abs(b) ? a : b;
	}

	template <typename T>
	constexpr inline bool rangeIntersection(T s1, T e1, T s2, T e2)
	{
		return (s1 < e2) && (s2 < e1);
	}

	constexpr double pi()
	{
		return 3.1415926535897932384626433832795;
	}

	// Endianness conversion
	//const bool IS_BIG_ENDIAN = *((short*)"AB") == 0x4243;
	constexpr bool IS_BIG_ENDIAN = false;

	template <typename T>
	constexpr inline T FixEndian(T t)
	{
		if (IS_BIG_ENDIAN) {
			T temp = t;
			unsigned char *v = reinterpret_cast<unsigned char*>(&temp);
			for (size_t i=0; i<sizeof(T)/2; i++) {
				std::swap(v[i], v[sizeof(T)-1-i]);
			}
			return temp;
		} else {
			return t;
		}
	}

	template<>
	constexpr inline char FixEndian<char>(char t) {
		return t;
	}

	template<>
	constexpr inline unsigned char FixEndian<unsigned char>(unsigned char t) {
		return t;
	}

	// True modulo definition
	template <typename T> constexpr inline T modulo (T a, T b)
	{
		static_assert(std::is_signed<T>::value, "Must be signed to use modulo operation");
		T res = a % b;
		if (res < 0) res = b+res;
		return res;
	}

	template <typename T> constexpr inline T floatModulo (T a, T b)
	{
		if (b == 0) return a;
		return a - b * std::floor(a/b);
	}
	template <> constexpr inline float modulo (float a, float b) { return floatModulo(a, b); }
	template <> constexpr inline double modulo (double a, double b) { return floatModulo(a, b); }

	// Float floor division (e.g. -0.5 / 2 = -1.0)
	template <typename T>
	constexpr inline T floorDivFloat (T a, T b)
	{
		return floor(a / b);
	}

	// Int floor division (e.g. -2 / 3 = -1)
	template <typename T>
	constexpr inline T floorDivInt (T a, T b)
	{
		T result = a / b;
		if (a % b < 0) {
			--result;
		}
		return result;
	}

	// Generic floor divs
	constexpr inline float floorDiv (float a, float b) { return floorDivFloat(a, b); }
	constexpr inline double floorDiv (double a, double b) { return floorDivFloat(a, b); }
	constexpr inline long floorDiv (long a, long b) { return floorDivInt(a, b); }
	constexpr inline int floorDiv (int a, int b) { return floorDivInt(a, b); }
	constexpr inline short floorDiv (short a, short b) { return floorDivInt(a, b); }
	constexpr inline char floorDiv (char a, char b) { return floorDivInt(a, b); }

	// Interpolation
	template <typename T>
	constexpr inline T interpolate(T a, T b, float factor) {
		return static_cast<T>(a * (1 - factor) + b * factor);
	}

	template <typename T>
	constexpr inline T lerp(T a, T b, float factor) {
		return static_cast<T>(a * (1 - factor) + b * factor);
	}

	// Smoothing
	template <typename T>
	constexpr inline T smoothCos(T a) {
		return T((1-cos(a * 3.1415926535897932384626433832795))*0.5);
	}

	// ASR (attack-sustain-release) envelope
	// Returns 0 at start of attack and end of release, 1 during sustain
	// Attack and release are linear interpolations
	template <typename T>
	constexpr inline T asr(T x, T a, T s, T r) {
		if (x < a) return x/a;
		T as = a+s;
		if (x < as) return 1;
		return 1 - ((x-as)/r);
	}

	// Next power of 2
	template<typename T>
	constexpr static T nextPowerOf2(T val)
	{
		--val;
		val = (val >> 1) | val;
		val = (val >> 2) | val;
		val = (val >> 4) | val;
		val = (val >> 8) | val;
		val = (val >> 16) | val;
		return val+1;
	}

	constexpr inline int fastLog2Floor (uint32_t value) {
		// From https://stackoverflow.com/questions/11376288/fast-computing-of-log2-for-64-bit-integers
		constexpr int tab32[32] = { 0, 9, 1, 10, 13, 21, 2, 29, 11, 14, 16, 18, 22, 25, 3, 30, 8, 12, 20, 28, 15, 17, 24, 7, 19, 27, 23, 6, 26, 5, 4, 31 };

		Expects(value > 0);
		value |= value >> 1;
		value |= value >> 2;
		value |= value >> 4;
		value |= value >> 8;
		value |= value >> 16;
		return tab32[uint32_t(value * 0x07C4ACDD) >> 27];
	}

	constexpr inline int fastLog2Floor (uint64_t value)
	{
		// From https://stackoverflow.com/questions/11376288/fast-computing-of-log2-for-64-bit-integers
		constexpr int tab64[64] = { 63,  0, 58,  1, 59, 47, 53,  2, 60, 39, 48, 27, 54, 33, 42,  3, 61, 51, 37, 40, 49, 18, 28, 20,  55, 30, 34, 11, 43, 14, 22,  4, 62, 57, 46, 52, 38, 26, 32, 41, 50, 36, 17, 19, 29, 10, 13, 21, 56, 45, 25, 31, 35, 16,  9, 12, 44, 24, 15,  8, 23,  7,  6,  5};

		Expects(value > 0);
		value |= value >> 1;
		value |= value >> 2;
		value |= value >> 4;
		value |= value >> 8;
		value |= value >> 16;
		value |= value >> 32;
		return tab64[uint64_t((value - (value >> 1)) * 0x07EDD5E59A4E28C2) >> 58];
	}

	constexpr inline int fastLog2Ceil (uint32_t value)
	{
		return fastLog2Floor(value - 1) + 1;
	}

	constexpr inline int fastLog2Ceil (uint64_t value)
	{
		return fastLog2Floor(value - 1) + 1;
	}

	// Advance a to b by up to inc
	template<typename T>
	constexpr static T advance(T a, T b, T inc)
	{
		if (a < b) return std::min(a+inc, b);
		else return std::max(a-inc, b);
	}

	// Align address
	template <typename T>
	constexpr T alignUp(T val, T align)
	{
		return val + (align - (val % align)) % align;
	}

	template <typename T>
	constexpr T alignDown(T val, T align)
	{
		return (val / align) * align;
	}

	// Prefetch data from memory
	static inline void prefetchL1(void* p) {
#ifdef _MSC_VER
		_mm_prefetch(static_cast<const char*>(p), _MM_HINT_T0);
#else
		__builtin_prefetch(p);
#endif
	}
	static inline void prefetchL2(void* p) {
#ifdef _MSC_VER
		_mm_prefetch(static_cast<const char*>(p), _MM_HINT_T1);
#else
		__builtin_prefetch(p);
#endif
	}
}


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

double A[] = { 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0 };

double x[] = { 1.0, -1.0, 2.0 };
double y[] = { -1.0, 0.0, 2.0 };

int main() {
  int i;
  cblas_dgemv(CblasRowMajor, CblasNoTrans, 3, 3, 2.5, A, 3, x, 1, 1.2, y, 1);
  for (i = 0; i < 3; ++i) { printf("%6.1f\n", y[i]); }

  return 0;
}


/* gsl_histogram_maxval.c
 * Copyright (C) 2000  Simone Piccardi
 *
 * This library 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 library; if not, write to the Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */
/***************************************************************
 *
 * File gsl_histogram_maxval.c:
 * Routine to find maximum and minumum content of a hisogram.
 * Need GSL library and header.
 * Contains the routines:
 * gsl_histogram_max_val find max content values
 * gsl_histogram_min_val find min content values
 * gsl_histogram_bin_max find coordinates of max contents bin
 * gsl_histogram_bin_min find coordinates of min contents bin
 *
 * Author: S. Piccardi
 * Jan. 2000
 *
 ***************************************************************/
#include <config.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_histogram.h>

double
gsl_histogram_max_val (const gsl_histogram * h)
{
  const size_t n = h->n;
  size_t i;
  double max = h->bin[0];
  for (i = 0; i < n; i++)
    {
      if (h->bin[i] > max)
        {
          max = h->bin[i];
        }
    }
  return max;
}

size_t
gsl_histogram_max_bin (const gsl_histogram * h)
{
  size_t i;
  size_t imax = 0;
  double max = h->bin[0];
  for (i = 0; i < h->n; i++)
    {
      if (h->bin[i] > max)
        {
          max = h->bin[i];
          imax = i;
        }
    }
  return imax;
}

double
gsl_histogram_min_val (const gsl_histogram * h)
{
  size_t i;
  double min = h->bin[0];
  for (i = 0; i < h->n; i++)
    {
      if (h->bin[i] < min)
        {
          min = h->bin[i];
        }
    }
  return min;
}

size_t
gsl_histogram_min_bin (const gsl_histogram * h)
{
  size_t i;
  size_t imin = 0;
  double min = h->bin[0];
  for (i = 0; i < h->n; i++)
    {
      if (h->bin[i] < min)
        {
          min = h->bin[i];
          imin = i;
        }
    }
  return imin;
}


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

/**
  This is a lagged Fibonacci generator which supposedly excellent
  statistical properties (I do not concur)

  I got it from the net and translated into C.

* ======================================================================
* NIST Guide to Available Math Software.
* Fullsource for module UNI from package CMLIB.
* Retrieved from CAMSUN on Tue Oct  8 14:04:10 1996.
* ======================================================================

C***BEGIN PROLOGUE  UNI
C***DATE WRITTEN   810915
C***REVISION DATE  830805
C***CATEGORY NO.  L6A21
C***KEYWORDS  RANDOM NUMBERS, UNIFORM RANDOM NUMBERS
C***AUTHOR    BLUE, JAMES, SCIENTIFIC COMPUTING DIVISION, NBS
C             KAHANER, DAVID, SCIENTIFIC COMPUTING DIVISION, NBS
C             MARSAGLIA, GEORGE, COMPUTER SCIENCE DEPT., WASH STATE UNIV
C
C***PURPOSE  THIS ROUTINE GENERATES QUASI UNIFORM RANDOM NUMBERS ON [0,1
C             AND CAN BE USED ON ANY COMPUTER WITH WHICH ALLOWS INTEGERS
C             AT LEAST AS LARGE AS 32767.
C***DESCRIPTION
C
C       THIS ROUTINE GENERATES QUASI UNIFORM RANDOM NUMBERS ON THE INTER
C       [0,1).  IT CAN BE USED WITH ANY COMPUTER WHICH ALLOWS
C       INTEGERS AT LEAST AS LARGE AS 32767.
C
C
C   USE
C       FIRST TIME....
C                   Z = UNI(JD)
C                     HERE JD IS ANY  N O N - Z E R O  INTEGER.
C                     THIS CAUSES INITIALIZATION OF THE PROGRAM
C                     AND THE FIRST RANDOM NUMBER TO BE RETURNED AS Z.
C       SUBSEQUENT TIMES...
C                   Z = UNI(0)
C                     CAUSES THE NEXT RANDOM NUMBER TO BE RETURNED AS Z.
C
C
C..................................................................
C   NOTE: USERS WHO WISH TO TRANSPORT THIS PROGRAM FROM ONE COMPUTER
C         TO ANOTHER SHOULD READ THE FOLLOWING INFORMATION.....
C
C   MACHINE DEPENDENCIES...
C      MDIG = A LOWER BOUND ON THE NUMBER OF BINARY DIGITS AVAILABLE
C              FOR REPRESENTING INTEGERS, INCLUDING THE SIGN BIT.
C              THIS VALUE MUST BE AT LEAST 16, BUT MAY BE INCREASED
C              IN LINE WITH REMARK A BELOW.
C
C   REMARKS...
C     A. THIS PROGRAM CAN BE USED IN TWO WAYS:
C        (1) TO OBTAIN REPEATABLE RESULTS ON DIFFERENT COMPUTERS,
C            SET 'MDIG' TO THE SMALLEST OF ITS VALUES ON EACH, OR,
C        (2) TO ALLOW THE LONGEST SEQUENCE OF RANDOM NUMBERS TO BE
C            GENERATED WITHOUT CYCLING (REPEATING) SET 'MDIG' TO THE
C            LARGEST POSSIBLE VALUE.
C     B. THE SEQUENCE OF NUMBERS GENERATED DEPENDS ON THE INITIAL
C          INPUT 'JD' AS WELL AS THE VALUE OF 'MDIG'.
C          IF MDIG=16 ONE SHOULD FIND THAT
   Editors Note: set the seed using 152 in order to get uni(305)
   -jt
C            THE FIRST EVALUATION
C              Z=UNI(305) GIVES Z=.027832881...
C            THE SECOND EVALUATION
C              Z=UNI(0) GIVES   Z=.56102176...
C            THE THIRD EVALUATION
C              Z=UNI(0) GIVES   Z=.41456343...
C            THE THOUSANDTH EVALUATION
C              Z=UNI(0) GIVES   Z=.19797357...
C
C***REFERENCES  MARSAGLIA G., "COMMENTS ON THE PERFECT UNIFORM RANDOM
C                 NUMBER GENERATOR", UNPUBLISHED NOTES, WASH S. U.
C***ROUTINES CALLED  I1MACH,XERROR
C***END PROLOGUE  UNI

  **/

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_rng.h>

static inline unsigned long int uni_get (void *vstate);
static double uni_get_double (void *vstate);
static void uni_set (void *state, unsigned long int s);

static const unsigned int MDIG = 16;    /* Machine digits in int */
static const unsigned int m1 = 32767;   /* 2^(MDIG-1) - 1 */
static const unsigned int m2 = 256;     /* 2^(MDIG/2) */

typedef struct
  {
    int i, j;
    unsigned long m[17];
  }
uni_state_t;

static inline unsigned long
uni_get (void *vstate)
{
  uni_state_t *state = (uni_state_t *) vstate;
  const int i = state->i;
  const int j = state->j;

  /* important k not be unsigned */
  long k = state->m[i] - state->m[j];

  if (k < 0)
    k += m1;
  state->m[j] = k;

  if (i == 0)
    {
      state->i = 16;
    }
  else
    {
      (state->i)--;
    }

  if (j == 0)
    {
      state->j = 16;
    }
  else
    {
      (state->j)--;
    }

  return k;
}

static double
uni_get_double (void *vstate)
{
  return uni_get (vstate) / 32767.0 ;
}

static void
uni_set (void *vstate, unsigned long int s)
{
  unsigned int i, seed, k0, k1, j0, j1;

  uni_state_t *state = (uni_state_t *) vstate;

  /* For this routine, the seeding is very elaborate! */
  /* A flaw in this approach is that seeds 1,2 give exactly the
     same random number sequence!  */

  s = 2 * s + 1;        /* enforce seed be odd */
  seed = (s < m1 ? s : m1);     /* seed should be less than m1 */

  k0 = 9069 % m2;
  k1 = 9069 / m2;
  j0 = seed % m2;
  j1 = seed / m2;

  for (i = 0; i < 17; ++i)
    {
      seed = j0 * k0;
      j1 = (seed / m2 + j0 * k1 + j1 * k0) % (m2 / 2);
      j0 = seed % m2;
      state->m[i] = j0 + m2 * j1;
    }
  state->i = 4;
  state->j = 16;

  return;
}

static const gsl_rng_type uni_type =
{"uni",                         /* name */
 32766,                         /* RAND_MAX */
 0,                             /* RAND_MIN */
 sizeof (uni_state_t),
 &uni_set,
 &uni_get,
 &uni_get_double};

const gsl_rng_type *gsl_rng_uni = &uni_type;


/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Set of C routines to use FFTW1.3 library
* 2D- FFT's for any size
*
*
* JLP
* Version 12/12/2006 (from my BORLAND version)
*--------------------------------------------------------*/
#include <stdio.h>
#include <math.h>
#include "jlp_fftw.h"
#include "jlp_num_rename.h"

/* JLP2001: To be compatible with "matlab"
* I should not normalize by sqrt(nx*ny): */
/*
#define NORMALIZE_SQRT
*/

/* Content of jlp_fftw.h:
#include <fftw.h>

int FFTW_1D_Y_FLT(float *re, float *im, INT4 *nx, INT4 *ny, INT4 *idim,
                   INT4 *direct);
int fftw_1D_Y_float(float *re, float *im, int nx, int ny, int direct);
int FFTW_1D_Y_DBLE(double *re, double *im, INT4 *nx, INT4 *ny, INT4 *idim,
             INT4 *direct);
int fftw_1D_Y_double(double *re, double *im, int nx, int ny, int direct);
int FFTW_2D_DBLE(double *re, double *im, int *nx, int *ny, int *direct);
int fftw_2D_double(double *re, double *im, int nx, int ny, int direct);
int FFTW_2D_FLT(float *re, float *im, INT4 *nx, INT4 *ny, INT4 *idim,
                  INT4 *kod);
int fftw_2D_float(float *re, float *im, int nx, int ny, int direct);
int fftw_setup(int nx, int ny);
int FFTW_SETUP(int *nx, int *ny);
int fftw_fast(FFTW_COMPLEX *image, int nx, int ny, int direct);
int fftw_shutdown();
*/

/* Static variables: */
static fftwnd_plan plan_fwd, plan_bkwd;

/*
#define MAIN_TEST
*/

#ifdef MAIN_TEST
main()
{
register int i;
int nx = 9, ny = 8;
double re[128], im[128];
FFTW_COMPLEX* image;
char s[80];

for(i = 0; i < nx * ny; i++)
  {
  re[i] = i;
  im[i] = -i;
  }

fftw_2D_double(re, im, nx, ny, 1);
fftw_2D_double(re, im, nx, ny, -1);

for(i = 0; i < nx * ny; i++)
  {
  printf(" i=%d re=%f im=%f \n", i, re[i], im[i]);
  }

printf(" Now going to fast fft...");
gets(s);

nx = 388; ny = 128;
image = (FFTW_COMPLEX *) malloc(nx * ny * sizeof(FFTW_COMPLEX));
for(i = 0; i < nx * ny; i++)
  {
  c_re(image[i]) = i;
  c_im(image[i]) = -i;
  }
  printf("OK, I go on, with nx=%d ny=%d",nx,ny);

fftw_setup(nx, ny);
fftw_fast(image, nx, ny, 1);
fftw_fast(image, nx, ny, -1);
fftw_shutdown();
for(i = 0; i < 200; i++)
  {
  printf(" i=%d re=%f im=%f \n", i, c_re(image[i]), c_im(image[i]));
  }

gets(s);
free(image);
}
#endif
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Interface with Fortran routines (with arrays of first dimension = idim)
* kod=1, or 2 direct
* kod=-1, or -2 inverse
* kod=-2, or 2: power spectrum, and phase
*--------------------------------------------------------------*/
int FFTW_2D_FLT(float *re, float *im, INT4 *nx, INT4 *ny, INT4 *idim,
                  INT4 *kod)
{
int nx1, ny1, direct, status;
if(*idim != *nx)
 {printf("FFTW_2D/Fatal error idim=%d while nx=%d \n", *idim, *nx);
  exit(-1);
 }
if(*kod != 1 && *kod != -1)
 {printf("FFTW_2D/Fatal error kod=%d (option not yet implemented)\n", *kod);
  exit(-1);
 }
nx1 = *nx; ny1 = *ny; direct = *kod;
status = fftw_2D_float(re, im, nx1, ny1, direct);
return(status);
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Interface with Fortran routines
* kod=1, or 2 direct
* kod=-1, or -2 inverse
* kod=-2, or 2: power spectrum, and phase
* --------------------------------------------------------------*/
int FFTW_1D_Y_DBLE(double *re, double *im, INT4 *nx, INT4 *ny,
                   INT4 *idim, INT4 *kod)
{
int nx1, ny1, direct, status;
if(*idim != *nx)
 {printf("FFTW_1D_Y/Fatal error idim=%d while nx=%d \n", *idim, *nx);
  exit(-1);
 }
if(*kod != 1 && *kod != -1)
 {printf("FFTW_1D_Y/Fatal error kod=%d (option not yet implemented)\n", *kod);
  exit(-1);
 }
nx1 = *nx; ny1 = *ny; direct = *kod;
printf("FFTW_1D_Y/ nx1=%d ny1=%d direct=%d\n",nx1,ny1,direct);
status = fftw_1D_Y_double(re, im, nx1, ny1, direct);
return(status);
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Interface with Fortran routines
* kod=1, or 2 direct
* kod=-1, or -2 inverse
* kod=-2, or 2: power spectrum, and phase
* --------------------------------------------------------------*/
int FFTW_1D_Y_FLT(float *re, float *im, INT4 *nx, INT4 *ny,
                   INT4 *idim, INT4 *kod)
{
int nx1, ny1, direct, status;

#ifndef TOTO_
double sum;
register int i;
#endif

if(*idim != *nx)
 {printf("FFTW_1D_Y_FLT/Fatal error idim=%d while nx=%d \n", *idim, *nx);
  exit(-1);
 }
if(*kod != 1 && *kod != -1)
 {printf("FFTW_1D_Y_FLT/Fatal error kod=%d (option not yet implemented)\n", *kod);
  exit(-1);
 }
nx1 = *nx; ny1 = *ny; direct = *kod;
#ifdef DEBUG
printf("FFTW_1D_Y_FLT/ nx1=%d ny1=%d direct=%d (fftw)\n",nx1,ny1,direct);
#endif

#ifndef TOTO_
sum =0;
for(i=0; i < ny1; i++) sum += re[nx1/2 + i * nx1];
printf(" Sum of central line = %e and sum/sqrt(ny)=%e \n",
        sum,sum/sqrt((double)ny1));
#endif

status = fftw_1D_Y_float(re, im, nx1, ny1, direct);

/* JLP99: to check that FFT is correct: */
/* Not recentred yet !! */
#ifndef TOTO_
for(i=0; i <= 2; i++)
printf("re,im [ixc,%d]: %e %e \n",i,
        re[nx1/2 + i*nx1],im[nx1/2 + i*nx1]);
#endif

return(status);
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* 2D FFT routine for double precision arrays
* direct = 1  : direct or forward
* direct = -1 : reverse or backward
* (FORTRAN version)
* --------------------------------------------------------------*/
int FFTW_2D_DBLE(double *re, double *im, int *nx, int *ny, int *direct)
{
int status;
status = fftw_2D_double(re, im, *nx, *ny, *direct);
return(status);
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* 2D FFT routine for double precision arrays
* direct = 1  : direct or forward
* direct = -1 : reverse or backward
*
* --------------------------------------------------------------*/
int fftw_2D_double(double *re, double *im, int nx, int ny, int direct)
{
register int i;
double norm;
int isize;
fftwnd_plan plan;
fftw_direction dir;
FFTW_COMPLEX* in_out;

isize = nx * ny;
in_out = (FFTW_COMPLEX *) malloc(isize * sizeof(FFTW_COMPLEX));

/* Transfer to complex array structure: */
for(i = 0; i < nx * ny; i++)
  {
   c_re(in_out[i]) = re[i];
   c_im(in_out[i]) = im[i];
  }

/* direct = 1  : forward
   direct = -1 : backward
*/
dir = (direct == 1) ? FFTW_FORWARD : FFTW_BACKWARD;
/* Warning: inversion nx-ny here: */
plan = fftw2d_create_plan(ny, nx, dir,
                              FFTW_ESTIMATE | FFTW_IN_PLACE);
if(plan == NULL)
{printf("FFTW_2D_DBLE: fatal error creating plan\n"); exit(-1);}

/* Compute the FFT: */
   fftwnd(plan, 1, in_out, 1, 0, 0, 0, 0);

/* Transfer back to original arrays (and normalize by sqrt(nx * ny): */
#ifdef NORMALIZE_SQRT
norm = nx * ny; norm = sqrt(norm);
for(i = 0; i < nx * ny; i++)
  {
  re[i] = c_re(in_out[i]) /norm;
  im[i] = c_im(in_out[i]) /norm;
  }
#else
if(direct == 1)
  {
  for(i = 0; i < nx * ny; i++)
    {
    re[i] = c_re(in_out[i]);
    im[i] = c_im(in_out[i]);
    }
  }
else
  {
  norm = nx * ny;
  for(i = 0; i < nx * ny; i++)
    {
    re[i] = c_re(in_out[i]) /norm;
    im[i] = c_im(in_out[i]) /norm;
    }
  }
#endif

free(in_out);

/* Delete plan: */
  fftwnd_destroy_plan(plan);

return(0);
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* 2D FFT routine for single precision arrays (for which idim=nx)
* (well suited to C arrays since nx=idim...)
* direct = 1  : direct or forward
* direct = -1 : reverse or backward
* --------------------------------------------------------------*/
int fftw_2D_float(float *re, float *im, int nx, int ny, int direct)
{
register int i;
double norm;
int isize;
fftwnd_plan plan;
fftw_direction dir;
FFTW_COMPLEX* in_out;

isize = nx * ny;
in_out = (FFTW_COMPLEX *) malloc(isize * sizeof(FFTW_COMPLEX));

/* Transfer to complex array structure: */
for(i = 0; i < nx * ny; i++)
  {
   c_re(in_out[i]) = re[i];
   c_im(in_out[i]) = im[i];
  }

/* direct = 1  : forward
   direct = -1 : backward
*/
dir = (direct == 1) ? FFTW_FORWARD : FFTW_BACKWARD;
/* Warning: inversion nx-ny here: */
plan = fftw2d_create_plan(ny, nx, dir,
                              FFTW_ESTIMATE | FFTW_IN_PLACE);
if(plan == NULL)
{printf("fftw_2D_float: fatal error creating plan\n"); exit(-1);}

/* Compute the FFT: */
   fftwnd(plan, 1, in_out, 1, 0, 0, 0, 0);

/* Transfer back to original arrays (and normalize by sqrt(nx * ny): */
#ifdef NORMALIZE_SQRT
norm = nx * ny; norm = sqrt(norm);
for(i = 0; i < nx * ny; i++)
  {
  re[i] = c_re(in_out[i]) /norm;
  im[i] = c_im(in_out[i]) /norm;
  }
#else
if(direct == 1)
  {
  for(i = 0; i < nx * ny; i++)
    {
    re[i] = c_re(in_out[i]);
    im[i] = c_im(in_out[i]);
    }
  }
else
  {
  norm = nx * ny;
  for(i = 0; i < nx * ny; i++)
    {
    re[i] = c_re(in_out[i]) /norm;
    im[i] = c_im(in_out[i]) /norm;
    }
  }
#endif

free(in_out);

/* Delete plan: */
  fftwnd_destroy_plan(plan);

return(0);
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* 1D FFT routine for single precision arrays
* along the columns (for spectroscopic mode)
* direct = 1  : direct or forward
* direct = -1 : reverse or backward
*
* --------------------------------------------------------------*/
int fftw_1D_Y_float(float *re, float *im, int nx, int ny, int direct)
{
register int i, j;
double norm;
fftw_plan plan;
fftw_direction dir;
FFTW_COMPLEX* in_out;

in_out = (FFTW_COMPLEX *) malloc(ny * sizeof(FFTW_COMPLEX));
if(in_out == NULL)
{printf("fftw_1D_Y_float: fatal error allocating memory\n"); exit(-1);}

/* direct = 1  : forward
   direct = -1 : backward
*/
dir = (direct == 1) ? FFTW_FORWARD : FFTW_BACKWARD;
plan = fftw_create_plan(ny, dir, FFTW_ESTIMATE | FFTW_IN_PLACE);
if(plan == NULL)
{printf("fftw_1D_Y_float: fatal error creating plan\n"); exit(-1);}

for(i = 0; i < nx; i++)
{
/* Transfer to complex array structure: */
  for(j = 0; j < ny; j++)
  {
   c_re(in_out[j]) = re[i + j * nx];
   c_im(in_out[j]) = im[i + j * nx];
  }

/* Compute the FFT: */
   fftw(plan, 1, in_out, 1, 0, 0, 0, 0);

/* Transfer back to original arrays (and normalize by sqrt(ny): */
#ifdef NORMALIZE_SQRT
norm = sqrt((double)ny);
for(j = 0; j < ny; j++)
  {
  re[i + j * nx] = c_re(in_out[j]) /norm;
  im[i + j * nx] = c_im(in_out[j]) /norm;
  }
#else
if(direct == 1)
  {
  for(j = 0; j < ny; j++)
    {
    re[i + j * nx] = c_re(in_out[j]);
    im[i + j * nx] = c_im(in_out[j]);
    }
  }
else
  {
  norm = (double)ny;
  for(j = 0; j < ny; j++)
    {
    re[i + j * nx] = c_re(in_out[j]) /norm;
    im[i + j * nx] = c_im(in_out[j]) /norm;
    }
  }
#endif
/* End of loop on i (from 0 to nx-1) */
}

free(in_out);

/* Delete plan: */
  fftw_destroy_plan(plan);

return(0);
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* 1D FFT routine for double precision arrays
* along the columns (for spectroscopic mode)
* direct = 1  : direct or forward
* direct = -1 : reverse or backward
*
* --------------------------------------------------------------*/
int fftw_1D_Y_double(double *re, double *im, int nx, int ny, int direct)
{
register int i, j;
double norm;
fftw_plan plan;
fftw_direction dir;
FFTW_COMPLEX* in_out;

in_out = (FFTW_COMPLEX *) malloc(ny * sizeof(FFTW_COMPLEX));

/* direct = 1  : forward
   direct = -1 : backward
*/
dir = (direct == 1) ? FFTW_FORWARD : FFTW_BACKWARD;
plan = fftw_create_plan(ny, dir, FFTW_ESTIMATE | FFTW_IN_PLACE);
if(plan == NULL)
{printf("fftw_1D_Y: fatal error creating plan\n"); exit(-1);}

for(i = 0; i < nx; i++)
{
/* Transfer to complex array structure: */
  for(j = 0; j < ny; j++)
  {
   c_re(in_out[j]) = re[i + j * nx];
   c_im(in_out[j]) = im[i + j * nx];
  }

/* Compute the FFT: */
   fftw(plan, 1, in_out, 1, 0, 0, 0, 0);

/* Transfer back to original arrays (and normalize by sqrt(ny): */
#ifdef NORMALIZE_SQRT
norm = sqrt((double)ny);
for(j = 0; j < ny; j++)
  {
  re[i + j * nx] = c_re(in_out[j]) /norm;
  im[i + j * nx] = c_im(in_out[j]) /norm;
  }
#else
if(direct == 1)
  {
  for(j = 0; j < ny; j++)
    {
    re[i + j * nx] = c_re(in_out[j]);
    im[i + j * nx] = c_im(in_out[j]);
    }
  }
else
  {
  norm = (double)ny;
  for(j = 0; j < ny; j++)
    {
    re[i + j * nx] = c_re(in_out[j]) /norm;
    im[i + j * nx] = c_im(in_out[j]) /norm;
    }
  }
#endif
/* End of loop on i (from 0 to nx-1) */
}

free(in_out);

/* Delete plan: */
  fftw_destroy_plan(plan);

return(0);
}
/****************************************************************
* Fortran interface to fftw_setup
****************************************************************/
int FFTW_FSETUP(int *nx, int *ny)
{
fftw_setup(*nx, *ny);
return(0);
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// fftw_setup, to initialize FFTW routines
// Create forward and backward plans
//
// --------------------------------------------------------------*/
int fftw_setup(int nx, int ny)
{
char fwd_name[60], bkwd_name[60];
FILE *fp_wisd;
int ToOutput;

/********************** With "forward"  ***************************/
#ifdef BORLAND
sprintf(fwd_name,"c:\\tc\\fftw13\\jlp\\fwd_%d.wis",nx);
#else
sprintf(fwd_name,"/d/fftw/fwd_%d%d.wis",nx,ny);
#endif

/* Check if wisdom file already exits: */
printf("fftw_setup/Read wisdom file: %s\n",fwd_name);
if((fp_wisd = fopen(fwd_name,"r")) == NULL)
  {
  printf("fftw_setup/Failure to open wisdom file: %s\n",fwd_name);
  ToOutput = 1;
  }
else
  {
  ToOutput = 0;
  if(fftw_import_wisdom_from_file(fp_wisd) == FFTW_FAILURE) ToOutput = 1;
  fclose(fp_wisd);
  }

/* Create  "plan" for fftw (speed is wanted for subsequent FFT's) */
/* Warning: inversion nx-ny here: */
plan_fwd = fftw2d_create_plan(ny, nx, FFTW_FORWARD,
                              FFTW_MEASURE | FFTW_IN_PLACE | FFTW_USE_WISDOM);

/* Output wisdom file if needed: */
if(ToOutput)
  {
  printf("fftw_setup/Write wisdom file: %s\n",fwd_name);
  if((fp_wisd = fopen(fwd_name,"w")) != NULL)
                                       fftw_export_wisdom_to_file(fp_wisd);
  fclose(fp_wisd);
  }

/********************** With "backward"  ***************************/
#ifdef BORLAND
sprintf(bkwd_name,"c:\\tc\\fftw13\\jlp\\bk_%d%d.wis",nx,ny);
#else
sprintf(bkwd_name,"/d/fftw/bk_%d%d.wis",nx,ny);
#endif

/* Check if wisdom file already exits: */
printf("fftw_setup/Read wisdom file: %s\n",bkwd_name);
if((fp_wisd = fopen(bkwd_name,"r")) == NULL)
  {
  printf("fftw_setup/Failure to open wisdom file: %s\n",bkwd_name);
  ToOutput = 1;
  }
else
  {
  ToOutput = 0;
  if(fftw_import_wisdom_from_file(fp_wisd) == FFTW_FAILURE) ToOutput = 1;
  fclose(fp_wisd);
  }

/* Create  "plan" for fftw (speed is wanted for subsequent FFT's) */
/* Warning: inversion nx-ny here: */
plan_bkwd = fftw2d_create_plan(ny, nx, FFTW_BACKWARD,
                              FFTW_MEASURE | FFTW_IN_PLACE | FFTW_USE_WISDOM);

/* Output wisdom file if needed: */
if(ToOutput)
  {
  printf("fftw_setup/Write wisdom file: %s\n",bkwd_name);
  if((fp_wisd = fopen(bkwd_name,"w")) != NULL)
                                        fftw_export_wisdom_to_file(fp_wisd);
  fclose(fp_wisd);
  }

return(0);
}
/*************************************************************
*
**************************************************************/
int FFTW_SHUTDOWN()
{
/* Delete plans: */
  fftwnd_destroy_plan(plan_fwd);
  fftwnd_destroy_plan(plan_bkwd);
return(0);
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* 2D FFT routine for double precision fftw_complex arrays
* direction = 1  : forward
* direction = -1 : backward
*  Should be called after fftw_setup and before fftw_shutdown
* --------------------------------------------------------------*/
int fftw_fast(FFTW_COMPLEX *image, int nx, int ny, int dir)
{
register int i;
double norm;

/* Compute the FFT: */
if(dir == 1)
   fftwnd(plan_fwd, 1, image, 1, 0, 0, 0, 0);
else
   fftwnd(plan_bkwd, 1, image, 1, 0, 0, 0, 0);

/* Normalize by sqrt(nx * ny): */
#ifdef NORMALIZE_SQRT
norm = nx * ny; norm = sqrt(norm);
for(i = 0; i < nx * ny; i++)
  {
  c_re(image[i]) /= norm;
  c_im(image[i]) /= norm;
  }
#else
if(dir == -1)
  {
  norm = (double)(nx * ny);
  for(i = 0; i < nx * ny; i++)
    {
    c_re(image[i]) /= norm;
    c_im(image[i]) /= norm;
    }
  }
#endif

return(0);
}


/* min/test.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 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 <stdlib.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_min.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_ieee_utils.h>

#include "test.h"
#include "min.h"

/* stopping parameters */

const double EPSABS = 0.001 ;
const double EPSREL = 0.001 ;

const unsigned int MAX_ITERATIONS = 100;

void my_error_handler (const char *reason, const char *file,
                       int line, int err);

#define WITHIN_TOL(a, b, epsrel, epsabs) \
 (fabs((a) - (b)) < (epsrel) * GSL_MIN(fabs(a), fabs(b)) + (epsabs))

int
main (void)
{
  gsl_function F_cos, F_func1, F_func2, F_func3, F_func4;

  const gsl_min_fminimizer_type * fminimizer[4] ;
  const gsl_min_fminimizer_type ** T;

  gsl_ieee_env_setup ();

  fminimizer[0] = gsl_min_fminimizer_goldensection;
  fminimizer[1] = gsl_min_fminimizer_brent;
  fminimizer[2] = gsl_min_fminimizer_quad_golden;
  fminimizer[3] = 0;

  F_cos = create_function (f_cos) ;
  F_func1 = create_function (func1) ;
  F_func2 = create_function (func2) ;
  F_func3 = create_function (func3) ;
  F_func4 = create_function (func4) ;

  gsl_set_error_handler (&my_error_handler);

  for (T = fminimizer ; *T != 0 ; T++)
    {
      test_f (*T, "cos(x) [0 (3) 6]", &F_cos, 0.0, 3.0, 6.0, M_PI);
      test_f (*T, "x^4 - 1 [-3 (-1) 17]", &F_func1, -3.0, -1.0, 17.0, 0.0);
      test_f (*T, "sqrt(|x|) [-2 (-1) 1.5]", &F_func2, -2.0, -1.0, 1.5, 0.0);
      test_f (*T, "func3(x) [-2 (3) 4]", &F_func3, -2.0, 3.0, 4.0, 1.0);
      test_f (*T, "func4(x) [0 (0.782) 1]", &F_func4, 0, 0.782, 1.0, 0.8);

      test_f_e (*T, "invalid range check [4, 0]", &F_cos, 4.0, 3.0, 0.0, M_PI);
      test_f_e (*T, "invalid range check [1, 1]", &F_cos, 1.0, 1.0, 1.0, M_PI);
      test_f_e (*T, "invalid range check [-1, 1]", &F_cos, -1.0, 0.0, 1.0, M_PI);
    }
  test_bracket("cos(x) [1,2]",&F_cos,1.0,2.0,15);
  test_bracket("sqrt(|x|) [-1,0]",&F_func2,-1.0,0.0,15);
  test_bracket("sqrt(|x|) [-1,-0.6]",&F_func2,-1.0,-0.6,15);
  test_bracket("sqrt(|x|) [-1,1]",&F_func2,-1.0,1.0,15);

  exit (gsl_test_summary ());
}

void
test_f (const gsl_min_fminimizer_type * T,
        const char * description, gsl_function *f,
        double lower_bound, double middle, double upper_bound,
        double correct_minimum)
{
  int status;
  size_t iterations = 0;
  double m, a, b;
  double x_lower, x_upper;
  gsl_min_fminimizer * s;

  x_lower = lower_bound;
  x_upper = upper_bound;

  s = gsl_min_fminimizer_alloc (T) ;
  gsl_min_fminimizer_set (s, f, middle, x_lower, x_upper) ;

  do
    {
      iterations++ ;

      status = gsl_min_fminimizer_iterate (s);

      m = gsl_min_fminimizer_x_minimum(s);
      a = gsl_min_fminimizer_x_lower(s);
      b = gsl_min_fminimizer_x_upper(s);

#ifdef DEBUG
      printf("%.12f %.18f %.12f %.18f %.12f %.18f status=%d\n",
             a, GSL_FN_EVAL(f, a), m, GSL_FN_EVAL(f, m), b, GSL_FN_EVAL(f, b), status);
#endif

      if (a > b)
        gsl_test (GSL_FAILURE, "interval is invalid (%g,%g)", a, b);

      if (m < a || m > b)
        gsl_test (GSL_FAILURE, "m lies outside interval %g (%g,%g)", m, a, b);

      if (status) break ;

      status = gsl_min_test_interval (a, b, EPSABS, EPSREL);
    }
  while (status == GSL_CONTINUE && iterations < MAX_ITERATIONS);

  gsl_test (status, "%s, %s (%g obs vs %g expected) ",
            gsl_min_fminimizer_name(s), description,
            gsl_min_fminimizer_x_minimum(s), correct_minimum);

  /* check the validity of the returned result */

  if (!WITHIN_TOL (m, correct_minimum, EPSREL, EPSABS))
    {
      gsl_test (GSL_FAILURE, "incorrect precision (%g obs vs %g expected)",
                m, correct_minimum);
    }

  gsl_min_fminimizer_free (s);

}

void
test_f_e (const gsl_min_fminimizer_type * T,
          const char * description, gsl_function *f,
          double lower_bound, double middle, double upper_bound,
          double correct_minimum)
{
  int status;
  size_t iterations = 0;
  double x_lower, x_upper;
  double a, b;
  gsl_min_fminimizer * s;

  x_lower = lower_bound;
  x_upper = upper_bound;

  s = gsl_min_fminimizer_alloc (T) ;
  status = gsl_min_fminimizer_set (s, f, middle, x_lower, x_upper) ;

  if (status != GSL_SUCCESS)
    {
      gsl_min_fminimizer_free (s) ;
      gsl_test (status == GSL_SUCCESS, "%s, %s", T->name, description);
      return ;
    }

  do
    {
      iterations++ ;
      gsl_min_fminimizer_iterate (s);
      a = gsl_min_fminimizer_x_lower(s);
      b = gsl_min_fminimizer_x_upper(s);

      status = gsl_min_test_interval (a, b, EPSABS, EPSREL);
    }
  while (status == GSL_CONTINUE && iterations < MAX_ITERATIONS);

  gsl_test (!status, "%s, %s", gsl_min_fminimizer_name(s), description,
            gsl_min_fminimizer_x_minimum(s) - correct_minimum);

  gsl_min_fminimizer_free (s);
}

void
my_error_handler (const char *reason, const char *file, int line, int err)
{
  if (0)
    printf ("(caught [%s:%d: %s (%d)])\n", file, line, reason, err);
}

int
test_bracket (const char * description,gsl_function *f,double lower_bound,
              double upper_bound, unsigned int max)
{
  int status;
  double x_lower, x_upper;
  double f_upper,f_lower,f_minimum;
  double x_minimum;

  x_lower=lower_bound;
  x_upper=upper_bound;
  SAFE_FUNC_CALL (f,x_lower,&f_lower);
  SAFE_FUNC_CALL (f,x_upper,&f_upper);
  status=gsl_min_find_bracket(f,&x_minimum,&f_minimum,&x_lower,&f_lower,&x_upper,&f_upper,max);
  gsl_test (status,"%s, interval: [%g,%g], values: (%g,%g), minimum at: %g, value: %g",
            description,x_lower,x_upper,f_lower,f_upper,x_minimum,f_minimum);
  return status;
}


#include <gsl/gsl_rng.h>

#include "allvars.h"

// file names
char fname_param[100];
char fname_con[100], fname_line[100], fname_results[100];

FILE *fp_results;

// transfer function parameters
double tau_lim_low, tau_lim_up;
int nc, nc_lim_low, nc_lim_up, nc_best;
double *grid_tau;
const int ntau=200;
double *TF_tau, *TF;

// mcmc
const int ntheta_max = 200;
int ntheta;
int nmcmc, nbuilt;
double *workspace;
int flag_detrend, flag_sim, flag_mcmc;
double *Cmat, *ICmat, *Smat, *Nmat, *USmat, *ASmat;
double *ICvmat, *Tmat1, *Tmat2, *Tmat3, *Tmat4;

double *cov_matrix;
double **theta_range, *theta_best, *theta_best_var;
double *theta_input, *sigma_input;
int *theta_fixed;

double *theta_best_con, *theta_best_var_con;

const gsl_rng_type * gsl_T;
const gsl_rng * gsl_r;

// light curve data
const int ndata_max=500;
int ncon_data, nline_data, nall_data;
double *Tcon_data, *Tline_data, *Fcon_data, *Fcerrs_data, *Fline_data, *Flerrs_data;
double *Fall_data;
double scale_con, scale_line;
double len_con, len_line, cad_con, cad_line;

int ncon=500, nline=500, nall;
double *Tcon, *Fcon, *Fcerrs, *Tline, *Fline, *Flerrs;

// error exit
char str_error_exit[200];

// mathematic functions
int *workspace_ipiv;


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

#ifndef __GSL_STATISTICS_LONG_H__
#define __GSL_STATISTICS_LONG_H__

#include <stddef.h>
#include <gsl/gsl_types.h>

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

__BEGIN_DECLS

GSL_EXPORT double gsl_stats_long_mean (const long data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_long_variance (const long data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_long_sd (const long data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_long_variance_with_fixed_mean (const long data[], const size_t stride, const size_t n, const double mean);
GSL_EXPORT double gsl_stats_long_sd_with_fixed_mean (const long data[], const size_t stride, const size_t n, const double mean);
GSL_EXPORT double gsl_stats_long_absdev (const long data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_long_skew (const long data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_long_kurtosis (const long data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_long_lag1_autocorrelation (const long data[], const size_t stride, const size_t n);

GSL_EXPORT double gsl_stats_long_covariance (const long data1[], const size_t stride1,const long data2[], const size_t stride2, const size_t n);

GSL_EXPORT double gsl_stats_long_variance_m (const long data[], const size_t stride, const size_t n, const double mean);
GSL_EXPORT double gsl_stats_long_sd_m (const long data[], const size_t stride, const size_t n, const double mean);
GSL_EXPORT double gsl_stats_long_absdev_m (const long data[], const size_t stride, const size_t n, const double mean);
GSL_EXPORT double gsl_stats_long_skew_m_sd (const long data[], const size_t stride, const size_t n, const double mean, const double sd);
GSL_EXPORT double gsl_stats_long_kurtosis_m_sd (const long data[], const size_t stride, const size_t n, const double mean, const double sd);
GSL_EXPORT double gsl_stats_long_lag1_autocorrelation_m (const long data[], const size_t stride, const size_t n, const double mean);

GSL_EXPORT double gsl_stats_long_covariance_m (const long data1[], const size_t stride1,const long data2[], const size_t stride2, const size_t n, const double mean1, const double mean2);

GSL_EXPORT double gsl_stats_long_pvariance (const long data1[], const size_t stride1, const size_t n1, const long data2[], const size_t stride2, const size_t n2);
GSL_EXPORT double gsl_stats_long_ttest (const long data1[], const size_t stride1, const size_t n1, const long data2[], const size_t stride2, const size_t n2);

GSL_EXPORT long gsl_stats_long_max (const long data[], const size_t stride, const size_t n);
GSL_EXPORT long gsl_stats_long_min (const long data[], const size_t stride, const size_t n);
GSL_EXPORT void gsl_stats_long_minmax (long * min, long * max, const long data[], const size_t stride, const size_t n);

GSL_EXPORT size_t gsl_stats_long_max_index (const long data[], const size_t stride, const size_t n);
GSL_EXPORT size_t gsl_stats_long_min_index (const long data[], const size_t stride, const size_t n);
GSL_EXPORT void gsl_stats_long_minmax_index (size_t * min_index, size_t * max_index, const long data[], const size_t stride, const size_t n);

GSL_EXPORT double gsl_stats_long_median_from_sorted_data (const long sorted_data[], const size_t stride, const size_t n) ;
GSL_EXPORT double gsl_stats_long_quantile_from_sorted_data (const long sorted_data[], const size_t stride, const size_t n, const double f) ;

__END_DECLS

#endif /* __GSL_STATISTICS_LONG_H__ */


/**
 * @file testmatmultc.c
 * @author Jeff Daily, PNNL jeff.daily@pnl.gov
 *
 * This is the C version of testmatmul.F.  The port was as direct as possible,
 * hopefully.
 */

#if HAVE_CONFIG_H
#   include "config.h"
#endif

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

#if defined(USE_ELEMENTAL)
#include <cblas.h>
#include "El.h"
int me, nproc;
ElGlobalArrays_d eldga;
#else
#include "mp3.h"
#include "ga.h"
#include "macdecls.h"
#include "xgemm.h"
#endif

void load_ga(int handle, double *f, int dim1, int dim2);
void verify_ga_dgemm(char xt1, char xt2, int num_m, int num_n, int num_k,
        double alpha, int g_a, int g_b, double beta, int g_c,
        double *tmpa, double *tmpb, double *tmpc);

#define dgemm_verify 1
#define nummax 1024
#define howmany 2
#define ntrans 4

#if defined(USE_ELEMENTAL)
#define MP_TIMER MPI_Wtime
void GA_Error (const char *str, int item)
{
    printf ("%s: %d\n", str, item);
    MPI_Abort(MPI_COMM_WORLD, -101);
}
#endif

/*
 * test ga_dgemm
 * Note: - change nummax for large arrays
 *       - turn off "dgemm_verify" for large arrays due to memory
 *         limitations, as dgemm_verify=1 for large arrays produces
 *         segfault, dumps core,or any crap.
 */
int main(int argc, char **argv)
{
    int num_m;
    int num_n;
    int num_k;
    int i;
    int ii;
    double *h0;
    int g_c;
    int g_b;
    int g_a;
    double a;
    double t1;
    double mf;
    double avg_t[ntrans];
    double avg_mf[ntrans];
    int itime;
    int ntimes;
    int nums_m[/*howmany*/] = {512,1024};
    int nums_n[/*howmany*/] = {512,1024};
    int nums_k[/*howmany*/] = {512,1024};
    char transa[/*ntrans*/] = "ntnt";
    char transb[/*ntrans*/] = "nntt";
    char ta;
    char tb;
    double *tmpa;
    double *tmpb;
    double *tmpc;
    int ndim;
    int dims[2];
#ifdef BLOCK_CYCLIC
    int block_size[2];
#endif

#if defined(USE_ELEMENTAL)
    // initialize Elemental (which will initialize MPI)
    ElInitialize( &argc, &argv );
    ElMPICommRank( MPI_COMM_WORLD, &me );
    ElMPICommSize( MPI_COMM_WORLD, &nproc );
    // instantiate el::global array
    ElGlobalArraysConstruct_d( &eldga );
    // initialize global arrays
    ElGlobalArraysInitialize_d( eldga );
#else
    MP_INIT(argc,argv);
    if (!MA_init(MT_DBL,1,20000000)) {
        GA_Error("failed: ma_init(MT_DBL,1,20000000)",10);
    }
    GA_INIT(argc,argv);
    me = GA_Nodeid();
#endif

    h0 = (double*)malloc(sizeof(double) * nummax*nummax);
    tmpa = (double*)malloc(sizeof(double) * nummax*nummax);
    tmpb = (double*)malloc(sizeof(double) * nummax*nummax);
    tmpc = (double*)malloc(sizeof(double) * nummax*nummax);

    ii = 0;
    for (i=0; i<nummax*nummax; i++) {
        ii = ii + 1;
        if (ii > nummax) {
            ii = 0;
        }
        h0[i] = ii;
    }

    /* Compute times assuming 500 mflops and 5 second target time */
    /* ntimes = max(3.0d0,5.0d0/(4.0d-9*num**3)); */
    ntimes = 5;

    for (ii=0; ii<howmany; ii++) {
        num_m = nums_m[ii];
        num_n = nums_n[ii];
        num_k = nums_k[ii];
        a = 0.5/(num_m*num_n);
        if (num_m > nummax || num_n > nummax || num_k > nummax) {
            GA_Error("Insufficient memory: check nummax", 1);
        }

#ifndef BLOCK_CYCLIC
        ndim = 2;

	/*
        dims[0] = num_m;
        dims[1] = num_n;
	*/
        dims[1] = num_m;
        dims[0] = num_n;

#if defined(USE_ELEMENTAL)
        ElGlobalArraysCreate_d( eldga, ndim, dims, "g_c", NULL, &g_c );
#else
        if (!((g_c = NGA_Create(MT_DBL,ndim,dims,"g_c",NULL)))) {
            GA_Error("failed: create g_c",20);
        }
#endif
	/*
        dims[0] = num_k;
        dims[1] = num_n;
	*/
        dims[1] = num_k;
        dims[0] = num_n;
#if defined(USE_ELEMENTAL)
        ElGlobalArraysCreate_d( eldga, ndim, dims, "g_b", NULL, &g_b );
#else
        if (!((g_b = NGA_Create(MT_DBL,ndim,dims,"g_b",NULL)))) {
            GA_Error("failed: create g_b",30);
        }
#endif
	/*
        dims[0] = num_m;
        dims[1] = num_k;
	*/
        dims[1] = num_m;
        dims[0] = num_k;
#if defined(USE_ELEMENTAL)
        ElGlobalArraysCreate_d( eldga, ndim, dims, "g_a", NULL, &g_a );
#else
        if (!((g_a = NGA_Create(MT_DBL,ndim,dims,"g_a",NULL)))) {
            GA_Error("failed: create g_a",40);
        }
#endif
#else
        ndim = 2;
        block_size[0] = 128;
        block_size[1] = 128;

        dims[0] = num_m;
        dims[1] = num_n;
        g_c = GA_Create_handle();
        GA_Set_data(g_c,ndim,dims,MT_DBL);
        GA_Set_array_name(g_c,"g_c");
        GA_Set_block_cyclic(g_c,block_size);
        if (!GA_Allocate(g_c)) {
            GA_Error("failed: create g_c",40);
        }

        dims[0] = num_k;
        dims[1] = num_n;
        g_b = GA_Create_handle();
        GA_Set_data(g_b,ndim,dims,MT_DBL);
        GA_Set_array_name(g_b,"g_b");
        GA_Set_block_cyclic(g_b,block_size);
        if (!ga_allocate(g_b)) {
            GA_Error("failed: create g_b",40);
        }

        dims[0] = num_m;
        dims[1] = num_k;
        g_a = GA_Create_handle();
        GA_Set_data(g_a,ndim,dims,MT_DBL);
        GA_Set_array_name(g_a,"g_a");
        GA_Set_block_cyclic(g_a,block_size);
        if (!ga_allocate(g_a)) {
            GA_Error('failed: create g_a',40);
        }
#endif

        /* Initialize matrices A and B */
        if (me == 0) {
            load_ga(g_a, h0, num_m, num_k);
            load_ga(g_b, h0, num_k, num_n);
        }
#if defined(USE_ELEMENTAL)
        double zero = 0.0;
        ElGlobalArraysFill_d( eldga, g_c, &zero );
	ElGlobalArraysSync_d( eldga );
#else
        GA_Zero(g_c);
        GA_Sync();
#endif
#if defined(USE_ELEMENTAL)
        if (me == 0) {
#else
        if (GA_Nodeid() == 0) {
#endif
            printf("\nMatrix Multiplication on C = A[%ld,%ld]xB[%ld,%ld]\n",
                    (long)num_m, (long)num_k, (long)num_k, (long)num_n);
            fflush(stdout);
        }

        for (i=0; i<ntrans; i++) {
            avg_t[i]  = 0.0;
            avg_mf[i] = 0.0;
        }

        for (itime=0; itime<ntimes; itime++) {
            for (i=0; i<ntrans; i++) {
#if defined(USE_ELEMENTAL)
	        ElGlobalArraysSync_d( eldga );
#else
                GA_Sync();
#endif
                ta = transa[i];
                tb = transb[i];
                t1 = MP_TIMER();
#if defined(USE_ELEMENTAL)
		ElGlobalArraysDgemm_d( eldga, ta, tb, num_m, num_n, num_k, 1.0, g_a, g_b, 0.0, g_c );
#else
                GA_Dgemm(ta,tb,num_m,num_n,num_k,1.0, g_a, g_b, 0.0, g_c);
#endif
                t1 = MP_TIMER() - t1;
#if defined(USE_ELEMENTAL)
                if (me == 0) {
#else
                if (GA_Nodeid() == 0) {
#endif
#if defined(USE_ELEMENTAL)
                    mf = 2e0*num_m*num_n*num_k/t1*1e-6/nproc;
#else
                    mf = 2e0*num_m*num_n*num_k/t1*1e-6/GA_Nnodes();
#endif
                    avg_t[i]  = avg_t[i]+t1;
                    avg_mf[i] = avg_mf[i] + mf;
                    printf("%15s%2d: %12.4f seconds %12.1f mflops/proc  %c %c\n",
                            "Run#", itime, t1, mf, ta, tb);
                    fflush(stdout);
                    if (dgemm_verify && itime == 0) {
                        /* recall the C API swaps the matrix order */
                        /* we swap it here for the Fortran-based verify */
                        verify_ga_dgemm(tb, ta, num_n, num_m, num_k, 1.0,
                                g_b, g_a, 0.0, g_c, tmpb, tmpa, tmpc);
                    }
                }
            }
        }
#if defined(USE_ELEMENTAL)
        if (me == 0) {
#else
        if (GA_Nodeid() == 0) {
#endif
            printf("\n");
            for (i=0; i<ntrans; i++) {
                printf("%17s: %12.4f seconds %12.1f mflops/proc  %c %c\n",
                        "Average", avg_t[i]/ntimes, avg_mf[i]/ntimes,
                        transa[i], transb[i]);
            }
            if(dgemm_verify) {
                printf("All GA_Dgemms are verified...O.K.\n");
            }
            fflush(stdout);
        }

        /*
           GA_Print(g_a);
           GA_Print(g_b);
           GA_Print(g_c);
           */
#if defined(USE_ELEMENTAL)
        ElGlobalArraysDestroy_d( eldga, g_a );
        ElGlobalArraysDestroy_d( eldga, g_b );
        ElGlobalArraysDestroy_d( eldga, g_c );
#else
        GA_Destroy(g_c);
        GA_Destroy(g_b);
        GA_Destroy(g_a);
#endif
    }

    /* ???
       format(a15, i2, ': ', e12.4, ' seconds ',f12.1,
       .     ' mflops/proc ', 3a2)
       */
#if defined(USE_ELEMENTAL)
    if (me == 0) {
#else
    if (GA_Nodeid() == 0) {
#endif
        printf("All tests successful\n");
    }

    free(h0);
    free(tmpa);
    free(tmpb);
    free(tmpc);
#if defined(USE_ELEMENTAL)
    // call el::global arrays destructor
    ElGlobalArraysTerminate_d( eldga );
    ElGlobalArraysDestruct_d( eldga );
    ElFinalize();
#else
    GA_Terminate();
    MP_FINALIZE();
#endif
    return 0;
}

/*
 * Verify for correctness. Process 0 computes BLAS dgemm
 * locally. For larger arrays, disbale this test as memory
 * might not be sufficient
 */
void verify_ga_dgemm(char xt1, char xt2, int num_m, int num_n, int num_k,
        double alpha, int g_a, int g_b, double beta, int g_c,
        double *tmpa, double *tmpb, double *tmpc)
{
    int i,j,type,ndim,dims[2],lo[2],hi[2];
    double abs_value;

    for (i=0; i<num_n; i++) {
        for (j=0; j<num_m; j++) {
            tmpc[j+i*num_m] = -1.0;
            tmpa[j+i*num_m] = -2.0;
        }
    }

#if defined(USE_ELEMENTAL)
    ElGlobalArraysInquire_d( eldga, g_a, &ndim, dims );
#else
    NGA_Inquire(g_a, &type, &ndim, dims);
#endif
    lo[0] = 0;
    lo[1] = 0;
    hi[0] = dims[0]-1;
    hi[1] = dims[1]-1;
#if defined(USE_ELEMENTAL)
    ElGlobalArraysGet_d( eldga, g_a, lo, hi, tmpa, &dims[1] );
#else
    NGA_Get(g_a, lo, hi, tmpa, &dims[1]);
#endif

#if defined(USE_ELEMENTAL)
    ElGlobalArraysInquire_d( eldga, g_a, &ndim, dims );
#else
    NGA_Inquire(g_a, &type, &ndim, dims);
#endif
    lo[0] = 0;
    lo[1] = 0;
    hi[0] = dims[0]-1;
    hi[1] = dims[1]-1;
#if defined(USE_ELEMENTAL)
    ElGlobalArraysGet_d( eldga, g_b, lo, hi, tmpb, &dims[1] );
#else
    NGA_Get(g_b, lo, hi, tmpb, &dims[1]);
#endif

    /* compute dgemm sequentially */
#if defined(USE_ELEMENTAL)
    cblas_dgemm ( CblasRowMajor, ( xt1 == 'n'? CblasNoTrans: CblasTrans ),
	    ( xt2 == 'n'? CblasNoTrans: CblasTrans ),
	    num_m /* M */, num_n /* N */, num_k /* K */,
	    alpha, tmpa, num_m, /* lda */
	    tmpb, num_k, /* ldb */ beta,
	    tmpc, num_m /* ldc */);
#else
    xb_dgemm(&xt1, &xt2, &num_m, &num_n, &num_k,
            &alpha, tmpa, &num_m,
            tmpb, &num_k, &beta,
            tmpc, &num_m);
#endif

    /* after computing c locally, verify it with the values in g_c */

#if defined(USE_ELEMENTAL)
    ElGlobalArraysInquire_d( eldga, g_a, &ndim, dims );
#else
    NGA_Inquire(g_a, &type, &ndim, dims);
#endif
    lo[0] = 0;
    lo[1] = 0;
    hi[0] = dims[0]-1;
    hi[1] = dims[1]-1;
#if defined(USE_ELEMENTAL)
    ElGlobalArraysGet_d( eldga, g_c, lo, hi, tmpa, &dims[1] );
#else
    NGA_Get(g_c, lo, hi, tmpa, &dims[1]);
#endif

    for (i=0; i<num_n; i++) {
        for (j=0; j<num_m; j++) {
            abs_value = fabs(tmpc[j+i*num_m]-tmpa[j+i*num_m]);
            if(abs_value > 1.0 || abs_value < -1.0) {
                printf("Values are = %f %f\n",
                        tmpc[j+i*num_m], tmpa[j+i*num_m]);
                printf("Values are = %f %f\n",
                        fabs(tmpc[j+i*num_m]-tmpa[j*i*num_m]), abs_value);
                fflush(stdout);
                GA_Error("verify ga_dgemm failed", 1);
            }
        }
    }
}

/**
 * called by process '0' (or your master process )
 */
void load_ga(int handle, double *f, int dim1, int dim2)
{
      int lo[2], hi[2];

      if (dim1 < 0 || dim2 < 0) {
          return;
      }

      lo[0] = 0;
      lo[1] = 0;
      hi[0] = dim1-1;
      hi[1] = dim2-1;
#if defined(USE_ELEMENTAL)
      ElGlobalArraysPut_d( eldga, handle, lo, hi, f, &dim1 );
#else
      NGA_Put(handle, lo, hi, f, &dim1);
#endif
}

/*
c
c-----------------------------------------------------------------------
c     must be called by all processors, if you need to fillup the
c     entire array
c
      subroutine load_ga_from_square(handle,num,f,ndim)
      implicit none
      integer handle, memhandle
      integer num,ndim
      real*8 f(ndim,ndim)
      integer ilo, ihi, jlo, jhi, nx, ny, ibuff
      integer ga_nodeid, i1, i2, i, j, ix, jx
#include "mafdecls.fh"

      call ga_distribution(handle, ga_nodeid(), ilo, ihi, jlo, jhi)

      if(ihi.le.0)return
      if(jhi.le.0)return

c     nx = ihi - ilo + 1
c     ny = jhi - jlo + 1

      do i = ilo,ihi,ndim
         do j = jlo,jhi,ndim
            call ga_put(handle,i,min(ihi,i+ndim),j,min(jhi,j+ndim),
     &                    f,ndim)
         enddo
      enddo

      return
      end
*/

/*
c
c-----------------------------------------------------------------------
c     must be called by all processors, if you need to fillup the
c     entire array
c
      subroutine load_ga_from_triangle(handle,f,ndim)
      implicit none
      integer handle, memhandle
      real*8 f(*)
      integer ndim
      integer ilo, ihi, jlo, jhi, nx, ny, ibuff
      integer ga_nodeid, i1, i2, i, j, ix, jx
#include "mafdecls.fh"

      call ga_distribution(handle, ga_nodeid(), ilo, ihi, jlo, jhi)

      if(ihi.le.0)return
      if(jhi.le.0)return

      nx = ihi - ilo + 1
      ny = jhi - jlo + 1

      if (.not.ma_alloc_get(MT_DBL,nx*ny,'flap',memhandle,ibuff)) then
         call ga_error('failed: allocate triangle',100)
      endif

      do i = 1,nx
         do j = 1,ny
            ix = i + ilo - 1
            jx = j + jlo - 1
            i1 = min(ix,jx)
            i2 = max(ix,jx)
            dbl_mb(ibuff + nx*(j-1) + (i-1) ) = f(i2*(i2-1)/2 + i1)
         enddo
      enddo

      call ga_put(handle,ilo,ihi,jlo,jhi,
     &              dbl_mb(ibuff),nx)

      if (.not.ma_free_heap(memhandle)) then
         call ga_error('failed: free triangle',100)
      endif

      return
      end
*/


/*
ODE: a program to get optime Runge-Kutta and multi-steps methods.

Copyright 2011-2019, Javier Burguete Tolosa.

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.

THIS SOFTWARE IS PROVIDED BY Javier Burguete Tolosa ``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 Javier Burguete Tolosa 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.
*/

/**
 * \file rk_4_2.c
 * \brief Source file to optimize Runge-Kutta 4 steps 2nd order methods.
 * \author Javier Burguete Tolosa.
 * \copyright Copyright 2011-2019.
 */
#define _GNU_SOURCE
#include <string.h>
#include <math.h>
#include <libxml/parser.h>
#include <glib.h>
#include <libintl.h>
#include <gsl/gsl_rng.h>
#include "config.h"
#include "utils.h"
#include "optimize.h"
#include "rk.h"
#include "rk_4_2.h"

#define DEBUG_RK_4_2 0          ///< macro to debug.

/**
 * Function to obtain the coefficients of a 4 steps 2nd order Runge-Kutta
 * method.
 */
int
rk_tb_4_2 (Optimize * optimize) ///< Optimize struct.
{
  long double *tb, *r;
#if DEBUG_RK_4_2
  fprintf (stderr, "rk_tb_4_2: start\n");
#endif
  tb = optimize->coefficient;
  r = optimize->random_data;
  t4 (tb) = 1.L;
  t1 (tb) = r[0];
  t2 (tb) = r[1];
  b21 (tb) = r[2];
  t3 (tb) = r[3];
  b31 (tb) = r[4];
  b32 (tb) = r[5];
  b41 (tb) = r[6];
  b42 (tb) = r[7];
  b43 (tb) = (0.5L - b41 (tb) * t1 (tb) - b42 (tb) * t2 (tb)) / t3 (tb);
  if (isnan (b43 (tb)))
    return 0;
  rk_b_4 (tb);
#if DEBUG_RK_4_2
  fprintf (stderr, "rk_tb_4_2: end\n");
#endif
  return 1;
}

/**
 * Function to obtain the coefficients of a 4 steps 2nd order, 3rd order in
 * equations depending only in time, Runge-Kutta method.
 */
int
rk_tb_4_2t (Optimize * optimize)        ///< Optimize struct.
{
  long double *tb, *r;
#if DEBUG_RK_4_2
  fprintf (stderr, "rk_tb_4_2t: start\n");
#endif
  tb = optimize->coefficient;
  r = optimize->random_data;
  t4 (tb) = 1.L;
  t1 (tb) = r[0];
  t2 (tb) = r[1];
  b21 (tb) = r[2];
  t3 (tb) = r[3];
  b31 (tb) = r[4];
  b32 (tb) = r[5];
  b41 (tb) = r[6];
  b42 (tb) = (1.L / 3.L - 0.5L * t3 (tb)
              - b41 (tb) * t1 (tb) * (t1 (tb) - t3 (tb)))
    / (t2 (tb) * (t2 (tb) - t3 (tb)));
  if (isnan (b42 (tb)))
    return 0;
  b43 (tb) = (0.5L - b41 (tb) * t1 (tb) - b42 (tb) * t2 (tb)) / t3 (tb);
  if (isnan (b43 (tb)))
    return 0;
  rk_b_4 (tb);
#if DEBUG_RK_4_2
  fprintf (stderr, "rk_tb_4_2t: end\n");
#endif
  return 1;
}

/**
 * Function to obtain the coefficients of a 4 steps 1st-2nd order Runge-Kutta
 * pair.
 */
int
rk_tb_4_2p (Optimize * optimize)        ///< Optimize struct.
{
  long double *tb;
#if DEBUG_RK_4_2
  fprintf (stderr, "rk_tb_4_2p: start\n");
#endif
  if (!rk_tb_4_2 (optimize))
    return 0;
  tb = optimize->coefficient;
  e41 (tb) = e42 (tb) = 0.L;
  rk_e_4 (tb);
#if DEBUG_RK_4_2
  rk_print_e (optimize, "rk_tb_4_2p", stderr);
  fprintf (stderr, "rk_tb_4_2p: end\n");
#endif
  return 1;
}

/**
 * Function to obtain the coefficients of a 4 steps 1st-2nd order, 1st-3rd order
 * in equations depending only in time, Runge-Kutta method.
 */
int
rk_tb_4_2tp (Optimize * optimize)       ///< Optimize struct.
{
  long double *tb;
#if DEBUG_RK_4_2
  fprintf (stderr, "rk_tb_4_2tp: start\n");
#endif
  if (!rk_tb_4_2t (optimize))
    return 0;
  tb = optimize->coefficient;
  e41 (tb) = e42 (tb) = 0.L;
  rk_e_4 (tb);
#if DEBUG_RK_4_2
  rk_print_tb (optimize, "rk_tb_4_2tp", stderr);
  fprintf (stderr, "rk_tb_4_2pt: end\n");
#endif
  return 1;
}

/**
 * Function to calculate the objective function of a 4 steps 2nd order
 * Runge-Kutta method.
 *
 * \return objective function value.
 */
long double
rk_objective_tb_4_2 (RK * rk)   ///< RK struct.
{
  long double *tb;
  long double o;
#if DEBUG_RK_4_2
  fprintf (stderr, "rk_objective_tb_4_2: start\n");
#endif
  tb = rk->tb->coefficient;
  o = fminl (0.L, b20 (tb));
  if (b30 (tb) < 0.L)
    o += b30 (tb);
  if (b40 (tb) < 0.L)
    o += b40 (tb);
  if (b43 (tb) < 0.L)
    o += b43 (tb);
  if (o < 0.L)
    {
      o = 40.L - o;
      goto end;
    }
  o = 30.L + fmaxl (1.L, fmaxl (t1 (tb), fmaxl (t2 (tb), t3 (tb))));
  if (rk->strong)
    {
      rk_bucle_ac (rk);
      o = fminl (o, *rk->ac0->optimal);
    }
end:
#if DEBUG_RK_4_2
  fprintf (stderr, "rk_objective_tb_4_2: optimal=%Lg\n", o);
  fprintf (stderr, "rk_objective_tb_4_2: end\n");
#endif
  return o;
}

/**
 * Function to calculate the objective function of a 4 steps 2nd order, 3rd
 * order in equations depending only in time, Runge-Kutta method.
 *
 * \return objective function value.
 */
long double
rk_objective_tb_4_2t (RK * rk)  ///< RK struct.
{
  long double *tb;
  long double o;
#if DEBUG_RK_4_2
  fprintf (stderr, "rk_objective_tb_4_2t: start\n");
#endif
  tb = rk->tb->coefficient;
  o = fminl (0.L, b20 (tb));
  if (b30 (tb) < 0.L)
    o += b30 (tb);
  if (b40 (tb) < 0.L)
    o += b40 (tb);
  if (b42 (tb) < 0.L)
    o += b42 (tb);
  if (b43 (tb) < 0.L)
    o += b43 (tb);
  if (o < 0.L)
    {
      o = 40.L - o;
      goto end;
    }
  o = 30.L + fmaxl (1.L, fmaxl (t1 (tb), fmaxl (t2 (tb), t3 (tb))));
  if (rk->strong)
    {
      rk_bucle_ac (rk);
      o = fminl (o, *rk->ac0->optimal);
    }
end:
#if DEBUG_RK_4_2
  fprintf (stderr, "rk_objective_tb_4_2t: optimal=%Lg\n", o);
  fprintf (stderr, "rk_objective_tb_4_2t: end\n");
#endif
  return o;
}


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

int main (void)
{
  double x, y;
  x = 5.0;
  y = gsl_sf_bessel_J0 (x);
  printf ("J0(%g) = %.18e\n", x, y);
  return 0;
}