/*
** pseudoinverse
**
** G.Lohmann, July 2004
*/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "viaio/Vlib.h"

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

extern void printmat(gsl_matrix *R,char *str);
extern void printvec(gsl_vector *x,char *str);

/*
**    B = A^-1 = A^+
*/
gsl_matrix *PseudoInv(gsl_matrix *A,gsl_matrix *B)
{
  int j,k,l,n,m;
  double u,x;
  gsl_matrix *U=NULL,*V=NULL;
  gsl_vector *w=NULL;

  m = A->size1;
  n = A->size2;

  if (B == NULL) {
    B = gsl_matrix_calloc (n,m);
  }

  U = gsl_matrix_calloc (m, n);
  V = gsl_matrix_calloc (n, n);
  w = gsl_vector_calloc (n);

  /* singular value decomposition */
  gsl_matrix_memcpy (U,A);
  gsl_linalg_SV_decomp_jacobi(U,V,w);
  gsl_matrix_set_zero(B);

  k=0;
  for (j=0; j<n; j++) {
    if (fabs(w->data[j]) > 1.0e-6) k++;
  }
  if (k < 1) VError(" Design matrix is singular\n");

  for (k=0; k<n; k++) {
    for (l=0; l<m; l++) {
      for (j=0; j<n; j++) {
	u = gsl_vector_get(w,j);
        if (fabs(u) > 1.0e-6) {
          x = gsl_matrix_get(B,k,l);
	  x += gsl_matrix_get(V,k,j)*gsl_matrix_get(U,l,j)/u;
          gsl_matrix_set(B,k,l,x);
        }
      }
    }
  }
  gsl_matrix_free(U);
  gsl_matrix_free(V);
  gsl_vector_free(w);
  return B;
}


#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_siman.h>

/* set up parameters for this simulated annealing run */

/* how many points do we try before stepping */
#define N_TRIES 200

/* how many iterations for each T? */
#define ITERS_FIXED_T 1000

/* max step size in random walk */
#define STEP_SIZE 1.0

/* Boltzmann constant */
#define K 1.0

/* initial temperature */
#define T_INITIAL 0.008

/* damping factor for temperature */
#define MU_T 1.003
#define T_MIN 2.0e-6

gsl_siman_params_t params
  = {N_TRIES, ITERS_FIXED_T, STEP_SIZE,
     K, T_INITIAL, MU_T, T_MIN};

/* now some functions to test in one dimension */
double E1(void *xp)
{
  double x = * ((double *) xp);

  return exp(-pow((x-1.0),2.0))*sin(8*x);
}

double M1(void *xp, void *yp)
{
  double x = *((double *) xp);
  double y = *((double *) yp);

  return fabs(x - y);
}

void S1(const gsl_rng * r, void *xp, double step_size)
{
  double old_x = *((double *) xp);
  double new_x;

  double u = gsl_rng_uniform(r);
  new_x = u * 2 * step_size - step_size + old_x;

  memcpy(xp, &new_x, sizeof(new_x));
}

void P1(void *xp)
{
  printf ("%12g", *((double *) xp));
}

int
main(int argc, char *argv[])
{
  const gsl_rng_type * T;
  gsl_rng * r;

  double x_initial = 15.5;

  gsl_rng_env_setup();

  T = gsl_rng_default;
  r = gsl_rng_alloc(T);

  gsl_siman_solve(r, &x_initial, E1, S1, M1, P1,
                  NULL, NULL, NULL,
                  sizeof(double), params);

  gsl_rng_free (r);
  return 0;
}


#include <stdio.h>
#include <stdlib.h>
#include <gbpLib.h>
#include <gbpMisc.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_roots.h>

double bisect_array_function(double x_interp, void *params) {
    double interp;
    double r_val;
    interp = interpolate(((bisect_af_params *)params)->interp, x_interp);
    r_val  = interp - ((bisect_af_params *)params)->value;
    return (r_val);
}

double bisect_array(interp_info *interp, double value, double threshold) {
    double *                     x;
    double *                     y;
    double *                     d2y;
    double                       x_lo;
    double                       x_hi;
    double                       r_val;
    int                          i;
    int                          alloc_flag = GBP_FALSE;
    int                          iter       = 0;
    int                          max_iter   = 500;
    int                          status     = GSL_CONTINUE;
    bisect_af_params             params;
    gsl_function                 F;
    const gsl_root_fsolver_type *T;
    gsl_root_fsolver *           s;

    params.interp = interp;
    params.value  = value;
    F.function    = bisect_array_function;
    F.params      = (void *)(&params);

    x_lo = interp->x[0];
    x_hi = interp->x[interp->n - 1];

    if(fabs(bisect_array_function(x_lo, &params)) < threshold)
        return (x_lo);
    if(fabs(bisect_array_function(x_hi, &params)) < threshold)
        return (x_hi);

    T = gsl_root_fsolver_brent;
    s = gsl_root_fsolver_alloc(T);
    gsl_root_fsolver_set(s, &F, x_lo, x_hi);

    while(status == GSL_CONTINUE && iter < max_iter) {
        status = gsl_root_fsolver_iterate(s);
        r_val  = gsl_root_fsolver_root(s);
        x_lo   = gsl_root_fsolver_x_lower(s);
        x_hi   = gsl_root_fsolver_x_upper(s);
        status = gsl_root_test_interval(x_lo, x_hi, 0, threshold);
        iter++;
    }

    // Clean-up
    gsl_root_fsolver_free(s);

    return (r_val);
}


/*! \file Integrate.h
 *  \brief header file for integrating functions or data.
 */

#ifndef INTEGRATE_H
#define INTEGRATE_H

#include <cmath>
#include <iostream>
#include <gsl/gsl_math.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_interp.h>
#include <gsl/gsl_monte.h>
#include <gsl/gsl_monte_plain.h>
#include <gsl/gsl_monte_miser.h>
#include <gsl/gsl_monte_vegas.h>
#include <Precision.h>
#include <Function.h>
#include <Interpolate.h>
#include <Random.h>

using namespace std;
namespace Math
{
    /// Integrate function f(x) from x[lower] to x[upper].  Uses some
    /// algorithm I found somewhere, sometime.
    Double_t IntegrateData(const Double_t x[], const Double_t f[], const int lower,
                         const int upper);

    /// Integrate function f(x) from x[lower] to x[upper].  Uses trapeziod
    /// algorithm.
    Double_t IntegrateTrap(const Double_t x[], const Double_t f[], const int a,
                         const int b);
    ///uses adaptive trapezoidal rule based on numerical recipes
    Double_t IntegrateTrapezoidal(const math_function * f, const Double_t a,
                         const Double_t b, const int n);
    Double_t IntegrateQTrap(const math_function * f, const Double_t a,
                         const Double_t b, const Double_t epsrel, int IMAX=16);
    ///uses simple trapezoidal
    Double_t IntegrateSimpleTrapezoidal(const math_function * f, const Double_t a,
                         const Double_t b, const int n);

    ///extended closed integration
    Double_t IntegrateClosed(const math_function * f, const Double_t a,
                         const Double_t b, const int n);

    ///simpsons 3/8 rule
    Double_t IntegrateSimpson(const math_function * f, const Double_t a,
                         const Double_t b);

    ///Romberg integration based on numerical recipes
    Double_t IntegrateRomberg(const math_function * f, const Double_t a,
                         const Double_t b, const Double_t epsrel, const int n, const int K);

    ///integration based vegas monte carlo integrator. can also return error and chisq
    ///vegas monte is either gsl or NR based vegas integrator.
    Double_t IntegrateVegasMonte(math_multidim_function * f, Double_t *a, Double_t *b, const int n, Double_t *error=0,Double_t*chisq=0,int interations=6);
    double IntegrateVegasMonte(gsl_monte_function * f, double *a, double *b, const int n, double *error=0,double *chisq=0);

    ///Romberg integration based on numerical recipes but uses vegas monte carlo integrator.
    Double_t IntegrateRombergMonte(math_multidim_function * f, Double_t *a,
                         Double_t *b, const Double_t epsrel, const int sampling, const int n, const int K, int interations=6);

    ///subroutine used in my nr vegas integrator.
    void rebin(Double_t rc, int nd, Double_t r[], Double_t xin[], Double_t xi[]);
    ///numerical recipes vegas integrator. for simplicity use IntegrateVegasMonte as interface
    void vegas(math_multidim_function *fxn, Double_t regn[], int ndim, int init,
    unsigned long ncall, int itmx, int nprn, long int idum, Double_t *tgral, Double_t *sd,
    Double_t *chi2a);
}

#endif


#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_roots.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_sf_expint.h>
#include "ccl.h"

static double einasto_norm_integrand(double x, void *params)
{
  double alpha = *((double *)(params));
  return x*x*exp(-2*(pow(x,alpha)-1)/alpha);
}

void ccl_einasto_norm_integral(int n_m, double *r_s, double *r_delta, double *alpha,
			       double *norm_out,int *status)
{
#pragma omp parallel default(none)			\
  shared(n_m, r_s, r_delta, alpha, norm_out, status)
  {
    int ii;
    int status_this=0;
    gsl_function F;
    gsl_integration_workspace *w = gsl_integration_workspace_alloc(1000);

    if (w == NULL)
      status_this = CCL_ERROR_MEMORY;

    if(status_this == 0) {
#pragma omp for
      for(ii=0;ii<n_m;ii++) {
	int qagstatus;
	double result, eresult;
	double x_max = r_delta[ii]/r_s[ii];
	F.function = &einasto_norm_integrand;
	F.params = &(alpha[ii]);
	qagstatus = gsl_integration_qag(&F, 0, x_max, 0, 1E-4,
					1000, GSL_INTEG_GAUSS31,
					w, &result, &eresult);
	if(qagstatus != GSL_SUCCESS) {
	  ccl_raise_gsl_warning(qagstatus, "ccl_haloprofile.c: ccl_einasto_norm_integral():");
	  status_this = CCL_ERROR_INTEG;
	  result = NAN;
	}
	norm_out[ii] = 4 * M_PI * r_s[ii] * r_s[ii] * r_s[ii] * result;
      }
    } //end omp for

    gsl_integration_workspace_free(w);
    if(status_this) {
      #pragma omp atomic write
      *status = status_this;
    }
  } //end omp parallel
}

static double hernquist_norm_integrand(double x, void *params)
{
  double opx=1+x;
  return x*x/(x*opx*opx*opx);
}

void ccl_hernquist_norm_integral(int n_m, double *r_s, double *r_delta,
			       double *norm_out,int *status)
{
#pragma omp parallel default(none)		\
  shared(n_m, r_s, r_delta, norm_out, status)
  {
    int ii;
    int status_this=0;
    gsl_function F;
    gsl_integration_workspace *w = gsl_integration_workspace_alloc(1000);

    if (w == NULL)
      status_this = CCL_ERROR_MEMORY;

    if(status_this == 0) {
#pragma omp for
      for(ii=0;ii<n_m;ii++) {
	int qagstatus;
	double result, eresult;
	double x_max = r_delta[ii]/r_s[ii];
	F.function = &hernquist_norm_integrand;
	F.params = NULL;
	qagstatus = gsl_integration_qag(&F, 0, x_max, 0, 1E-4,
					1000, GSL_INTEG_GAUSS31,
					w, &result, &eresult);
	if(qagstatus != GSL_SUCCESS) {
	  ccl_raise_gsl_warning(qagstatus, "ccl_haloprofile.c: ccl_hernquist_norm_integral():");
	  status_this = CCL_ERROR_INTEG;
	  result = NAN;
	}
	norm_out[ii] = 4 * M_PI * r_s[ii] * r_s[ii] * r_s[ii] * result;
      }
    } //end omp for

    gsl_integration_workspace_free(w);
    if(status_this) {
      #pragma omp atomic write
      *status = status_this;
    }
  } //end omp parallel
}


//
// Created by karl_ on 2020-5-3.
//

#include <math.h>
#include <cblas.h>
#include <imgproc/color-convert.h>

gboolean imgproc_to_HSL(CoreImage *src, CoreImage **dst) {
    CoreSize *size;
    CoreColorSpace src_color_space;
    CorePixelType src_pixel_type;
    gdouble *src_data_cast, *dst_data;
    gdouble *hsl_pixel, *rgb_pixel, c_max, c_min, c;
    gsize i, j, area;

    size = core_image_get_size(src);
    area = core_size_get_area(size);
    src_color_space = core_image_get_color_space(src);
    src_pixel_type = core_image_get_pixel_type(src);

    g_return_val_if_fail(src_color_space == CORE_COLOR_SPACE_RGB, FALSE);

    if (core_pixel_is_double(src_pixel_type)) {
        src_data_cast = core_image_get_data(src);
    } else if (core_pixel_is_uint8(src_pixel_type)) {
        src_data_cast = malloc(sizeof(gdouble) * core_size_get_area(size));
        for (i = 0; i < core_size_get_area(size) * 3; ++i) {
            src_data_cast[i] = ((guint8 *) core_image_get_data(src))[i] / core_pixel_get_range(src_pixel_type);
        }
    } else {
        return FALSE;
    }

    dst_data = g_malloc(sizeof(gdouble) * core_size_get_area(size));
    for (i = 0; i < area; ++i) {
        hsl_pixel = dst_data + i * 3;
        rgb_pixel = src_data_cast + i * 3;
        c_min = 1;
        c_max = 0;
        c = c_max - c_min;
        for (j = 0; j < 3; ++j) {
            if (rgb_pixel[j] > c_max) {
                c_max = rgb_pixel[j];
            }
            if (rgb_pixel[j] < c_min) {
                c_max = rgb_pixel[j];
            }
        }

        /* Lightness */
        hsl_pixel[2] = (c_min + c_max) / 2;

        /* Saturation */
        if (!c) {
            hsl_pixel[1] = 0;
        } else {
            hsl_pixel[1] = c / (1 - fabs(hsl_pixel[2] * 2 - 1));
        }

        /* Hue */
        if (!c) {
            hsl_pixel[0] = 0;
        } else {
            if (rgb_pixel[0] == c_max) {
                hsl_pixel[0] = 1.0 / 6 * (rgb_pixel[1] - rgb_pixel[2]) / c;
            } else if (rgb_pixel[1] == c_max) {
                hsl_pixel[0] = 1.0 / 6 * (2 + (rgb_pixel[2] - rgb_pixel[0]) / c);
            } else if (rgb_pixel[2] == c_max) {
                hsl_pixel[0] = 1.0 / 6 * (4 + (rgb_pixel[0] - rgb_pixel[1]) / c);
            }
        }
    }

    if (*dst == NULL) {
        *dst = core_image_new_with_data(dst_data, CORE_COLOR_SPACE_HSL, CORE_PIXEL_D3, size, FALSE);
    } else {
        core_image_assign_data(*dst, dst_data, CORE_COLOR_SPACE_HSL, CORE_PIXEL_D3, size, FALSE);
    }

    if (core_pixel_is_uint8(src_pixel_type)) {
        free(src_data_cast);
    }
    g_object_unref(size);

    return TRUE;
}

