/*******************************************************************************
*
*       This file is part of the General Hidden Markov Model Library,
*       GHMM version __VERSION__, see http://ghmm.org
*
*       Filename: ghmm/ghmm/randvar.c
*       Authors:  Bernhard Knab, Benjamin Rich, Janne Grunau
*
*       Copyright (C) 1998-2004 Alexander Schliep
*       Copyright (C) 1998-2001 ZAIK/ZPR, Universitaet zu Koeln
*       Copyright (C) 2002-2004 Max-Planck-Institut fuer Molekulare Genetik,
*                               Berlin
*
*       Contact: schliep@ghmm.org
*
*       This library is free software; you can redistribute it and/or
*       modify it under the terms of the GNU Library General Public
*       License as published by the Free Software Foundation; either
*       version 2 of the License, or (at your option) any later version.
*
*       This library 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
*       Library General Public License for more details.
*
*       You should have received a copy of the GNU Library General Public
*       License along with this library; if not, write to the Free
*       Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*
*       This file is version $Revision: 2310 $
*                       from $Date: 2013-06-14 10:36:57 -0400 (Fri, 14 Jun 2013) $
*             last change by $Author: ejb177 $.
*
*******************************************************************************/

#ifdef HAVE_CONFIG_H
#  include "../config.h"
#endif

#include <math.h>
#include <float.h>
#include <string.h>
#ifdef HAVE_LIBPTHREAD
# include <pthread.h>
#endif /* HAVE_LIBPTHREAD */

#include "ghmm.h"
#include "mes.h"
#include "mprintf.h"
#include "randvar.h"
#include "rng.h"

#ifdef DO_WITH_GSL

# include <gsl/gsl_math.h>
# include <gsl/gsl_sf_erf.h>
# include <gsl/gsl_randist.h>

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

#endif /* DO_WITH_GSL */

#if defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 199901L)
#else
static double ighmm_erf (double x);
static double ighmm_erfc (double x);
#endif /* check for ISO C99 */

/* A list of already calculated values of the density function of a
   N(0,1)-distribution, with x in [0.00, 19.99] */
#define PDFLEN 2000
#define X_STEP_PDF 0.01         /* step size */
#define X_FAKT_PDF 100          /* equivalent to step size */
static double pdf_stdnormal[PDFLEN];
static int pdf_stdnormal_exists = 0;

/* A list of already calulated values PHI of the Gauss distribution is
   read in, x in [-9.999, 0] */
#define X_STEP_PHI 0.001        /* step size */
#define X_FAKT_PHI 1000         /* equivalent to step size */
static double x_PHI_1 = -1.0;

#ifndef M_SQRT1_2
#define M_SQRT1_2  0.70710678118654752440084436210
#endif

/*============================================================================*/
/* needed by ighmm_gtail_pmue_interpol */

double ighmm_rand_get_xfaktphi ()
{
  return X_FAKT_PHI;
}

double ighmm_rand_get_xstepphi ()
{
  return X_STEP_PHI;
}

double ighmm_rand_get_philen ()
{
#ifdef DO_WITH_GSL
  return 0 /*PHI_len*/;
#else
  return ighmm_rand_get_xPHIless1 () / X_STEP_PHI;
#endif
}

/*============================================================================*/
double ighmm_rand_get_PHI (double x)
{
#if defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 199901L)
  return (erf (x * M_SQRT1_2) + 1.0) / 2.0;
#else
  return (ighmm_erf (x * M_SQRT1_2) + 1.0) / 2.0;
#endif
}                               /* randvar_get_PHI */

/*============================================================================*/
/* When is PHI[x,0,1] == 1? */
double ighmm_rand_get_xPHIless1 ()
{
# define CUR_PROC "ighmm_rand_get_xPHIless1"

  if (x_PHI_1 == -1) {
    double low, up, half;
    low = 0;
    up = 100;
    while (up - low > 0.001) {
      half = (low + up) / 2.0;
      if (ighmm_rand_get_PHI (half) < 1.0)
        low = half;
      else
        up = half;
    }
    x_PHI_1 = low;
  }
  return (x_PHI_1);

# undef CUR_PROC
}

/*============================================================================*/
double ighmm_rand_get_1overa (double x, double mean, double u)
{
  /* Calulates 1/a(x, mean, u), with a = the integral from x til \infty over
     the Gauss density function */
# define CUR_PROC "ighmm_rand_get_1overa"

  double erfc_value;

  if (u <= 0.0) {
    GHMM_LOG(LCONVERTED, "u <= 0.0 not allowed\n");
    goto STOP;
  }

#if defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 199901L)
  erfc_value = erfc ((x - mean) / sqrt (u * 2));
#else
  erfc_value = ighmm_erfc ((x - mean) / sqrt (u * 2));
#endif

  if (erfc_value <= DBL_MIN) {
    ighmm_mes (MES_WIN, "a ~= 0.0 critical! (mue = %.2f, u =%.2f)\n", mean, u);
    return (erfc_value);
  }
  else
    return (2.0 / erfc_value);

STOP:
  return (-1.0);
# undef CUR_PROC
}                               /* ighmm_rand_get_1overa */

/*============================================================================*/
/* REMARK:
   The calulation of this density function was testet, by calculating the
   following integral sum for arbitrary mue and u:
     for (x = 0, x < ..., x += step(=0.01/0.001/0.0001))
       isum += step * ighmm_rand_normal_density_pos(x, mue, u);
   In each case, the sum "converged" evidently towards 1!
   (BK, 14.6.99)
   CHANGE:
   Truncate at -EPS_NDT (const.h), so that x = 0 doesn't lead to a problem.
   (BK, 15.3.2000)
*/
double ighmm_rand_normal_density_pos (double x, double mean, double u)
{
# define CUR_PROC "ighmm_rand_normal_density_pos"
  return ighmm_rand_normal_density_trunc (x, mean, u, -GHMM_EPS_NDT);
# undef CUR_PROC
}                               /* double ighmm_rand_normal_density_pos */

/*============================================================================*/
double ighmm_rand_normal_density_trunc(double x, double mean, double u,
                                       double a)
{
# define CUR_PROC "ighmm_rand_normal_density_trunc"
#ifndef DO_WITH_GSL
  double c;
#endif /* DO_WITH_GSL */

  if (u <= 0.0) {
    GHMM_LOG(LERROR, "u <= 0.0 not allowed");
    goto STOP;
  }
  if (x < a)
    return 0.0;

#ifdef DO_WITH_GSL
  /* move mean to the right position */
  return gsl_ran_gaussian_tail_pdf(x - mean, a - mean, sqrt(u));
#else
  if ((c = ighmm_rand_get_1overa(a, mean, u)) == -1) {
    GHMM_LOG_QUEUED(LERROR);
    goto STOP;
  };
  return c * ighmm_rand_normal_density(x, mean, u);
#endif /* DO_WITH_GSL */

STOP:
  return -1.0;
# undef CUR_PROC
}                               /* double ighmm_rand_normal_density_trunc */

/*============================================================================*/
double ighmm_rand_normal_density (double x, double mean, double u)
{
# define CUR_PROC "ighmm_rand_normal_density"
#ifndef DO_WITH_GSL
  double expo;
#endif
  if (u <= 0.0) {
    GHMM_LOG(LCONVERTED, "u <= 0.0 not allowed\n");
    goto STOP;
  }
  /* The denominator is possibly < EPS??? Check that ? */
#ifdef DO_WITH_GSL
  /* double gsl_ran_gaussian_pdf (double x, double sigma) */
  return gsl_ran_gaussian_pdf (x - mean, sqrt (u));
#else
  expo = exp (-1 * m_sqr (mean - x) / (2 * u));
  return (1 / (sqrt (2 * PI * u)) * expo);
#endif

STOP:
  return (-1.0);
# undef CUR_PROC
}                               /* double ighmm_rand_normal_density */

/*============================================================================*/
/* covariance matrix is linearized */
double ighmm_rand_binormal_density(const double *x, double *mean, double *cov)
{
# define CUR_PROC "ighmm_rand_binormal_density"
  double rho;
#ifndef DO_WITH_GSL
  double numerator,part1,part2,part3;
#endif
  if (cov[0] <= 0.0 || cov[2 + 1] <= 0.0) {
    GHMM_LOG(LCONVERTED, "variance <= 0.0 not allowed\n");
    goto STOP;
  }
  rho = cov[1] / ( sqrt (cov[0]) * sqrt (cov[2 + 1]) );
  /* The denominator is possibly < EPS??? Check that ? */
#ifdef DO_WITH_GSL
  /* double gsl_ran_bivariate_gaussian_pdf (double x, double y, double sigma_x,
                                            double sigma_y, double rho) */
  return gsl_ran_bivariate_gaussian_pdf (x[0], x[1], sqrt (cov[0]),
                                         sqrt (cov[2 + 1]), rho);
#else
  part1 = (x[0] - mean[0]) / sqrt (cov[0]);
  part2 = (x[1] - mean[1]) / sqrt (cov[2 + 1]);
  part3 = m_sqr (part1) - 2 * part1 * part2 + m_sqr (part2);
  numerator = exp ( -1 * (part3) / ( 2 * (1 - m_sqr(rho)) ) );
  return (numerator / ( 2 * PI * sqrt(1 - m_sqr(rho)) ));
#endif

STOP:
  return (-1.0);
# undef CUR_PROC
}                               /* double ighmm_rand_binormal_density */

/*============================================================================*/
/* matrices are linearized */
double ighmm_rand_multivariate_normal_density(int length, const double *x, double *mean, double *sigmainv, double det)
{
# define CUR_PROC "ighmm_rand_multivariate_normal_density"
  /* multivariate normal density function    */
  /*
   *       length     dimension of the random vetor
   *       x          point at which to evaluate the pdf
   *       mean       vector of means of size n
   *       sigmainv   inverse variance matrix of dimension n x n
   *       det        determinant of covariance matrix
   */

#ifdef DO_WITH_GSL
  int i, j;
  double ax,ay;
  gsl_vector *ym, *xm, *gmean;
  gsl_matrix *inv = gsl_matrix_alloc(length, length);

  for (i=0; i<length; ++i) {
    for (j=0; j<length; ++j) {
      gsl_matrix_set(inv, i, j, sigmainv[i*length+j]);
    }
  }

  xm = gsl_vector_alloc(length);
  gmean = gsl_vector_alloc(length);
  /*gsl_vector_memcpy(xm, x);*/
  for (i=0; i<length; ++i) {
    gsl_vector_set(xm, i, x[i]);
    gsl_vector_set(gmean, i, mean[i]);
  }

  gsl_vector_sub(xm, gmean);
  ym = gsl_vector_alloc(length);
  gsl_blas_dsymv(CblasUpper, 1.0, inv, xm, 0.0, ym);
  gsl_matrix_free(inv);
  gsl_blas_ddot(xm, ym, &ay);
  gsl_vector_free(xm);
  gsl_vector_free(ym);
  ay = exp(-0.5*ay) / sqrt(pow((2*M_PI), length) * det);

  return ay;
#else
  /* do without GSL */
  int i, j;
  double ay, tempv;

  ay = 0;
  for (i=0; i<length; ++i) {
    tempv = 0;
    for (j=0; j<length; ++j) {
      tempv += (x[j]-mean[j])*sigmainv[j*length+i];
    }
    ay += tempv*(x[i]-mean[i]);
  }
  ay = exp(-0.5*ay) / sqrt(pow((2*PI), length) * det);

  return ay;
#endif
# undef CUR_PROC
}                               /* double ighmm_rand_multivariate_normal_density */

/*============================================================================*/
double ighmm_rand_uniform_density (double x, double max, double min)
{
# define CUR_PROC "ighmm_rand_uniform_density"
  double prob;
  if (max <= min) {
    GHMM_LOG(LCONVERTED, "max <= min not allowed \n");
    goto STOP;
  }
  prob = 1.0/(max-min);

  if ( (x <= max) && (x>=min) ){
    return prob;
  }else{
    return 0.0;
  }
STOP:
  return (-1.0);
# undef CUR_PROC
}                               /* double ighmm_rand_uniform_density */

/*============================================================================*/
/* special ghmm_cmodel pdf need it: smo->density==normal_approx: */
/* generates a table of of aequidistant samples of gaussian pdf */

static int randvar_init_pdf_stdnormal ()
{
# define CUR_PROC "randvar_init_pdf_stdnormal"
  int i;
  double x = 0.00;
  for (i = 0; i < PDFLEN; i++) {
    pdf_stdnormal[i] = 1 / (sqrt (2 * PI)) * exp (-1 * x * x / 2);
    x += (double) X_STEP_PDF;
  }
  pdf_stdnormal_exists = 1;
  /* printf("pdf_stdnormal_exists = %d\n", pdf_stdnormal_exists); */
  return (0);
# undef CUR_PROC
}                               /* randvar_init_pdf_stdnormal */

double ighmm_rand_normal_density_approx (double x, double mean, double u)
{
# define CUR_PROC "ighmm_rand_normal_density_approx"
#ifdef HAVE_LIBPTHREAD
  static pthread_mutex_t lock;
#endif /* HAVE_LIBPTHREAD */
  int i;
  double y, z, pdf_x;
  if (u <= 0.0) {
    GHMM_LOG(LCONVERTED, "u <= 0.0 not allowed\n");
    goto STOP;
  }
  if (!pdf_stdnormal_exists) {
#ifdef HAVE_LIBPTHREAD
    pthread_mutex_lock (&lock); /* Put on a lock, because the clustering is parallel   */
#endif /* HAVE_LIBPTHREAD */
    randvar_init_pdf_stdnormal ();
#ifdef HAVE_LIBPTHREAD
    pthread_mutex_unlock (&lock);       /* Take the lock off */
#endif /* HAVE_LIBPTHREAD */
  }
  y = 1 / sqrt (u);
  z = fabs ((x - mean) * y);
  i = (int) (z * X_FAKT_PDF);
  /* linear interpolation: */
  if (i >= PDFLEN - 1) {
    i = PDFLEN - 1;
    pdf_x = y * pdf_stdnormal[i];
  }
  else
    pdf_x = y * (pdf_stdnormal[i] +
                 (z - i * X_STEP_PDF) *
                 (pdf_stdnormal[i + 1] - pdf_stdnormal[i]) / X_STEP_PDF);
  return (pdf_x);
STOP:
  return (-1.0);
# undef CUR_PROC
}                               /* double ighmm_rand_normal_density_approx */
double ighmm_rand_dirichlet(int seed, int len, double *alpha, double *theta){
  if (seed != 0) {
    GHMM_RNG_SET(RNG, seed);
  }
#ifdef DO_WITH_GSL
   gsl_ran_dirichlet(RNG, len, alpha, theta);
#else
  printf("not implemted without gsl. Compile with gsl to use dirichlet");
#endif
}

/*============================================================================*/
double ighmm_rand_std_normal (int seed)
{
# define CUR_PROC "ighmm_rand_std_normal"
  if (seed != 0) {
    GHMM_RNG_SET (RNG, seed);
  }

#ifdef DO_WITH_GSL
    return (gsl_ran_gaussian (RNG, 1.0));
#else
    /* Use the polar Box-Mueller transform */
    /*
       double x, y, r2;

       do {
       x = 2.0 * GHMM_RNG_UNIFORM(RNG) - 1.0;
       y = 2.0 * GHMM_RNG_UNIFORM(RNG) - 1.0;
       r2 = (x * x) + (y * y);
       } while (r2 >= 1.0);

       return x * sqrt((-2.0 * log(r2)) / r2);
     */

    double r2, theta;

    r2 = -2.0 * log (GHMM_RNG_UNIFORM (RNG));   /* r2 ~ chi-square(2) */
    theta = 2.0 * PI * GHMM_RNG_UNIFORM (RNG);  /* theta ~ uniform(0, 2 \pi) */
    return sqrt (r2) * cos (theta);
#endif

# undef CUR_PROC
}                               /* ighmm_rand_std_normal */

/*============================================================================*/
double ighmm_rand_normal(double mue, double u, int seed)
{
# define CUR_PROC "ighmm_rand_normal"
  if (seed != 0) {
    GHMM_RNG_SET(RNG, seed);
  }

#ifdef DO_WITH_GSL
    return gsl_ran_gaussian(RNG, sqrt (u)) + mue;
#else
    double x;
    x = sqrt(u) * ighmm_rand_std_normal(seed) + mue;
    return x;
#endif

# undef CUR_PROC
}                               /* ighmm_rand_normal */

/*============================================================================*/
int ighmm_rand_multivariate_normal (int dim, double *x, double *mue, double *sigmacd, int seed)
{
# define CUR_PROC "ighmm_rand_multivariate_normal"
  /* generate random vector of multivariate normal
   *
   *     dim     number of dimensions
   *     x       space to store resulting vector in
   *     mue     vector of means
   *     sigmacd linearized cholesky decomposition of cov matrix
   *     seed    RNG seed
   *
   *     see Barr & Slezak, A Comparison of Multivariate Normal Generators */
  int i, j;
#ifdef DO_WITH_GSL
  gsl_vector *y = gsl_vector_alloc(dim);
  gsl_vector *xgsl = gsl_vector_alloc(dim);
  gsl_matrix *cd = gsl_matrix_alloc(dim, dim);
#endif
  if (seed != 0) {
    GHMM_RNG_SET (RNG, seed);
    /* do something here */
    return 0;
  }
  else {
#ifdef DO_WITH_GSL
    /* cholesky decomposition matrix */
    for (i=0;i<dim;i++) {
      for (j=0;j<dim;j++) {
        gsl_matrix_set(cd, i, j, sigmacd[i*dim+j]);
      }
    }
    /* generate a random vector N(O,I) */
    for (i=0;i<dim;i++) {
      gsl_vector_set(y, i, ighmm_rand_std_normal(seed));
    }
    /* multiply cd with y */
    gsl_blas_dgemv(CblasNoTrans, 1.0, cd, y, 0.0, xgsl);
    for (i=0;i<dim;i++) {
      x[i] = gsl_vector_get(xgsl, i) + mue[i];
    }
    gsl_vector_free(y);
    gsl_vector_free(xgsl);
    gsl_matrix_free(cd);
#else
    /* multivariate random numbers without gsl */
    double randuni;
    for (i=0;i<dim;i++) {
      randuni = ighmm_rand_std_normal(seed);
      for (j=0;j<dim;j++) {
        if (i==0)
          x[j] = mue[j];
        x[j] += randuni * sigmacd[j*dim+i];
      }
    }
#endif
    return 0;
  }
# undef CUR_PROC
}                               /* ighmm_rand_multivariate_normal */

/*============================================================================*/
#ifndef DO_WITH_GSL
# define C0 2.515517
# define C1 0.802853
# define C2 0.010328
# define D1 1.432788
# define D2 0.189269
# define D3 0.001308
#endif

double ighmm_rand_normal_right (double a, double mue, double u, int seed)
{
# define CUR_PROC "ighmm_rand_normal_right"
  double x = -1;
  double sigma;
#ifdef DO_WITH_GSL
  double s;
#else
  double U, Us, Us1, Feps, t, T;
#endif

  if (u <= 0.0) {
    GHMM_LOG(LCONVERTED, "u <= 0.0 not allowed\n");
    goto STOP;
  }
  sigma = sqrt(u);

  if (seed != 0) {
    GHMM_RNG_SET (RNG, seed);
  }

#ifdef DO_WITH_GSL
  /* move boundary to lower values in order to achieve maximum at mue
     gsl_ran_gaussian_tail(generator, lower_boundary, sigma)
   */
  return mue + gsl_ran_gaussian_tail(RNG, a - mue, sqrt (u));

#else /* DO_WITH_GSL */

  /* Inverse transformation with restricted sampling by Fishman */
  U = GHMM_RNG_UNIFORM(RNG);
  Feps = ighmm_rand_get_PHI((a-mue) / sigma);

  Us = Feps + (1-Feps) * U;
  Us1 = 1-Us;
  t = m_min (Us, Us1);

  t = sqrt (-log (t * t));

  T =
    sigma * (t - (C0 + t * (C1 + t * C2))
                 / (1 + t * (D1 + t * (D2 + t * D3))));

  if (Us < Us1)
    x = mue - T;
  else
    x = mue + T;
#endif /* DO_WITH_GSL */

STOP:
  return x;
# undef CUR_PROC
}                               /* randvar_normal_pos */

/*============================================================================*/
double ighmm_rand_uniform_int (int seed, int K)
{
# define CUR_PROC "ighmm_rand_uniform_int"
  if (seed != 0) {
    GHMM_RNG_SET (RNG, seed);
  }

#ifdef DO_WITH_GSL
    /* more direct solution than old version ! */
    return (double) gsl_rng_uniform_int (RNG, K);
#else
    return (double) ((int) (((double) K) * GHMM_RNG_UNIFORM (RNG)));
#endif

# undef CUR_PROC
}                               /* ighmm_rand_uniform_int */

/*===========================================================================*/
double ighmm_rand_uniform_cont (int seed, double max, double min)
{
# define CUR_PROC "ighmm_rand_uniform_cont"
  if (max <= min) {
    GHMM_LOG(LCONVERTED, "max <= min not allowed\n");
    goto STOP;
  }
  if (seed != 0) {
    GHMM_RNG_SET (RNG, seed);
  }

#ifdef DO_WITH_GSL
    return (double)(((double)gsl_rng_uniform (RNG)*(max-min)) + min);
#else
    return (double)((GHMM_RNG_UNIFORM (RNG))*(max-min) + min );
#endif

STOP:
  return (-1.0);
# undef CUR_PROC
}                               /* ighmm_rand_uniform_cont */

/*============================================================================*/
/* cumalative distribution function of N(mean, u) */
double ighmm_rand_normal_cdf (double x, double mean, double u)
{
# define CUR_PROC "ighmm_rand_normal_cdf"
  if (u <= 0.0) {
    GHMM_LOG(LCONVERTED, "u <= 0.0 not allowed\n");
    goto STOP;
  }
#if defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 199901L)
  /* PHI(x)=erf(x/sqrt(2))/2+0.5 */
  return (erf ((x - mean) / sqrt (u * 2.0)) + 1.0) / 2.0;
#else
  return (ighmm_erf ((x - mean) / sqrt (u * 2.0)) + 1.0) / 2.0;
#endif /* Check for ISO C99 */
STOP:
  return (-1.0);
# undef CUR_PROC
}                               /* double ighmm_rand_normal_cdf */

/*============================================================================*/
/* cumalative distribution function of a-truncated N(mean, u) */
double ighmm_rand_normal_right_cdf (double x, double mean, double u, double a)
{
# define CUR_PROC "ighmm_rand_normal_right_cdf"

  if (x <= a)
    return (0.0);
  if (u <= a) {
    GHMM_LOG(LCONVERTED, "u <= a not allowed\n");
    goto STOP;
  }
#if defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 199901L)
  /*
     Function: int erfc (double x, gsl_sf_result * result)
     These routines compute the complementary error function
     erfc(x) = 1 - erf(x) = 2/\sqrt(\pi) \int_x^\infty \exp(-t^2).
   */
  return 1.0 + (erf ((x - mean) / sqrt (u * 2)) -
                1.0) / erfc ((a - mean) / sqrt (u * 2));
#else
  return 1.0 + (ighmm_erf ((x - mean) / sqrt (u * 2)) -
                1.0) / ighmm_erfc ((a - mean) / sqrt (u * 2));
#endif /* Check for ISO C99 */
STOP:
  return (-1.0);
# undef CUR_PROC
}                               /* double ighmm_rand_normal_cdf */

/*============================================================================*/
/* cumalative distribution function of a uniform distribution in the range [min,max] */
double ighmm_rand_uniform_cdf (double x, double max, double min)
{
# define CUR_PROC "ighmm_rand_uniform_cdf"
  if (max <= min) {
    GHMM_LOG(LCONVERTED, "max <= min not allowed\n");
    goto STOP;
  }
  if (x < min) {
    return 0.0;
  }
  if (x >= max) {
    return 1.0;
  }
  return (x-min)/(max-min);
STOP:
  return (-1.0);
# undef CUR_PROC
}                               /* ighmm_rand_uniform_cdf */

#if defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 199901L)
#else

/* THIS WILL BE OBSOLETE WHEN WE USE ISO C99 */

/*===========================================================================
 *
 * The following functions for the error function and the complementory
 * error function are taken from
 * http://www.mathematik.uni-bielefeld.de/~sillke/ALGORITHMS/special-functions/erf.c
 * and have following different copyright.

 * ====================================================
 * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
 *
 * Developed at SunPro, a Sun Microsystems, Inc. business.
 * Permission to use, copy, modify, and distribute this
 * software is freely granted, provided that this notice
 * is preserved.
 * ====================================================
 */

/*
 * ====================================================
 *
 * Reference:
 *
 * W. J. Cody,
 *   Rational Chebychev approximations for the
 *   error function.
 *   Mathematics of Computations 23 (1969) 631-637
 *
 * W. J. Cody,
 *   Performance evaluations of programs for the
 *   error and complementary error function.
 *   Transactions of the ACM on Mathematical Software,
 *   16:1 (March 1990) 38-46
 *
 * W. J. Cody,
 *   SPECFUN - A portable special function package,
 *   In: New Computing environments;
 *   Microcomputers in Large-Scale Scientific Computing,
 *   A. Wouk, SIAM, 1987, 1-12
 *
 * W. J. Cody,
 *   http://www.netlib.org/specfun/erf.
 *
 * For calculations with complex arguments see:
 *
 * Walter Gautschi
 *   "Efficient computation of the complex error function"
 *   SIAM J. Numer. Anal.
 *   7:1 (1970), 187-198
 *
 * J.A.C. Weideman
 *   "Computation of the complex error function"
 *   SIAM J. Numer. Anal.
 *   31:5 (1994), 1497-1518
 *
 * ====================================================
 */