gboolean imgproc_to_RGB(CoreImage *src, CoreImage **dst) {
    CoreSize *size;
    CoreColorSpace color_space;
    CorePixelType pixel_type;
    gdouble *src_data, *dst_data;
    gdouble *hsl_pixel, *rgb_pixel;
    gdouble c, h, h_ceil, x, m, r, g, b;
    gsize i, area;

    g_return_val_if_fail(src, FALSE);
    color_space = core_image_get_color_space(src);
    pixel_type = core_image_get_pixel_type(src);
    g_return_val_if_fail(color_space == CORE_COLOR_SPACE_HSL, FALSE);
    g_return_val_if_fail(pixel_type == CORE_PIXEL_D3, FALSE);
    size = core_image_get_size(src);
    area = core_size_get_area(size);
    src_data = core_image_get_data(src);
    dst_data = g_malloc(sizeof(gdouble) * area);
    for (i = 0; i < area; ++i) {
        hsl_pixel = src_data + i * 3;
        rgb_pixel = dst_data + i * 3;

        c = (1 - fabs(2 * hsl_pixel[2] - 1));
        h = hsl_pixel[0] * 6.0;
        x = c * (1 - fabs(h - floor(h / 2) * 2 - 1));
        h_ceil = ceil(h) + DBL_EPSILON;
        if (h_ceil >= 6) {
            r = c;
            g = 0;
            b = x;
        } else if (h_ceil >= 5) {
            r = x;
            g = 0;
            b = c;
        } else if (h_ceil >= 4) {
            r = 0;
            g = x;
            b = c;
        } else if (h_ceil >= 3) {
            r = 0;
            g = c;
            b = x;
        } else if (h_ceil >= 2) {
            r = x;
            g = c;
            b = 0;
        } else if (h_ceil >= 1) {
            r = c;
            g = x;
            b = 0;
        } else {
            r = g = b = 0;
        }
        m = hsl_pixel[2] - c / 2;
        rgb_pixel[0] = r + m;
        rgb_pixel[1] = g + m;
        rgb_pixel[2] = b + m;
    }

    if (*dst == NULL) {
        *dst = core_image_new_with_data(dst_data, CORE_COLOR_SPACE_RGB, CORE_PIXEL_D3, size, FALSE);
    } else {
        core_image_assign_data(*dst, dst_data, CORE_COLOR_SPACE_RGB, CORE_PIXEL_D3, size, FALSE);
    }

    g_object_unref(size);
    return TRUE;
}

gboolean imgproc_to_grayscale(CoreImage *src, CoreImage **dst) {
    gsize i;
    CoreSize *size;
    gdouble *dst_data_cast, *src_data_cast;
    gpointer src_data, dst_data;
    gsize channel, area;
    CorePixelType pixel_type;
    g_return_val_if_fail(src != NULL, FALSE);
    g_return_val_if_fail(dst != NULL, FALSE);

    size = core_image_get_size(src);
    area = core_size_get_area(size);
    pixel_type = core_image_get_pixel_type(src);
    channel = core_image_get_channel(src);
    src_data = core_image_get_data(src);

    if (core_pixel_is_uint8(pixel_type)) {
        src_data_cast = g_malloc(area * sizeof(gdouble) * channel);
        for (i = 0; i < area * channel; ++i) {
            src_data_cast[i] = (gdouble) ((guint8 *) src_data)[i];
        }
    } else {
        /* do not need to cast if pixel type is Dx */
        src_data_cast = src_data;
    }
    dst_data_cast = g_malloc(area * sizeof(gdouble));

    cblas_dcopy(area, src_data_cast, channel, dst_data_cast, 1);
    for (i = 1; i < channel; ++i) {
        cblas_daxpy(area, 1.0, src_data_cast + i, channel, dst_data_cast, 1);
    }
    if (channel > 1) {
        cblas_daxpy(area, 1.0 / channel - 1.0, dst_data_cast, 1, dst_data_cast, 1);
    }

    if (core_pixel_is_uint8(pixel_type)) {
        pixel_type = CORE_PIXEL_U1;
    } else if (core_pixel_is_double(pixel_type)) {
        pixel_type = CORE_PIXEL_D1;
    } else {
        g_return_val_if_fail(FALSE, FALSE);
    }

    /* cast dst_data from double back to uchar if needed */
    if (core_pixel_is_uint8(pixel_type)) {
        for (i = 0; i < area; ++i) {
            ((guint8 *) dst_data_cast)[i] = dst_data_cast[i];
        }
    }
    dst_data = dst_data_cast;
    if (core_pixel_is_uint8(pixel_type)) {
        dst_data = g_realloc(dst_data, sizeof(guint8) * area);
    }

    /* assign result */
    if (*dst == NULL) {
        *dst = core_image_new_with_data(dst_data, CORE_COLOR_SPACE_GRAY_SCALE, pixel_type, size, FALSE);
    } else {
        core_image_assign_data(*dst, dst_data, CORE_COLOR_SPACE_GRAY_SCALE, pixel_type, size, FALSE);
    }

    /* free additional allocated memory */
    if (core_pixel_is_uint8(pixel_type)) {
        g_free(src_data_cast);
    }
    g_object_unref(size);
    return TRUE;
}

gboolean imgproc_to_binary_threshold(CoreImage *src, CoreImage **dst, gdouble threshold, gboolean inverse) {
    CoreImage *gray_src = NULL;
    CoreSize *size;
    gsize i, area;
    gdouble *src_data, *dst_data;
    CoreColorSpace color_space;
    CorePixelType pixel_type;
    g_return_val_if_fail(src != NULL, FALSE);
    g_return_val_if_fail(dst != NULL, FALSE);

    if (core_image_get_channel(src) > 1) {
        imgproc_to_grayscale(src, &gray_src);
    } else {
        gray_src = g_object_ref(src);
    }
    size = core_image_get_size(gray_src);
    area = core_size_get_area(size);
    src_data = core_image_get_data(gray_src);
    dst_data = g_malloc(sizeof(gdouble) * area);
    for (i = 0; i < area; ++i) {
        dst_data[i] = ((unsigned) (src_data[i] > threshold) ^ (unsigned) inverse) * 255.0;
    }
    if (*dst == NULL) {
        core_image_new_with_data(dst_data, CORE_COLOR_SPACE_BIN, CORE_PIXEL_U1, size, FALSE);
    } else {
        core_image_assign_data(*dst, dst_data, CORE_COLOR_SPACE_BIN, CORE_PIXEL_U1, size, FALSE);
    }
    g_object_unref(size);
    g_object_unref(gray_src);
    return TRUE;
}


/*---
  Flow*: A Verification Tool for Cyber-Physical Systems.
  Authors: Xin Chen, Sriram Sankaranarayanan, and Erika Abraham.
  Email: Xin Chen <chenxin415@gmail.com> if you have questions or comments.

  The code is released as is under the GNU General Public License (GPL).
---*/

#ifndef MATRIX_H_
#define MATRIX_H_

#include "include.h"
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>

namespace flowstar
{

class Real;
class Interval;
class UnivariatePolynomial;
class Polynomial;
class RowVector;
class ColVector;
class rMatrix;
class iMatrix;
class upMatrix;
class mpMatrix;
class iMatrix2;
class HybridSystem;
class TaylorModelVec;

// The matrix class is implemented based on the data structure of gsl matrix.
class Matrix
{
private:
	gsl_matrix *data;
public:
	Matrix();
	Matrix(const int m, const int n);	// Create an m x n matrix, all of the entries are 0.
	Matrix(const int n);				// Create an n x n matrix, all of the entries are 0.
	Matrix(const Matrix & A);
	~Matrix();

	double get(const int i, const int j) const;				// Get the entry at position [i,j].
	void set(const double v, const int i, const int j);		// Set A[i,j] = v.

	int rows() const;
	int cols() const;

	void row(RowVector & result, const int i) const;			// Return the (i+1)-st row.

	void sortColumns();		// Sort the columns by size in descending order.
	int rank() const;

	void neg(Matrix & result) const;
	void neg_assign();

	void inverse(Matrix & result) const;
	void inverse_assign();

	void transpose(Matrix & result) const;
	void svd(Matrix & U) const;

	void QR(Matrix & D);
	void QRfactor(Matrix & Q);

	void output(FILE *fp) const;

	Matrix & operator += (const Matrix & A);
	Matrix & operator -= (const Matrix & A);
	Matrix & operator *= (const Matrix & A);

	Matrix operator + (const Matrix & A) const;
	Matrix operator - (const Matrix & A) const;
	Matrix operator * (const Matrix & A) const;

	Matrix & operator = (const Matrix & A);
};

class RowVector
{
private:
	Matrix vec;
public:
	RowVector();
	RowVector(const int n);
	RowVector(const RowVector & v);
	~RowVector();

	void set(const double v, const int pos);
	double get(const int pos) const;
	int size() const;

	void transpose(ColVector & result) const;

	void neg(RowVector & result) const;
	void neg_assign();

	void dump(FILE *fp) const;

	double innerProd(const RowVector & v) const;
	double EuclideanNorm() const;
	void normalize();

	bool operator == (const RowVector & v) const;

	RowVector & operator += (const RowVector & v);
	RowVector & operator -= (const RowVector & v);
	RowVector operator + (const RowVector & v) const;
	RowVector operator - (const RowVector & v) const;

	RowVector & operator = (const RowVector & v);

	friend class ColVector;
};

class ColVector
{
private:
	Matrix vec;
public:
	ColVector();
	ColVector(const int n);
	ColVector(const ColVector & v);
	~ColVector();

	void set(const double v, const int pos);
	double get(const int pos) const;
	int size() const;

	void transpose(RowVector & result) const;

	void neg(ColVector & result) const;
	void neg_assign();

	void mul(ColVector & result, const Matrix & m) const;
	void mul_assign(const Matrix & m);

	ColVector & operator += (const ColVector & v);
	ColVector & operator -= (const ColVector & v);
	ColVector operator + (const ColVector & v) const;
	ColVector operator - (const ColVector & v) const;

	ColVector & operator = (const ColVector & v);

	friend class RowVector;
};

class bMatrix
{
protected:
	bool *data;
	int size1;
	int size2;

public:
	bMatrix();
	bMatrix(const int m, const int n);
	bMatrix(const bMatrix & B);
	~bMatrix();

	int rows() const;
	int cols() const;

	void output(FILE *fp) const;

	bMatrix & operator += (const bMatrix & B);
	bool * operator [] (const int i);

	bMatrix & operator = (const bMatrix & B);
};

class rMatrix
{
protected:
	Real *data;
	int size1;
	int size2;

public:
	rMatrix();
	rMatrix(const int m, const int n);	// zero matrix
	rMatrix(const int n);				// identity matrix
	rMatrix(const rMatrix & rmatrix);
	~rMatrix();

	int rows() const;
	int cols() const;

	void abs(rMatrix & result) const;
	void transpose(rMatrix & result) const;

	void add_RNDD(rMatrix & result, const rMatrix & rmatrix) const;
	void add_assign_RNDD(const rMatrix & rmatrix);
	void add_RNDU(rMatrix & result, const rMatrix & rmatrix) const;
	void add_assign_RNDU(const rMatrix & rmatrix);

	void mul_RNDD(rMatrix & result, const rMatrix & rmatrix) const;
	void mul_RNDU(rMatrix & result, const rMatrix & rmatrix) const;

	void output(FILE *fp) const;

	rMatrix & operator += (const rMatrix & B);
	rMatrix & operator -= (const rMatrix & B);
	rMatrix & operator *= (const rMatrix & B);
	rMatrix operator + (const rMatrix & B) const;
	rMatrix operator - (const rMatrix & B) const;
	rMatrix operator * (const rMatrix & B) const;

	rMatrix & operator *= (const Real & r);
	rMatrix & operator *= (const double d);

	rMatrix & operator = (const rMatrix & rmatrix);		// should always be the same precision

	Real * operator [] (const int i);

	friend void to_iMatrix2(iMatrix2 & result, const rMatrix & lo, const rMatrix & up);

	friend class iMatrix;
	friend class iMatrix2;
	friend class upMatrix;
};

// matrix for intervals
class iMatrix
{
protected:
	Interval *data;
	int size1;
	int size2;

public:
	iMatrix();
	iMatrix(const int m, const int n);	// zero matrix
	iMatrix(const int n);				// identity matrix
	iMatrix(const iMatrix & A);
	iMatrix(const rMatrix & A);
	iMatrix(const iMatrix2 & A);
	iMatrix(const std::vector<Interval> & box);
	iMatrix(const std::string & matlab_format);
	~iMatrix();

	void clear();

	void mul(iMatrix & result, iMatrix & A);

	int rows() const;
	int cols() const;

	bool isZero() const;

	void pow(iMatrix & result, const int order) const;
	void pow_assign(const int order);

	double max_norm() const;
	void max_norm(Real & norm) const;

	void transpose(iMatrix & result) const;
	void times_pars(mpMatrix & result) const;
	void center();

	void linearTrans(std::vector<Polynomial> & result, const std::vector<Polynomial> & polyVec) const;

	void right_scale_assign(const std::vector<Interval> & scalars);

	void to_iMatrix2(iMatrix2 & A) const;

	void output(FILE *fp) const;

	iMatrix & operator += (const iMatrix & A);
	iMatrix & operator += (const Real & rad);
	iMatrix & operator -= (const iMatrix & A);
	iMatrix & operator *= (const iMatrix & A);
	iMatrix & operator *= (const Interval & I);
	iMatrix & operator *= (const double c);
	iMatrix & operator /= (const Interval & I);
	iMatrix & operator /= (const double c);

	iMatrix operator + (const iMatrix & A) const;
	iMatrix operator - (const iMatrix & A) const;
	iMatrix operator * (const iMatrix & A) const;
	iMatrix operator * (const Interval & I) const;
	iMatrix operator * (const double c) const;
	upMatrix operator * (const upMatrix & upm) const;

	mpMatrix operator * (const mpMatrix & mpm) const;
	TaylorModelVec operator * (const TaylorModelVec & tmv) const;

	Interval * operator [] (const int i);
	iMatrix & operator = (const iMatrix & A);
	iMatrix & operator = (const rMatrix & A);
	iMatrix & operator = (const iMatrix2 & A);
	iMatrix & operator = (const std::vector<Interval> & box);

	friend class upMatrix;
	friend class mpMatrix;
	friend class rMatrix;
	friend class iMatrix2;
	friend class Zonotope;
};

class iMatrix2
{
public:
	rMatrix center;
	rMatrix radius;

public:
	iMatrix2();
	iMatrix2(const int m, const int n);	// zero matrix
	iMatrix2(const int n);				// identity matrix
	iMatrix2(const iMatrix2 & A);
	iMatrix2(const iMatrix & A);
	iMatrix2(const std::vector<Interval> & box);
	~iMatrix2();

	int rows() const;
	int cols() const;

	void to_iMatrix(iMatrix & A) const;
	void transpose(iMatrix2 & result) const;
	void mag(Interval & I, const int i, const int j);
	void mag(Real & r, const int i, const int j);
	void add_assign(const Interval & I, const int i, const int j);

	iMatrix2 & operator += (const iMatrix2 & A);
	iMatrix2 & operator += (const iMatrix & A);
	iMatrix2 & operator += (const Real & rad);
	iMatrix2 & operator *= (const iMatrix2 & A);
	iMatrix2 & operator *= (const Interval & I);

	iMatrix2 operator + (const iMatrix2 & A) const;
	iMatrix2 operator * (const iMatrix2 & A) const;
	iMatrix operator * (const iMatrix & A) const;
	upMatrix operator * (const upMatrix & upm) const;
	TaylorModelVec operator * (const TaylorModelVec & tmv) const;

	iMatrix2 & operator = (const iMatrix2 & A);

	void output(FILE *fp) const;

	friend void to_iMatrix2(iMatrix2 & result, const rMatrix & lo, const rMatrix & up);

	friend class upMatrix;
	friend class mpMatrix;
	friend class rMatrix;
	friend class iMatrix;
	friend class Zonotope;
	friend class Polynomial;
	friend class HybridSystem;
};

// matrix for univariate polynomials
class upMatrix
{
protected:
	UnivariatePolynomial *data;
	int size1;
	int size2;

public:
	upMatrix();
	upMatrix(const int m, const int n);
	upMatrix(const int n);
	upMatrix(const iMatrix & A);
	upMatrix(const iMatrix2 & A);
	upMatrix(const upMatrix & upm);
	~upMatrix();

	int rows() const;
	int cols() const;
	int degree() const;

	bool isZero() const;

	void intEval(iMatrix & result, const std::vector<Interval> & val_exp_table) const;	// interval evaluation based on the monomial form
	void intEval(iMatrix & result, const Interval & val) const;							// interval evaluation based on the Horner form

	void intEval(iMatrix2 & result, const std::vector<Interval> & val_exp_table) const;
	void intEval(iMatrix2 & result, const Interval & val) const;

	void integral();
	void times_x(const int order);
	void transpose(upMatrix & result) const;

	void times_pars(mpMatrix & result) const;

	void ctrunc(iMatrix & rem, const int order, const std::vector<Interval> & val_exp_table);
	void ctrunc(iMatrix & rem, const int order, const Interval & val);

	void ctrunc(iMatrix & rem1, iMatrix & rem2, const int order, const std::vector<Interval> & val1_exp_table, const std::vector<Interval> & val2_exp_table);
	void ctrunc(iMatrix & rem1, iMatrix & rem2, const int order, const Interval & val1, const Interval & val2);

	void ctrunc(const int order, const std::vector<Interval> & val_exp_table);
	void ctrunc(const int order, const Interval & val);

	void nctrunc(const int order);

	void substitute(upMatrix & result, const std::vector<UnivariatePolynomial> & t_exp_table) const;
	void substitute(upMatrix & result, const UnivariatePolynomial & t) const;

	void output(FILE *fp) const;

	void decompose(upMatrix & positive, upMatrix & negative, iMatrix2 & im2_rem) const;

	upMatrix & operator += (const upMatrix & upm);
	upMatrix & operator += (const iMatrix & A);
	upMatrix & operator += (const Real & rad);
	upMatrix & operator -= (const upMatrix & upm);
	upMatrix & operator -= (const iMatrix & A);
	upMatrix & operator *= (const upMatrix & upm);
	upMatrix & operator *= (const iMatrix & A);
	upMatrix & operator *= (const Interval & I);

	upMatrix operator + (const upMatrix & upm) const;
	upMatrix operator + (const iMatrix & A) const;
	upMatrix operator - (const upMatrix & upm) const;
	upMatrix operator - (const iMatrix & A) const;
	upMatrix operator * (const upMatrix & upm) const;
	upMatrix operator * (const iMatrix & A) const;
	upMatrix operator * (const iMatrix2 & A) const;
	upMatrix operator * (const Interval & I) const;
	mpMatrix operator * (const mpMatrix & mpm) const;

	UnivariatePolynomial * operator [] (const int i);

	upMatrix & operator = (const upMatrix & upm);

	friend class iMatrix;
	friend class iMatrix2;
};

// matrix for multivariate polynomials
class mpMatrix
{
protected:
	Polynomial *data;
	int size1;
	int size2;

public:
	mpMatrix();
	mpMatrix(const int m, const int n);
	mpMatrix(const int n);
	mpMatrix(const mpMatrix & mpm);
	~mpMatrix();

	int rows() const;
	int cols() const;

	void intEval(mpMatrix & result, const std::vector<Interval> & val_exp_table) const;
	void intEval(iMatrix & result, const std::vector<Interval> & domain) const;

	void output(FILE *fp, const std::vector<std::string> & varNames) const;

	mpMatrix & operator += (const mpMatrix & mpm);

	mpMatrix operator + (const mpMatrix & mpm) const;

	Polynomial * operator [] (const int i);
	mpMatrix & operator = (const mpMatrix & mpm);

	friend class iMatrix;
	friend class upMatrix;
};

class MatrixParseSetting
{
public:
	std::string strExpression;
	iMatrix result;

public:
	MatrixParseSetting();
	MatrixParseSetting(const MatrixParseSetting & setting);
	~MatrixParseSetting();

	MatrixParseSetting & operator = (const MatrixParseSetting & setting);
};

void to_iMatrix2(iMatrix2 & result, const rMatrix & lo, const rMatrix & up);

extern MatrixParseSetting matrixParseSetting;

}

void parse_Matrix();

#endif /* MATRIX_H_ */


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

#include <gsl/gsl_rng.h>
#include <gsl/gsl_statistics.h>

#define N (4096*4*16)

int main (void)
{
    long countsq;
    double x, r2;
    //6 dimensional space
    int dim = 6;

    gsl_rng *r;

    r = gsl_rng_alloc (gsl_rng_taus2);
    gsl_rng_set (r, 1UL);

    long i, j, nn;

    printf ("# Iter        Npoints   Volume     AbsErr\n");
    nn = N;
    for (j = 0; j < 16; j++)
    {
        countsq = 0L;
        for (i = 0; i < nn; i++)
        {
            // 4-dimensional distance from the origin, squared
            r2 = 0.;
            for (int k = 0; k < dim; k++)
            {
                x = 2 * gsl_rng_uniform (r) - 1.;
                r2 += x * x;
            }

            // are we inside the unit ball
            if (r2 <= 1.)
            {
                countsq++;
            }
        }
        // ratio of the volume of the 4-dimensional unit ball and
        // and 4-dimensional cube of side length 2.
        double vol = ((double) countsq) * pow (2, (double) dim) / nn;
        double exact = M_PI * M_PI / 2;

        printf ("%3ld  %16ld   %f   %f\n", j, nn, vol, fabs (exact - vol));
        nn *= 2;
    }

    gsl_rng_free (r);

    return 0;
}


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

double fn1 (double x, void * params)
{
  return cos(x) + 1.0;
}

int
main (void)
{
  int status;
  int iter = 0, max_iter = 100;
  const gsl_min_fminimizer_type *T;
  gsl_min_fminimizer *s;
  double m = 2.0, m_expected = M_PI;
  double a = 0.0, b = 6.0;
  gsl_function F;

  F.function = &fn1;
  F.params = 0;

  T = gsl_min_fminimizer_brent;
  s = gsl_min_fminimizer_alloc (T);
  gsl_min_fminimizer_set (s, &F, m, a, b);

  printf ("using %s method\n",
          gsl_min_fminimizer_name (s));

  printf ("%5s [%9s, %9s] %9s %10s %9s\n",
          "iter", "lower", "upper", "min",
          "err", "err(est)");

  printf ("%5d [%.7f, %.7f] %.7f %+.7f %.7f\n",
          iter, a, b,
          m, m - m_expected, b - a);

  do
    {
      iter++;
      status = gsl_min_fminimizer_iterate (s);

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

      status
        = gsl_min_test_interval (a, b, 0.001, 0.0);

      if (status == GSL_SUCCESS)
        printf ("Converged:\n");

      printf ("%5d [%.7f, %.7f] "
              "%.7f %+.7f %.7f\n",
              iter, a, b,
              m, m - m_expected, b - a);
    }
  while (status == GSL_CONTINUE && iter < max_iter);

  gsl_min_fminimizer_free (s);

  return status;
}


/*
 * cube.c -- solutions of a cubic system
 *
 * (c) 2016 Prof Dr Andreas Mueller, Hochschule Rapperswil
 */
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv2.h>

int	f(double x, const double y[], double f[], void *params) {
	f[0] = (y[0] - 1) * y[0] * (y[0] + 1);
	return GSL_SUCCESS;
}

int	singlecurve(FILE *out, const char *name, double x0, double y0) {
	printf("%s: %f\n", name, y0);
	gsl_odeiv2_system	system = { f, NULL, 1, NULL };
	gsl_odeiv2_driver	*driver
		= gsl_odeiv2_driver_alloc_y_new(&system, gsl_odeiv2_step_rk8pd,
			1e-6, 1e-6, 0.0);
	fprintf(out, "path %s;\n", name);
	double	x = x0;
	double	y[1] = { y0 };
	fprintf(out, "%s = (%.3f,%.3f)", name, x, y[0]);
	for (int i = 1; i <= 400; i++) {
		double	xnext = x + 0.01;
		int	status = gsl_odeiv2_driver_apply(driver, &x, xnext, y);
		if (status != GSL_SUCCESS) {
			fprintf(stderr, "error: rc = %d\n", status);
			fprintf(out, ";\n");
			return -1;
		}
		if (fabs(y[0]) > 100) {
			goto end;
		}
		fprintf(out, "\n    --(%.3f,%.3f)", x, y[0]);
	}
end:
	fprintf(out, ";\n");
	gsl_odeiv2_driver_free(driver);
	return 1;
}

int	centercurve(FILE *out, const char *name, double y0) {
	gsl_odeiv2_system	system = { f, NULL, 1, NULL };
	gsl_odeiv2_driver	*driver
		= gsl_odeiv2_driver_alloc_y_new(&system, gsl_odeiv2_step_rk8pd,
			-1e-6, 1e-6, 0.0);
	double	x = 0;
	double	y[1] = { y0 };
	double	xnext = -2;
	// integrate backwards to the initial value
	int	status = gsl_odeiv2_driver_apply(driver, &x, xnext, y);
	gsl_odeiv2_driver_free(driver);
	driver = gsl_odeiv2_driver_alloc_y_new(&system, gsl_odeiv2_step_rk8pd,
			1e-6, 1e-6, 0.0);
/*
	if (fabs(y[0]) > 100) {
		return -1;
	}
*/
	singlecurve(out, name, -2, y[0]);
	return 1;
}

int	main(int argc, char *argv[]) {
	FILE	*out = fopen("cube.mp", "w");
	char	suffix = 'a';
	char	pathname[6];

	for (int i = 0; i < 16; i++) {
		double	y0 = -1.5 + i * 0.2;
		snprintf(pathname, sizeof(pathname), "path%c", suffix++);
		centercurve(out, pathname, y0);
	}

	return EXIT_SUCCESS;
}


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <time.h>
#define PI 3.1416
#define USAGE "./mcmc_solar.x n_steps n_burn"