/* double erf(double x)
 * double erfc(double x)
 *			     x
 *		      2      |\
 *     erf(x)  =  ---------  | exp(-t*t)dt
 *	 	   sqrt(pi) \|
 *			     0
 *
 *     erfc(x) =  1-erf(x)
 *  Note that
 *		erf(-x) = -erf(x)
 *		erfc(-x) = 2 - erfc(x)
 *
 * Method:
 *	1. For |x| in [0, 0.84375]
 *	    erf(x)  = x + x*R(x^2)
 *          erfc(x) = 1 - erf(x)           if x in [-.84375,0.25]
 *                  = 0.5 + ((0.5-x)-x*R)  if x in [0.25,0.84375]
 *	   where R = P/Q where P is an odd poly of degree 8 and
 *	   Q is an odd poly of degree 10.
 *						 -57.90
 *			| R - (erf(x)-x)/x | <= 2
 *
 *
 *	   Remark. The formula is derived by noting
 *          erf(x) = (2/sqrt(pi))*(x - x^3/3 + x^5/10 - x^7/42 + ....)
 *	   and that
 *          2/sqrt(pi) = 1.128379167095512573896158903121545171688
 *	   is close to one. The interval is chosen because the fix
 *	   point of erf(x) is near 0.6174 (i.e., erf(x)=x when x is
 *	   near 0.6174), and by some experiment, 0.84375 is chosen to
 * 	   guarantee the error is less than one ulp for erf.
 *
 *      2. For |x| in [0.84375,1.25], let s = |x| - 1, and
 *         c = 0.84506291151 rounded to single (24 bits)
 *         	erf(x)  = sign(x) * (c  + P1(s)/Q1(s))
 *         	erfc(x) = (1-c)  - P1(s)/Q1(s) if x > 0
 *			  1+(c+P1(s)/Q1(s))    if x < 0
 *         	|P1/Q1 - (erf(|x|)-c)| <= 2**-59.06
 *	   Remark: here we use the taylor series expansion at x=1.
 *		erf(1+s) = erf(1) + s*Poly(s)
 *			 = 0.845.. + P1(s)/Q1(s)
 *	   That is, we use rational approximation to approximate
 *			erf(1+s) - (c = (single)0.84506291151)
 *	   Note that |P1/Q1|< 0.078 for x in [0.84375,1.25]
 *	   where
 *		P1(s) = degree 6 poly in s
 *		Q1(s) = degree 6 poly in s
 *
 *      3. For x in [1.25,1/0.35(~2.857143)],
 *         	erfc(x) = (1/x)*exp(-x*x-0.5625+R1/S1)
 *         	erf(x)  = 1 - erfc(x)
 *	   where
 *		R1(z) = degree 7 poly in z, (z=1/x^2)
 *		S1(z) = degree 8 poly in z
 *
 *      4. For x in [1/0.35,28]
 *         	erfc(x) = (1/x)*exp(-x*x-0.5625+R2/S2) if x > 0
 *			= 2.0 - (1/x)*exp(-x*x-0.5625+R2/S2) if -6<x<0
 *			= 2.0 - tiny		(if x <= -6)
 *         	erf(x)  = sign(x)*(1.0 - erfc(x)) if x < 6, else
 *         	erf(x)  = sign(x)*(1.0 - tiny)
 *	   where
 *		R2(z) = degree 6 poly in z, (z=1/x^2)
 *		S2(z) = degree 7 poly in z
 *
 *      Note1:
 *	   To compute exp(-x*x-0.5625+R/S), let s be a single
 *	   precision number and s := x; then
 *		-x*x = -s*s + (s-x)*(s+x)
 *	        exp(-x*x-0.5626+R/S) =
 *			exp(-s*s-0.5625)*exp((s-x)*(s+x)+R/S);
 *      Note2:
 *	   Here 4 and 5 make use of the asymptotic series
 *			  exp(-x*x)
 *		erfc(x) ~ ---------- * ( 1 + Poly(1/x^2) )
 *			  x*sqrt(pi)
 *	   We use rational approximation to approximate
 *      	g(s)=f(1/x^2) = log(erfc(x)*x) - x*x + 0.5625
 *	   Here is the error bound for R1/S1 and R2/S2
 *      	|R1/S1 - f(x)|  < 2**(-62.57)
 *      	|R2/S2 - f(x)|  < 2**(-61.52)
 *
 *      5. For inf > x >= 28
 *         	erf(x)  = sign(x) *(1 - tiny)  (raise inexact)
 *         	erfc(x) = tiny*tiny (raise underflow) if x > 0
 *			= 2 - tiny if x<0
 *
 *      7. Special case:
 *         	erf(0)  = 0, erf(inf)  = 1, erf(-inf) = -1,
 *         	erfc(0) = 1, erfc(inf) = 0, erfc(-inf) = 2,
 *	   	erfc/erf(NaN) is NaN
 */

static const double

tiny =  1e-300,
half =  5.00000000000000000000e-01, /* 0x3FE00000, 0x00000000 */
one  =  1.00000000000000000000e+00, /* 0x3FF00000, 0x00000000 */
two  =  2.00000000000000000000e+00, /* 0x40000000, 0x00000000 */

erx  =  8.45062911510467529297e-01, /* 0x3FEB0AC1, 0x60000000 */
/*
 * Coefficients for approximation to  erf on [0,0.84375]
 */
efx  =  1.28379167095512586316e-01, /* 0x3FC06EBA, 0x8214DB69 */
efx8 =  1.02703333676410069053e+00, /* 0x3FF06EBA, 0x8214DB69 */
pp0  =  1.28379167095512558561e-01, /* 0x3FC06EBA, 0x8214DB68 */
pp1  = -3.25042107247001499370e-01, /* 0xBFD4CD7D, 0x691CB913 */
pp2  = -2.84817495755985104766e-02, /* 0xBF9D2A51, 0xDBD7194F */
pp3  = -5.77027029648944159157e-03, /* 0xBF77A291, 0x236668E4 */
pp4  = -2.37630166566501626084e-05, /* 0xBEF8EAD6, 0x120016AC */
qq1  =  3.97917223959155352819e-01, /* 0x3FD97779, 0xCDDADC09 */
qq2  =  6.50222499887672944485e-02, /* 0x3FB0A54C, 0x5536CEBA */
qq3  =  5.08130628187576562776e-03, /* 0x3F74D022, 0xC4D36B0F */
qq4  =  1.32494738004321644526e-04, /* 0x3F215DC9, 0x221C1A10 */
qq5  = -3.96022827877536812320e-06, /* 0xBED09C43, 0x42A26120 */
/*
 * Coefficients for approximation to  erf  in [0.84375,1.25]
 */
pa0  = -2.36211856075265944077e-03, /* 0xBF6359B8, 0xBEF77538 */
pa1  =  4.14856118683748331666e-01, /* 0x3FDA8D00, 0xAD92B34D */
pa2  = -3.72207876035701323847e-01, /* 0xBFD7D240, 0xFBB8C3F1 */
pa3  =  3.18346619901161753674e-01, /* 0x3FD45FCA, 0x805120E4 */
pa4  = -1.10894694282396677476e-01, /* 0xBFBC6398, 0x3D3E28EC */
pa5  =  3.54783043256182359371e-02, /* 0x3FA22A36, 0x599795EB */
pa6  = -2.16637559486879084300e-03, /* 0xBF61BF38, 0x0A96073F */
qa1  =  1.06420880400844228286e-01, /* 0x3FBB3E66, 0x18EEE323 */
qa2  =  5.40397917702171048937e-01, /* 0x3FE14AF0, 0x92EB6F33 */
qa3  =  7.18286544141962662868e-02, /* 0x3FB2635C, 0xD99FE9A7 */
qa4  =  1.26171219808761642112e-01, /* 0x3FC02660, 0xE763351F */
qa5  =  1.36370839120290507362e-02, /* 0x3F8BEDC2, 0x6B51DD1C */
qa6  =  1.19844998467991074170e-02, /* 0x3F888B54, 0x5735151D */
/*
 * Coefficients for approximation to  erfc in [1.25,1/0.35]
 */
ra0  = -9.86494403484714822705e-03, /* 0xBF843412, 0x600D6435 */
ra1  = -6.93858572707181764372e-01, /* 0xBFE63416, 0xE4BA7360 */
ra2  = -1.05586262253232909814e+01, /* 0xC0251E04, 0x41B0E726 */
ra3  = -6.23753324503260060396e+01, /* 0xC04F300A, 0xE4CBA38D */
ra4  = -1.62396669462573470355e+02, /* 0xC0644CB1, 0x84282266 */
ra5  = -1.84605092906711035994e+02, /* 0xC067135C, 0xEBCCABB2 */
ra6  = -8.12874355063065934246e+01, /* 0xC0545265, 0x57E4D2F2 */
ra7  = -9.81432934416914548592e+00, /* 0xC023A0EF, 0xC69AC25C */
sa1  =  1.96512716674392571292e+01, /* 0x4033A6B9, 0xBD707687 */
sa2  =  1.37657754143519042600e+02, /* 0x4061350C, 0x526AE721 */
sa3  =  4.34565877475229228821e+02, /* 0x407B290D, 0xD58A1A71 */
sa4  =  6.45387271733267880336e+02, /* 0x40842B19, 0x21EC2868 */
sa5  =  4.29008140027567833386e+02, /* 0x407AD021, 0x57700314 */
sa6  =  1.08635005541779435134e+02, /* 0x405B28A3, 0xEE48AE2C */
sa7  =  6.57024977031928170135e+00, /* 0x401A47EF, 0x8E484A93 */
sa8  = -6.04244152148580987438e-02, /* 0xBFAEEFF2, 0xEE749A62 */
/*
 * Coefficients for approximation to  erfc in [1/.35,28]
 */
rb0  = -9.86494292470009928597e-03, /* 0xBF843412, 0x39E86F4A */
rb1  = -7.99283237680523006574e-01, /* 0xBFE993BA, 0x70C285DE */
rb2  = -1.77579549177547519889e+01, /* 0xC031C209, 0x555F995A */
rb3  = -1.60636384855821916062e+02, /* 0xC064145D, 0x43C5ED98 */
rb4  = -6.37566443368389627722e+02, /* 0xC083EC88, 0x1375F228 */
rb5  = -1.02509513161107724954e+03, /* 0xC0900461, 0x6A2E5992 */
rb6  = -4.83519191608651397019e+02, /* 0xC07E384E, 0x9BDC383F */
sb1  =  3.03380607434824582924e+01, /* 0x403E568B, 0x261D5190 */
sb2  =  3.25792512996573918826e+02, /* 0x40745CAE, 0x221B9F0A */
sb3  =  1.53672958608443695994e+03, /* 0x409802EB, 0x189D5118 */
sb4  =  3.19985821950859553908e+03, /* 0x40A8FFB7, 0x688C246A */
sb5  =  2.55305040643316442583e+03, /* 0x40A3F219, 0xCEDF3BE6 */
sb6  =  4.74528541206955367215e+02, /* 0x407DA874, 0xE79FE763 */
sb7  = -2.24409524465858183362e+01; /* 0xC03670E2, 0x42712D62 */

static double ighmm_erf (double x) {
  double R,S,P,Q,s,y,z,r;
  double ax = fabs(x);
#ifdef HAVE_IEEE754
  if (!isfinite(x)) {
    if (isnan(x)) return x;     /* erf(nan)=nan */
    return (x==ax) ? 1 : -1;	/* erf(+-inf)=+-1 */
  }
#endif
  if (ax < 0.84375) {		/* |x|<0.84375 */
    if (ax < 3.7252903e-9) { 	/* |x|<2**-28 */
      if (ax < tiny)
	return 0.125*(8.0*x+efx8*x);  /*avoid underflow */
      return x + efx*x;
    }
    z = x*x;
    r = pp0+z*(pp1+z*(pp2+z*(pp3+z*pp4)));
    s = one+z*(qq1+z*(qq2+z*(qq3+z*(qq4+z*qq5))));
    y = r/s;
    return x + x*y;
  }
  if (ax < 1.25) {			/* 0.84375 <= |x| < 1.25 */
    s = ax-one;
    P = pa0+s*(pa1+s*(pa2+s*(pa3+s*(pa4+s*(pa5+s*pa6)))));
    Q = one+s*(qa1+s*(qa2+s*(qa3+s*(qa4+s*(qa5+s*qa6)))));
    if (x>=0) return erx + P/Q;
    else return -erx - P/Q;
  }
  if (ax >= 6.0) {		/* inf>|x|>=6 */
    if (x>=0) return one-tiny;
    else return tiny-one;
  }
  s = one/(x*x);
  if (ax < 2.857142857) {	/* |x| < 1/0.35 */
    R=ra0+s*(ra1+s*(ra2+s*(ra3+s*(ra4+s*(ra5+s*(ra6+s*ra7))))));
    S=one+s*(sa1+s*(sa2+s*(sa3+s*(sa4+s*(sa5+s*(sa6+s*(sa7+s*sa8)))))));
  }
  else {		/* |x| >= 1/0.35 */
    R=rb0+s*(rb1+s*(rb2+s*(rb3+s*(rb4+s*(rb5+s*rb6)))));
    S=one+s*(sb1+s*(sb2+s*(sb3+s*(sb4+s*(sb5+s*(sb6+s*sb7))))));
  }
  z  = (double)(float) ax;
  r  = exp(-z*z-0.5625)*exp((z-ax)*(z+ax)+R/S);
  if (x>=0) return one-r/ax;
  else return  r/ax-one;
}