void likelihood(float *chi, float *y_obs, float *y_model, int n_puntos);
void my_model(float *mine, float *anio ,float a, float b, float c, float d, int n_puntos);
void mcmc(int n_steps, float *a, float *b, float *c, float *d, float *l, float *x_obs, float *y_obs,int n_puntos, float *mine);
void min(float *mine, float *x_obs, float *a, float *b, float *c, float *d, float *l, int n_steps, int n_puntos);

void leer(float *anio, float *manc, int n_puntos);
float *reserva(int n_puntos);
void print_array(float *a, float *b, float *c, float *d , float *l, int n_steps, int n_burn);

int main(int argc, char **argv){

	srand48(time(NULL));

	//Valores que entran
	int n_steps=atof(argv[1]);
	int n_burn=atof(argv[2]);
	//Inicializacion de los arrays que guardan los datos
	int n_puntos= 4141;
	float *manc;
	float *anio;
	anio=reserva(n_puntos);
	manc=reserva(n_puntos);

	leer(anio, manc, n_puntos);

	//Array con los resultados del ajuste
	float *mine;
	mine=reserva(n_puntos);

	//Arrays que guardan los datos de las variables
	float *a;
	float *b;
	float *c;
	float *d;
	float *l;
	a=reserva(n_steps);
	b=reserva(n_steps);
	c=reserva(n_steps);
	d=reserva(n_steps);
	l=reserva(n_steps);
	//Primer paso en la cadena
	a[0]=drand48();
	b[0]=drand48();
	c[0]=drand48();
	d[0]=drand48();
	float chi;
	my_model(mine,anio,a[0],b[0],c[0],d[0],n_puntos);
	likelihood(&chi,manc,mine,n_puntos);
	l[0]=chi;

	//Iteraciones
	mcmc(n_steps,a,b,c,d,l,anio,manc,n_puntos,mine);
	min(mine,anio,a,b,c,d,l,n_steps,n_puntos);

	print_array(a,b,c,d,l,n_steps,n_burn);

	free(mine);
	free(anio);
	free(manc);
	free(a);
	free(b);
	free(c);
	free(d);

	return 0;
}

//OBTENCION DEL MEJOR AJUSTE
void min(float *mine, float *x_obs, float *a, float *b, float *c, float *d, float *l, int n_steps, int n_puntos){
	int i;
	float temp, a2, b2, c2, d2;
	temp=l[0];
	a2=a[0];
	b2=b[0];
	c2=c[0];
	d2=d[0];
	for(i=1;i<n_steps;i++){
		if(temp > l[i] ){
			temp=l[i];
			a2=a[i];
			b2=b[i];
			c2=c[i];
			d2=d[i];
		}
	}
	my_model(mine,x_obs,a2,b2,c2,d2,n_puntos);
}

//ITERACIONES
void mcmc(int n_steps, float *a, float *b, float *c, float *d, float *l, float *x_obs, float *y_obs,int n_puntos, float *mine){
	int i;
	float a2,b2,c2,d2, chi, chi2, alpha, beta;
	const gsl_rng_type * T;
	gsl_rng * r;
	gsl_rng_env_setup();
	T = gsl_rng_default;
	r = gsl_rng_alloc (T);

    for(i=0;i<(n_steps-1);i++){
		a2 = a[i] + gsl_ran_gaussian(r, 0.1);
		b2 = b[i] + gsl_ran_gaussian(r, 0.1);
		c2 = c[i] + gsl_ran_gaussian(r, 0.1);
		d2 = c[i] + gsl_ran_gaussian(r, 0.1);

		float *y_init;
		float *y_prime;
		y_init=reserva(n_puntos);
		y_prime=reserva(n_puntos);

		my_model(y_init,x_obs,a[i],b[i],c[i],d[i],n_puntos);
		my_model(y_prime,x_obs,a2,b2,c2,d2,n_puntos);

		likelihood(&chi,y_obs,y_init,n_puntos);
		likelihood(&chi2,y_obs,y_prime,n_puntos);

		alpha = -chi2 + chi;
		if(alpha >= 0.0){
			a[i+1]=a2;
			b[i+1]=b2;
			c[i+1]=c2;
			d[i+1]=d2;
			l[i+1]=chi2;
		}
		else{
			beta = drand48();
			if(alpha < -6000.0 ){
				a[i+1]=a[i];
				b[i+1]=b[i];
				c[i+1]=c[i];
				d[i+1]=d[i];
				l[i+1]=chi;
			}
			else if(beta <= exp(alpha)){
				a[i+1]=a2;
				b[i+1]=b2;
				c[i+1]=c2;
				d[i+1]=d2;
				l[i+1]=chi2;
			}
			else{
				a[i+1]=a[i];
				b[i+1]=b[i];
				c[i+1]=c[i];
				d[i+1]=d[i];
				l[i+1]=chi;
			}
		}

		free(y_init);
		free(y_prime);
	}

  gsl_rng_free (r);
}

//LECTURA DEL ARCHIVO
void leer(float *anio, float *manc, int n_puntos){

	float *mes;
	mes=reserva(n_puntos);

	FILE *in;
	int i;
	in=fopen("monthrg.dat","r");
	if(!in){
    printf("problems opening the file %s\n","monthrg.dat");
    exit(1);
	}
	float temp1;
	float temp2;
	for(i=0;i<n_puntos;i++){
	  do{
		  fscanf(in, "%f %f %f %f %f\n", &anio[i], &mes[i], &temp1, &manc[i] , &temp2);
	  }while(manc[i] == -99.0);
	}
	fclose(in);

	//Calculo del anio como decimal
	for(i=0;i<n_puntos;i++){
		anio[i] = anio[i] + (mes[i]-1.0)/12.0;
	}
	free(mes);
}

//IMPRESION DE LOS ARRAYS
void print_array(float *a, float *b, float *c, float *d , float *l, int n_steps, int n_burn){
  int i;
  for(i=n_burn;i<n_steps;i++){
    printf("%f %f %f %f %f\n", a[i], b[i], c[i] , d[i], l[i]);
  }
}

//EVALUAR LOS PUNTOS CON EL AJUSTE
void my_model(float *mine, float *anio ,float a, float b, float c, float d, int n_puntos){
	int i;
	for(i=0;i<n_puntos;i++){
		mine[i] = a * cos((2*PI/d)*anio[i] + b) + c;
	}
}

//RESERVA DE MEMORIA
float *reserva(int n_puntos){
  float *array;
  int i;
  if(!(array = malloc(n_puntos * sizeof(float)))){
    printf("Problema en reserva\n");
    exit(1);
  }
  for(i=0;i<n_puntos;i++){
    array[i] = 0.0;
  }
  return array;
}

//CALCULO DE LA VARIACION DEL AJUSTE
void likelihood(float *chi, float *y_obs, float *y_model, int n_puntos){
	int i;
	float temp=0;
	for(i=0;i<n_puntos;i++){
		temp=temp+pow(y_obs[i]-y_model[i],2.0);
	}
	*chi=(1.0/2.0)*temp;
}


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

double myfunc(unsigned n, const double *x, double *grad, void *my_func_data)
{
    //printf("%f\t%f\t", grad[0], grad[1]);
    //printf("%f\t%f\n", x[0], x[1]);
    if (grad) {
        grad[0] = 0.0;
        grad[1] = 0.5 / sqrt(x[1]);
    }
    //printf("%f\n", sqrt(x[1]));
    //sleep(1);
    return sqrt(x[1]);
}

typedef struct {
    double a, b;
} my_constraint_data;

double myconstraint(unsigned n, const double *x, double *grad, void *data)
{
    my_constraint_data *d = (my_constraint_data *) data;
    double a = d->a, b = d->b;
    if (grad) {
        grad[0] = 3 * a * (a*x[0] + b) * (a*x[0] + b);
        grad[1] = -1.0;
    }
    return ((a*x[0] + b) * (a*x[0] + b) * (a*x[0] + b) - x[1]);
 }

int main(){
    double lb[2] = { -HUGE_VAL, 0 }; /* lower bounds */
    nlopt_opt opt;

    //opt = nlopt_create(NLOPT_LD_MMA, 2); /* algorithm and dimensionality */
    opt = nlopt_create(NLOPT_LN_COBYLA, 2); /* algorithm and dimensionality */
    nlopt_set_lower_bounds(opt, lb);
    nlopt_set_min_objective(opt, myfunc, NULL);

    my_constraint_data data[2] = { {2,0}, {-1,1} };

    nlopt_add_inequality_constraint(opt, myconstraint, &data[0], 1e-8);
    nlopt_add_inequality_constraint(opt, myconstraint, &data[1], 1e-8);

    //nlopt_set_xtol_rel(opt, 1e-4);
    nlopt_set_stopval(opt, (sqrt(8./27.) + 1e-3));

    double x[2] = { 1.234, 5.678 };  /* `*`some` `initial` `guess`*` */
    double minf; /* `*`the` `minimum` `objective` `value,` `upon` `return`*` */
    if (nlopt_optimize(opt, x, &minf) < 0) {
	printf("nlopt failed!\n");
    }
    else {
	printf("found minimum at f(%g,%g) = %0.10g\n", x[0], x[1], minf);
    }

    nlopt_destroy(opt);
}


#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <assert.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_sort.h>
#include <gsl/gsl_roots.h>
#include <gsl/gsl_sf_bessel.h>

#include "cosmocalc.h"
#include "weaklens.h"

double chiLim;

static void comp_lens_power_spectrum(lensPowerSpectra lps);

double lens_power_spectrum(double ell, lensPowerSpectra lps)
{
  if(lps->initFlag == 1 || lps->currCosmoNum != cosmoData.cosmoNum || lps->currWLNum != wlData.wlNum)
    {
      lps->initFlag = 0;
      lps->currCosmoNum = cosmoData.cosmoNum;
      lps->currWLNum = wlData.wlNum;

      comp_lens_power_spectrum(lps);
    }

  return exp(gsl_spline_eval(lps->spline,log(ell),lps->accel));
}

double nonlinear_powspec_for_lens(double k, double a)
{
  return nonlinear_powspec(k,a);
}

static double lenskern(double chi, double chis)
{
  if(chi > chis)
    return 0.0;
  else
    return 1.5*cosmoData.OmegaM*(100.0/CSOL)*(100.0/CSOL)/acomvdist(chi)*(chis-chi)/chis;
}

static double lenspk_integrand(double chi, void *p)
{
  lensPowerSpectra lps = (lensPowerSpectra) p;
  double sn = lps->sn;

  if(chi == 0.0 || chi < chiLim)
    return 0.0;
  else
    return lenskern(chi,lps->chis1)*lenskern(chi,lps->chis2)*(nonlinear_powspec_for_lens(lps->ell/chi,acomvdist(chi)) + sn);
}

static void comp_lens_power_spectrum(lensPowerSpectra lps)
{
#define WORKSPACE_NUM 100000
#define ABSERR 1e-12
#define RELERR 1e-12
#define TABLE_LENGTH 1000

  gsl_integration_workspace *workspace;
  gsl_function F;
  double result,abserr;
  double logltab[TABLE_LENGTH];
  double logpkltab[TABLE_LENGTH];
  double chimax;
  int i;

  //fill in bin information
  chiLim = lps->chiLim;
  if(lps->chis1 > lps->chis2)
    chimax = lps->chis1;
  else
    chimax = lps->chis2;

  fprintf(stderr,"doing lens pk - chiLim = %lf, chiMax = %lf\n",chiLim,chimax);

  //init
  workspace = gsl_integration_workspace_alloc((size_t) WORKSPACE_NUM);
  F.function = &lenspk_integrand;
  F.params = lps;

  //make table
  double lnlmin = log(wlData.lmin);
  double lnlmax = log(wlData.lmax);
  for(i=0;i<TABLE_LENGTH;++i)
    {
      logltab[i] = i*(lnlmax-lnlmin)/(TABLE_LENGTH-1) + lnlmin;

      lps->ell = exp(logltab[i]);
      gsl_integration_qag(&F,0.0,chimax,ABSERR,RELERR,(size_t) WORKSPACE_NUM,GSL_INTEG_GAUSS51,workspace,&result,&abserr);

      logpkltab[i] = log(result);
    }

  //free
  gsl_integration_workspace_free(workspace);

  //init splines and accels
  if(lps->spline != NULL)
    gsl_spline_free(lps->spline);
  lps->spline = gsl_spline_alloc(gsl_interp_akima,(size_t) (TABLE_LENGTH));
  gsl_spline_init(lps->spline,logltab,logpkltab,(size_t) (TABLE_LENGTH));
  if(lps->accel != NULL)
    gsl_interp_accel_reset(lps->accel);
  else
    lps->accel = gsl_interp_accel_alloc();

#undef TABLE_LENGTH
#undef ABSERR
#undef RELERR
#undef WORKSPACE_NUM
}

lensPowerSpectra init_lens_power_spectrum(double zs1, double zs2)
{
  lensPowerSpectra lps;

  lps = (lensPowerSpectra)malloc(sizeof(_lensPowerSpectra));
  assert(lps != NULL);

  lps->initFlag = 1;
  lps->zs1 = zs1;
  lps->zs2 = zs2;
  lps->chis1 = comvdist(1.0/(1.0 + zs1));
  lps->chis2 = comvdist(1.0/(1.0 + zs2));
  lps->spline = NULL;
  lps->accel = NULL;

  return lps;
}

void free_lens_power_spectrum(lensPowerSpectra lps)
{
  if(lps->spline != NULL)
    gsl_spline_free(lps->spline);
  if(lps->accel != NULL)
    gsl_interp_accel_free(lps->accel);
  free(lps);
}

////////////////////////////////////////
// corr. funcs!
//////////////////////////////////////

static double lenscfp_integrand(double ell, void *p)
{
  lensCorrFunc lcf = (lensCorrFunc) p;

  return ell/2.0/M_PI*lens_power_spectrum(ell,lcf->lps)*gsl_sf_bessel_J0(ell*lcf->theta/60.0/180.0*M_PI);

}

static double lenscfm_integrand(double ell, void *p)
{
  lensCorrFunc lcf = (lensCorrFunc) p;

  return ell/2.0/M_PI*lens_power_spectrum(ell,lcf->lps)*gsl_sf_bessel_Jn(4,ell*lcf->theta/60.0/180.0*M_PI);
}

static void comp_lens_corr_funcs(lensCorrFunc lcf)
{
#define WORKSPACE_NUM 100000
#define ABSERR 1e-12
#define RELERR 1e-12
#define TABLE_LENGTH 1000

  gsl_integration_workspace *workspace;
  gsl_function F;
  double result,abserr;
  double logttab[TABLE_LENGTH];
  double logcfptab[TABLE_LENGTH];
  double logcfmtab[TABLE_LENGTH];
  int i;
  double lntmin;
  double lntmax;

  //init
  workspace = gsl_integration_workspace_alloc((size_t) WORKSPACE_NUM);
  F.params = lcf;
  lntmin = log(wlData.tmin);
  lntmax = log(wlData.tmax);

  //make tables
  F.function = &lenscfp_integrand;
  for(i=0;i<TABLE_LENGTH;++i)
    {
      logttab[i] = i*(lntmax-lntmin)/(TABLE_LENGTH-1) + lntmin;

      lcf->theta = exp(logttab[i]);
      gsl_integration_qag(&F,wlData.lmin,wlData.lmax,ABSERR,RELERR,(size_t) WORKSPACE_NUM,GSL_INTEG_GAUSS51,workspace,&result,&abserr);

      logcfptab[i] = log(result);
    }

  F.function = &lenscfm_integrand;
  for(i=0;i<TABLE_LENGTH;++i)
    {
      logttab[i] = i*(lntmax-lntmin)/(TABLE_LENGTH-1) + lntmin;

      lcf->theta = exp(logttab[i]);
      gsl_integration_qag(&F,wlData.lmin,wlData.lmax,ABSERR,RELERR,(size_t) WORKSPACE_NUM,GSL_INTEG_GAUSS51,workspace,&result,&abserr);

      logcfmtab[i] = log(result);
    }

  //free
  gsl_integration_workspace_free(workspace);

  //init splines and accels
  if(lcf->splineP != NULL)
    gsl_spline_free(lcf->splineP);
  lcf->splineP = gsl_spline_alloc(gsl_interp_akima,(size_t) (TABLE_LENGTH));
  gsl_spline_init(lcf->splineP,logttab,logcfptab,(size_t) (TABLE_LENGTH));
  if(lcf->accelP != NULL)
    gsl_interp_accel_reset(lcf->accelP);
  else
    lcf->accelP = gsl_interp_accel_alloc();

  if(lcf->splineM != NULL)
    gsl_spline_free(lcf->splineM);
  lcf->splineM = gsl_spline_alloc(gsl_interp_akima,(size_t) (TABLE_LENGTH));
  gsl_spline_init(lcf->splineM,logttab,logcfmtab,(size_t) (TABLE_LENGTH));
  if(lcf->accelM != NULL)
    gsl_interp_accel_reset(lcf->accelM);
  else
    lcf->accelM = gsl_interp_accel_alloc();

#undef TABLE_LENGTH
#undef ABSERR
#undef RELERR
#undef WORKSPACE_NUM
}

double lens_corr_func_minus(double theta, lensCorrFunc lcf)
{
  if(lcf->initFlag == 1 || lcf->currCosmoNum != cosmoData.cosmoNum || lcf->currWLNum != wlData.wlNum)
    {
      lcf->initFlag = 0;
      lcf->currCosmoNum = cosmoData.cosmoNum;
      lcf->currWLNum = wlData.wlNum;

      comp_lens_corr_funcs(lcf);
    }

  return exp(gsl_spline_eval(lcf->splineM,log(theta),lcf->accelM));
}

double lens_corr_func_plus(double theta, lensCorrFunc lcf)
{
  if(lcf->initFlag == 1 || lcf->currCosmoNum != cosmoData.cosmoNum || lcf->currWLNum != wlData.wlNum)
    {
      lcf->initFlag = 0;
      lcf->currCosmoNum = cosmoData.cosmoNum;
      lcf->currWLNum = wlData.wlNum;

      comp_lens_corr_funcs(lcf);
    }

  return exp(gsl_spline_eval(lcf->splineP,log(theta),lcf->accelP));
}

lensCorrFunc init_lens_corr_func(lensPowerSpectra lps)
{
  lensCorrFunc lcf;

  lcf = (lensCorrFunc)malloc(sizeof(_lensCorrFunc));
  assert(lcf != NULL);

  lcf->initFlag = 1;
  lcf->lps = lps;
  lcf->splineM = NULL;
  lcf->accelM = NULL;
  lcf->splineP = NULL;
  lcf->accelP = NULL;

  return lcf;
}

void free_lens_corr_func(lensCorrFunc lcf)
{
  if(lcf->splineM != NULL)
    gsl_spline_free(lcf->splineM);
  if(lcf->accelM != NULL)
    gsl_interp_accel_free(lcf->accelM);

  if(lcf->splineP != NULL)
    gsl_spline_free(lcf->splineP);
  if(lcf->accelP != NULL)
    gsl_interp_accel_free(lcf->accelP);

  free(lcf);
}


/** @file
 * Based on the paper:
 * Quintana, F.A. (1998) ``Nonparametric Bayesian analysis for assessing homogeneity in $ k\times l$
 * contingency tables with fixed right margin totals''.
 * Also as Technical Report PUC/FM-96/7. Journal of the American Statistical Association, 93(443), 1140-1149.
 * Available as compressed postscript.
 * http://www.mat.puc.cl/~quintana/hcttr.ps.gz
 */

#ifndef BIO_CONTINGENCY_HOMOGENEITY_H_
#define BIO_CONTINGENCY_HOMOGENEITY_H_

#include "bio/defs.h"
#include "bio/partition.h"

#include <boost/assign/list_of.hpp>

#include <gsl/gsl_sf_log.h>
#include <gsl/gsl_sf_exp.h>
#include <gsl/gsl_machine.h>
#include <gsl/gsl_sf_gamma.h>

#include <numeric>
#include <stdexcept>

BIO_NS_START

typedef std::vector<double> gamma_vec_t;
extern gamma_vec_t contingency_gamma_exponential_prior;

template <class LambdaIt>
double
calculate_ln_D(
	LambdaIt lambda_begin,
	LambdaIt lambda_end)
{
	double result = gsl_sf_lngamma(std::accumulate(lambda_begin, lambda_end, double(0)));
	for (LambdaIt i = lambda_begin; lambda_end != i; ++i)
	{
		result -= gsl_sf_lngamma(*i);
	}

	return result;
}

/** Calculate natural logarithm of a term for one partition in Lm(n). */
template <class PartitionIt, class CountIt, class LambdaIt>
double
contingency_calculate_ln_L_term(
	PartitionIt part_it,
	unsigned m,
	CountIt n_begin,
	CountIt n_end,
	LambdaIt lambda_begin,
	LambdaIt lambda_end)
{
	double ln_result = 1.0;

#ifdef BIO_CONTINGENCY_PRINT_PARTITIONS
	//can only do this for smallish numbers
	if (26 >= part_it.M.size())
	{
		std::vector<char> v;
		for (unsigned i = 0; i != part_it.M.size(); ++i)
		{
			v.push_back('a' + i);
		}
		std::cout << *part_it[v] << '\n';
	}
#endif //BIO_CONTINGENCY_PRINT_PARTITIONS

	//for each set in the partition
	for (unsigned i = 0; i < m; ++i)
	{
		//calculate lambda + n
		typedef std::vector<double> lambda_vec_t;
		lambda_vec_t lambda_plus_n;
		{
			std::copy(lambda_begin, lambda_end, std::inserter(lambda_plus_n, lambda_plus_n.begin()));
			unsigned e_i = 0;
			unsigned j = 0;
			//for each index
			for (CountIt n = n_begin; n_end != n; ++j, ++n)
			{
				//is this j in this partition?
				if (i == part_it.M[j])
				{
					for (unsigned k = 0; k != (*n).size(); ++k)
					{
						lambda_plus_n[k] += (*n)[k];
					}
					if (1 < e_i)
					{
						ln_result += gsl_sf_log(double(e_i)); //the factorial term
					}
					++e_i;
				}
			}
		}

		ln_result += calculate_ln_D(lambda_begin, lambda_end);
		ln_result -= calculate_ln_D(lambda_plus_n.begin(), lambda_plus_n.end());
	}

	return ln_result;
}

/** Calculate Lm(n). */
template <class CountIt, class LambdaIt>
double
contingency_calculate_L(
	unsigned m,
	CountIt n_begin,
	CountIt n_end,
	LambdaIt lambda_begin,
	LambdaIt lambda_end)
{
	unsigned set_size = unsigned(n_end - n_begin);
	BOOST_ASSERT(unsigned(lambda_end - lambda_begin) == set_size);

	double result = 0;

	//iterate over all the partitions
	partition::iterator_k part_it(set_size, m);
	try
	{
		while (true)
		{
			const double ln_L_term =
				contingency_calculate_ln_L_term(
					part_it,
					m,
					n_begin,
					n_end,
					lambda_begin,
					lambda_end);

			//check for underflow - in which case it will be 0.
			if (ln_L_term > GSL_LOG_DBL_MIN)
			{
				result += gsl_sf_exp(ln_L_term);
			}

			++part_it;
		}
	}
	catch (std::overflow_error&) //how the partition code determines there are no more partitions!
	{
	}

	return result;
}

template <class Value>
Value
factorial(Value value)
{
	Value result = Value(1);
	while (value != 0)
	{
		result *= value;
		--value;
	}
	return result;
}

typedef double(* contingency_cluster_prior)(double c, void * void_params);

struct ContingencyClusterPrior
{
	virtual double operator()(double c) = 0;
	virtual double get_ln_prior(double c);
};

struct ExponentialPrior : ContingencyClusterPrior
{
	double mean;
	ExponentialPrior(double mean = 1.0);
	double operator()(double c);
	double get_ln_prior(double c);
};

struct GammaParams
{
	unsigned m;
	unsigned k;
	ContingencyClusterPrior * prior;
};

double
gamma_exponential_prior_integrand(double c, void * void_params);

double
contingency_calculate_gamma(
	unsigned m,
	unsigned k,
	ContingencyClusterPrior * prior);

template <class GammaIt, class CountIt, class LambdaIt>
double
contingency_calculate_bayes_factor(
	GammaIt gamma_begin,
	GammaIt gamma_end,
	CountIt n_begin,
	CountIt n_end,
	LambdaIt lambda_begin,
	LambdaIt lambda_end)
{
	const unsigned k = unsigned(n_end == n_begin ? 0 : n_begin->end() - n_begin->begin());

	double result = contingency_calculate_L(1, n_begin, n_end, lambda_begin, lambda_end);

	result *= (1 - factorial(k-1) * (*gamma_begin));

	result /= factorial(k-1);

	double sum = 0;
	for (unsigned i = 2; k + 1 != i; ++i)
	{
		sum += *(gamma_begin + i - 1) * contingency_calculate_L(i, n_begin, n_end, lambda_begin, lambda_end);
	}
	result /= sum;

	return result;
}

/** Contains the data for a contingency table and defines types. */
template <unsigned K, unsigned L>
struct ContingencyTable
{
	typedef boost::array<unsigned, K> row_t;
	typedef boost::array<row_t, L> table_t;
	typedef boost::array<double, L> lambda_vec_t;

	table_t data;

	static unsigned get_k() { return K; }
	static unsigned get_l() { return L; }
};

template <unsigned K, unsigned L>
std::ostream &
operator<<(std::ostream & os, const ContingencyTable<K, L> & table)
{
	for (unsigned l = 0; L != l; ++l)
	{
		for (unsigned k = 0; K != k; ++k)
		{
			os << table.data[l][k] << '\t';
		}
		os << '\n';
	}
	return os;
}

BIO_NS_END

#endif //BIO_CONTINGENCY_HOMOGENEITY_H_


//
//  her2.h
//  Linear Algebra Template Library
//
//  Created by Rodney James on 12/23/11.
//  Copyright (c) 2011 University of Colorado Denver. All rights reserved.
//

#ifndef _her2_h
#define _her2_h

/// @file her2.h Performs complex outer product of two vectors.

#include <cctype>
#include "latl.h"

namespace LATL
{
   /// @brief Performs a vector outer product of two complex vectors.
   ///
   /// For a complex Hermitian matrix A, complex vectors x and y, and complex scalar alpha,
   ///
   ///        A := alpha * x * y' + conj(alpha) * y * x' + A
   ///
   /// is computed. The result is Hermitian and is stored as either upper or lower triangular in A.
   /// @return 0 if success.
   /// @return -i if the ith argument is invalid.
   /// @tparam real_t Floating point type.
   /// @param uplo Specifies whether A is stored as upper or lower triangular.
   ///
   ///        if uplo = 'U' or 'u' then A is upper triangular
   ///        if uplo = 'L' or 'l' then A is lower triangular
   ///
   /// @param n Specifies the order of the matrix A.  n>=0
   /// @param alpha Complex scalar.
   /// @param x Pointer to complex vector x.
   /// @param incx Increment of the vector x.  x!=0
   /// @param y Pointer to complex vector y.
   /// @param incy Increment of the vector y.  y!=0
   /// @param A Pointer to complex Hermitian n-by-n matrix A.If uplo = 'U' or 'u' then only the upper
   /// triangular part of A is referenced and the lower part is not referenced.  If uplo = 'L' or 'l' then
   /// only the lower triangular part of A is referenced and the upper part is not referenced.
   /// @param ldA Column length of matrix A.  ldA>=n.
   /// @ingroup BLAS