static double ighmm_erfc (double x) {
  double R,S,P,Q,s,y,z,r;
  double ax = fabs(x);
#ifdef HAVE_IEEE754
  if (!isfinite(x)) {
    if (isnan(x)) return x;     /* erfc(nan)=nan */
    return (x==ax) ? 0 : 2;	/* erfc(+-inf)=0,2 */
  }
#endif
  if (ax < 0.84375) {		/* |x|<0.84375 */
    if (ax < 13.8777878e-18)  	/* |x|<2**-56 */
      return one-x;
    z = x*x;
    r = pp0+z*(pp1+z*(pp2+z*(pp3+z*pp4)));
    s = one+z*(qq1+z*(qq2+z*(qq3+z*(qq4+z*qq5))));
    y = r/s;
    if (ax < 0.25) {  		/* x<1/4 */
      return one-(x+x*y);
    }
    else {
      r = x*y;
      r += (x-half);
      return half - r ;
    }
  }
  if (ax < 1.25) {			/* 0.84375 <= |x| < 1.25 */
    s = fabs(x)-one;
    P = pa0+s*(pa1+s*(pa2+s*(pa3+s*(pa4+s*(pa5+s*pa6)))));
    Q = one+s*(qa1+s*(qa2+s*(qa3+s*(qa4+s*(qa5+s*qa6)))));
    if (x>=0) {
      z  = one-erx; return z - P/Q;
    }
    else {
      z = erx+P/Q; return one+z;
    }
  }
  if (ax < 28.0) {		/* |x|<28 */
    s = one/(x*x);
    if (ax < 2.857142857) {	/* |x| < 1/.35 ~ 2.857143*/
      R=ra0+s*(ra1+s*(ra2+s*(ra3+s*(ra4+s*(ra5+s*(ra6+s*ra7))))));
      S=one+s*(sa1+s*(sa2+s*(sa3+s*(sa4+s*(sa5+s*(sa6+s*(sa7+s*sa8)))))));
    }
    else {			/* |x| >= 1/.35 ~ 2.857143 */
      if (x < -6.0) return two-tiny;/* x < -6 */
      R=rb0+s*(rb1+s*(rb2+s*(rb3+s*(rb4+s*(rb5+s*rb6)))));
      S=one+s*(sb1+s*(sb2+s*(sb3+s*(sb4+s*(sb5+s*(sb6+s*sb7))))));
    }
    z  = (double)(float) ax;
    r  = exp(-z*z-0.5625)*exp((z-ax)*(z+ax)+R/S);
    if (x>0) return r/ax;
    else return two-r/ax;
  }
  else {
    if(x>0) return tiny*tiny;
    else return two-tiny;
  }
}
#endif /* check for ISO C99 */


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

#pragma once

#ifndef type_3f1a28e2_0da6_44b6_84db_29542f5a65c0_h
#define type_3f1a28e2_0da6_44b6_84db_29542f5a65c0_h

#include <utility>
#include <gslib/basetype.h>
#include <gslib/math.h>

__gslib_begin__

template<class _ty, class _protopt>
struct point_t:
    public _protopt
{
    typedef _ty type;
    typedef _protopt proto;
    typedef point_t<_ty, _protopt> myref;

public:
    point_t() { this->x = 0; this->y = 0; }
    point_t(const proto& p): proto(p) {}
    point_t(type a, type b) { this->x = a; this->y = b; }
    void offset(type u, type v) { this->x += u; this->y += v; }
    void offset(const proto& p) { this->x += p.x; this->y += p.y; }
    void set_point(type a, type b) { this->x = a; this->y = b; }
    bool operator == (const myref& that) const { return this->x == that.x && this->y == that.y; }
    bool operator != (const myref& that) const { return this->x != that.x || this->y != that.y; }
};

struct vec2i { int x, y; };
typedef point_t<int, vec2i> point;
typedef point_t<float, vec2> pointf;

template<class _ty, class _ptcls>
struct rect_t
{
    typedef _ty type;
    typedef rect_t<_ty, _ptcls> myref;
    typedef _ptcls point;

public:
    type    left, top, right, bottom;

public:
    rect_t()
    {
        left = 0;
        top = 0;
        right = 0;
        bottom = 0;
    }
    rect_t(type l, type t, type w, type h) { set_rect(l, t, w, h); }
    type width() const { return right - left; }
    type height() const { return bottom - top; }
    void set_rect(type l, type t, type w, type h)
    {
        left = l;
        top = t;
        right = l + w;
        bottom = t + h;
    }
    void set_ltrb(type l, type t, type r, type b)
    {
        left = l;
        top = t;
        right = r;
        bottom = b;
    }
    void set_by_pts(const point& p1, const point& p2)
    {
        left = gs_min(p1.x, p2.x);
        top = gs_min(p1.y, p2.y);
        right = gs_max(p1.x, p2.x);
        bottom = gs_max(p1.y, p2.y);
    }
    bool in_rect(const point& pt) const { return pt.x >= left && pt.x < right && pt.y >= top && pt.y < bottom; }
    void offset(type x, type y) { left += x; right += x; top += y; bottom += y; }
    void deflate(type u, type v);
    void move_to(const point& pt) { move_to(pt.x, pt.y); }
    void move_to(type x, type y);
    bool operator == (const myref& that) const { return left == that.left && right == that.right && top == that.top && bottom == that.bottom; }
    bool operator != (const myref& that) const { return left != that.left || right != that.right || top != that.top || bottom != that.bottom; }
    type area() const { return width() * height(); }
    point center() const
    {
        point c;
        c.x = (left + right) / 2;
        c.y = (top + bottom) / 2;
        return c;
    }
    point top_left() const { return point(left, top); }
    point top_right() const { return point(right, top); }
    point bottom_left() const { return point(left, bottom); }
    point bottom_right() const { return point(right, bottom); }
};

typedef rect_t<int, point> rect;
typedef rect_t<float, pointf> rectf;

inline rectf to_rectf(const rect& rc)
{
    return std::move(rectf((float)rc.left, (float)rc.top, (float)rc.width(), (float)rc.height()));
}

inline rect to_aligned_rect(const rectf& rc)
{
    return std::move(rect(round(rc.left), round(rc.top), round(rc.width()), round(rc.height())));
}

gs_export extern bool intersect_rect(rect& rc, const rect& rc1, const rect& rc2);
gs_export extern bool is_rect_intersected(const rect& rc1, const rect& rc2);
gs_export extern void union_rect(rect& rc, const rect& rc1, const rect& rc2);
gs_export extern bool substract_rect(rect& rc, const rect& rc1, const rect& rc2);
gs_export extern bool intersect_rect(rectf& rc, const rectf& rc1, const rectf& rc2);
gs_export extern bool is_rect_intersected(const rectf& rc1, const rectf& rc2);
gs_export extern bool is_rect_contained(const rect& rc1, const rect& rc2);
gs_export extern bool is_rect_contained(const rectf& rc1, const rectf& rc2);
gs_export extern void union_rect(rectf& rc, const rectf& rc1, const rectf& rc2);
gs_export extern bool substract_rect(rectf& rc, const rectf& rc1, const rectf& rc2);
gs_export extern bool is_line_rect_overlapped(const point& p1, const point& p2, const rect& rc);
gs_export extern bool is_line_rect_overlapped(const pointf& p1, const pointf& p2, const rectf& rc);

__gslib_end__

#endif


/*
This file is a part of NNTL project (https://github.com/Arech/nntl)

Copyright (c) 2015-2021, Arech (aradvert@gmail.com; https://github.com/Arech)
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.

* Neither the name of NNTL 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.
*/
#pragma once

#include <cblas.h>
//TODO: function definitions (like dgemm()) conflicts with similar function definitions in ACML. It builds successfully, but
//links to wrong library and access violation happens in run-time.

#include <complex>
#define lapack_complex_float ::std::complex<float>
#define lapack_complex_double ::std::complex<double>
#include <lapacke.h>

//#include "../../../utils/denormal_floats.h"

#pragma comment(lib,"libopenblas.dll.a")

//what else to do to use OpenBLAS:
// -in the Solution's VC++ Directories property page set parameter Library Directories to point to a folder with the libopenblas.dll.a file
// -copy correct libopenblas.dll and another dlls that the libopenblas.dll require to the debug/release solution's folder
// -if you're going to use any calling convetion except for __cdecl, then most likely, you'll have to update function declarations
//		within the cblas.h and other blas's .h files included to contain the __cdecl keyword (it's absent for some reason).
//		Check the b_OpenBLAS:: methods to find out which function definitions should be changed.

//http://www.christophlassner.de/using-blas-from-c-with-row-major-data.html

// BTW: lda,ldb,ldc is a "major stride". The stride represents the distance in memory between elements in adjacent rows
// (if row-major) or in adjacent columns (if column-major). This means that the stride is usually equal to the number
// of rows/columns in the matrix.
// Matrix A = [1 2 3]
//            [4 5 6]
// Row-major stores values as {1,2,3,4,5,6} Stride here is 3
// Col-major stores values as {1,4,2,5,3,6} Stride here is 2
// (https://www.physicsforums.com/threads/understanding-blas-dgemm-in-c.543110/)

namespace nntl {
namespace math {

	// wrapper around BLAS API. Should at least isolate from double/float differences
	// Also, we are going to use ColMajor ordering in all math libraries (most of them use it by default)
	// EXPECTING data to be in COL-MAJOR mode!
	//
	// NB: Leading dimension is the number of elements in major dimension. We're using Col-Major ordering,
	// therefore it is the number of ROWs of a matrix
	struct b_OpenBLAS {
	private:
		typedef utils::_scoped_restore_FPU _restoreFPU;
	public:
		//TODO: beware that sz_t type used as substitution of blasint can overflow blasint and silencing conversion warnings here can make it difficult to debug!
		//TODO: May be there should be some preliminary check for this condition.

		//////////////////////////////////////////////////////////////////////////
		//////////////////////////////////////////////////////////////////////////
		// LEVEL 1
		// AXPY y=a*x+y
		template<typename sz_t, typename fl_t>
		static typename ::std::enable_if_t< ::std::is_same< ::std::remove_pointer_t<fl_t>, double>::value > axpy(
			const sz_t n, const fl_t alpha, const fl_t *x, const sz_t incx, fl_t *y, const sz_t incy)
		{
			_restoreFPU r;
			cblas_daxpy(static_cast<blasint>(n), alpha, x, static_cast<blasint>(incx), y, static_cast<blasint>(incy));
		}
		template<typename sz_t, typename fl_t>
		static typename ::std::enable_if_t< ::std::is_same< ::std::remove_pointer_t<fl_t>, float>::value > axpy(
			const sz_t n, const fl_t alpha, const fl_t *x, const sz_t incx, fl_t *y, const sz_t incy)
		{
			_restoreFPU r;
			cblas_saxpy(static_cast<blasint>(n), alpha, x, static_cast<blasint>(incx), y, static_cast<blasint>(incy));
		}

		//////////////////////////////////////////////////////////////////////////
		// cblas_?dot
		// Computes a vector - vector dot product.
		//	Input Parameters
		// n - Specifies the number of elements in vectors x and y.
		// x - Array, size at least(1 + (n - 1)*abs(incx)).
		// incx - Specifies the increment for the elements of x.
		// y - Array, size at least(1 + (n - 1)*abs(incy)).
		// incy - Specifies the increment for the elements of y.
		// Return Values - The result of the dot product of x and y, if n is positive.Otherwise, returns 0.
		//https://software.intel.com/en-us/mkl-developer-reference-c-cblas-dot
		template<typename sz_t, typename fl_t>
		static typename ::std::enable_if_t< ::std::is_same< ::std::remove_pointer_t<fl_t>, double>::value, double >
			dot(const sz_t n, const fl_t *x, const sz_t incx, const fl_t *y, const sz_t incy)
		{
			_restoreFPU r;
			return cblas_ddot(static_cast<blasint>(n), x, static_cast<blasint>(incx), y, static_cast<blasint>(incy));
		}
		template<typename sz_t, typename fl_t>
		static typename ::std::enable_if_t< ::std::is_same< ::std::remove_pointer_t<fl_t>, float>::value, float >
			dot(const sz_t n, const fl_t *x, const sz_t incx, const fl_t *y, const sz_t incy)
		{
			_restoreFPU r;
			return cblas_sdot(static_cast<blasint>(n), x, static_cast<blasint>(incx), y, static_cast<blasint>(incy));
		}

		//////////////////////////////////////////////////////////////////////////
		//////////////////////////////////////////////////////////////////////////
		// LEVEL 2