   template <typename real_t>
   int HER2(char uplo, int_t n, complex<real_t> alpha, complex<real_t> *x, int_t incx, complex<real_t> *y, int_t incy, complex<real_t> *A, int_t ldA)
   {
      using std::conj;
      using std::real;
      using std::toupper;
      const complex<real_t> zero(0.0,0.0);
      complex<real_t> tx,ty;
      int_t i,j,kx,ky,jx,ix,jy,iy;

      uplo=toupper(uplo);

      if((uplo!='U')&&(uplo!='L'))
         return -1;
      else if(n<0)
         return -2;
      else if(incx==0)
         return -5;
      else if(incy==0)
         return -7;
      else if(ldA<n)
         return -9;
      else if((n==0)||(alpha==zero))
         return 0;

      if((incx==1)&&(incy==1))
      {
         if(uplo=='U')
         {
            for(j=0;j<n;j++)
            {
               tx=conj(alpha*x[j]);
               ty=alpha*conj(y[j]);
               for(i=0;i<j;i++)
                  A[i]+=x[i]*ty+y[i]*tx;
               A[j]=real(A[j])+real(x[j]*ty+y[j]*tx);
               A+=ldA;
            }
         }
         else
         {
            for(j=0;j<n;j++)
            {
               tx=conj(alpha*x[j]);
               ty=alpha*conj(y[j]);
               A[j]=real(A[j])+real(x[j]*ty+y[j]*tx);
               for(i=j+1;i<n;i++)
                  A[i]+=x[i]*ty+y[i]*tx;
               A+=ldA;
            }
         }
      }
      else
      {
         kx=(incx>0)?0:(1-n)*incx;
         ky=(incy>0)?0:(1-n)*incy;
         if(uplo=='U')
         {
            jx=kx;
            jy=ky;
            for(j=0;j<n;j++)
            {
               tx=conj(alpha*x[jx]);
               ty=alpha*conj(y[jy]);
               ix=kx;
               iy=ky;
               for(i=0;i<j;i++)
               {
                  A[i]+=x[ix]*ty+y[iy]*tx;
                  ix+=incx;
                  iy+=incy;
               }
               A[j]=real(A[j])+real(x[jx]*ty+y[jy]*tx);
               A+=ldA;
               jx+=incx;
               jy+=incy;
            }
         }
         else
         {
            jx=kx;
            jy=ky;
            for(j=0;j<n;j++)
            {
               tx=conj(alpha*x[jx]);
               ty=alpha*conj(y[jy]);
               ix=jx;
               iy=jy;
               A[j]=real(A[j])+real(x[jx]*ty+y[jy]*tx);
               for(i=j+1;i<n;i++)
               {
                  ix+=incx;
                  iy+=incy;
                  A[i]+=x[ix]*ty+y[iy]*tx;
               }
               jx+=incx;
               jy+=incy;
               A+=ldA;
            }
         }
      }
      return 0;
   }
#ifdef __latl_cblas
#include <cblas.h>

   template <> int HER2<float>(char uplo, int_t n, complex<float> alpha, complex<float> *x, int_t incx, complex<float> *y, int_t incy, complex<float> *A, int_t ldA)
   {
      uplo=std::toupper(uplo);
      if((uplo!='U')&&(uplo!='L'))
         return -1;
      else if(n<0)
         return -2;
      else if(incx==0)
         return -5;
      else if(ldA<n)
         return -7;

      const CBLAS_UPLO Uplo=(uplo=='U')?CblasUpper:CblasLower;

      cblas_cher2(CblasColMajor,Uplo,n,&alpha,x,incx,y,incy,A,ldA);

      return 0;
   }

   template <> int HER2<double>(char uplo, int_t n, complex<double> alpha, complex<double> *x, int_t incx, complex<double> *y, int_t incy, complex<double> *A, int_t ldA)
   {
      uplo=std::toupper(uplo);
      if((uplo!='U')&&(uplo!='L'))
         return -1;
      else if(n<0)
         return -2;
      else if(incx==0)
         return -5;
      else if(ldA<n)
         return -7;

      const CBLAS_UPLO Uplo=(uplo=='U')?CblasUpper:CblasLower;

      cblas_zher2(CblasColMajor,Uplo,n,&alpha,x,incx,y,incy,A,ldA);

      return 0;
   }
#endif

}
#endif


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

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

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

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

#pragma once

#include <cmath>
#include <iostream>
#include "angle.h"
#include <halley/utils/utils.h>
#include "halley/text/string_converter.h"
#include <gsl/gsl>

#ifdef min
#undef min
#endif
#ifdef max
#undef max
#endif

namespace Halley {
	//////////////////////////////
	// Vector2D class declaration
	template <typename T=float, class U=Angle<float>>
	class Vector2D {
	private:
		constexpr T mod(T a, T b) const { return a % b; }

	public:
		T x,y;

		// Constructors
		constexpr Vector2D () : x(0), y(0) {}

		constexpr Vector2D (T _x, T _y) : x(_x), y(_y) {}

		template <typename V>
		constexpr explicit Vector2D (Vector2D<V> vec) : x(T(vec.x)), y(T(vec.y)) {}

		Vector2D (T length, U angle)
		{
			float s, c;
			angle.sincos(s, c);
			x = s*length;
			y = c*length;
		}

		// Getter
		constexpr inline T& operator[](size_t n)
		{
			Expects(n <= 1);
			return (&x)[n];
		}
		constexpr inline T operator[](size_t n) const
		{
			Expects(n <= 1);
			return (&x)[n];
		}

		// Assignment and comparison
		constexpr inline Vector2D& operator = (Vector2D param) { x = param.x; y = param.y; return *this; }
		constexpr inline Vector2D& operator = (T param) { x = param; y = param; return *this; }
		constexpr inline bool operator == (Vector2D param) const { return x == param.x && y == param.y; }
		constexpr inline bool operator != (Vector2D param) const { return x != param.x || y != param.y; }
		constexpr inline bool operator < (Vector2D param) const { return y != param.y ? y < param.y : x < param.x; }

		// Basic algebra
		constexpr inline Vector2D operator + (Vector2D param) const { return Vector2D(x + param.x,y + param.y); }
		constexpr inline Vector2D operator - (Vector2D param) const { return Vector2D(x - param.x,y - param.y); }
		constexpr inline Vector2D operator * (Vector2D param) const { return Vector2D(x * param.x,y * param.y); }
		constexpr inline Vector2D operator / (Vector2D param) const { return Vector2D(x / param.x,y / param.y); }
		constexpr inline Vector2D operator % (Vector2D param) const { return Vector2D(mod(x, param.x), mod(y, param.y)); }

		constexpr inline Vector2D modulo(Vector2D param) const { return Vector2D(Halley::modulo<T>(x, param.x), Halley::modulo<T>(y, param.y)); }
		constexpr inline Vector2D floorDiv(Vector2D param) const { return Vector2D(Halley::floorDiv(x, param.x), Halley::floorDiv(y, param.y)); }

		constexpr inline Vector2D operator - () const { return Vector2D(-x,-y); }

		template <typename V>
		constexpr inline Vector2D operator * (V param) const { return Vector2D(T(x * param), T(y * param)); }

		template <typename V>
		constexpr inline Vector2D operator / (V param) const { return Vector2D(T(x / param), T(y / param)); }

		// In-place operations
		constexpr inline Vector2D& operator += (Vector2D param) { x += param.x; y += param.y; return *this; }
		constexpr inline Vector2D& operator -= (Vector2D param) { x -= param.x; y -= param.y; return *this; }
		constexpr inline Vector2D& operator *= (const T param) { x *= param; y *= param; return *this; }
		constexpr inline Vector2D& operator /= (const T param) { x /= param; y /= param; return *this; }

		// Get the normalized vector (unit vector)
		constexpr inline Vector2D unit () const
		{
			float len = length();
			if (len != 0) {
				return (*this) / len;
			} else {
				return Vector2D(0, 0);
			}
		}
		constexpr inline Vector2D normalized() const
		{
			return unit();
		}
		constexpr inline void normalize()
		{
			*this = unit();
		}

		// Get the orthogonal vector
		constexpr inline Vector2D orthoLeft () const { return Vector2D(-y, x); }
		constexpr inline Vector2D orthoRight () const { return Vector2D(y, -x); }

		// Cross product (the Z component of it)
		constexpr inline T cross (Vector2D param) const { return x * param.y - y * param.x; }

		// Dot product
		constexpr inline T dot (Vector2D param) const { return (x * param.x) + (y * param.y); }

		// Length
		constexpr inline T length () const { return static_cast<T>(std::sqrt(squaredLength())); }
		constexpr inline T len () const { return length(); }
		constexpr inline T manhattanLength() const { return std::abs(x) + std::abs(y); }

		// Squared length, often useful and much faster
		constexpr inline T squaredLength () const { return x*x+y*y; }

		// Projection on another vector
		inline Vector2D projection (Vector2D param) const
		{
			Vector2D unit = param.unit();
			return this->dot(unit) * unit;
		}
		inline T projectionLength (Vector2D param) const
		{
			return this->dot(param.unit());
		}

		// Rounding
		constexpr inline Vector2D floor() const { return Vector2D(std::floor(x), std::floor(y)); }
		constexpr inline Vector2D ceil() const { return Vector2D(std::ceil(x), std::ceil(y)); }
		constexpr inline Vector2D round() const { return Vector2D(std::round(x), std::round(y)); }

		// Gets the angle that this vector is pointing to
		inline U angle () const
		{
			U angle;
			angle.setRadians(atan2(y, x));
			return angle;
		}

		// Rotates vector by an angle
		inline Vector2D rotate (U angle) const
		{
			T cos, sin;
			angle.sincos(sin, cos);
			return Vector2D(x*cos - y*sin, x*sin + y*cos);
		}

		// Rotates vector by sine and cosine
		inline Vector2D rotate (T sine, T cosine) const
		{
			return Vector2D(x*cosine - y*sine, x*sine + y*cosine);
		}

		// Removes the length of the vector along the given axis
		constexpr inline Vector2D neutralize (Vector2D param) const
		{
			return *this - dot(param)*param;
		}

		String toString() const
		{
			return String("(") + x + ", " + y + ")";
		}

		constexpr Vector2D abs() const
		{
			return Vector2D(std::abs(x), std::abs(y));
		}

		constexpr inline static Vector2D<T,U> min(Vector2D<T,U> a, Vector2D<T,U> b)
		{
			return Vector2D<T,U>(std::min(a.x, b.x), std::min(a.y, b.y));
		}

		constexpr inline static Vector2D<T,U> max(Vector2D<T,U> a, Vector2D<T,U> b)
		{
			return Vector2D<T,U>(std::max(a.x, b.x), std::max(a.y, b.y));
		}
	};

	////////////////////
	// Global operators
	template <typename T,class U,typename V>
	inline Vector2D<T,U> operator * (V f, const Vector2D<T,U> &v)
	{
		return Vector2D<T,U>(T(v.x * f),T(v.y * f));
	}

	template <typename T,class U>
	std::ostream& operator<< (std::ostream& ostream, const Vector2D<T, U>& v)
	{
		ostream << "(" << v.x << "," << v.y << ")" ; return ostream;
	}

	////////////
	// Typedefs
	typedef Vector2D<double,Angle<double> > Vector2d;
	typedef Vector2D<> Vector2f;
	typedef Vector2D<int> Vector2i;
	typedef Vector2D<short> Vector2s;
	typedef Vector2D<char> Vector2c;
	typedef Vector2f Position;
	typedef Vector2f Size;
}

namespace std {
	template<>
	struct hash<Halley::Vector2i>
	{
		size_t operator()(const Halley::Vector2i& v) const
		{
			return std::hash<long long>()(*reinterpret_cast<const long long*>(&v));
		}
	};

	template<>
	struct hash<Halley::Vector2f>
	{
		size_t operator()(const Halley::Vector2f& v) const
		{
			return std::hash<long long>()(*reinterpret_cast<const long long*>(&v));
		}
	};
}


/* =======================================
 * Code for 8.06 Term Paper (Spring 2015)
 *
 * Author : To Chin Yu (ytc@mit.edu)
 * Purpose: 1D lattice simulation
 * V(x) = alpha*x^2 + lambda*x^4
 * =======================================
 */
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#define sq(x) pow((x),2.0)
#define cb(x) pow((x),3.0)
#define qd(x) pow((x),4.0)

// GSL Random Number Generator (https://www.gnu.org/software/gsl/)
#include <gsl/gsl_rng.h>
const gsl_rng_type * T; gsl_rng * r;

// Parameters
#define N_lat   5000  // Lattice size
#define a       0.1   // Lattice spacing
#define N_therm 500   // Thermalization
#define N_meas  1000  // Number of configuration to measure
#define N_skip  100   // Decorrelation
#define alpha   0     // Strength of potential (harmonic)
#define lambda  1     // Strength of potential (anharmonic)
#define epsilon 0.2   // Size of fluctuation

#define N_bin 1000
#define dx 0.01
#define x_range 5.0 // x in [-range, range]

// Globals
double x[N_lat];      // Lattice
int acc,tot;          // Acceptance rate = acc/tot
double E0;            // Ground state energy
double C[N_lat];      // Correlator
double psi[N_bin];    // Wavefunction

// Output Files
FILE *fE0, *fCorRatio, *fpsi;

// Prototypes
void setup();
void cleanup();
void update();

int main(void){
  setup();

  // Thermalize
  for(int n=0;n<N_therm;n++){
    update();

    // Plot E0 during thermalization
    if(0){
      E0=0;
      for(int i=0;i<N_lat;i++){
        E0 += ( 2*x[i]*lambda*cb(x[i]) + lambda*qd(x[i]) )/N_lat;
      }
      fprintf(fE0,"%f\n", E0);
    }
  }

  // Do updates and measurements
  for(int n=0;n<N_meas;n++){
    // Decorrelate
    for(int m=0;m<N_skip;m++){ update(); }
    // Measurements:
    // 1. E0

    // Plot E0
    E0=0;
    for(int i=0;i<N_lat;i++){
      E0 += ( 2*x[i]*lambda*cb(x[i]) + lambda*qd(x[i]) )/N_lat;
    }
    fprintf(fE0,"%f\n", E0);

    // 2. Correlator
    for(int i=0;i<N_lat;i++){
      for(int T=0;T<N_lat;T++){
        C[T] += x[i]*x[(i+T)%N_lat] / (N_lat*N_meas);
      }
    }

    // 3. Wavefunction
    for(int i=0;i<N_lat;i++){
      if(abs(x[i])<x_range){
        psi[(int)floor((x[i]+x_range)/dx)]+=1;
      }
    }

  }

  // Plot correlator ratio
  for(int T=0;T<N_lat;T++){
    fprintf(fCorRatio,"%f \n", C[T]/C[(T+1)%N_lat]); // for large T, this gives a(E1-E0)
  }

  // Plot wavefunction
  for(int i=0;i<N_bin;i++){
    fprintf(fpsi,"%f, %f\n", i*dx-x_range, sqrt(psi[i]/(dx*N_meas*N_lat)));
  }

  printf("Acceptance rate: %f \n", (double)acc/tot);

  cleanup();
  return 0;
}

// Calculate difference in action
double dS(int i, double x_old){
  return (x[i]*(x[i]-x[(i+1)%N_lat]-x[(i-1+N_lat)%N_lat])/a
          + alpha*a*sq(x[i]) + lambda*a*qd(x[i]))
          - (x_old*(x_old-x[(i+1)%N_lat]-x[(i-1+N_lat)%N_lat])/a
               + alpha*a*sq(x_old) + lambda*a*qd(x_old));
}

void update(){
  double x_old;
  double d; // Difference in action
  for(int i=0;i<N_lat;i++){
    x_old=x[i];
    x[i]=x[i] + epsilon * (gsl_rng_uniform(r)-0.5)*2;
    acc++; tot++;
    d=dS(i,x_old);
    if(d>0){
      if(exp(-d)<gsl_rng_uniform(r)){ x[i]=x_old; acc--; } // Revert to old value
    }
  }
}

void setup(){
  // Cold start
  for(int i=0;i<N_lat;i++){ x[i]=0; C[i]=0; }

  // Initialize variables
  acc = 0;
  tot = 0;
  E0 = 0;
  for(int i=0;i<N_bin;i++){ psi[i]=0; }

  // Create Files
  fE0=fopen("E0.txt", "w+");
  fCorRatio=fopen("CorRatio.txt", "w+");
  fpsi=fopen("psi.txt", "w+");

  // Setup random environment
  gsl_rng_env_setup();
  T = gsl_rng_mt19937;
  r = gsl_rng_alloc (T);
}

void cleanup(){
  gsl_rng_free (r);

  // Close Files
  fclose(fE0);
  fclose(fCorRatio);
  fclose(fpsi);
}


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

struct data
{
  double *t;
  double *y;
  size_t n;
};

/* model function: a * exp( -1/2 * [ (t - b) / c ]^2 ) */
double
gaussian(const double a, const double b, const double c, const double t)
{
  const double z = (t - b) / c;
  return (a * exp(-0.5 * z * z));
}

int
func_f (const gsl_vector * x, void *params, gsl_vector * f)
{
  struct data *d = (struct data *) params;
  double a = gsl_vector_get(x, 0);
  double b = gsl_vector_get(x, 1);
  double c = gsl_vector_get(x, 2);
  size_t i;

  for (i = 0; i < d->n; ++i)
    {
      double ti = d->t[i];
      double yi = d->y[i];
      double y = gaussian(a, b, c, ti);

      gsl_vector_set(f, i, yi - y);
    }

  return GSL_SUCCESS;
}

int
func_df (const gsl_vector * x, void *params, gsl_matrix * J)
{
  struct data *d = (struct data *) params;
  double a = gsl_vector_get(x, 0);
  double b = gsl_vector_get(x, 1);
  double c = gsl_vector_get(x, 2);
  size_t i;

  for (i = 0; i < d->n; ++i)
    {
      double ti = d->t[i];
      double zi = (ti - b) / c;
      double ei = exp(-0.5 * zi * zi);

      gsl_matrix_set(J, i, 0, -ei);
      gsl_matrix_set(J, i, 1, -(a / c) * ei * zi);
      gsl_matrix_set(J, i, 2, -(a / c) * ei * zi * zi);
    }

  return GSL_SUCCESS;
}

int
func_fvv (const gsl_vector * x, const gsl_vector * v,
          void *params, gsl_vector * fvv)
{
  struct data *d = (struct data *) params;
  double a = gsl_vector_get(x, 0);
  double b = gsl_vector_get(x, 1);
  double c = gsl_vector_get(x, 2);
  double va = gsl_vector_get(v, 0);
  double vb = gsl_vector_get(v, 1);
  double vc = gsl_vector_get(v, 2);
  size_t i;

  for (i = 0; i < d->n; ++i)
    {
      double ti = d->t[i];
      double zi = (ti - b) / c;
      double ei = exp(-0.5 * zi * zi);
      double Dab = -zi * ei / c;
      double Dac = -zi * zi * ei / c;
      double Dbb = a * ei / (c * c) * (1.0 - zi*zi);
      double Dbc = a * zi * ei / (c * c) * (2.0 - zi*zi);
      double Dcc = a * zi * zi * ei / (c * c) * (3.0 - zi*zi);
      double sum;

      sum = 2.0 * va * vb * Dab +
            2.0 * va * vc * Dac +
                  vb * vb * Dbb +
            2.0 * vb * vc * Dbc +
                  vc * vc * Dcc;

      gsl_vector_set(fvv, i, sum);
    }

  return GSL_SUCCESS;
}

void
callback(const size_t iter, void *params,
         const gsl_multifit_nlinear_workspace *w)
{
  gsl_vector *f = gsl_multifit_nlinear_residual(w);
  gsl_vector *x = gsl_multifit_nlinear_position(w);
  double avratio = gsl_multifit_nlinear_avratio(w);
  double rcond;

  (void) params; /* not used */

  /* compute reciprocal condition number of J(x) */
  gsl_multifit_nlinear_rcond(&rcond, w);

  fprintf(stderr, "iter %2zu: a = %.4f, b = %.4f, c = %.4f, |a|/|v| = %.4f cond(J) = %8.4f, |f(x)| = %.4f\n",
          iter,
          gsl_vector_get(x, 0),
          gsl_vector_get(x, 1),
          gsl_vector_get(x, 2),
          avratio,
          1.0 / rcond,
          gsl_blas_dnrm2(f));
}

void
solve_system(gsl_vector *x, gsl_multifit_nlinear_fdf *fdf,
             gsl_multifit_nlinear_parameters *params)
{
  const gsl_multifit_nlinear_type *T = gsl_multifit_nlinear_trust;
  const size_t max_iter = 200;
  const double xtol = 1.0e-8;
  const double gtol = 1.0e-8;
  const double ftol = 1.0e-8;
  const size_t n = fdf->n;
  const size_t p = fdf->p;
  gsl_multifit_nlinear_workspace *work =
    gsl_multifit_nlinear_alloc(T, params, n, p);
  gsl_vector * f = gsl_multifit_nlinear_residual(work);
  gsl_vector * y = gsl_multifit_nlinear_position(work);
  int info;
  double chisq0, chisq, rcond;

  /* initialize solver */
  gsl_multifit_nlinear_init(x, fdf, work);

  /* store initial cost */
  gsl_blas_ddot(f, f, &chisq0);

  /* iterate until convergence */
  gsl_multifit_nlinear_driver(max_iter, xtol, gtol, ftol,
                              callback, NULL, &info, work);

  /* store final cost */
  gsl_blas_ddot(f, f, &chisq);

  /* store cond(J(x)) */
  gsl_multifit_nlinear_rcond(&rcond, work);

  gsl_vector_memcpy(x, y);

  /* print summary */

  fprintf(stderr, "NITER         = %zu\n", gsl_multifit_nlinear_niter(work));
  fprintf(stderr, "NFEV          = %zu\n", fdf->nevalf);
  fprintf(stderr, "NJEV          = %zu\n", fdf->nevaldf);
  fprintf(stderr, "NAEV          = %zu\n", fdf->nevalfvv);
  fprintf(stderr, "initial cost  = %.12e\n", chisq0);
  fprintf(stderr, "final cost    = %.12e\n", chisq);
  fprintf(stderr, "final x       = (%.12e, %.12e, %12e)\n",
          gsl_vector_get(x, 0), gsl_vector_get(x, 1), gsl_vector_get(x, 2));
  fprintf(stderr, "final cond(J) = %.12e\n", 1.0 / rcond);

  gsl_multifit_nlinear_free(work);
}

int
main (void)
{
  const size_t n = 300;  /* number of data points to fit */
  const size_t p = 3;    /* number of model parameters */
  const double a = 5.0;  /* amplitude */
  const double b = 0.4;  /* center */
  const double c = 0.15;  /* width */
  const gsl_rng_type * T = gsl_rng_default;
  gsl_vector *f = gsl_vector_alloc(n);
  gsl_vector *x = gsl_vector_alloc(p);
  gsl_multifit_nlinear_fdf fdf;
  gsl_multifit_nlinear_parameters fdf_params =
    gsl_multifit_nlinear_default_parameters();
  struct data fit_data;
  gsl_rng * r;
  size_t i;

  gsl_rng_env_setup ();
  r = gsl_rng_alloc (T);

  fit_data.t = malloc(n * sizeof(double));
  fit_data.y = malloc(n * sizeof(double));
  fit_data.n = n;

  /* generate synthetic data with noise */
  for (i = 0; i < n; ++i)
    {
      double t = (double)i / (double) n;
      double y0 = gaussian(a, b, c, t);
      double dy = gsl_ran_gaussian (r, 0.1 * y0);

      fit_data.t[i] = t;
      fit_data.y[i] = y0 + dy;
    }

  /* define function to be minimized */
  fdf.f = func_f;
  fdf.df = func_df;
  fdf.fvv = func_fvv;
  fdf.n = n;
  fdf.p = p;
  fdf.params = &fit_data;

  /* starting point */
  gsl_vector_set(x, 0, 1.0);
  gsl_vector_set(x, 1, 0.0);
  gsl_vector_set(x, 2, 1.0);

  fdf_params.trs = gsl_multifit_nlinear_trs_lmaccel;
  solve_system(x, &fdf, &fdf_params);

  /* print data and model */
  {
    double A = gsl_vector_get(x, 0);
    double B = gsl_vector_get(x, 1);
    double C = gsl_vector_get(x, 2);

    for (i = 0; i < n; ++i)
      {
        double ti = fit_data.t[i];
        double yi = fit_data.y[i];
        double fi = gaussian(A, B, C, ti);

        printf("%f %f %f\n", ti, yi, fi);
      }
  }

  gsl_vector_free(f);
  gsl_vector_free(x);
  gsl_rng_free(r);

  return 0;
}


/* randist/chisq.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 James Theiler, Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 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 <math.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

/* The chisq distribution has the form

   p(x) dx = (1/(2*Gamma(nu/2))) (x/2)^(nu/2 - 1) exp(-x/2) dx

   for x = 0 ... +infty */

double
gsl_ran_chisq (const gsl_rng * r, const double nu)
{
  double chisq = 2 * gsl_ran_gamma (r, nu / 2, 1.0);
  return chisq;
}

double
gsl_ran_chisq_pdf (const double x, const double nu)
{
  if (x < 0)
    {
      return 0 ;
    }
  else
    {
      if(nu == 2.0)
        {
          return exp(-x/2.0) / 2.0;
        }
      else
        {
          double p;
          double lngamma = gsl_sf_lngamma (nu / 2);

          p = exp ((nu / 2 - 1) * log (x/2) - x/2 - lngamma) / 2;
          return p;
        }
    }
}


/** @file */
#ifndef __CCL_UTILS_H_INCLUDED__
#define __CCL_UTILS_H_INCLUDED__

#include <gsl/gsl_spline.h>

#define CCL_MIN(a, b)  (((a) < (b)) ? (a) : (b))
#define CCL_MAX(a, b)  (((a) > (b)) ? (a) : (b))

CCL_BEGIN_DECLS

/**
 * Compute bin edges of N-1 linearly spaced bins on the interval [xmin,xmax]
 * @param xmin minimum value of spacing
 * @param xmax maximum value of spacing
 * @param N number of bins plus one (number of bin edges)
 * @return x, bin edges in range [xmin, xmax]
 */
double * ccl_linear_spacing(double xmin, double xmax, int N);

/**
 * Compute bin edges of N-1 logarithmically and then linearly spaced bins on the interval [xmin,xmax]
 * @param xminlog minimum value of spacing
 * @param xmin value when logarithmical ends and linear spacing begins
 * @param xmax maximum value of spacing
 * @param Nlin number of linear bins plus one (number of bin edges)
 * @param Nlog number of logarithmic bins plus one (number of bin edges)
 * @return x, bin edges in range [xminlog, xmax]
 */