		//////////////////////////////////////////////////////////////////////////
		//////////////////////////////////////////////////////////////////////////
		// LEVEL 3
		//
		//////////////////////////////////////////////////////////////////////////
		// General matrix multiplication
		// GEMM C := alpha*op(A)*op(B) + beta*C
		// https://software.intel.com/en-us/node/520775
		// where:
		// op(X) is one of op(X) = X, or op(X) = XT, or op(X) = XH,
		// alpha and beta are scalars,
		// A, B and C are matrices :
		// op(A) is an m - by - k matrix,
		// op(B) is a k - by - n matrix,
		// C is an m - by - n matrix.
		//
		// M - Specifies the number of rows of the matrix op(A) and of the matrix C. The value of m must be at least zero.
		// N - Specifies the number of columns of the matrix op(B) and the number of columns of the matrix C. The value of n must be at least zero.
		// K - Specifies the number of columns of the matrix op(A) and the number of rows of the matrix op(B). The value of k must be at least zero.
		// A - {transa=CblasNoTrans : Array, size lda*k. Before entry, the leading m-by-k part of the array a must contain the matrix A.
		//		transa=CblasTrans : Array, size lda*m. Before entry, the leading k-by-m part of the array a must contain the matrix A.}
		// lda - Specifies the leading dimension of A as declared in the calling (sub)program.
		//		{transa=CblasNoTrans, lda must be at least max(1, m).
		//		transa=CblasTrans, lda must be at least max(1, k)}
		// B - {transb=CblasNoTrans : Array, size ldb by n. Before entry, the leading k-by-n part of the array b must contain the matrix B.
		//		transb=CblasTrans : Array, size ldb by k. Before entry the leading n-by-k part of the array b must contain the matrix B.}
		// ldb - Specifies the leading dimension of B as declared in the calling (sub)program.
		//		{transb = CblasNoTrans : ldb must be at least max(1, k).
		//		transb=CblasTrans : ldb must be at least max(1, n).}
		// C - Array, size ldc by n. Before entry, the leading m-by-n part of the array c must contain the matrix C, except when beta is
		//		equal to zero, in which case c need not be set on entry.
		// ldc - Specifies the leading dimension of c as declared in the calling (sub)program. ldc must be at least max(1, m).
		template<typename sz_t, typename fl_t>
		static typename ::std::enable_if_t< ::std::is_same< ::std::remove_pointer_t<fl_t>, double>::value >
			gemm( //const enum CBLAS_TRANSPOSE TransA, const enum CBLAS_TRANSPOSE TransB,
			const bool bTransposeA, const bool bTransposeB,
			const sz_t M, const sz_t N, const sz_t K, const fl_t alpha, const fl_t *A, const sz_t lda,
			const fl_t *B, const sz_t ldb, const fl_t beta, fl_t *C, const sz_t ldc)
		{
			_restoreFPU r;
			cblas_dgemm(CblasColMajor, bTransposeA ? CblasTrans : CblasNoTrans, bTransposeB ? CblasTrans : CblasNoTrans,
				static_cast<blasint>(M), static_cast<blasint>(N), static_cast<blasint>(K),
				alpha, A, static_cast<blasint>(lda), B, static_cast<blasint>(ldb), beta, C, static_cast<blasint>(ldc));
			//global_denormalized_floats_mode();
		}
		template<typename sz_t, typename fl_t>
		static typename ::std::enable_if_t< ::std::is_same< ::std::remove_pointer_t<fl_t>, float>::value >
			gemm( //const enum CBLAS_TRANSPOSE TransA, const enum CBLAS_TRANSPOSE TransB,
			const bool bTransposeA, const bool bTransposeB,
			const sz_t M, const sz_t N, const sz_t K, const fl_t alpha, const fl_t *A, const sz_t lda,
			const fl_t *B, const sz_t ldb, const fl_t beta, fl_t *C, const sz_t ldc)
		{
			_restoreFPU r;
			cblas_sgemm(CblasColMajor, bTransposeA ? CblasTrans: CblasNoTrans, bTransposeB ? CblasTrans : CblasNoTrans,
				static_cast<blasint>(M), static_cast<blasint>(N), static_cast<blasint>(K),
				alpha, A, static_cast<blasint>(lda), B, static_cast<blasint>(ldb), beta, C, static_cast<blasint>(ldc));
			//global_denormalized_floats_mode();
		}

		//////////////////////////////////////////////////////////////////////////
		// cblas_?syrk, https://software.intel.com/en-us/node/520780
		// Performs a symmetric rank-k update.
		// The ?syrk routines perform a rank-k matrix-matrix operation for a symmetric matrix C using a general matrix A.
		// The operation is defined as:
		// C := alpha*A*A' + beta*C,
		//		or
		// C : = alpha*A'*A + beta*C,
		// where :
		//		alpha and beta are scalars,
		//		C is an n-by-n symmetric matrix,
		//		A is an n-by-k matrix in the first case and a k-by-n matrix in the second case.
		template<typename sz_t, typename fl_t>
		static typename ::std::enable_if_t< ::std::is_same< ::std::remove_pointer_t<fl_t>, double>::value >
			syrk( const bool bCLowerTriangl, const bool bFirstATransposed, const sz_t N, const sz_t K, const fl_t alpha
				, const fl_t *A, const sz_t lda, const fl_t beta, fl_t *C, const sz_t ldc)
		{
			_restoreFPU r;
			cblas_dsyrk(CblasColMajor, bCLowerTriangl ? CblasLower : CblasUpper, bFirstATransposed ? CblasTrans : CblasNoTrans
				, static_cast<blasint>(N), static_cast<blasint>(K), alpha, A, static_cast<blasint>(lda), beta, C, static_cast<blasint>(ldc));
			//global_denormalized_floats_mode();
		}
		template<typename sz_t, typename fl_t>
		static typename ::std::enable_if_t< ::std::is_same< ::std::remove_pointer_t<fl_t>, float>::value >
			syrk(const bool bCLowerTriangl, const bool bFirstATransposed, const sz_t N, const sz_t K, const fl_t alpha
				, const fl_t *A, const sz_t lda, const fl_t beta, fl_t *C, const sz_t ldc)
		{
			_restoreFPU r;
			cblas_ssyrk(CblasColMajor, bCLowerTriangl ? CblasLower : CblasUpper, bFirstATransposed ? CblasTrans : CblasNoTrans
				, static_cast<blasint>(N), static_cast<blasint>(K), alpha, A, static_cast<blasint>(lda), beta, C, static_cast<blasint>(ldc));
			//global_denormalized_floats_mode();
		}

		//////////////////////////////////////////////////////////////////////////
		// cblas_?symm, https://software.intel.com/en-us/node/520779
		// Computes a matrix - matrix product where one input matrix is symmetric.
		// The ?symm routines compute a scalar-matrix-matrix product with one symmetric matrix and add the
		// result to a scalar-matrix product. The operation is defined as
		// C: = alpha*A*B + beta*C,
		//		or
		// C : = alpha*B*A + beta*C,
		// where :
		//		alpha and beta are scalars,
		//		A is a symmetric matrix,
		//		B and C are m - by - n matrices.
		template<typename sz_t, typename fl_t>
		static typename ::std::enable_if_t< ::std::is_same< ::std::remove_pointer_t<fl_t>, double>::value >
			symm(const bool bSymmAatLeft, const bool bALowerTriangl, const sz_t M, const sz_t N, const fl_t alpha
				, const fl_t *A, const sz_t lda, const fl_t *B, const sz_t ldb, const fl_t beta, fl_t *C, const sz_t ldc)
		{
			_restoreFPU r;
			cblas_dsymm(CblasColMajor, bSymmAatLeft ? CblasLeft : CblasRight, bALowerTriangl ? CblasLower : CblasUpper
				, static_cast<blasint>(M), static_cast<blasint>(N), alpha, A, static_cast<blasint>(lda)
				, B, static_cast<blasint>(ldb), beta, C, static_cast<blasint>(ldc));
			//global_denormalized_floats_mode();
		}
		template<typename sz_t, typename fl_t>
		static typename ::std::enable_if_t< ::std::is_same< ::std::remove_pointer_t<fl_t>, float>::value >
			symm(const bool bSymmAatLeft, const bool bALowerTriangl, const sz_t M, const sz_t N, const fl_t alpha
				, const fl_t *A, const sz_t lda, const fl_t *B, const sz_t ldb, const fl_t beta, fl_t *C, const sz_t ldc)
		{
			_restoreFPU r;
			cblas_ssymm(CblasColMajor, bSymmAatLeft ? CblasLeft : CblasRight, bALowerTriangl ? CblasLower : CblasUpper
				, static_cast<blasint>(M), static_cast<blasint>(N), alpha, A, static_cast<blasint>(lda)
				, B, static_cast<blasint>(ldb), beta, C, static_cast<blasint>(ldc));
			//global_denormalized_floats_mode();
		}

		//////////////////////////////////////////////////////////////////////////
		//////////////////////////////////////////////////////////////////////////
		// LAPACKE

		// ?gesvd
		// https://software.intel.com/en-us/node/521150
		template<typename sz_t, typename fl_t>
		static typename ::std::enable_if_t< ::std::is_same< ::std::remove_pointer_t<fl_t>, double>::value, int>
			gesvd(/*int matrix_layout,*/ const char jobu, const char jobvt,
				const sz_t m, const sz_t n, fl_t* A,
				const sz_t lda, fl_t* S, fl_t* U, const sz_t ldu,
				fl_t* Vt, const sz_t ldvt, fl_t* superb)
		{
			_restoreFPU r;
			return static_cast<int>(LAPACKE_dgesvd(LAPACK_COL_MAJOR, jobu, jobvt, static_cast<lapack_int>(m), static_cast<lapack_int>(n)
				, A, static_cast<lapack_int>(lda), S, U, static_cast<lapack_int>(ldu), Vt, static_cast<lapack_int>(ldvt), superb));
		}
		template<typename sz_t, typename fl_t>
		static typename ::std::enable_if_t< ::std::is_same< ::std::remove_pointer_t<fl_t>, float>::value, int>
			gesvd(/*int matrix_layout,*/ const char jobu, const char jobvt,
				const sz_t m, const sz_t n, fl_t* A,
				const sz_t lda, fl_t* S, fl_t* U, const sz_t ldu,
				fl_t* Vt, const sz_t ldvt, fl_t* superb)
		{
			_restoreFPU r;
			return static_cast<int>(LAPACKE_sgesvd(LAPACK_COL_MAJOR, jobu, jobvt, static_cast<lapack_int>(m), static_cast<lapack_int>(n)
				, A, static_cast<lapack_int>(lda), S, U, static_cast<lapack_int>(ldu), Vt, static_cast<lapack_int>(ldvt), superb));
		}

		//////////////////////////////////////////////////////////////////////////
		//////////////////////////////////////////////////////////////////////////
		// Extensions

		// The omatcopy routine performs scaling and out-of-place transposition/copying of matrices. A transposition
		// operation can be a normal matrix copy, a transposition, a conjugate transposition, or just a conjugation.
		// The operation is defined as follows:
		// B : = alpha*op(A)
		//
		// Parameters
		// rows - The number of rows in the source matrix.
		// cols - The number of columns in the source matrix.
		// alpha - This parameter scales the input matrix by alpha.
		// pA - Array.
		// lda - Distance between the first elements in adjacent columns(in the case of the column - major order)
		//		or rows(in the case of the row - major order) in the source matrix; measured in the number of elements.
		//		This parameter must be at least max(1, rows) if ordering = 'C' or 'c', and max(1, cols) otherwise.
		// b - Array.
		// ldb - Distance between the first elements in adjacent columns(in the case of the column - major order)
		//		or rows(in the case of the row - major order) in the destination matrix; measured in the number of elements.
		//		To determine the minimum value of ldb on output, consider the following guideline :
		//		If ordering = 'C' or 'c', then
		//			If trans = 'T' or 't' or 'C' or 'c', this parameter must be at least max(1, cols)
		//			If trans = 'N' or 'n' or 'R' or 'r', this parameter must be at least max(1, rows)
		//		If ordering = 'R' or 'r', then
		//			If trans = 'T' or 't' or 'C' or 'c', this parameter must be at least max(1, rows)
		//			If trans = 'N' or 'n' or 'R' or 'r', this parameter must be at least max(1, cols)
		//
		// #warning current OpenBLAS implementation is slower, than it can be. See TEST(TestPerfDecisions, mTranspose) in test_perf_decisions.cpp
		//and https://github.com/xianyi/OpenBLAS/issues/1243
		// https://github.com/xianyi/OpenBLAS/issues/2532
		// https://stackoverflow.com/questions/16737298/what-is-the-fastest-way-to-transpose-a-matrix-in-c/16743203#16743203
		template<typename sz_t, typename fl_t>
		static typename ::std::enable_if_t< ::std::is_same< ::std::remove_pointer_t<fl_t>, double>::value >
			omatcopy(const bool bTranspose, const sz_t rows, const sz_t cols, const fl_t alpha,
				const fl_t* pA, const sz_t lda, fl_t* pB, const sz_t ldb)
		{
			_restoreFPU r;
			cblas_domatcopy(CblasColMajor, bTranspose ? CblasTrans : CblasNoTrans,
				static_cast<blasint>(rows), static_cast<blasint>(cols), alpha,
				pA, static_cast<blasint>(lda), pB, static_cast<blasint>(ldb));
		}
		template<typename sz_t, typename fl_t>
		static typename ::std::enable_if_t< ::std::is_same< ::std::remove_pointer_t<fl_t>, float>::value >
			omatcopy(const bool bTranspose, const sz_t rows, const sz_t cols, const fl_t alpha,
				const fl_t* pA, const sz_t lda, fl_t* pB, const sz_t ldb)
		{
			_restoreFPU r;
			cblas_somatcopy(CblasColMajor, bTranspose ? CblasTrans : CblasNoTrans,
				static_cast<blasint>(rows), static_cast<blasint>(cols), alpha,
				pA, static_cast<blasint>(lda), pB, static_cast<blasint>(ldb));
		}

		template<typename sz_t, typename fl_t>
		static typename ::std::enable_if_t< ::std::is_same< ::std::remove_pointer_t<fl_t>, double>::value >
			imatcopy(const bool bTranspose, const sz_t rows, const sz_t cols, const fl_t alpha,
				fl_t* pA, const sz_t lda, const sz_t ldb)
		{
			_restoreFPU r;
			cblas_dimatcopy(CblasColMajor, bTranspose ? CblasTrans : CblasNoTrans,
				static_cast<blasint>(rows), static_cast<blasint>(cols), alpha,
				pA, static_cast<blasint>(lda), static_cast<blasint>(ldb));
		}
		template<typename sz_t, typename fl_t>
		static typename ::std::enable_if_t< ::std::is_same< ::std::remove_pointer_t<fl_t>, float>::value >
			imatcopy(const bool bTranspose, const sz_t rows, const sz_t cols, const fl_t alpha,
				fl_t* pA, const sz_t lda, const sz_t ldb)
		{
			_restoreFPU r;
			cblas_simatcopy(CblasColMajor, bTranspose ? CblasTrans : CblasNoTrans,
				static_cast<blasint>(rows), static_cast<blasint>(cols), alpha,
				pA, static_cast<blasint>(lda), static_cast<blasint>(ldb));
		}
	};

}
}


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

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

#include "endian.c"
#include "standardize.c"

static void sprint_nybble(int i, char *s) ;
static void sprint_byte(int i, char *s) ;
static int determine_ieee_type (int non_zero, int exponent, int max_exponent);

/* For the IEEE float format the bits are found from the following
   masks,

   sign      = 0x80000000
   exponent  = 0x7f800000
   mantisssa = 0x007fffff

   For the IEEE double format the masks are,

   sign      = 0x8000000000000000
   exponent  = 0x7ff0000000000000
   mantissa  = 0x000fffffffffffff

   */

void
gsl_ieee_float_to_rep (const float * x, gsl_ieee_float_rep * r)
{
  int e, non_zero;

  union {
    float f;
    struct  {
      unsigned char byte[4] ;
    } ieee ;
  } u;

  u.f = *x ;

  if (little_endian_p())
    make_float_bigendian(&(u.f)) ;

  /* note that r->sign is signed, u.ieee.byte is unsigned */

  if (u.ieee.byte[3]>>7)
    {
      r->sign = 1 ;
    }
  else
    {
      r->sign = 0 ;
    }

  e = (u.ieee.byte[3] & 0x7f) << 1 | (u.ieee.byte[2] & 0x80)>>7 ;

  r->exponent = e - 127 ;

  sprint_byte((u.ieee.byte[2] & 0x7f) << 1,r->mantissa) ;
  sprint_byte(u.ieee.byte[1],r->mantissa + 7) ;
  sprint_byte(u.ieee.byte[0],r->mantissa + 15) ;

  r->mantissa[23] = '\0' ;

  non_zero = u.ieee.byte[0] || u.ieee.byte[1] || (u.ieee.byte[2] & 0x7f);

  r->type = determine_ieee_type (non_zero, e, 255) ;
}

void
gsl_ieee_double_to_rep (const double * x, gsl_ieee_double_rep * r)
{

  int e, non_zero;

  union
  {
    double d;
    struct  {
      unsigned char byte[8];
    } ieee ;
  } u;

  u.d= *x ;

  if (little_endian_p())
    make_double_bigendian(&(u.d)) ;

  /* note that r->sign is signed, u.ieee.byte is unsigned */

  if (u.ieee.byte[7]>>7)
    {
      r->sign = 1 ;
    }
  else
    {
      r->sign = 0 ;
    }

  e =(u.ieee.byte[7] & 0x7f)<<4 ^ (u.ieee.byte[6] & 0xf0)>>4 ;

  r->exponent = e - 1023 ;

  sprint_nybble(u.ieee.byte[6],r->mantissa) ;
  sprint_byte(u.ieee.byte[5],r->mantissa + 4) ;
  sprint_byte(u.ieee.byte[4],r->mantissa + 12) ;
  sprint_byte(u.ieee.byte[3],r->mantissa + 20) ;
  sprint_byte(u.ieee.byte[2],r->mantissa + 28) ;
  sprint_byte(u.ieee.byte[1],r->mantissa + 36) ;
  sprint_byte(u.ieee.byte[0],r->mantissa + 44) ;

  r->mantissa[52] = '\0' ;

  non_zero = (u.ieee.byte[0] || u.ieee.byte[1] || u.ieee.byte[2]
              || u.ieee.byte[3] || u.ieee.byte[4] || u.ieee.byte[5]
              || (u.ieee.byte[6] & 0x0f)) ;

  r->type = determine_ieee_type (non_zero, e, 2047) ;
}

/* A table of character representations of nybbles */

static char nybble[16][5]={ /* include space for the \0 */
  "0000", "0001", "0010", "0011",
  "0100", "0101", "0110", "0111",
  "1000", "1001", "1010", "1011",
  "1100", "1101", "1110", "1111"
}  ;

static void
sprint_nybble(int i, char *s)
{
  char *c ;
  c=nybble[i & 0x0f ];
  *s=c[0] ;  *(s+1)=c[1] ;  *(s+2)=c[2] ;  *(s+3)=c[3] ;
}

static void
sprint_byte(int i, char *s)
{
  char *c ;
  c=nybble[(i & 0xf0)>>4];
  *s=c[0] ;  *(s+1)=c[1] ;  *(s+2)=c[2] ;  *(s+3)=c[3] ;
  c=nybble[i & 0x0f];
  *(s+4)=c[0] ;  *(s+5)=c[1] ;  *(s+6)=c[2] ;  *(s+7)=c[3] ;
}

static int
determine_ieee_type (int non_zero, int exponent, int max_exponent)
{
  if (exponent == max_exponent)
    {
      if (non_zero)
        {
          return GSL_IEEE_TYPE_NAN ;
        }
      else
        {
          return GSL_IEEE_TYPE_INF ;
        }
    }
  else if (exponent == 0)
    {
      if (non_zero)
        {
          return GSL_IEEE_TYPE_DENORMAL ;
        }
      else
        {
          return GSL_IEEE_TYPE_ZERO ;
        }
    }
  else
    {
      return GSL_IEEE_TYPE_NORMAL ;
    }
}


/* Copyright (C) 2010-2021 Barcelona Supercomputing Center and University of
 * Illinois at Urbana-Champaign
 * SPDX-License-Identifier: MIT
 */

/** \file
 * \brief Wrapper routines for GSL random number functions.
 */

/* clang-format off */

#include <gsl/gsl_errno.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_rng.h>
#include <stdio.h>
#include <time.h>

/** \brief Private internal-use variable to store the random number
 * generator.
 */
static gsl_rng *camp_rand_gsl_rng = NULL;

/** \brief Result code indicating successful completion.
 */
#define CAMP_RAND_GSL_SUCCESS      0
/** \brief Result code indicating initialization failure.
 */
#define CAMP_RAND_GSL_INIT_FAIL    1
/** \brief Result code indicating the generator was not initialized
 * when it should have been.
 */
#define CAMP_RAND_GSL_NOT_INIT     2
/** \brief Result code indicating the generator was already
 * initialized when an initialization was attempted.
 */
#define CAMP_RAND_GSL_ALREADY_INIT 3

/** \brief Initialize the random number generator with the given seed.
 *
 * This must be called before any other GSL random number functions
 * are called.
 *
 * \param seed The random seed to use.
 * \return CAMP_RAND_GSL_SUCCESS on success, otherwise an error code.
 * \sa camp_rand_finalize_gsl() to cleanup the generator.
 */
int camp_srand_gsl(int seed)
{
        if (camp_rand_gsl_rng) {
                return CAMP_RAND_GSL_ALREADY_INIT;
        }
        gsl_set_error_handler_off(); // turn off automatic error handling
        camp_rand_gsl_rng = gsl_rng_alloc(gsl_rng_mt19937);
        if (camp_rand_gsl_rng == NULL) {
                return CAMP_RAND_GSL_INIT_FAIL;
        }
        gsl_rng_set(camp_rand_gsl_rng, seed);
        return CAMP_RAND_GSL_SUCCESS;
}

/** \brief Cleanup and deallocate the random number generator.
 *
 * This must be called after camp_srand_gsl().
 *
 * \return CAMP_RAND_GSL_SUCCESS on success, otherwise an error code.
 */
int camp_rand_finalize_gsl()
{
        if (!camp_rand_gsl_rng) {
                return CAMP_RAND_GSL_NOT_INIT;
        }
        gsl_rng_free(camp_rand_gsl_rng);
        camp_rand_gsl_rng = NULL;
        return CAMP_RAND_GSL_SUCCESS;
}

/** \brief Generate a uniform random number in \f$[0,1)\f$.
 *
 * \param harvest A pointer to the generated random number.
 * \return CAMP_RAND_GSL_SUCCESS on success, otherwise an error code.
 */
int camp_rand_gsl(double *harvest)
{
        if (!camp_rand_gsl_rng) {
                return CAMP_RAND_GSL_NOT_INIT;
        }
        *harvest = gsl_rng_uniform(camp_rand_gsl_rng);
        return CAMP_RAND_GSL_SUCCESS;
}

/** \brief Generate a uniform random integer in \f$[1,n]\f$.
 *
 * \param n The upper limit of the random integer.
 * \param harvest A pointer to the generated random number.
 * \return CAMP_RAND_GSL_SUCCESS on success, otherwise an error code.
 */
int camp_rand_int_gsl(int n, int *harvest)
{
        if (!camp_rand_gsl_rng) {
                return CAMP_RAND_GSL_NOT_INIT;
        }
        *harvest = gsl_rng_uniform_int(camp_rand_gsl_rng, n) + 1;
        return CAMP_RAND_GSL_SUCCESS;
}

/** \brief Generate a normally-distributed random number.
 *
 * \param mean The mean of the distribution.
 * \param stddev The standard deviation of the distribution.
 * \param harvest A pointer to the generated random number.
 * \return CAMP_RAND_GSL_SUCCESS on success, otherwise an error code.
 */
int camp_rand_normal_gsl(double mean, double stddev, double *harvest)
{
        if (!camp_rand_gsl_rng) {
                return CAMP_RAND_GSL_NOT_INIT;
        }
        *harvest = gsl_ran_gaussian(camp_rand_gsl_rng, stddev) + mean;
        return CAMP_RAND_GSL_SUCCESS;
}

/** \brief Generate a Poisson-distributed random integer.
 *
 * \param mean The mean of the distribution.
 * \param harvest A pointer to the generated random number.
 * \return CAMP_RAND_GSL_SUCCESS on success, otherwise an error code.
 */
int camp_rand_poisson_gsl(double mean, int *harvest)
{
        if (!camp_rand_gsl_rng) {
                return CAMP_RAND_GSL_NOT_INIT;
        }
        *harvest = gsl_ran_poisson(camp_rand_gsl_rng, mean);
        return CAMP_RAND_GSL_SUCCESS;
}

/** \brief Generate a Binomial-distributed random integer.
 *
 * \param n The sample size for the distribution.
 * \param p The sample probability for the distribution.
 * \param harvest A pointer to the generated random number.
 * \return CAMP_RAND_GSL_SUCCESS on success, otherwise an error code.
 */
int camp_rand_binomial_gsl(int n, double p, int *harvest)
{
        unsigned int u;

        if (!camp_rand_gsl_rng) {
                return CAMP_RAND_GSL_NOT_INIT;
        }
        u = n;
        *harvest = gsl_ran_binomial(camp_rand_gsl_rng, p, u);
        return CAMP_RAND_GSL_SUCCESS;
}

/* clang-format on */


/* Useful general-purpose functions.
 *
 * Krzysztof Chalupka, 2017.
 */

#include <stdio.h>
#include <gsl/gsl_blas.h>
#include "utils.h"

void crit_err(char *msg){
  fprintf(stderr, "%s, exiting.\n", msg);
  exit(1);
}

int get_nlines(char *fname){
  /* Check how many lines there are in a file. */
  int nlines = 0;
  FILE *f = fopen(fname, "r");
  while(!feof(f))
    if (fgetc(f) == '\n')
      nlines++;
  fclose(f);
  printf("%d lines read from %s.\n\n", nlines, fname);
  return nlines;
}

void print_intarray(int *arr, int n){
  int i;
  printf("[");
  for(i = 0; i < n; i++)
    printf("%d ", arr[i]);
  printf("]");
}

void print_gsl_vector(gsl_vector *vec){
  int i;
  for (i = 0; i < vec->size; i++)
    printf("%.4f ", vec->data[i]);
}

gsl_vector *vector_sub(gsl_vector *v1, gsl_vector *v2){
  /* Create a new vector equal to v1 - v2. */
  gsl_vector *res = gsl_vector_calloc(v1->size);
  gsl_vector_add(res, v1);
  gsl_vector_sub(res, v2);
  return res;
}

double vector_dist(gsl_vector *v1, gsl_vector *v2){
  /* Compute the Euclidean distance between vectors. */
  double dist;
  gsl_vector *tmp =  vector_sub(v1, v2);
  dist = gsl_blas_dnrm2(tmp);
  free(tmp);
  return dist;
}