double * ccl_linlog_spacing(double xminlog, double xmin, double xmax, int Nlin, int Nlog);

/**
 * Compute bin edges of N-1 logarithmically spaced bins on the interval [xmin,xmax]
 * @param xmin minimum value of spacing
 * @param xmax maximum value of spacing
 * @param N number of bins plus one (number of bin edges)
 * @return x, bin edges in range [xmin, xmax]
 */
double * ccl_log_spacing(double xmin, double xmax, int N);
//Returns array of N logarithmically-spaced values between xmin and xmax

double ccl_j_bessel(int l,double x);
//Spherical Bessel function of order l (adapted from CAMB)

/**
 * Spline wrapper
 * Used to take care of evaluations outside the supported range
 */
typedef struct {
  gsl_interp_accel *intacc; //GSL spline
  gsl_spline *spline;
  double x0,xf; //Interpolation limits
  double y0,yf; //Constant values to use beyond interpolation limit
} SplPar;

SplPar *ccl_spline_init(int n,double *x,double *y,double y0,double yf);

double ccl_spline_eval(double x,SplPar *spl);

void ccl_spline_free(SplPar *spl);

CCL_END_DECLS

#endif


/* -*- Mode: c++ -*- */
// copyright (c) 2006-2010 by Christos Dimitrakakis
// <christos.dimitrakakis@gmail.com>
/***************************************************************************
 *                                                                         *
 *   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.                                   *
 *                                                                         *
 ***************************************************************************/

#ifndef MATRIX_H
#define MATRIX_H

#include <gsl/gsl_cblas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_matrix.h>
#include <exception>
#include <iostream>
#include <stdexcept>
#include <vector>
#include "Vector.h"
#include "real.h"
//#include <atlas/cblas.h>

#undef REFERENCE_ACCESS

#define ACCURACY_LIMIT 1e-12

/** \brief An n-by-m dimensional matrix.

    TODO: Use the BLAS interface for some / most routines.
 */
class Matrix {
 public:
  /// Check bounds?
  enum BoundsCheckingStatus { NO_CHECK_BOUNDS = 0, CHECK_BOUNDS = 1 };
#ifdef NDEBUG
  static Matrix Null(int rows, int columns,
                     enum BoundsCheckingStatus check = NO_CHECK_BOUNDS);
  static Matrix Unity(int rows, int columns,
                      enum BoundsCheckingStatus check = NO_CHECK_BOUNDS);

  Matrix(int rows = 1, int columns = 1,
         enum BoundsCheckingStatus check = NO_CHECK_BOUNDS);
  Matrix(int rows, int columns, real* y,
         enum BoundsCheckingStatus check = NO_CHECK_BOUNDS);
  explicit Matrix(const Vector& v,
                  enum BoundsCheckingStatus check_ = NO_CHECK_BOUNDS);
#else
  static Matrix Null(int rows, int columns,
                     enum BoundsCheckingStatus check = CHECK_BOUNDS);
  static Matrix Unity(int rows, int columns,
                      enum BoundsCheckingStatus check = CHECK_BOUNDS);

  Matrix(int rows = 1, int columns = 1,
         enum BoundsCheckingStatus check = CHECK_BOUNDS);
  Matrix(int rows, int columns, real* y,
         enum BoundsCheckingStatus check = CHECK_BOUNDS);
  explicit Matrix(const Vector& v,
                  enum BoundsCheckingStatus check_ = CHECK_BOUNDS);
#endif
  Matrix(const Matrix& rhs, bool clone = true);
  ~Matrix();
  void Resize(int rows_, int columns_);
  Matrix AddRow(const Vector& rhs);
  Matrix AddColumn(const Vector& rhs);
  Matrix& operator=(const Matrix& rhs);
  bool operator==(const Matrix& rhs) const;
  bool operator!=(const Matrix& rhs) const;
  Matrix operator+(const Matrix& rhs);
  Matrix& operator+=(const Matrix& rhs);
  Matrix operator-(const Matrix& rhs);
  Matrix& operator-=(const Matrix& rhs);
  Matrix& operator*=(const real& rhs);
  Matrix operator*(const Matrix& rhs);
  Matrix operator*(const real& rhs);
  Matrix operator+(const real& rhs);
  Matrix operator-(const real& rhs);
  Matrix operator/(const real& rhs);
  /// Matrix inversion (defaults to GSL with LU)
  Matrix Inverse(real epsilon = ACCURACY_LIMIT) const {
    return GSL_Inverse();
    // return Inverse_LU();
  }
  Vector SVD_Solve(const Vector& b) const;

  Matrix GSL_Inverse() const;

  /** Matrix inversion using the Cholesky decomposition.

      First call Cholesky with accuracy epsilon to obtain $\fL,
      L'\f$ such that \f$A = LL'\f$.  Then solve for \f$X = A^{-1}\f$:
      \f[
      LL'X = I,
      \f]
      by dynamic programming.
  */
  Matrix Inverse_Cholesky(real epsilon = ACCURACY_LIMIT) const {
    Matrix U = Cholesky(epsilon);
    Matrix L(U, false);
    L.Transpose();
    return Inverse(L, U);
  }
  /** Matrix inversion using the LU decomposition.

      First call LUDecomposition with accuracy epsilon to obtain $\fL,
      U\f$ such that \f$A = LU\f$.  Then solve for \f$X = A^{-1}\f$:
      \f[
      LUX = I,
      \f]
      by dynamic programming.
  */
  Matrix Inverse_LU(real epsilon = ACCURACY_LIMIT) const {
    real det;
    Matrix tmp(*this);
    std::vector<Matrix> A = tmp.LUDecomposition(det, epsilon);
    return Inverse(A[0], A[1]);
  }

  Matrix Inverse(const Matrix& L, const Matrix& U) const;
  bool isSymmetric() const;
  bool isTriangular() const;
  bool isUpperTriangular() const;
  bool isLowerTriangular() const;
  real det() const;
  real tr() const;
  real L1Norm() const;
  real L2Norm() const;
  Matrix Multiple(const Matrix& rhs) const;
  Matrix Kron(const Matrix& rhs) const;
  real ColumnSum(int c) const;
  real RowSum(int r) const;
  real compute_det_triangular() const;
  real gaussian_elimination_forward(real epsilon = ACCURACY_LIMIT);
  std::vector<Matrix> QRDecomposition() const;
  std::vector<Matrix> LUDecomposition(real& determinant,
                                      real epsilon = ACCURACY_LIMIT);
  Matrix Cholesky(real epsilon = ACCURACY_LIMIT) const;
  void Cholesky(Matrix& chol, real epsilon = ACCURACY_LIMIT) const;
  void Clear();
  void Transpose();
  Vector getColumn(int c) const;
  Vector getRow(int r) const;
  void setColumn(int c, const Vector& x);
  void setRow(int r, const Vector& x);
  void SortRow(int r);
  void SortColumn(int r);
  int Rows() const;
  int Columns() const;
  Vector Vec() const;
  void Vec(const Vector& x);
  Vector RowMax() const;
  Vector ColumnMax() const;
  Vector RowMin() const;
  Vector ColumnMin() const;
  real& operator()(int i, int j);
  const real& operator()(int i, int j) const;
  void print(FILE* f) const;
  friend Matrix operator*(const real& lhs, const Matrix& rhs);
  friend Matrix operator*(const Vector& lhs, const Matrix& rhs);
  friend Vector operator*(const Matrix& lhs, const Vector& rhs);

 protected:
  int rows;     ///< number of rows in the matrix
  int columns;  ///< number of columns in the matrix
  real* x;      ///< data
#ifdef REFERENCE_ACCESS
  real** x_list;  ///< data pointers
  void MakeReferences();
#endif
  const enum BoundsCheckingStatus checking_bounds;
  bool transposed;
  bool clear_data;
  const real& qGet(int i, int j);
  void qSet(int i, int j, real v);
};

Matrix operator*(const real& lhs, const Matrix& rhs);
Matrix operator*(const Vector& lhs, const Matrix& rhs);
Vector operator*(const Matrix& lhs, const Vector& rhs);
real Mahalanobis2(const Vector& x, const Matrix& A, const Vector& y);

/// Kronecker product
inline Matrix Kron(const Matrix& lhs, const Matrix& rhs) {
  return lhs.Kron(rhs);
}

inline Matrix OuterProduct(const Vector& lhs, const Vector& rhs) {
  Matrix R(lhs.n, rhs.n);
  MatrixProduct(lhs, rhs, R);
  return R;
}

Matrix Transpose(const Matrix& rhs);

/// In-place transpose matrix
inline void Matrix::Transpose() { transposed = !transposed; }

inline int Matrix::Rows() const {
  if (transposed) {
    return columns;
  }
  return rows;
}

inline int Matrix::Columns() const {
  if (transposed) {
    return rows;
  }
  return columns;
}

inline real& Matrix::operator()(int i, int j) {
  if (transposed) {
    int tmp = i;
    i = j;
    j = tmp;
  }

  if (checking_bounds) {
    if (i < 0 || j < 0 || i >= rows || j >= columns) {
      fprintf(stderr, "bad access: (%d, %d) on a %d x %d matrix\n", i, j, rows,
              columns);
      fflush(stderr);
      throw std::out_of_range("matrix index out of range");
    }
  }
#ifdef REFERENCE_ACCESS
  return x_list[i][j];
#else
  return x[i * columns + j];
#endif
}

inline const real& Matrix::operator()(int i, int j) const {
  if (transposed) {
    int tmp = i;
    i = j;
    j = tmp;
  }

  if (checking_bounds) {
    if (i < 0 || j < 0 || i >= rows || j >= columns) {
      fprintf(stderr, "bad access: (%d, %d) on a %d x %d matrix\n", i, j, rows,
              columns);
      fflush(stderr);
      throw std::out_of_range("matrix index out of range");
    }
  }
#ifdef REFERENCE_ACCESS
  return x_list[i][j];
#else
  return x[i * columns + j];
#endif
}

#if 0
inline const real& Matrix::qGet(int i, int j) {
#ifdef REFERENCE_ACCESS
  return x_list[i][j];
#else
  return x[i * columns + j];
#endif
}

inline void Matrix::qSet(int i, int j, real v) {
#ifdef REFERENCE_ACCESS
  x_list[i][j] = v;
#else
  x[i * columns + j] = v;
#endif
}
#endif

#endif


/* specfunc/elljac.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 <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_pow_int.h>
#include <gsl/gsl_sf_elljac.h>

/* GJ: See [Thompson, Atlas for Computing Mathematical Functions] */

/* BJG 2005-07: New algorithm based on Algorithm 5 from Numerische
   Mathematik 7, 78-90 (1965) "Numerical Calculation of Elliptic
   Integrals and Elliptic Functions" R. Bulirsch.

   Minor tweak is to avoid division by zero when sin(x u_l) = 0 by
   computing reflected values sn(K-u) cn(K-u) dn(K-u) and using
   transformation from Abramowitz & Stegun table 16.8 column "K-u"*/

int
gsl_sf_elljac_e(double u, double m, double * sn, double * cn, double * dn)
{
  if(fabs(m) > 1.0) {
    *sn = 0.0;
    *cn = 0.0;
    *dn = 0.0;
    GSL_ERROR ("|m| > 1.0", GSL_EDOM);
  }
  else if(fabs(m) < 2.0*GSL_DBL_EPSILON) {
    *sn = sin(u);
    *cn = cos(u);
    *dn = 1.0;
    return GSL_SUCCESS;
  }
  else if(fabs(m - 1.0) < 2.0*GSL_DBL_EPSILON) {
    *sn = tanh(u);
    *cn = 1.0/cosh(u);
    *dn = *cn;
    return GSL_SUCCESS;
  }
  else {
    int status = GSL_SUCCESS;
    const int N = 16;
    double mu[16];
    double nu[16];
    double c[16];
    double d[16];
    double sin_umu, cos_umu, t, r;
    int n = 0;

    mu[0] = 1.0;
    nu[0] = sqrt(1.0 - m);

    while( fabs(mu[n] - nu[n]) > 4.0 * GSL_DBL_EPSILON * fabs(mu[n]+nu[n])) {
      mu[n+1] = 0.5 * (mu[n] + nu[n]);
      nu[n+1] = sqrt(mu[n] * nu[n]);
      ++n;
      if(n >= N - 1) {
        status = GSL_EMAXITER;
        break;
      }
    }

    sin_umu = sin(u * mu[n]);
    cos_umu = cos(u * mu[n]);

    /* Since sin(u*mu(n)) can be zero we switch to computing sn(K-u),
       cn(K-u), dn(K-u) when |sin| < |cos| */

    if (fabs(sin_umu) < fabs(cos_umu))
      {
        t = sin_umu / cos_umu;

        c[n] = mu[n] * t;
        d[n] = 1.0;

        while(n > 0) {
          n--;
          c[n] = d[n+1] * c[n+1];
          r = (c[n+1] * c[n+1]) / mu[n+1];
          d[n] = (r + nu[n]) / (r + mu[n]);
          }

        *dn = sqrt(1.0-m) / d[n];
        *cn = (*dn) * GSL_SIGN(cos_umu) / gsl_hypot(1.0, c[n]);
        *sn = (*cn) * c[n] /sqrt(1.0-m);
      }
    else
      {
        t = cos_umu / sin_umu;

        c[n] = mu[n] * t;
        d[n] = 1.0;

        while(n > 0) {
          --n;
          c[n] = d[n+1] * c[n+1];
          r = (c[n+1] * c[n+1]) / mu[n+1];
          d[n] = (r + nu[n]) / (r + mu[n]);
        }

        *dn = d[n];
        *sn = GSL_SIGN(sin_umu) / gsl_hypot(1.0, c[n]);
        *cn = c[n] * (*sn);
      }

    return status;
  }
}