gsl_vector **load_vectors_from_file(char *fname, int nvecs)
{
  int vec_id;
  FILE *f;
  gsl_vector **vecs;
  double vals[2] = {0., 0.};

  /* Allocate the vector array. */
  vecs = (gsl_vector **) calloc(nvecs, sizeof(gsl_vector *));

  /* Load the vectors from a file into an array. */
  f = fopen(fname, "r");
  vec_id = 0;
  while(fscanf(f,"%lf %lf", &vals[0], &vals[1]) != EOF){
    vecs[vec_id] = gsl_vector_alloc(2);
    gsl_vector_set(vecs[vec_id], 0, vals[0]);
    gsl_vector_set(vecs[vec_id], 1, vals[1]);
    vec_id++;
  }

  /* Clean up. */
  fclose(f);
  printf("Data: \n");
  for (vec_id = 0; vec_id < nvecs; vec_id++)
    printf("[%f, %f]\n",
	   vecs[vec_id]->data[0],
	   vecs[vec_id]->data[1]);
  printf("\n");
  return vecs;
}


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

/* Driver routine for odeiv2. This is a wrapper for low level GSL
   functions that allows a simple interface to step, control and
   evolve layers.
 */

#include <config.h>
#include <math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv2.h>
#include <gsl/gsl_machine.h>

static gsl_odeiv2_driver *
driver_alloc (const gsl_odeiv2_system * sys, const double hstart,
              const gsl_odeiv2_step_type * T)
{
  /* Allocates and initializes an ODE driver system. Step and evolve
     objects are allocated here, but control object is allocated in
     another function.
   */

  gsl_odeiv2_driver *state =
    (gsl_odeiv2_driver *) malloc (sizeof (gsl_odeiv2_driver));

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

  if (sys == NULL)
    {
      GSL_ERROR_NULL ("gsl_odeiv2_system must be defined", GSL_EINVAL);
    }

  {
    const size_t dim = sys->dimension;

    if (dim == 0)
      {
        GSL_ERROR_NULL
          ("gsl_odeiv2_system dimension must be a positive integer",
           GSL_EINVAL);
      }

    state->sys = sys;

    state->s = gsl_odeiv2_step_alloc (T, dim);

    if (state->s == NULL)
      {
        free (state);
        GSL_ERROR_NULL ("failed to allocate step object", GSL_ENOMEM);
      }

    state->e = gsl_odeiv2_evolve_alloc (dim);
  }

  if (state->e == NULL)
    {
      gsl_odeiv2_step_free (state->s);
      free (state);
      GSL_ERROR_NULL ("failed to allocate evolve object", GSL_ENOMEM);
    }

  if (hstart > 0.0 || hstart < 0.0)
    {
      state->h = hstart;
    }
  else
    {
      GSL_ERROR_NULL ("invalid hstart", GSL_EINVAL);
    }

  state->h = hstart;
  state->hmin = 0.0;
  state->hmax = GSL_DBL_MAX;
  state->nmax = 0;
  state->n = 0;
  state->c = NULL;

  return state;
}

int
gsl_odeiv2_driver_set_hmin (gsl_odeiv2_driver * d, const double hmin)
{
  /* Sets minimum allowed step size fabs(hmin) for driver. It is
     required that hmin <= fabs(h) <= hmax. */

  if ((fabs (hmin) > fabs (d->h)) || (fabs (hmin) > d->hmax))
    {
      GSL_ERROR_NULL ("hmin <= fabs(h) <= hmax required", GSL_EINVAL);
    }

  d->hmin = fabs (hmin);

  return GSL_SUCCESS;
}

int
gsl_odeiv2_driver_set_hmax (gsl_odeiv2_driver * d, const double hmax)
{
  /* Sets maximum allowed step size fabs(hmax) for driver. It is
     required that hmin <= fabs(h) <= hmax. */

  if ((fabs (hmax) < fabs (d->h)) || (fabs (hmax) < d->hmin))
    {
      GSL_ERROR_NULL ("hmin <= fabs(h) <= hmax required", GSL_EINVAL);
    }

  if (hmax > 0.0 || hmax < 0.0)
    {
      d->hmax = fabs (hmax);
    }
  else
    {
      GSL_ERROR_NULL ("invalid hmax", GSL_EINVAL);
    }

  return GSL_SUCCESS;
}

int
gsl_odeiv2_driver_set_nmax (gsl_odeiv2_driver * d,
                            const unsigned long int nmax)
{
  /* Sets maximum number of allowed steps (nmax) for driver */

  d->nmax = nmax;

  return GSL_SUCCESS;
}

gsl_odeiv2_driver *
gsl_odeiv2_driver_alloc_y_new (const gsl_odeiv2_system * sys,
                               const gsl_odeiv2_step_type * T,
                               const double hstart,
                               const double epsabs, const double epsrel)
{
  /* Initializes an ODE driver system with control object of type y_new. */

  gsl_odeiv2_driver *state = driver_alloc (sys, hstart, T);

  if (state == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate driver object", GSL_ENOMEM);
    }

  if (epsabs >= 0.0 && epsrel >= 0.0)
    {
      state->c = gsl_odeiv2_control_y_new (epsabs, epsrel);

      if (state->c == NULL)
        {
          gsl_odeiv2_driver_free (state);
          GSL_ERROR_NULL ("failed to allocate control object", GSL_ENOMEM);
        }
    }
  else
    {
      gsl_odeiv2_driver_free (state);
      GSL_ERROR_NULL ("epsabs and epsrel must be positive", GSL_EINVAL);
    }

  /* Distribute pointer to driver object */

  gsl_odeiv2_step_set_driver (state->s, state);
  gsl_odeiv2_evolve_set_driver (state->e, state);
  gsl_odeiv2_control_set_driver (state->c, state);

  return state;
}

gsl_odeiv2_driver *
gsl_odeiv2_driver_alloc_yp_new (const gsl_odeiv2_system * sys,
                                const gsl_odeiv2_step_type * T,
                                const double hstart,
                                const double epsabs, const double epsrel)
{
  /* Initializes an ODE driver system with control object of type yp_new. */

  gsl_odeiv2_driver *state = driver_alloc (sys, hstart, T);

  if (state == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate driver object", GSL_ENOMEM);
    }

  if (epsabs >= 0.0 && epsrel >= 0.0)
    {
      state->c = gsl_odeiv2_control_yp_new (epsabs, epsrel);

      if (state->c == NULL)
        {
          gsl_odeiv2_driver_free (state);
          GSL_ERROR_NULL ("failed to allocate control object", GSL_ENOMEM);
        }
    }
  else
    {
      gsl_odeiv2_driver_free (state);
      GSL_ERROR_NULL ("epsabs and epsrel must be positive", GSL_EINVAL);
    }

  /* Distribute pointer to driver object */

  gsl_odeiv2_step_set_driver (state->s, state);
  gsl_odeiv2_evolve_set_driver (state->e, state);
  gsl_odeiv2_control_set_driver (state->c, state);

  return state;
}

gsl_odeiv2_driver *
gsl_odeiv2_driver_alloc_standard_new (const gsl_odeiv2_system * sys,
                                      const gsl_odeiv2_step_type * T,
                                      const double hstart,
                                      const double epsabs,
                                      const double epsrel, const double a_y,
                                      const double a_dydt)
{
  /* Initializes an ODE driver system with control object of type
     standard_new.
   */

  gsl_odeiv2_driver *state = driver_alloc (sys, hstart, T);

  if (state == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate driver object", GSL_ENOMEM);
    }

  if (epsabs >= 0.0 && epsrel >= 0.0)
    {
      state->c =
        gsl_odeiv2_control_standard_new (epsabs, epsrel, a_y, a_dydt);

      if (state->c == NULL)
        {
          gsl_odeiv2_driver_free (state);
          GSL_ERROR_NULL ("failed to allocate control object", GSL_ENOMEM);
        }
    }
  else
    {
      gsl_odeiv2_driver_free (state);
      GSL_ERROR_NULL ("epsabs and epsrel must be positive", GSL_EINVAL);
    }

  /* Distribute pointer to driver object */

  gsl_odeiv2_step_set_driver (state->s, state);
  gsl_odeiv2_evolve_set_driver (state->e, state);
  gsl_odeiv2_control_set_driver (state->c, state);

  return state;
}

gsl_odeiv2_driver *
gsl_odeiv2_driver_alloc_scaled_new (const gsl_odeiv2_system * sys,
                                    const gsl_odeiv2_step_type * T,
                                    const double hstart,
                                    const double epsabs, const double epsrel,
                                    const double a_y, const double a_dydt,
                                    const double scale_abs[])
{
  /* Initializes an ODE driver system with control object of type
     scaled_new.
   */

  gsl_odeiv2_driver *state = driver_alloc (sys, hstart, T);

  if (state == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate driver object", GSL_ENOMEM);
    }

  if (epsabs >= 0.0 && epsrel >= 0.0)
    {
      state->c = gsl_odeiv2_control_scaled_new (epsabs, epsrel, a_y, a_dydt,
                                                scale_abs, sys->dimension);

      if (state->c == NULL)
        {
          gsl_odeiv2_driver_free (state);
          GSL_ERROR_NULL ("failed to allocate control object", GSL_ENOMEM);
        }
    }
  else
    {
      gsl_odeiv2_driver_free (state);
      GSL_ERROR_NULL ("epsabs and epsrel must be positive", GSL_EINVAL);
    }

  /* Distribute pointer to driver object */

  gsl_odeiv2_step_set_driver (state->s, state);
  gsl_odeiv2_evolve_set_driver (state->e, state);
  gsl_odeiv2_control_set_driver (state->c, state);

  return state;
}

int
gsl_odeiv2_driver_apply (gsl_odeiv2_driver * d, double *t,
                         const double t1, double y[])
{
  /* Main driver function that evolves the system from t to t1. In
     beginning vector y contains the values of dependent variables at
     t. This function returns values at t=t1 in y. In case of
     unrecoverable error, y and t contains the values after the last
     successful step.
   */

  int sign = 0;
  d->n = 0;

  /* Determine integration direction sign */

  if (d->h > 0.0)
    {
      sign = 1;
    }
  else
    {
      sign = -1;
    }

  /* Check that t, t1 and step direction are sensible */

  if (sign * (t1 - *t) < 0.0)
    {
      GSL_ERROR_NULL
        ("integration limits and/or step direction not consistent",
         GSL_EINVAL);
    }

  /* Evolution loop */

  while (sign * (t1 - *t) > 0.0)
    {
      int s = gsl_odeiv2_evolve_apply (d->e, d->c, d->s, d->sys,
                                       t, t1, &(d->h), y);

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

      /* Check for maximum allowed steps */

      if ((d->nmax > 0) && (d->n > d->nmax))
        {
          return GSL_EMAXITER;
        }

      /* Set step size if maximum size is exceeded */

      if (fabs (d->h) > d->hmax)
        {
          d->h = sign * d->hmax;
        }

      /* Check for too small step size */

      if (fabs (d->h) < d->hmin)
        {
          return GSL_ENOPROG;
        }

      d->n++;
    }

  return GSL_SUCCESS;
}

int
gsl_odeiv2_driver_apply_fixed_step (gsl_odeiv2_driver * d, double *t,
                                    const double h, const unsigned long int n,
                                    double y[])
{
  /* Alternative driver function that evolves the system from t using
   * n steps of size h. In the beginning vector y contains the values
   * of dependent variables at t. This function returns values at t =
   * t + n * h in y. In case of an unrecoverable error, y and t
   * contains the values after the last successful step.
   */

  unsigned long int i;
  d->n = 0;

  /* Evolution loop */

  for (i = 0; i < n; i++)
    {
      int s = gsl_odeiv2_evolve_apply_fixed_step (d->e, d->c, d->s, d->sys,
                                                  t, h, y);

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

      d->n++;
    }

  return GSL_SUCCESS;
}

int
gsl_odeiv2_driver_reset (gsl_odeiv2_driver * d)
{
  /* Reset the driver. Resets evolve and step objects. */

  {
    int s = gsl_odeiv2_evolve_reset (d->e);

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

  {
    int s = gsl_odeiv2_step_reset (d->s);

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

  return GSL_SUCCESS;
}

int
gsl_odeiv2_driver_reset_hstart (gsl_odeiv2_driver * d, const double hstart)
{
  /* Resets current driver and sets initial step size to hstart */

  gsl_odeiv2_driver_reset (d);

  if ((d->hmin > fabs (hstart)) || (fabs (hstart) > d->hmax))
    {
      GSL_ERROR_NULL ("hmin <= fabs(h) <= hmax required", GSL_EINVAL);
    }

  if (hstart > 0.0 || hstart < 0.0)
    {
      d->h = hstart;
    }
  else
    {
      GSL_ERROR_NULL ("invalid hstart", GSL_EINVAL);
    }

  return GSL_SUCCESS;
}

void
gsl_odeiv2_driver_free (gsl_odeiv2_driver * state)
{
  if (state->c != NULL)
    {
      gsl_odeiv2_control_free (state->c);
    }

  gsl_odeiv2_evolve_free (state->e);
  gsl_odeiv2_step_free (state->s);
  free (state);
}


//  This random generator is a C++ wrapper for the GNU Scientific Library
//  Copyright (C) 2001 Torbjorn Vik

//  This program is free software; you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation; either version 2 of the License, or
//  (at your option) any later version.

//  This program is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details.

//  You should have received a copy of the GNU General Public License
//  along with this program; if not, write to the Free Software
//  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
#ifndef __min_fminimizer_h
#define __min_fminimizer_h

#include <gsl/gsl_errno.h>
#include <gsl/gsl_min.h>

namespace gsl{

//! Derive this class provide a user defined function for minimisation
struct min_f
{
	//! This operator must be overridden
	virtual double operator()(const double& x)=0;

	//! This is the function gsl calls to optimize f
	static double f(double x, void *p)
	{
		return (*(min_f *)p)(x);
	}
};

//! Class for minimizing one dimensional functions.
/*!
  Usage:
       - Create with optional minimize type
	   - Set with function object and inital bounds
	   - Loop the  iterate function until convergence or maxIterations (extra facility)

	   - Recover minimum and bounds
 */
class min_fminimizer
{
 public:
	//! choose between gsl_min_fminimizer_goldensection and gsl_min_fminimizer_brent
	min_fminimizer(const gsl_min_fminimizer_type* type=gsl_min_fminimizer_brent) : s(NULL), maxIterations(100), isSet(false)
	{
		s=gsl_min_fminimizer_alloc(type);
		nIterations=0;
		if (!s)
		{
			//error
			//cout << "ERROR Couldn't allocate memory for minimizer" << endl;
			//throw ?
			exit(-1);
		}
	}
	~min_fminimizer(){if (s) gsl_min_fminimizer_free(s);}
	//! returns GSL_FAILURE if the interval does not contain a minimum
	int set(min_f& function, double minimum, double x_lower, double x_upper)
	{
		isSet=false;
		f.function = &function.f;
		f.params = &function;
		int status=	gsl_min_fminimizer_set(s, &f, minimum, x_lower, x_upper);
		if (!status)
		{
			isSet=true;
			nIterations=0;
		}
		return status;
	}
	int set_with_values(min_f& function,
						double minimum, double f_minimum,
						double x_lower,double f_lower,
						double x_upper, double f_upper)
	{
		isSet=false;
		f.function = &function.f;
		f.params = &function;
		int status=	gsl_min_fminimizer_set_with_values(s, &f, minimum, f_minimum, x_lower, f_lower, x_upper, f_upper);
		if (!status)
		{
			isSet=true;
			nIterations=0;
		}
		return status;
	}
	int iterate()
	{
		assert_set();
		int status=gsl_min_fminimizer_iterate(s);
		nIterations++;
		if (status==GSL_FAILURE)
			isConverged=true;
		return status;
	}
	double minimum(){assert_set();return gsl_min_fminimizer_minimum(s);}
	double x_upper(){assert_set();return gsl_min_fminimizer_x_upper(s);}
	double x_lower(){assert_set();return gsl_min_fminimizer_x_lower(s);}
	void SetMaxIterations(int n){maxIterations=n;}
	int GetNIterations(){return nIterations;}
	bool is_converged(){if (nIterations>=maxIterations) return true; if (isConverged) return true; return false;}
	//string name() const;

 private:
	void assert_set(){if (!isSet)exit(-1);} // Old problem of error handling: TODO

	bool isSet;
	bool isConverged;
	int nIterations;
	int maxIterations;
	gsl_min_fminimizer* s;
	gsl_function f;
};
};	 // namespace gsl

#endif //__min_fminimizer_h


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

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

#include "odeiv_util.h"

gsl_odeiv_evolve *
gsl_odeiv_evolve_alloc (size_t dim)
{
  gsl_odeiv_evolve *e =
    (gsl_odeiv_evolve *) malloc (sizeof (gsl_odeiv_evolve));

  if (e == 0)
    {
      GSL_ERROR_NULL ("failed to allocate space for evolve struct",
                      GSL_ENOMEM);
    }

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

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

  e->yerr = (double *) malloc (dim * sizeof (double));

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

  e->dydt_in = (double *) malloc (dim * sizeof (double));

  if (e->dydt_in == 0)
    {
      free (e->yerr);
      free (e->y0);
      free (e);
      GSL_ERROR_NULL ("failed to allocate space for dydt_in", GSL_ENOMEM);
    }

  e->dydt_out = (double *) malloc (dim * sizeof (double));

  if (e->dydt_out == 0)
    {
      free (e->dydt_in);
      free (e->yerr);
      free (e->y0);
      free (e);
      GSL_ERROR_NULL ("failed to allocate space for dydt_out", GSL_ENOMEM);
    }

  e->dimension = dim;
  e->count = 0;
  e->failed_steps = 0;
  e->last_step = 0.0;

  return e;
}

int
gsl_odeiv_evolve_reset (gsl_odeiv_evolve * e)
{
  e->count = 0;
  e->failed_steps = 0;
  e->last_step = 0.0;
  return GSL_SUCCESS;
}

void
gsl_odeiv_evolve_free (gsl_odeiv_evolve * e)
{
  RETURN_IF_NULL (e);
  free (e->dydt_out);
  free (e->dydt_in);
  free (e->yerr);
  free (e->y0);
  free (e);
}

/* Evolution framework method.
 *
 * Uses an adaptive step control object
 */
int
gsl_odeiv_evolve_apply (gsl_odeiv_evolve * e,
                        gsl_odeiv_control * con,
                        gsl_odeiv_step * step,
                        const gsl_odeiv_system * dydt,
                        double *t, double t1, double *h, double y[])
{
  const double t0 = *t;
  double h0 = *h;
  int step_status;
  int final_step = 0;
  double dt = t1 - t0;  /* remaining time, possibly less than h */

  if (e->dimension != step->dimension)
    {
      GSL_ERROR ("step dimension must match evolution size", GSL_EINVAL);
    }

  if ((dt < 0.0 && h0 > 0.0) || (dt > 0.0 && h0 < 0.0))
    {
      GSL_ERROR ("step direction must match interval direction", GSL_EINVAL);
    }

  /* No need to copy if we cannot control the step size. */

  if (con != NULL)
    {
      DBL_MEMCPY (e->y0, y, e->dimension);
    }

  /* Calculate initial dydt once if the method can benefit. */

  if (step->type->can_use_dydt_in)
    {
      int status = GSL_ODEIV_FN_EVAL (dydt, t0, y, e->dydt_in);

      if (status)
        {
          return status;
        }
    }

try_step:

  if ((dt >= 0.0 && h0 > dt) || (dt < 0.0 && h0 < dt))
    {
      h0 = dt;
      final_step = 1;
    }
  else
    {
      final_step = 0;
    }

  if (step->type->can_use_dydt_in)
    {
      step_status =
        gsl_odeiv_step_apply (step, t0, h0, y, e->yerr, e->dydt_in,
                              e->dydt_out, dydt);
    }
  else
    {
      step_status =
        gsl_odeiv_step_apply (step, t0, h0, y, e->yerr, NULL, e->dydt_out,
                              dydt);
    }

  /* Check for stepper internal failure */

  if (step_status != GSL_SUCCESS)
    {
      *h = h0;  /* notify user of step-size which caused the failure */
      *t = t0;  /* restore original t value */
      return step_status;
    }

  e->count++;
  e->last_step = h0;

  if (final_step)
    {
      *t = t1;
    }
  else
    {
      *t = t0 + h0;
    }

  if (con != NULL)
    {
      /* Check error and attempt to adjust the step. */

      double h_old = h0;

      const int hadjust_status
        = gsl_odeiv_control_hadjust (con, step, y, e->yerr, e->dydt_out, &h0);

      if (hadjust_status == GSL_ODEIV_HADJ_DEC)
        {
          /* Check that the reported status is correct (i.e. an actual
             decrease in h0 occured) and the suggested h0 will change
             the time by at least 1 ulp */

          double t_curr = GSL_COERCE_DBL(*t);
          double t_next = GSL_COERCE_DBL((*t) + h0);

          if (fabs(h0) < fabs(h_old) && t_next != t_curr)
            {
              /* Step was decreased. Undo step, and try again with new h0. */
              DBL_MEMCPY (y, e->y0, dydt->dimension);
              e->failed_steps++;
              goto try_step;
            }
          else
            {
              h0 = h_old; /* keep current step size */
            }
        }
    }

  *h = h0;  /* suggest step size for next time-step */

  return step_status;
}


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

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

#include <gsl/gsl_math.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spmatrix.h>
#include <gsl/gsl_spblas.h>

int
gsl_spmatrix_scale(gsl_spmatrix *m, const double x)
{
  size_t i;

  for (i = 0; i < m->nz; ++i)
    m->data[i] *= x;

  return GSL_SUCCESS;
} /* gsl_spmatrix_scale() */

int
gsl_spmatrix_minmax(const gsl_spmatrix *m, double *min_out, double *max_out)
{
  double min, max;
  size_t n;

  if (m->nz == 0)
    {
      GSL_ERROR("matrix is empty", GSL_EINVAL);
    }

  min = m->data[0];
  max = m->data[0];

  for (n = 1; n < m->nz; ++n)
    {
      double x = m->data[n];

      if (x < min)
        min = x;

      if (x > max)
        max = x;
    }

  *min_out = min;
  *max_out = max;

  return GSL_SUCCESS;
} /* gsl_spmatrix_minmax() */

/*
gsl_spmatrix_add()
  Add two sparse matrices

Inputs: c - (output) a + b
        a - (input) sparse matrix
        b - (input) sparse matrix

Return: success or error
*/

int
gsl_spmatrix_add(gsl_spmatrix *c, const gsl_spmatrix *a,
                 const gsl_spmatrix *b)
{
  const size_t M = a->size1;
  const size_t N = a->size2;

  if (b->size1 != M || b->size2 != N || c->size1 != M || c->size2 != N)
    {
      GSL_ERROR("matrices must have same dimensions", GSL_EBADLEN);
    }
  else if (a->sptype != b->sptype || a->sptype != c->sptype)
    {
      GSL_ERROR("matrices must have same sparse storage format",
                GSL_EINVAL);
    }
  else if (GSL_SPMATRIX_ISTRIPLET(a))
    {
      GSL_ERROR("triplet format not yet supported", GSL_EINVAL);
    }
  else
    {
      int status = GSL_SUCCESS;
      size_t *w = (size_t *) a->work;
      double *x = (double *) b->work;
      size_t *Cp, *Ci;
      double *Cd;
      size_t j, p;
      size_t nz = 0; /* number of non-zeros in c */
      size_t inner_size, outer_size;

      if (GSL_SPMATRIX_ISCCS(a))
        {
          inner_size = M;
          outer_size = N;
        }
      else if (GSL_SPMATRIX_ISCRS(a))
        {
          inner_size = N;
          outer_size = M;
        }
      else
        {
          GSL_ERROR("unknown sparse matrix type", GSL_EINVAL);
        }

      if (c->nzmax < a->nz + b->nz)
        {
          status = gsl_spmatrix_realloc(a->nz + b->nz, c);
          if (status)
            return status;
        }

      /* initialize w = 0 */
      for (j = 0; j < inner_size; ++j)
        w[j] = 0;

      Ci = c->i;
      Cp = c->p;
      Cd = c->data;

      for (j = 0; j < outer_size; ++j)
        {
          Cp[j] = nz;

          /* CCS: x += A(:,j); CRS: x += A(j,:) */
          nz = gsl_spblas_scatter(a, j, 1.0, w, x, j + 1, c, nz);

          /* CCS: x += B(:,j); CRS: x += B(j,:) */
          nz = gsl_spblas_scatter(b, j, 1.0, w, x, j + 1, c, nz);

          for (p = Cp[j]; p < nz; ++p)
            Cd[p] = x[Ci[p]];
        }

      /* finalize last column of c */
      Cp[j] = nz;
      c->nz = nz;

      return status;
    }
} /* gsl_spmatrix_add() */

/*
gsl_spmatrix_d2sp()
  Convert a dense gsl_matrix to sparse (triplet) format

Inputs: S - (output) sparse matrix in triplet format
        A - (input) dense matrix to convert
*/

int
gsl_spmatrix_d2sp(gsl_spmatrix *S, const gsl_matrix *A)
{
  int s = GSL_SUCCESS;
  size_t i, j;

  gsl_spmatrix_set_zero(S);
  S->size1 = A->size1;
  S->size2 = A->size2;

  for (i = 0; i < A->size1; ++i)
    {
      for (j = 0; j < A->size2; ++j)
        {
          double x = gsl_matrix_get(A, i, j);

          if (x != 0.0)
            gsl_spmatrix_set(S, i, j, x);
        }
    }

  return s;
} /* gsl_spmatrix_d2sp() */

/*
gsl_spmatrix_sp2d()
  Convert a sparse matrix to dense format
*/

int
gsl_spmatrix_sp2d(gsl_matrix *A, const gsl_spmatrix *S)
{
  if (A->size1 != S->size1 || A->size2 != S->size2)
    {
      GSL_ERROR("matrix sizes do not match", GSL_EBADLEN);
    }
  else
    {
      gsl_matrix_set_zero(A);

      if (GSL_SPMATRIX_ISTRIPLET(S))
        {
          size_t n;

          for (n = 0; n < S->nz; ++n)
            {
              size_t i = S->i[n];
              size_t j = S->p[n];
              double x = S->data[n];

              gsl_matrix_set(A, i, j, x);
            }
        }
      else
        {
          GSL_ERROR("non-triplet formats not yet supported", GSL_EINVAL);
        }

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