/*
** Kendall's coefficient of concordance - Kendall's W
**
** G.Lohmann, July 2010
*/
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_sort_double.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_cdf.h>

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

extern void gsl_sort_vector_index (gsl_permutation *,gsl_vector *);

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

double kendall(size_t **xrank,double *r,int n,int m,int otype)
{
  int i,j;
  double S,W,R,chi_square=0;
  double nx=0,mx=0;

  nx = (double)n;
  mx = (double)m;

  for (i=0; i<n; i++) {
    r[i] = 0;
    for (j=0; j<m; j++) r[i] += xrank[j][i];
  }

  R = 0;
  for (i=0; i<n; i++) R += r[i];
  R /= nx;

  S = 0;
  for (i=0; i<n; i++) S += SQR(r[i] - R);

  W = 12.0*S/(mx*mx*(nx*nx-1)*nx);
  if (otype == 0) return W;

  /* by central limit theorem, chi_square is approx normal for large n */
  /* see en.wikipedia.org/wiki/Chi-square_distribution */
  chi_square = (nx-1)*mx*W;
  return (chi_square - nx)/(sqrt(2.0*nx));
}

double VKendall_W(gsl_matrix *data,int nvoxels,int otype)
{
  int i,j,n,m;
  double W0;
  static double *r=NULL;
  static gsl_vector *vec = NULL;
  static size_t **xrank=NULL;
  static gsl_permutation *perm=NULL,*rank=NULL;

  m = data->size1;
  n = nvoxels;

  if (vec == NULL) {
    r    = (double *) calloc(data->size2,sizeof(double));
    vec  = gsl_vector_calloc(data->size2);
    perm = gsl_permutation_alloc(data->size2);
    rank = gsl_permutation_alloc(data->size2);

    xrank = (size_t **) calloc(m,sizeof(size_t *));
    for (i=0; i<m; i++) xrank[i] = (size_t *) calloc(data->size2,sizeof(size_t));
  }

  gsl_vector_set_all (vec,9999);
  for (i=0; i<m; i++) {
    for (j=0; j<n; j++) {
      gsl_vector_set(vec,j,gsl_matrix_get(data,i,j));
    }
    gsl_sort_vector_index (perm, vec);
    gsl_permutation_inverse (rank, perm);
    for (j=0; j<n; j++) xrank[i][j] = rank->data[j];
  }

  /* Kendall's W of measured data */
  W0 = kendall(xrank,r,n,m,otype);
  return W0;
}


#pragma once

#include <cmath>

#include <gsl/gsl_math.h>

bool to_fraction(double num, long& numerator, long& denominator){
    double sign = num < 0 ? -1 : 1;
    double g = std::abs(num);
    long a = 0, b = 1, c = 1, d = 0;
    long s;
    int iter = 0;
    numerator = 0, denominator = 1;
    while (iter++ < 1e6){
        s = (long) std::floor(g);
        numerator = a + s*c;
        denominator = b + s*d;
        a = c;
        b = d;
        c = numerator;
        d = denominator;
        g = 1.0/(g-s);
        if(gsl_fcmp(sign*numerator, num*denominator, 1e-13) == 0){
            numerator *= sign;
            return true;
        }
    }
    return false;
}


/*solver/integration.c
*
* Function for numerical integration
* using quadrature and Monte Carlo
* methods.
*
* Author: Benjamin Vatter.
* email: benjaminvatterj@gmail.com
* date: 15 August 2015
*/

#include <stdlib.h>
#include <assert.h>
#include <math.h>
#include <stdio.h>
#include "integration.h"
#include "dbg.h"
#include <gsl/gsl_math.h>
#include <gsl/gsl_monte.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_rng.h>

typedef struct {
	int dims;
	int depth;
	double *lb;
	double *ub;
	double *real_x;
	double epsabs;
	double epsrel;
	size_t limit;
	gsl_integration_workspace * workspace;
	double * abserr;
	nquad_function *f;
} nquad_params;

void gsl_error (const char * reason, const char * file, int line, int gsl_errno)
{
  gsl_stream_printf ("NQUAD ERROR", file, line, reason);

  if (gsl_errno != GSL_EROUND) {
	  fflush (stderr);
	  fflush (stdout);
	  abort ();
  } else {
  	log_warn("NQUAD ERROR: rounding error detected. This result might be unreliable.");
  	fflush (stderr);
  	return;
  }
}

/*
SUbroutine for recursive multidimensional integration
*/
double nquad_f(double x, void * params)
{
	nquad_params *in_pars = (nquad_params *) params;
	double result;
	double abserr;
	int step = in_pars->depth;
	// Add value
	if (step>0) {
	 	in_pars->real_x[step-1] = x;
	}
	if (in_pars->dims == in_pars->depth){
		result = in_pars->f->function(in_pars->real_x, in_pars->dims, in_pars->f->params);
		return result;
	} else {
		// Consistency
		assert((step+1) <= in_pars->dims);
		// increase depth
		in_pars->depth += 1;
		gsl_integration_workspace *w = gsl_integration_workspace_alloc(in_pars->limit);
		gsl_function F = {.function = &nquad_f, .params = in_pars};
		gsl_integration_qags(&F, in_pars->lb[step], in_pars->ub[step], in_pars->epsabs, in_pars->epsrel, in_pars->limit, w,&result, &abserr);
		//gsl_integration_qag(&F, in_pars->lb[step], in_pars->ub[step], in_pars->epsabs, in_pars->epsrel, in_pars->limit, 2, w,&result, &abserr);
		in_pars->abserr[step] = abserr;
		gsl_integration_workspace_free(w);
		in_pars->depth -= 1;
		return result;
	}
}

/*
An extension of gsl_integration_qags
to n dimensions. The first parameter is
the number or dimensions. the signature of
f must be
f(double *x, void * params)

Parameters:
n - dimensions
f - function and parameters
lb - lower bounds to integrals
ub - upper bounds to integrals
epsabs - desired absolute error in each integral (only in nested-quadrature)
epsrel - desired relative error in each integral (only in nested-quadrature)
limit - max number of iterations
result - on out contains result
abserr - on out contains estimated error
*/
void nquad(int n, nquad_function *f,  double *lb, double *ub,
           double epsabs, double epsrel, size_t limit, double * result, double *abserr)
{
	assert(n>=1);
	nquad_params *new_params = malloc(sizeof(nquad_params));
	new_params->dims = n;
	new_params->depth = 0;
	new_params->lb = lb;
	new_params->ub = ub;
	new_params->epsabs = epsabs;
	new_params->epsrel = epsrel;
	new_params->limit = limit;
	new_params->abserr = malloc(sizeof(double) * n);
	new_params->f = f;
	new_params->real_x = malloc(sizeof(double) * n);

	// Set error handle
	gsl_error_handler_t *old_handler = gsl_set_error_handler(&gsl_error);

	// integrate
	result[0] = nquad_f(0.0, new_params);
	abserr[0] = 1.0;
	int i;
	double err = 0.0;
	for (i = 0; i<n; i++){
		err = (err < new_params->abserr[i])? new_params->abserr[i]: err;
	}
	*abserr = err;

	// release real x
	free(new_params->real_x);
	free(new_params->abserr);
	free(new_params);
	// recover error handle
	gsl_set_error_handler(old_handler);
}

/* Monte Carlo integration routine
* It has the same signature as the nquad function
* but it doesn't really use the epsrel value. It is
* done so to allow interchanging those two functions
* during runtime.
*
* This is based on the Monte Carlo plain routine found
* in GSL, written by Michael Booth
*/
void mcint(int n, nquad_function *f, double *lb, double *ub, double epsabs,
           double epsrel, size_t limit, double *result, double *abserr)
{
	double vol=1.0, m = 0, q = 0;
	double *x = malloc(sizeof(double)*n);
	size_t i, j;
	const gsl_rng_type *T;
	gsl_rng *r;
	gsl_rng_env_setup ();
	T = gsl_rng_default;
	r = gsl_rng_alloc (T);

	/* Sanity check */
	for(i=0; i<n; i++) {
		if(lb[i] >= ub[i]) {
			GSL_ERROR("Lower bound is higher than upper bound. Null feasible region.", GSL_EINVAL);
		}
		if(ub[i] - lb[i] > GSL_DBL_MAX) {
			GSL_ERROR("Range of integration is too large, please rescale",
                     GSL_EINVAL);
		}
		vol *= ub[i] - lb[i];
	}

	j=0;

	do
	{
		for (i=0; i < n; i++) {
			x[i] = lb[i] + gsl_rng_uniform_pos(r) * (ub[i] - lb[i]);
		}
		double fval = (f->function)(x, n, f->params);
		double d = fval - m;
		m += d / (n + 1.0);
		q += d * d * (n/(n +1.0));

		if (j < 2){ *abserr = GSL_POSINF;}
		else {
			*abserr = vol * sqrt(q / (limit * (limit - 1.0)));
		}
		j++;
	} while((*abserr > epsabs) && j < limit);

	*result = vol * m;
	if (*abserr > epsabs){
		log_warn("Monte Carlo integration max evaluation %d was insufficient"
		"to achieve the desired absolute error of %e", (int)limit, epsabs);
	}

	free(x);
	gsl_rng_free (r);
}


#include <petsc.h>

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

    PetscErrorCode ierr;
    PetscMPIInt    rank;
    PetscInt       i;
    PetscReal      localval, globalsum;
    PetscReal      x = 1; // default value

    ierr = PetscInitialize(&argc, &argv, NULL,
                           "Compute e in parallel with PETSc.\n\n");
    if (ierr) return ierr;

    ierr = MPI_Comm_rank(PETSC_COMM_WORLD, &rank); CHKERRQ(ierr);

    ierr = PetscOptionsBegin(PETSC_COMM_WORLD, NULL,
                             "Maclaurin Series for exp(x)",
                             NULL); CHKERRQ(ierr);
      ierr = PetscOptionsReal("-x", "input to exp(x) function",
                              NULL, x, &x, NULL); CHKERRQ(ierr);
    ierr = PetscOptionsEnd(); CHKERRQ(ierr);

    // compute  x^n / n!  where n = (rank of process) + 1
    localval = 1.;
    for (i = 1; i < rank+1; i++) localval *= x/i;

    // sum the contributions over all processes
    ierr = MPI_Allreduce(&localval, &globalsum, 1, MPIU_REAL, MPIU_SUM,
                         PETSC_COMM_WORLD); CHKERRQ(ierr);

    // output estimate of e and report on work from each process
    ierr = PetscPrintf(PETSC_COMM_WORLD,
                       "exp(x) is about %17.15f\n",
                       globalsum); CHKERRQ(ierr);

    ierr = PetscPrintf(PETSC_COMM_SELF,
                       "rank %d did %d flops\n",
                       rank, (rank > 0) ? rank-1 : 0); CHKERRQ(ierr);

    return PetscFinalize();
}


#include <err.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv2.h>

#define X   y[0]
#define V_X y[1]
#define Y   y[2]
#define V_Y y[3]

const double g = 9.81;        /* m/s^2 */
const double v_0 = 700.0;     /* m/s */
const double B2_m = 4.0e-5;   /* 1/m */
const double y_d = 1.0e4;     /* m */

int func(double t, const double y[], double f[], void *params);

int main(int argc, char *argv[]) {
    gsl_odeiv2_system sys = {func, NULL, 4, NULL};
    gsl_odeiv2_driver *driver = gsl_odeiv2_driver_alloc_y_new(
            &sys, gsl_odeiv2_step_rkf45, 1.0e-6, 1.0e-6, 0.0
    );
    int i = 1;
    double t = 0.0;
    double alpha = argc > 1 ? strtod(argv[1], NULL) : 1.0;
    double y[4];
    X = Y = 0.0;
    V_X = v_0*cos(alpha);
    V_Y = v_0*sin(alpha);
    while (Y >= 0.0) {
        double t_i = i/50.0;
        int status = gsl_odeiv2_driver_apply(driver, &t, t_i, y);
        if (status != GSL_SUCCESS) {
            warnx("apply status = %d", status);
            break;
        }
        printf("%.12le %.12le %.12le %.12le %.12le\n", t, X, Y, V_X, V_Y);
        i++;
    }
    gsl_odeiv2_driver_free(driver);
    return EXIT_SUCCESS;
}

#define DX_DT f[0]
#define DVX_DT f[1]
#define DY_DT f[2]
#define DVY_DT f[3]

int func(double t, const double y[], double f[], void *params) {
    double v = sqrt(V_X*V_X + V_Y*V_Y);
    double corr = exp(-Y/y_d);
    DX_DT  = V_X;
    DVX_DT = -B2_m*corr*v*V_X;
    DY_DT  = V_Y;
    DVY_DT = -g - B2_m*corr*v*V_Y;
    return GSL_SUCCESS;
}


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

/* falsepos.c -- falsepos root finding algorithm

   The false position algorithm uses bracketing by linear interpolation.

   If a linear interpolation step would decrease the size of the
   bracket by less than a bisection step would then the algorithm
   takes a bisection step instead.

   The last linear interpolation estimate of the root is used. If a
   bisection step causes it to fall outside the brackets then it is
   replaced by the bisection estimate (x_upper + x_lower)/2.

*/

#include <config.h>

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

#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_roots.h>

#include "roots.h"

typedef struct
  {
    double f_lower, f_upper;
  }
falsepos_state_t;

static int falsepos_init (void * vstate, gsl_function * f, double * root, double x_lower, double x_upper);
static int falsepos_iterate (void * vstate, gsl_function * f, double * root, double * x_lower, double * x_upper);

static int
falsepos_init (void * vstate, gsl_function * f, double * root, double x_lower, double x_upper)
{
  falsepos_state_t * state = (falsepos_state_t *) vstate;

  double f_lower, f_upper ;

  *root = 0.5 * (x_lower + x_upper);

  SAFE_FUNC_CALL (f, x_lower, &f_lower);
  SAFE_FUNC_CALL (f, x_upper, &f_upper);

  state->f_lower = f_lower;
  state->f_upper = f_upper;

  if ((f_lower < 0.0 && f_upper < 0.0) || (f_lower > 0.0 && f_upper > 0.0))
    {
      GSL_ERROR ("endpoints do not straddle y=0", GSL_EINVAL);
    }

  return GSL_SUCCESS;

}

static int
falsepos_iterate (void * vstate, gsl_function * f, double * root, double * x_lower, double * x_upper)
{
  falsepos_state_t * state = (falsepos_state_t *) vstate;

  double x_linear, f_linear;
  double x_bisect, f_bisect;

  double x_left = *x_lower ;
  double x_right = *x_upper ;

  double f_lower = state->f_lower;
  double f_upper = state->f_upper;

  double w ;

  if (f_lower == 0.0)
    {
      *root = x_left ;
      *x_upper = x_left;
      return GSL_SUCCESS;
    }

  if (f_upper == 0.0)
    {
      *root = x_right ;
      *x_lower = x_right;
      return GSL_SUCCESS;
    }

  /* Draw a line between f(*lower_bound) and f(*upper_bound) and
     note where it crosses the X axis; that's where we will
     split the interval. */

  x_linear = x_right - (f_upper * (x_left - x_right) / (f_lower - f_upper));

  SAFE_FUNC_CALL (f, x_linear, &f_linear);

  if (f_linear == 0.0)
    {
      *root = x_linear;
      *x_lower = x_linear;
      *x_upper = x_linear;
      return GSL_SUCCESS;
    }

  /* Discard the half of the interval which doesn't contain the root. */

  if ((f_lower > 0.0 && f_linear < 0.0) || (f_lower < 0.0 && f_linear > 0.0))
    {
      *root = x_linear ;
      *x_upper = x_linear;
      state->f_upper = f_linear;
      w = x_linear - x_left ;
    }
  else
    {
      *root = x_linear ;
      *x_lower = x_linear;
      state->f_lower = f_linear;
      w = x_right - x_linear;
    }

  if (w < 0.5 * (x_right - x_left))
    {
      return GSL_SUCCESS ;
    }

  x_bisect = 0.5 * (x_left + x_right);

  SAFE_FUNC_CALL (f, x_bisect, &f_bisect);

  if ((f_lower > 0.0 && f_bisect < 0.0) || (f_lower < 0.0 && f_bisect > 0.0))
    {
      *x_upper = x_bisect;
      state->f_upper = f_bisect;
      if (*root > x_bisect)
        *root = 0.5 * (x_left + x_bisect) ;
    }
  else
    {
      *x_lower = x_bisect;
      state->f_lower = f_bisect;
      if (*root < x_bisect)
        *root = 0.5 * (x_bisect + x_right) ;
    }

  return GSL_SUCCESS;
}

static const gsl_root_fsolver_type falsepos_type =
{"falsepos",                            /* name */
 sizeof (falsepos_state_t),
 &falsepos_init,
 &falsepos_iterate};

const gsl_root_fsolver_type  * gsl_root_fsolver_falsepos = &falsepos_type;


#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <assert.h>
#include <gsl/gsl_vector.h>

#define MAX(A,B) (((A)>(B))?(A):(B))

void reoshift(gsl_vector *v, int h)
{
  if ( h > 0 ) {
    gsl_vector *temp = gsl_vector_alloc(v->size);
    gsl_vector_view p = gsl_vector_subvector(v, 0, v->size - h);
    gsl_vector_view p1 = gsl_vector_subvector(temp, h, v->size - h);
    gsl_vector_memcpy(&p1.vector, &p.vector);
    p = gsl_vector_subvector(temp, 0, h);
    gsl_vector_set_zero(&p.vector);
    gsl_vector_memcpy(v, temp);
    gsl_vector_free(temp);
  }
}

gsl_vector *poly_long_div(gsl_vector *n, gsl_vector *d, gsl_vector **r)
{
  gsl_vector *nt = NULL, *dt = NULL, *rt = NULL, *d2 = NULL, *q = NULL;
  int gn, gt, gd;

  if ( (n->size >= d->size) && (d->size > 0) && (n->size > 0) ) {
    nt = gsl_vector_alloc(n->size); assert(nt != NULL);
    dt = gsl_vector_alloc(n->size); assert(dt != NULL);
    rt = gsl_vector_alloc(n->size); assert(rt != NULL);
    d2 = gsl_vector_alloc(n->size); assert(d2 != NULL);
    gsl_vector_memcpy(nt, n);
    gsl_vector_set_zero(dt); gsl_vector_set_zero(rt);
    gsl_vector_view p = gsl_vector_subvector(dt, 0, d->size);
    gsl_vector_memcpy(&p.vector, d);
    gsl_vector_memcpy(d2, dt);
    gn = n->size - 1;
    gd = d->size - 1;
    gt = 0;

    while( gsl_vector_get(d, gd) == 0 ) gd--;

    while ( gn >= gd ) {
      reoshift(dt, gn-gd);
      double v = gsl_vector_get(nt, gn)/gsl_vector_get(dt, gn);
      gsl_vector_set(rt, gn-gd, v);
      gsl_vector_scale(dt, v);
      gsl_vector_sub(nt, dt);
      gt = MAX(gt, gn-gd);
      while( (gn>=0) && (gsl_vector_get(nt, gn) == 0.0) ) gn--;
      gsl_vector_memcpy(dt, d2);
    }

    q = gsl_vector_alloc(gt+1); assert(q != NULL);
    p = gsl_vector_subvector(rt, 0, gt+1);
    gsl_vector_memcpy(q, &p.vector);
    if ( r != NULL ) {
      if ( (gn+1) > 0 ) {
	*r = gsl_vector_alloc(gn+1); assert( *r != NULL );
	p = gsl_vector_subvector(nt, 0, gn+1);
	gsl_vector_memcpy(*r, &p.vector);
      } else {
	*r = gsl_vector_alloc(1); assert( *r != NULL );
	gsl_vector_set_zero(*r);
      }
    }
    gsl_vector_free(nt); gsl_vector_free(dt);
    gsl_vector_free(rt); gsl_vector_free(d2);
    return q;
  } else {
    q = gsl_vector_alloc(1); assert( q != NULL );
    gsl_vector_set_zero(q);
    if ( r != NULL ) {
      *r = gsl_vector_alloc(n->size); assert( *r != NULL );
      gsl_vector_memcpy(*r, n);
    }
    return q;
  }
}

void poly_print(gsl_vector *p)
{
  int i;
  for(i=p->size-1; i >= 0; i--) {
    if ( i > 0 )
      printf("%lfx^%d + ",
	     gsl_vector_get(p, i), i);
    else
      printf("%lf\n", gsl_vector_get(p, i));
  }
}

gsl_vector *create_poly(int d, ...)
{
  va_list al;
  int i;
  gsl_vector *r = NULL;

  va_start(al, d);
  r = gsl_vector_alloc(d); assert( r != NULL );

  for(i=0; i < d; i++)
    gsl_vector_set(r, i, va_arg(al, double));

  return r;
}


#include <math.h>

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

#include "regional.h"

void init_weights(t_stochasticity_grid *g) {
    /* The weights will be normalized s.t. the sum of squared weights is one */
    g->weights[0] = 1.0;
    double sum = 1.0;
    for (int i = 1; i<g->depth; i++) {
        double val = g->weights[i-1] / g->weight_factor;
        g->weights[i] = val;
        sum += val*val;
    }
    double N = sqrt(sum);
    for (int i = 0; i < g->depth; i++) {
        g->weights[i] /= N;
    }
}

/*
 'arr' should point to a double array of size at least height*width.
*/
t_stochasticity_grid *create_stochasticity_grid(double *arr, int height, int width, double weight_factor, double mean, double stddev, long int seed) {
    t_stochasticity_grid *g = malloc( sizeof(t_stochasticity_grid) );
    g->arr = arr;
    g->height = height;
    g->width = width;

    /* It is best to use array dimensions of power of 2 */

    int d = height > width ? height : width; // Take the max
    g->depth = ceil(log2(d)) + 1;
    g->stddev = stddev;
    g->mean = mean;
    g->weight_factor = weight_factor;

    g->rng = gsl_rng_alloc(gsl_rng_mt19937);
    gsl_rng_set(g->rng, seed);

    g->weights = malloc( sizeof(double) * g->depth );
    init_weights(g);

    return g;
}

void free_stochasticity_grid(t_stochasticity_grid *g) {
    gsl_rng_free(g->rng);
    free(g->weights);
    g->weights = NULL;
    g->arr = NULL;
    free(g);
}

static inline double gaussian(const gsl_rng *rng, double stddev) {
    return gsl_ran_gaussian_ziggurat(rng, stddev);
}

void fill_quadrant(const t_stochasticity_grid *g, int y0, int x0, int y1, int x1, int level) {
    double weight = g->weights[level];
    double rval = gaussian(g->rng, g->stddev);
    for(int i = y0; i < y1; i++) {
        for(int j = x0; j < x1; j++) {
            int offset = i * g->width + j;
            double val = rval*weight;
            g->arr[offset] += val;
        }
    }
}

void recursive_division(const t_stochasticity_grid *g, int level, int y0, int x0, int y1, int x1) {
    if (level >= g->depth) return;
    if (x1-x0 == 1 && y1-y0 == 1) return; // Skip 1x1 grids

    if (y0 >= y1) return;
    if (x0 >= x1) return;
    //printf("Depth: %i. (%i, %i, %i, %i)\n", level, y0, x0, y1, x1);

    fill_quadrant(g, y0, x0, y1, x1, level);

    if (level + 1 == g->depth) return; // Avoid unnecessary recursion; skip level with 1x1 grids

    int ydist = (y1-y0)/2.0 + 0.5;
    int xdist = (x1-x0)/2.0 + 0.5;
    int ymid = y0+ydist;
    int xmid = x0+xdist;

    /* Upper left */
    recursive_division(g, level+1, y0, x0, ymid, xmid);

    /* Upper right */
    recursive_division(g, level+1, ymid, x0, y1, xmid);

    /* Bottom left */
    recursive_division(g, level+1, y0, xmid, ymid, x1);

    /* Bottom right */
    recursive_division(g, level+1, ymid, xmid, y1, x1);
}

/**
 Generate spatially autocorrelated coefficients using a recursive quadtree subdivision.
 The pointer 'arr' is a 2-dimensional output array of size 'height' * 'width'.
 The variable 'variance' gives the variance of the log-norm distribution with mean and cut-off at 1.0.
*/
void generate_stochasticity(t_stochasticity_grid *g) {
    /* Initialize the array by generating the last level (i.e., 1x1 grids) */
    for(int i = 0; i < g->height; i++) {
        for(int j = 0; j < g->width; j++) {
            double weight = g->weights[g->depth-1];
            g->arr[i*g->width + j] = weight*gaussian(g->rng, g->stddev);
        }
    }

    recursive_division(g, 0, 0, 0, g->height, g->width);

    for(int i = 0; i < g->height; i++) {
        for(int j = 0; j < g->width; j++) {
            double y = g->arr[i*g->width + j];
            y = exp(y + g->mean);
            if (y > 1.0) y = 1.0;
            g->arr[i*g->width + j] = y;
        }
    }

}


/* randist/nbinomial.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_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_sf_gamma.h>

/* The negative binomial distribution has the form,

   prob(k) =  Gamma(n + k)/(Gamma(n) Gamma(k + 1))  p^n (1-p)^k

   for k = 0, 1, ... . Note that n does not have to be an integer.

   This is the Leger's algorithm (given in the answers in Knuth) */

unsigned int
gsl_ran_negative_binomial (const gsl_rng * r, double p, double n)
{
  double X = gsl_ran_gamma (r, n, 1.0) ;
  unsigned int k = gsl_ran_poisson (r, X*(1-p)/p) ;
  return k ;
}

double
gsl_ran_negative_binomial_pdf (const unsigned int k, const double p, double n)
{
  double P;

  double f = gsl_sf_lngamma (k + n) ;
  double a = gsl_sf_lngamma (n) ;
  double b = gsl_sf_lngamma (k + 1.0) ;

  P = exp(f-a-b) * pow (p, n) * pow (1 - p, (double)k);

  return P;
}


/* interpolation/interp_poly.c
 *
 * Copyright (C) 2001 DAN, HO-JIN
 *
 * 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.
 */

/* Modified for standalone use in polynomial directory, B.Gough 2001 */

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

int
gsl_poly_dd_init (double dd[], const double xa[], const double ya[],
                  size_t size)
{
  size_t i, j;

  /* Newton's divided differences */

  dd[0] = ya[0];

  for (j = size - 1; j >= 1; j--)
    {
      dd[j] = (ya[j] - ya[j - 1]) / (xa[j] - xa[j - 1]);
    }

  for (i = 2; i < size; i++)
    {
      for (j = size - 1; j >= i; j--)
        {
          dd[j] = (dd[j] - dd[j - 1]) / (xa[j] - xa[j - i]);
        }
    }

  return GSL_SUCCESS;
}

int
gsl_poly_dd_taylor (double c[], double xp,
                    const double dd[], const double xa[], size_t size,
                    double w[])
{
  size_t i, j;

  for (i = 0; i < size; i++)
    {
      c[i] = 0.0;
      w[i] = 0.0;
    }

  w[size - 1] = 1.0;

  c[0] = dd[0];

  for (i = size - 1; i-- > 0;)
    {
      w[i] = -w[i + 1] * (xa[size - 2 - i] - xp);

      for (j = i + 1; j < size - 1; j++)
        {
          w[j] = w[j] - w[j + 1] * (xa[size - 2 - i] - xp);
        }

      for (j = i; j < size; j++)
        {
          c[j - i] += w[j] * dd[size - i - 1];
        }
    }

  return GSL_SUCCESS;
}


#include <stdio.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_eigen.h>

int
main (void)
{
  double data[] = { -1.0, 1.0, -1.0, 1.0,
                    -8.0, 4.0, -2.0, 1.0,
                    27.0, 9.0, 3.0, 1.0,
                    64.0, 16.0, 4.0, 1.0 };

  gsl_matrix_view m
    = gsl_matrix_view_array (data, 4, 4);

  gsl_vector_complex *eval = gsl_vector_complex_alloc (4);
  gsl_matrix_complex *evec = gsl_matrix_complex_alloc (4, 4);

  gsl_eigen_nonsymmv_workspace * w =
    gsl_eigen_nonsymmv_alloc (4);

  gsl_eigen_nonsymmv (&m.matrix, eval, evec, w);

  gsl_eigen_nonsymmv_free (w);

  gsl_eigen_nonsymmv_sort (eval, evec,
                           GSL_EIGEN_SORT_ABS_DESC);

  {
    int i, j;

    for (i = 0; i < 4; i++)
      {
        gsl_complex eval_i
           = gsl_vector_complex_get (eval, i);
        gsl_vector_complex_view evec_i
           = gsl_matrix_complex_column (evec, i);

        printf ("eigenvalue = %g + %gi\n",
                GSL_REAL(eval_i), GSL_IMAG(eval_i));
        printf ("eigenvector = \n");
        for (j = 0; j < 4; ++j)
          {
            gsl_complex z =
              gsl_vector_complex_get(&evec_i.vector, j);
            printf("%g + %gi\n", GSL_REAL(z), GSL_IMAG(z));
          }
      }
  }

  gsl_vector_complex_free(eval);
  gsl_matrix_complex_free(evec);

  return 0;
}


/**
 *
 * @generated d Tue Jan  7 11:45:26 2014
 *
 **/
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <cblas.h>
#include <lapacke.h>
#include <plasma.h>
#include <core_blas.h>
#include "auxiliary.h"

/*-------------------------------------------------------------------
 * Check the orthogonality of Q
 */

int d_check_orthogonality(int M, int N, int LDQ, double *Q)
{
    double alpha, beta;
    double normQ;
    int info_ortho;
    int i;
    int minMN = min(M, N);
    double eps;
    double *work = (double *)malloc(minMN*sizeof(double));

    eps = LAPACKE_dlamch_work('e');
    alpha = 1.0;
    beta  = -1.0;

    /* Build the idendity matrix USE DLASET?*/
    double *Id = (double *) malloc(minMN*minMN*sizeof(double));
    memset((void*)Id, 0, minMN*minMN*sizeof(double));
    for (i = 0; i < minMN; i++)
        Id[i*minMN+i] = (double)1.0;

    /* Perform Id - Q'Q */
    if (M >= N)
        cblas_dsyrk(CblasColMajor, CblasUpper, CblasTrans, N, M, alpha, Q, LDQ, beta, Id, N);
    else
        cblas_dsyrk(CblasColMajor, CblasUpper, CblasNoTrans, M, N, alpha, Q, LDQ, beta, Id, M);

    normQ = LAPACKE_dlansy_work(LAPACK_COL_MAJOR, 'i', 'u', minMN, Id, minMN, work);

    printf("============\n");
    printf("Checking the orthogonality of Q \n");
    printf("||Id-Q'*Q||_oo / (N*eps) = %e \n",normQ/(minMN*eps));

    if ( isnan(normQ / (minMN * eps)) || (normQ / (minMN * eps) > 10.0) ) {
        printf("-- Orthogonality is suspicious ! \n");
        info_ortho=1;
    }
    else {
        printf("-- Orthogonality is CORRECT ! \n");
        info_ortho=0;
    }

    free(work); free(Id);

    return info_ortho;
}

/*------------------------------------------------------------
 *  Check the factorization QR
 */

int d_check_QRfactorization(int M, int N, double *A1, double *A2, int LDA, double *Q)
{
    double Anorm, Rnorm;
    double alpha, beta;
    int info_factorization;
    int i,j;
    double eps;

    eps = LAPACKE_dlamch_work('e');

    double *Ql       = (double *)malloc(M*N*sizeof(double));
    double *Residual = (double *)malloc(M*N*sizeof(double));
    double *work              = (double *)malloc(max(M,N)*sizeof(double));

    alpha=1.0;
    beta=0.0;

    if (M >= N) {
        /* Extract the R */
        double *R = (double *)malloc(N*N*sizeof(double));
        memset((void*)R, 0, N*N*sizeof(double));
        LAPACKE_dlacpy_work(LAPACK_COL_MAJOR,'u', M, N, A2, LDA, R, N);

        /* Perform Ql=Q*R */
        memset((void*)Ql, 0, M*N*sizeof(double));
        cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, M, N, N, (alpha), Q, LDA, R, N, (beta), Ql, M);
        free(R);
    }
    else {
        /* Extract the L */
        double *L = (double *)malloc(M*M*sizeof(double));
        memset((void*)L, 0, M*M*sizeof(double));
        LAPACKE_dlacpy_work(LAPACK_COL_MAJOR,'l', M, N, A2, LDA, L, M);

    /* Perform Ql=LQ */
        memset((void*)Ql, 0, M*N*sizeof(double));
        cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, M, N, M, (alpha), L, M, Q, LDA, (beta), Ql, M);
        free(L);
    }

    /* Compute the Residual */
    for (i = 0; i < M; i++)
        for (j = 0 ; j < N; j++)
            Residual[j*M+i] = A1[j*LDA+i]-Ql[j*M+i];

    Rnorm = LAPACKE_dlange_work(LAPACK_COL_MAJOR, 'i', M, N, Residual, M, work);
    Anorm = LAPACKE_dlange_work(LAPACK_COL_MAJOR, 'i', M, N, A2, LDA, work);

    if (M >= N) {
        printf("============\n");
        printf("Checking the QR Factorization \n");
        printf("-- ||A-QR||_oo/(||A||_oo.N.eps) = %e \n",Rnorm/(Anorm*N*eps));
    }
    else {
        printf("============\n");
        printf("Checking the LQ Factorization \n");
        printf("-- ||A-LQ||_oo/(||A||_oo.N.eps) = %e \n",Rnorm/(Anorm*N*eps));
    }

    if (isnan(Rnorm / (Anorm * N *eps)) || (Rnorm / (Anorm * N * eps) > 10.0) ) {
        printf("-- Factorization is suspicious ! \n");
        info_factorization = 1;
    }
    else {
        printf("-- Factorization is CORRECT ! \n");
        info_factorization = 0;
    }

    free(work); free(Ql); free(Residual);

    return info_factorization;
}

/*------------------------------------------------------------------------
 *  Check the factorization of the matrix A2
 */

int d_check_LLTfactorization(int N, double *A1, double *A2, int LDA, int uplo)
{
    double Anorm, Rnorm;
    double alpha;
    int info_factorization;
    int i,j;
    double eps;

    eps = LAPACKE_dlamch_work('e');

    double *Residual = (double *)malloc(N*N*sizeof(double));
    double *L1       = (double *)malloc(N*N*sizeof(double));
    double *L2       = (double *)malloc(N*N*sizeof(double));
    double *work              = (double *)malloc(N*sizeof(double));

    memset((void*)L1, 0, N*N*sizeof(double));
    memset((void*)L2, 0, N*N*sizeof(double));

    alpha= 1.0;

    LAPACKE_dlacpy_work(LAPACK_COL_MAJOR,' ', N, N, A1, LDA, Residual, N);

    /* Dealing with L'L or U'U  */
    if (uplo == PlasmaUpper){
        LAPACKE_dlacpy_work(LAPACK_COL_MAJOR,'u', N, N, A2, LDA, L1, N);
        LAPACKE_dlacpy_work(LAPACK_COL_MAJOR,'u', N, N, A2, LDA, L2, N);
        cblas_dtrmm(CblasColMajor, CblasLeft, CblasUpper, CblasTrans, CblasNonUnit, N, N, (alpha), L1, N, L2, N);
    }
    else{
        LAPACKE_dlacpy_work(LAPACK_COL_MAJOR,'l', N, N, A2, LDA, L1, N);
        LAPACKE_dlacpy_work(LAPACK_COL_MAJOR,'l', N, N, A2, LDA, L2, N);
        cblas_dtrmm(CblasColMajor, CblasRight, CblasLower, CblasTrans, CblasNonUnit, N, N, (alpha), L1, N, L2, N);
    }

    /* Compute the Residual || A -L'L|| */
    for (i = 0; i < N; i++)
        for (j = 0; j < N; j++)
           Residual[j*N+i] = L2[j*N+i] - Residual[j*N+i];

    Rnorm = LAPACKE_dlange_work(LAPACK_COL_MAJOR, 'i', N, N, Residual, N, work);
    Anorm = LAPACKE_dlange_work(LAPACK_COL_MAJOR, 'i', N, N, A1, LDA, work);

    printf("============\n");
    printf("Checking the Cholesky Factorization \n");
    printf("-- ||L'L-A||_oo/(||A||_oo.N.eps) = %e \n",Rnorm/(Anorm*N*eps));

    if ( isnan(Rnorm/(Anorm*N*eps)) || (Rnorm/(Anorm*N*eps) > 10.0) ){
        printf("-- Factorization is suspicious ! \n");
        info_factorization = 1;
    }
    else{
        printf("-- Factorization is CORRECT ! \n");
        info_factorization = 0;
    }

    free(Residual); free(L1); free(L2); free(work);

    return info_factorization;
}

/*--------------------------------------------------------------
 * Check the gemm
 */
double d_check_gemm(PLASMA_enum transA, PLASMA_enum transB, int M, int N, int K,
                   double alpha, double *A, int LDA,
                   double *B, int LDB,
                   double beta, double *Cplasma,
                   double *Cref, int LDC,
                   double *Cinitnorm, double *Cplasmanorm, double *Clapacknorm )
{
    double beta_const = -1.0;
    double Rnorm;
    double *work = (double *)malloc(max(K,max(M, N))* sizeof(double));

    *Cinitnorm   = LAPACKE_dlange_work(LAPACK_COL_MAJOR, 'i', M, N, Cref,    LDC, work);
    *Cplasmanorm = LAPACKE_dlange_work(LAPACK_COL_MAJOR, 'i', M, N, Cplasma, LDC, work);

    cblas_dgemm(CblasColMajor, (CBLAS_TRANSPOSE)transA, (CBLAS_TRANSPOSE)transB, M, N, K,
                (alpha), A, LDA, B, LDB, (beta), Cref, LDC);

    *Clapacknorm = LAPACKE_dlange_work(LAPACK_COL_MAJOR, 'i', M, N, Cref, LDC, work);

    cblas_daxpy(LDC * N, (beta_const), Cplasma, 1, Cref, 1);

    Rnorm = LAPACKE_dlange_work(LAPACK_COL_MAJOR, 'i', M, N, Cref, LDC, work);

    free(work);

    return Rnorm;
}

/*--------------------------------------------------------------
 * Check the trsm
 */
double d_check_trsm(PLASMA_enum side, PLASMA_enum uplo, PLASMA_enum trans, PLASMA_enum diag,
                   int M, int NRHS, double alpha,
                   double *A, int LDA,
                   double *Bplasma, double *Bref, int LDB,
                   double *Binitnorm, double *Bplasmanorm, double *Blapacknorm )
{
    double beta_const = -1.0;
    double Rnorm;
    double *work = (double *)malloc(max(M, NRHS)* sizeof(double));
    /*double eps = LAPACKE_dlamch_work('e');*/

    *Binitnorm   = LAPACKE_dlange_work(LAPACK_COL_MAJOR, 'i', M, NRHS, Bref,    LDB, work);
    *Bplasmanorm = LAPACKE_dlange_work(LAPACK_COL_MAJOR, 'm', M, NRHS, Bplasma, LDB, work);

    cblas_dtrsm(CblasColMajor, (CBLAS_SIDE)side, (CBLAS_UPLO)uplo,
                (CBLAS_TRANSPOSE)trans, (CBLAS_DIAG)diag, M, NRHS,
                (alpha), A, LDA, Bref, LDB);

    *Blapacknorm = LAPACKE_dlange_work(LAPACK_COL_MAJOR, 'm', M, NRHS, Bref, LDB, work);

    cblas_daxpy(LDB * NRHS, (beta_const), Bplasma, 1, Bref, 1);

    Rnorm = LAPACKE_dlange_work(LAPACK_COL_MAJOR, 'm', M, NRHS, Bref, LDB, work);
    Rnorm = Rnorm / *Blapacknorm;
    /* max(M,NRHS) * eps);*/

    free(work);

    return Rnorm;
}

/*--------------------------------------------------------------
 * Check the solution
 */

double d_check_solution(int M, int N, int NRHS, double *A, int LDA,
                      double *B,  double *X, int LDB,
                      double *anorm, double *bnorm, double *xnorm )
{
/*     int info_solution; */
    double Rnorm = -1.00;
    double zone  =  1.0;
    double mzone = -1.0;
    double *work = (double *)malloc(max(M, N)* sizeof(double));

    *anorm = LAPACKE_dlange_work(LAPACK_COL_MAJOR, 'i', M, N,    A, LDA, work);
    *xnorm = LAPACKE_dlange_work(LAPACK_COL_MAJOR, 'i', M, NRHS, X, LDB, work);
    *bnorm = LAPACKE_dlange_work(LAPACK_COL_MAJOR, 'i', N, NRHS, B, LDB, work);

    cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, M, NRHS, N, (zone), A, LDA, X, LDB, (mzone), B, LDB);

    Rnorm = LAPACKE_dlange_work(LAPACK_COL_MAJOR, 'i', N, NRHS, B, LDB, work);

    free(work);

    return Rnorm;
}


#ifndef LAPACKWRAPPER_H
#define LAPACKWRAPPER_H

#include <vector>

extern "C" {
    // void dbdsqr_(char *uplo, int *n, int *ncvt, int *nru, int *ncc, double *D, double *E, double *VT, int *ldvt, double *U, int *ldu, double *C, int *ldc, double *work, int *info);
    // void dgetrf_(int *m, int *n, double *A, int *lda, int *ipiv, int *info);
    // void dgetrs_(char *trans, int *n, int *nrhs, double *A, int *lda, int *ipiv, double *B, int *ldb, int *info);
    // void dgetri_(int *n, double *A, int *lda, int *ipiv, double *work, int *lwork, int *info);
    // void dgehrd_(int *n, int*ilo, int *ihi, double *A, int *lda, double *tau, double *work, int *lwork, int *info);
    // void dhseqr_(char *job, char *compz, int* n, int* ilo, int *ihi, double *H, int *ldh, double *WR, double *WI, double *Z, int *ldz, double *work, int *lwork, int *info);

    // void dorghr_(int *n, int *ilo, int *ihi, double *A, int *lda, double *tau, double *work, int *lwork, int *info);

    // void dtrsyl_(char *trana, char *tranb, int *isgn, int *m, int *n, double *A, int *lda, double *B, int *ldb, double *C, int *ldc, double *scale, int *info);
    #include <lapack.h>
    #include <cblas.h>
}

//---------------------------------------------------------------------------
// LAPACK FUNCTIONS
//---------------------------------------------------------------------------

// Compute the bidiagonal SVD
int cdbdsqr(char uplo, int n, int ncvt, int nru, int ncc,
            double *D, double *E, double *VT, int ldvt,
            double *U, int ldu, double *C, int ldc);

// Compute LU factorization of A
int cdgetrf(int m, int n, double *A, int lda, int *ipiv);
// Solve linear system from LU factorization from cdgetrf
int cdgetrs(char trans, int n, int nrhs, double *A, int lda, int *ipiv, double *B, int ldb);
// Invert matrix given the LU factorization from dgetrf_
int cdgetri(int n, double *A, int lda, int *ipiv);

//-----------------------------------------------------------------
// Computing the Schur Decompotision to apply preconditioner
// --- 1. Reduce to upper Hessenberg using cdgehrd
// --- 2. Generate Orthogonal Matrix using cdorghr
// --- 3. Compute Schur decomposition of Hessenberg Matrix using cdorghr
//----------------------------------------------------------------

// Reduce matrix to upper Hessenberg form
int cdgehrd(int n, int ilo, int ihi, double *A, int lda, double *tau);
// Schur decomposition of a Hessenberg matrix
int cdhseqr(char job, char compz, int n, int ilo, int ihi, double *H, int ldh, double *WR, double *WI, double *Z, int ldz);
// Generate the orthogonal matrix Q implictly created in dgehrd
int cdorghr(int n, int ilo, int ihi, double *A, int lda, double *tau);

// Solve Sylvester System
int cdtrsyl(char trana, char tranb, int isgn, int m, int n, double *A, int lda, double *B, int ldb, double *C, int ldc, double scale);

//-----------------------------------------------------------------------------------
// C++ Class interfaces for LAPACK FUNCTIONS
//-----------------------------------------------------------------------------------

void Print(const std::string str, const std::vector<int> &size, const double *A);
void Transpose(std::vector<int> &size, double *A, double *AT);
// LU decomposition
class LU{
private:
    // Data is stored column-major fashion
    double *data;
    int *ipiv;
public:
    std::vector<int> size; // (number of rows, number of cols)
    LU(const int m_, const int n_, const double *data_);
    ~LU();
    void Factor();
    void SetData(const double *data_);
    void Solve(const int mb, const int nb, double *B, bool trans);
    void GetInverse(const int m, const int n, double *invA);
    void PrintLU();
};

// Schur Decomposition
class Schur{
private:
    double *Qdata, *Tdata;
public:
    std::vector<int> size;
    Schur(const int m_, const int n_, const double *data);
    ~Schur();
    void Factor();
    double* Q() const;
    double* T() const;
    void PrintQ();
    void PrintT();
};

#endif


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

#include <lapacke.h>
#include <libhades.h>

#include "unittest.h"

int test_trace()
{
    matrix_t *A, *B;
    matrix_complex_t *C, *D;
    double trace;
    complex_t trace_cplx;
    unittest_t test;
    unittest_init(&test, "matrix_mult", "Test trace");

    A = matrix_alloc(3,3);
    B = matrix_alloc(3,3);

    matrix_set(A, 0,0, 1);
    matrix_set(A, 0,1, 2);
    matrix_set(A, 0,2, 3);
    matrix_set(A, 1,0, 4);
    matrix_set(A, 1,1, 5);
    matrix_set(A, 1,2, 6);
    matrix_set(A, 2,0, 7);
    matrix_set(A, 2,1, 8);
    matrix_set(A, 2,2, 9);

    matrix_set(B, 0,0, 9);
    matrix_set(B, 0,1, 8);
    matrix_set(B, 0,2, 7);
    matrix_set(B, 1,0, 6);
    matrix_set(B, 1,1, 5);
    matrix_set(B, 1,2, 4);
    matrix_set(B, 2,0, 3);
    matrix_set(B, 2,1, 2);
    matrix_set(B, 2,2, 1);

    trace = matrix_trace_AB(A, B);
    AssertEqual(&test, trace, 189);

    AssertEqual(&test, matrix_trace(A), 15);
    AssertEqual(&test, matrix_trace(B), 15);

    matrix_free(A);
    matrix_free(B);

    C = matrix_complex_alloc(3,3);
    D = matrix_complex_alloc(3,3);

    matrix_set(C, 0,0, CPLX(0,1));
    matrix_set(C, 0,1, CPLX(-2,0));
    matrix_set(C, 0,2, CPLX(3,1));
    matrix_set(C, 1,0, CPLX(-2,0));
    matrix_set(C, 1,1, CPLX(1,0));
    matrix_set(C, 1,2, CPLX(0,1));
    matrix_set(C, 2,0, CPLX(0,-2));
    matrix_set(C, 2,1, CPLX(1,3));
    matrix_set(C, 2,2, CPLX(5,0));

    matrix_set(D, 0,0, CPLX(0,-2));
    matrix_set(D, 0,1, CPLX(0,1));
    matrix_set(D, 0,2, CPLX(2,-2));
    matrix_set(D, 1,0, CPLX(1,0));
    matrix_set(D, 1,1, CPLX(5,0));
    matrix_set(D, 1,2, CPLX(0,1));
    matrix_set(D, 2,0, CPLX(-1,0));
    matrix_set(D, 2,1, CPLX(-4,-4));
    matrix_set(D, 2,2, CPLX(0,1));

    trace_cplx = matrix_complex_trace(C);
    AssertEqual(&test, CREAL(trace_cplx), 6);
    AssertEqual(&test, CIMAG(trace_cplx), 1);
    trace_cplx = matrix_complex_trace(D);
    AssertEqual(&test, CREAL(trace_cplx), 5);
    AssertEqual(&test, CIMAG(trace_cplx), -1);

    trace = matrix_trace_complex_AB_real(C, D);
    AssertEqual(&test, trace, -1);

    trace_cplx = matrix_trace_complex_AB(C, D);
    AssertEqual(&test, CREAL(trace_cplx), -1);
    AssertEqual(&test, CIMAG(trace_cplx), -5);

    matrix_complex_free(C);
    matrix_complex_free(D);

    return test_results(&test, stderr);
}

int test_matrix_mult()
{
    matrix_t *A, *B, *C;
    unittest_t test;
    unittest_init(&test, "matrix_mult", "Test matrix multiplication");

    A = matrix_alloc(2,3);
    B = matrix_alloc(3,2);

    matrix_set(A, 0,0, 3);
    matrix_set(A, 0,1, 2);
    matrix_set(A, 0,2, 1);
    matrix_set(A, 1,0, 1);
    matrix_set(A, 1,1, 0);
    matrix_set(A, 1,2, 2);

    matrix_set(B, 0,0, 1);
    matrix_set(B, 0,1, 2);
    matrix_set(B, 1,0, 0);
    matrix_set(B, 1,1, 1);
    matrix_set(B, 2,0, 4);
    matrix_set(B, 2,1, 0);

    C = matrix_mult(A,B,1,NULL);

    matrix_free(A);
    matrix_free(B);

    AssertEqual(&test, matrix_get(C,0,0), 7);
    AssertEqual(&test, matrix_get(C,0,1), 8);
    AssertEqual(&test, matrix_get(C,1,0), 9);
    AssertEqual(&test, matrix_get(C,1,1), 2);

    matrix_free(C);

    return test_results(&test, stderr);
}

int test_matrix_zeros()
{
    int m,n;
    matrix_t *A;
    matrix_complex_t *C;
    unittest_t test;

    unittest_init(&test, "matrix_zeros", "Test function matrix_zeros");

    for(m = 6; m < 20; m++)
        for(n = 1; n < 8; n++)
        {
            int im, in;
            A = matrix_zeros(m,n, NULL);
            C = matrix_complex_zeros(m,n, NULL);
            for(im = 0; im < m; im++)
                for(in = 0; in < n; in++)
                {
                    complex_t c = matrix_get(C,im,in);
                    AssertEqual(&test, matrix_get(A,im,in),0);
                    AssertEqual(&test, CREAL(c), 0);
                    AssertEqual(&test, CIMAG(c), 0);
                }

            matrix_free(A);
            matrix_complex_free(C);
        }

    return test_results(&test, stderr);
}

int test_zeros()
{
    matrix_t *A;
    matrix_complex_t *B;
    unittest_t test;
    unittest_init(&test, "matrix_zeros", "Test zeros");
    size_t rows = 100, columns = 150;

    A = matrix_zeros        (rows, columns, NULL);
    B = matrix_complex_zeros(rows, columns, NULL);

    for(size_t i = 0; i < rows; i++)
        for(size_t j = 0; j < columns; j++)
        {
            AssertEqual(&test, matrix_get(A, i,j), 0);
            AssertEqual(&test, matrix_get(B, i,j), 0);
        }

    matrix_free(A);
    matrix_complex_free(B);

    return test_results(&test, stderr);
}

int test_matrix_kron()
{
    matrix_t *A, *B, *AB;
    matrix_complex_t *C, *D, *CD;
    matrix_t *E, *EE;
    unittest_t test;
    unittest_init(&test, "matrix_kron", "Test function matrix_kron");

    A = matrix_alloc(3,2);
    B = matrix_alloc(2,2);

    matrix_set(A, 0,0, 1);
    matrix_set(A, 0,1, 2);
    matrix_set(A, 1,0, 3);
    matrix_set(A, 1,1, 4);
    matrix_set(A, 2,0, 5);
    matrix_set(A, 2,1, 6);

    matrix_set(B, 0,0, 7);
    matrix_set(B, 0,1, 8);
    matrix_set(B, 1,0, 9);
    matrix_set(B, 1,1, 0);

    AB = matrix_kron(A, B, NULL);

    matrix_free(A);
    matrix_free(B);

    AssertEqual(&test, matrix_get(AB, 0,0), 7);
    AssertEqual(&test, matrix_get(AB, 0,1), 8);
    AssertEqual(&test, matrix_get(AB, 0,2), 14);
    AssertEqual(&test, matrix_get(AB, 0,3), 16);

    AssertEqual(&test, matrix_get(AB, 1,0), 9);
    AssertEqual(&test, matrix_get(AB, 1,1), 0);
    AssertEqual(&test, matrix_get(AB, 1,2), 18);
    AssertEqual(&test, matrix_get(AB, 1,3), 0);

    AssertEqual(&test, matrix_get(AB, 2,0), 21);
    AssertEqual(&test, matrix_get(AB, 2,1), 24);
    AssertEqual(&test, matrix_get(AB, 2,2), 28);
    AssertEqual(&test, matrix_get(AB, 2,3), 32);

    AssertEqual(&test, matrix_get(AB, 3,0), 27);
    AssertEqual(&test, matrix_get(AB, 3,1), 0);
    AssertEqual(&test, matrix_get(AB, 3,2), 36);
    AssertEqual(&test, matrix_get(AB, 3,3), 0);

    AssertEqual(&test, matrix_get(AB, 4,0), 35);
    AssertEqual(&test, matrix_get(AB, 4,1), 40);
    AssertEqual(&test, matrix_get(AB, 4,2), 42);
    AssertEqual(&test, matrix_get(AB, 4,3), 48);

    AssertEqual(&test, matrix_get(AB, 5,0), 45);
    AssertEqual(&test, matrix_get(AB, 5,1), 0);
    AssertEqual(&test, matrix_get(AB, 5,2), 54);
    AssertEqual(&test, matrix_get(AB, 5,3), 0);

    matrix_free(AB);

    C = matrix_complex_alloc(2,2);
    D = matrix_complex_alloc(2,2);

    matrix_set(C, 0,0, CPLX(1,1));
    matrix_set(C, 0,1, CPLX(2,-2));
    matrix_set(C, 1,0, CPLX(5,0));
    matrix_set(C, 1,1, CPLX(0,-1));

    matrix_set(D, 0,0, CPLX(0,-1));
    matrix_set(D, 0,1, CPLX(2,0));
    matrix_set(D, 1,0, CPLX(5,0));
    matrix_set(D, 1,1, CPLX(0,7));

    CD = matrix_complex_alloc(4,4);
    matrix_complex_kron(C, D, CD);

    matrix_complex_free(C);
    matrix_complex_free(D);

    AssertEqual(&test, CREAL(matrix_get(CD, 0,0)), 1);
    AssertEqual(&test, CIMAG(matrix_get(CD, 0,0)), -1);
    AssertEqual(&test, CREAL(matrix_get(CD, 0,1)), 2);
    AssertEqual(&test, CIMAG(matrix_get(CD, 0,1)), 2);
    AssertEqual(&test, CREAL(matrix_get(CD, 0,2)), -2);
    AssertEqual(&test, CIMAG(matrix_get(CD, 0,2)), -2);
    AssertEqual(&test, CREAL(matrix_get(CD, 0,3)), 4);
    AssertEqual(&test, CIMAG(matrix_get(CD, 0,3)), -4);

    AssertEqual(&test, CREAL(matrix_get(CD, 1,0)), 5);
    AssertEqual(&test, CIMAG(matrix_get(CD, 1,0)), 5);
    AssertEqual(&test, CREAL(matrix_get(CD, 1,1)), -7);
    AssertEqual(&test, CIMAG(matrix_get(CD, 1,1)), 7);
    AssertEqual(&test, CREAL(matrix_get(CD, 1,2)), 10);
    AssertEqual(&test, CIMAG(matrix_get(CD, 1,2)), -10);
    AssertEqual(&test, CREAL(matrix_get(CD, 1,3)), 14);
    AssertEqual(&test, CIMAG(matrix_get(CD, 1,3)), 14);

    AssertEqual(&test, CREAL(matrix_get(CD, 2,0)), 0);
    AssertEqual(&test, CIMAG(matrix_get(CD, 2,0)), -5);
    AssertEqual(&test, CREAL(matrix_get(CD, 2,1)), 10);
    AssertEqual(&test, CIMAG(matrix_get(CD, 2,1)), 0);
    AssertEqual(&test, CREAL(matrix_get(CD, 2,2)), -1);
    AssertEqual(&test, CIMAG(matrix_get(CD, 2,2)), 0);
    AssertEqual(&test, CREAL(matrix_get(CD, 2,3)), 0);
    AssertEqual(&test, CIMAG(matrix_get(CD, 2,3)), -2);

    AssertEqual(&test, CREAL(matrix_get(CD, 3,0)), 25);
    AssertEqual(&test, CIMAG(matrix_get(CD, 3,0)), 0);
    AssertEqual(&test, CREAL(matrix_get(CD, 3,1)), 0);
    AssertEqual(&test, CIMAG(matrix_get(CD, 3,1)), 35);
    AssertEqual(&test, CREAL(matrix_get(CD, 3,2)), 0);
    AssertEqual(&test, CIMAG(matrix_get(CD, 3,2)), -5);
    AssertEqual(&test, CREAL(matrix_get(CD, 3,3)), 7);
    AssertEqual(&test, CIMAG(matrix_get(CD, 3,3)), 0);

    matrix_complex_free(CD);

    E = matrix_zeros(2,2, NULL);

    matrix_set(E, 0,0, 1);
    matrix_set(E, 1,1, 2);

    EE = matrix_kron(E, E, NULL);
    matrix_free(E);

    AssertEqual(&test, matrix_get(EE, 0,0), 1);
    AssertEqual(&test, matrix_get(EE, 1,1), 2);
    AssertEqual(&test, matrix_get(EE, 2,2), 2);
    AssertEqual(&test, matrix_get(EE, 3,3), 4);

    for(size_t i = 0; i < 4; i++)
        for(size_t j = 0; j < 4; j++)
            if(i != j)
                AssertEqual(&test, matrix_get(EE, i,j), 0);

    matrix_free(EE);

    return test_results(&test, stderr);
}

int main(int argc, char *argv[])
{
    test_matrix_zeros();
    test_matrix_kron();
    test_matrix_mult();
    test_zeros();
    test_trace();

    return 0;
}


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <nubspline.h>
#include "SimulationSteps.h"
#include <gsl/gsl_interp.h>
#include <gsl/gsl_spline.h>

void eval_NUBspline_1d_d(NUBspline_1d_d * restrict spline, double x,
	double* restrict val);

void eval_NUBspline_1d_d_vg(NUBspline_1d_d * restrict spline, double x,
	double* restrict val, double* restrict grad);

void eval_NUBspline_1d_d_vgl(NUBspline_1d_d * restrict spline, double x,
	double* restrict val, double* restrict grad, double* restrict lapl);

void InterpolateWithSplines(int NumPointsToInterpolate, int NumPointsToEvaluate, double* x, double* NodalX, double *func, double *NodalFunc, double* NodalDeriv)
{

	NUgrid* myGrid = create_general_grid(x, NumPointsToInterpolate);
	BCtype_d myBC;
	myBC.lCode = NATURAL;
	myBC.rCode = NATURAL;
	NUBspline_1d_d *mySpline = create_NUBspline_1d_d(myGrid, myBC, func);
	for(int i =0; i < NumPointsToEvaluate; i++)
	{
		if (NodalDeriv != NULL)
			eval_NUBspline_1d_d_vg(mySpline, NodalX[i], &NodalFunc[i], &NodalDeriv[i]);
		else
			eval_NUBspline_1d_d(mySpline, NodalX[i], &NodalFunc[i]);
	}

}

void InterpolateWithGSLSplines(int NumPointsToInterpolate, int NumPointsToEvaluate, double* x, double* NodalX, double* func, double* NodalFunc, double* NodalDeriv)
{
	gsl_interp_accel *acc = gsl_interp_accel_alloc();
	gsl_spline* spline = gsl_spline_alloc(gsl_interp_cspline, NumPointsToInterpolate);
	gsl_spline_init(spline, x, func, NumPointsToInterpolate);

	for(int j = 0; j < NumPointsToEvaluate; j++)
	{
		NodalFunc[j] = gsl_spline_eval(spline, NodalX[j], acc);
		if (NodalDeriv != NULL)
			NodalDeriv[j] = gsl_spline_eval_deriv(spline, NodalX[j], acc);
	}

}


#pragma once

#include "shapes3D.h"

#include <gsl/gsl>

namespace ad {

template <int N_baseEdges>
std::vector<vertex3D::Data> make_pyramid(float aBaseRadius, float aHeight)
{
    std::vector<vertex3D::Data> result;
    // There are twice the number of edges in the pyramid, each edge being two vertices
    constexpr std::size_t ElementsCount{N_baseEdges*2*2};
    result.reserve(ElementsCount);

    Vec<3> topPosition = {0.0f, 0.0f, aHeight};

    auto getBasePosition = [](int aId, float aRadius) -> Vec<3>
    {
        return {
            std::cos(2*pi<>*aId/N_baseEdges) * aRadius,
            std::sin(2*pi<>*aId/N_baseEdges) * aRadius,
            0.0f,
        };
    };

    Vec<3> position_prev = getBasePosition(-1, aBaseRadius);
    for(int i=0; i<N_baseEdges; ++i)
    {
        Vec<3> position_a = getBasePosition(i, aBaseRadius);
        Vec<3> position_b = getBasePosition(i+1, aBaseRadius);

        // The base segment
        {
            // The normal of BA vector (not AB)
            Vec<3> ba = (position_a - position_b).normalize();
            Vec<3> base_normal = {-ba.y(), ba.x(), 0.0f};

            result.emplace_back(vertex3D::Data{{position_a}, {base_normal}});
            result.emplace_back(vertex3D::Data{{position_b}, {base_normal}});
        }

        // The segment connecting base vertex "i" to the top
        {
            float teta = std::atan(aBaseRadius/aHeight); // angle between Z and a side edge
            Vec<3> b_prev = (position_prev - position_b).normalize();
            Vec<3> side_normal = {
                -b_prev.y() * std::cos(teta),
                b_prev.x() * std::cos(teta),
                std::sin(teta),
            };
            result.emplace_back(vertex3D::Data{{position_a}, {side_normal}});
            result.emplace_back(vertex3D::Data{{topPosition}, {side_normal}});
        }

        position_prev = position_a;
    }

    Expects(result.size() == ElementsCount);
    return result;
}

} // namespace ad


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

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

/* Fit the data (x_i, y_i) to the linear relationship

   Y = c0 + c1 x

   returning,

   c0, c1  --  coefficients
   cov00, cov01, cov11  --  variance-covariance matrix of c0 and c1,
   sumsq   --   sum of squares of residuals

   This fit can be used in the case where the errors for the data are
   uknown, but assumed equal for all points. The resulting
   variance-covariance matrix estimates the error in the coefficients
   from the observed variance of the points around the best fit line.
*/

int
gsl_fit_linear (const double *x, const size_t xstride,
                const double *y, const size_t ystride,
                const size_t n,
                double *c0, double *c1,
                double *cov_00, double *cov_01, double *cov_11, double *sumsq)
{
  double m_x = 0, m_y = 0, m_dx2 = 0, m_dxdy = 0;

  size_t i;

  for (i = 0; i < n; i++)
    {
      m_x += (x[i * xstride] - m_x) / (i + 1.0);
      m_y += (y[i * ystride] - m_y) / (i + 1.0);
    }

  for (i = 0; i < n; i++)
    {
      const double dx = x[i * xstride] - m_x;
      const double dy = y[i * ystride] - m_y;

      m_dx2 += (dx * dx - m_dx2) / (i + 1.0);
      m_dxdy += (dx * dy - m_dxdy) / (i + 1.0);
    }

  /* In terms of y = a + b x */

  {
    double s2 = 0, d2 = 0;
    double b = m_dxdy / m_dx2;
    double a = m_y - m_x * b;

    *c0 = a;
    *c1 = b;

    /* Compute chi^2 = \sum (y_i - (a + b * x_i))^2 */

    for (i = 0; i < n; i++)
      {
        const double dx = x[i * xstride] - m_x;
        const double dy = y[i * ystride] - m_y;
        const double d = dy - b * dx;
        d2 += d * d;
      }

    s2 = d2 / (n - 2.0);        /* chisq per degree of freedom */

    *cov_00 = s2 * (1.0 / n) * (1 + m_x * m_x / m_dx2);
    *cov_11 = s2 * 1.0 / (n * m_dx2);

    *cov_01 = s2 * (-m_x) / (n * m_dx2);

    *sumsq = d2;
  }

  return GSL_SUCCESS;
}

/* Fit the weighted data (x_i, w_i, y_i) to the linear relationship

   Y = c0 + c1 x

   returning,

   c0, c1  --  coefficients
   s0, s1  --  the standard deviations of c0 and c1,
   r       --  the correlation coefficient between c0 and c1,
   chisq   --  weighted sum of squares of residuals */

int
gsl_fit_wlinear (const double *x, const size_t xstride,
                 const double *w, const size_t wstride,
                 const double *y, const size_t ystride,
                 const size_t n,
                 double *c0, double *c1,
                 double *cov_00, double *cov_01, double *cov_11,
                 double *chisq)
{

  /* compute the weighted means and weighted deviations from the means */

  /* wm denotes a "weighted mean", wm(f) = (sum_i w_i f_i) / (sum_i w_i) */

  double W = 0, wm_x = 0, wm_y = 0, wm_dx2 = 0, wm_dxdy = 0;

  size_t i;

  for (i = 0; i < n; i++)
    {
      const double wi = w[i * wstride];

      if (wi > 0)
        {
          W += wi;
          wm_x += (x[i * xstride] - wm_x) * (wi / W);
          wm_y += (y[i * ystride] - wm_y) * (wi / W);
        }
    }

  W = 0;                        /* reset the total weight */

  for (i = 0; i < n; i++)
    {
      const double wi = w[i * wstride];

      if (wi > 0)
        {
          const double dx = x[i * xstride] - wm_x;
          const double dy = y[i * ystride] - wm_y;

          W += wi;
          wm_dx2 += (dx * dx - wm_dx2) * (wi / W);
          wm_dxdy += (dx * dy - wm_dxdy) * (wi / W);
        }
    }

  /* In terms of y = a + b x */

  {
    double d2 = 0;
    double b = wm_dxdy / wm_dx2;
    double a = wm_y - wm_x * b;

    *c0 = a;
    *c1 = b;

    *cov_00 = (1 / W) * (1 + wm_x * wm_x / wm_dx2);
    *cov_11 = 1 / (W * wm_dx2);

    *cov_01 = -wm_x / (W * wm_dx2);

    /* Compute chi^2 = \sum w_i (y_i - (a + b * x_i))^2 */

    for (i = 0; i < n; i++)
      {
        const double wi = w[i * wstride];

        if (wi > 0)
          {
            const double dx = x[i * xstride] - wm_x;
            const double dy = y[i * ystride] - wm_y;
            const double d = dy - b * dx;
            d2 += wi * d * d;
          }
      }

    *chisq = d2;
  }

  return GSL_SUCCESS;
}

int
gsl_fit_linear_est (const double x,
                    const double c0, const double c1,
                    const double c00, const double c01, const double c11,
                    double *y, double *y_err)
{
  *y = c0 + c1 * x;
  *y_err = sqrt (c00 + x * (2 * c01 + c11 * x));
  return GSL_SUCCESS;
}

int
gsl_fit_mul (const double *x, const size_t xstride,
             const double *y, const size_t ystride,
             const size_t n,
             double *c1, double *cov_11, double *sumsq)
{
  double m_x = 0, m_y = 0, m_dx2 = 0, m_dxdy = 0;

  size_t i;

  for (i = 0; i < n; i++)
    {
      m_x += (x[i * xstride] - m_x) / (i + 1.0);
      m_y += (y[i * ystride] - m_y) / (i + 1.0);
    }

  for (i = 0; i < n; i++)
    {
      const double dx = x[i * xstride] - m_x;
      const double dy = y[i * ystride] - m_y;

      m_dx2 += (dx * dx - m_dx2) / (i + 1.0);
      m_dxdy += (dx * dy - m_dxdy) / (i + 1.0);
    }

  /* In terms of y =  b x */

  {
    double s2 = 0, d2 = 0;
    double b = (m_x * m_y + m_dxdy) / (m_x * m_x + m_dx2);

    *c1 = b;

    /* Compute chi^2 = \sum (y_i -  b * x_i)^2 */

    for (i = 0; i < n; i++)
      {
        const double dx = x[i * xstride] - m_x;
        const double dy = y[i * ystride] - m_y;
        const double d = (m_y - b * m_x) + dy - b * dx;
        d2 += d * d;
      }

    s2 = d2 / (n - 1.0);        /* chisq per degree of freedom */

    *cov_11 = s2 * 1.0 / (n * (m_x * m_x + m_dx2));

    *sumsq = d2;
  }

  return GSL_SUCCESS;
}

int
gsl_fit_wmul (const double *x, const size_t xstride,
              const double *w, const size_t wstride,
              const double *y, const size_t ystride,
              const size_t n,
              double *c1, double *cov_11, double *chisq)
{

  /* compute the weighted means and weighted deviations from the means */

  /* wm denotes a "weighted mean", wm(f) = (sum_i w_i f_i) / (sum_i w_i) */

  double W = 0, wm_x = 0, wm_y = 0, wm_dx2 = 0, wm_dxdy = 0;

  size_t i;

  for (i = 0; i < n; i++)
    {
      const double wi = w[i * wstride];

      if (wi > 0)
        {
          W += wi;
          wm_x += (x[i * xstride] - wm_x) * (wi / W);
          wm_y += (y[i * ystride] - wm_y) * (wi / W);
        }
    }

  W = 0;                        /* reset the total weight */

  for (i = 0; i < n; i++)
    {
      const double wi = w[i * wstride];

      if (wi > 0)
        {
          const double dx = x[i * xstride] - wm_x;
          const double dy = y[i * ystride] - wm_y;

          W += wi;
          wm_dx2 += (dx * dx - wm_dx2) * (wi / W);
          wm_dxdy += (dx * dy - wm_dxdy) * (wi / W);
        }
    }

  /* In terms of y = b x */

  {
    double d2 = 0;
    double b = (wm_x * wm_y + wm_dxdy) / (wm_x * wm_x + wm_dx2);

    *c1 = b;

    *cov_11 = 1 / (W * (wm_x * wm_x + wm_dx2));

    /* Compute chi^2 = \sum w_i (y_i - b * x_i)^2 */

    for (i = 0; i < n; i++)
      {
        const double wi = w[i * wstride];

        if (wi > 0)
          {
            const double dx = x[i * xstride] - wm_x;
            const double dy = y[i * ystride] - wm_y;
            const double d = (wm_y - b * wm_x) + (dy - b * dx);
            d2 += wi * d * d;
          }
      }

    *chisq = d2;
  }

  return GSL_SUCCESS;
}

int
gsl_fit_mul_est (const double x,
                 const double c1, const double c11,
                 double *y, double *y_err)
{
  *y = c1 * x;
  *y_err = sqrt (c11) * fabs (x);
  return GSL_SUCCESS;
}


#include <stdio.h>
#include <gsl/gsl_statistics.h>
#include <stdlib.h>
#include <float.h>

#ifndef N
#define N 32
#endif

#define K N/2

#include <klee/klee.h>
int
main(void)
{
  double data[K], w[K];
  float input[N];

  // symbolic inputs
  klee_make_symbolic(&input, sizeof(input), "input");

  int i;
  for (i = 0; i < K; i++){
    w[i] = input[i]  / FLT_MAX * 100;
  }
  for (i = 0; i < K; i++){
    data[i] = input[i+K]  / FLT_MAX * 100;
  }

  double wtss, wmean;
  wmean    = gsl_stats_wmean(w, 1, data, 1, K);
  wtss     = gsl_stats_wtss_m(w, 1, data, 1, K, wmean);

  printf ("The sample variance is %g\n", wtss);
  return 0;
}


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

/* Modified for standalone use in polynomial directory, B.Gough 2001 */

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

int
gsl_poly_dd_init (double dd[], const double xa[], const double ya[],
                  size_t size)
{
  size_t i, j;

  /* Newton's divided differences */

  dd[0] = ya[0];

  for (j = size - 1; j >= 1; j--)
    {
      dd[j] = (ya[j] - ya[j - 1]) / (xa[j] - xa[j - 1]);
    }

  for (i = 2; i < size; i++)
    {
      for (j = size - 1; j >= i; j--)
        {
          dd[j] = (dd[j] - dd[j - 1]) / (xa[j] - xa[j - i]);
        }
    }

  return GSL_SUCCESS;
}

int
gsl_poly_dd_taylor (double c[], double xp,
                    const double dd[], const double xa[], size_t size,
                    double w[])
{
  size_t i, j;

  for (i = 0; i < size; i++)
    {
      c[i] = 0.0;
      w[i] = 0.0;
    }

  w[size - 1] = 1.0;

  c[0] = dd[0];

  for (i = size - 1; i-- > 0;)
    {
      w[i] = -w[i + 1] * (xa[size - 2 - i] - xp);

      for (j = i + 1; j < size - 1; j++)
        {
          w[j] = w[j] - w[j + 1] * (xa[size - 2 - i] - xp);
        }

      for (j = i; j < size; j++)
        {
          c[j - i] += w[j] * dd[size - i - 1];
        }
    }

  return GSL_SUCCESS;
}

/*
gsl_poly_dd_hermite_init()
  Compute divided difference representation of data
for Hermite polynomial interpolation

Inputs: dd   - (output) array of size 2*size containing
               divided differences, dd[k] = f[z_0,z_1,...,z_k]
        za   - (output) array of size 2*size containing
               z values
        xa   - x data
        ya   - y data
        dya  - dy/dx data
        size - size of xa,ya,dya arrays

Return: success
*/

int
gsl_poly_dd_hermite_init (double dd[], double za[], const double xa[], const double ya[],
                          const double dya[], const size_t size)
{
  const size_t N = 2 * size;
  size_t i, j;

  /* Hermite divided differences */

  dd[0] = ya[0];

  /* compute: dd[j] = f[z_{j-1},z_j] for j \in [1,N-1] */
  for (j = 0; j < size; ++j)
    {
      za[2*j] = xa[j];
      za[2*j + 1] = xa[j];

      if (j != 0)
        {
          dd[2*j] = (ya[j] - ya[j - 1]) / (xa[j] - xa[j - 1]);
          dd[2*j - 1] = dya[j - 1];
        }
    }

  dd[N - 1] = dya[size - 1];

  for (i = 2; i < N; i++)
    {
      for (j = N - 1; j >= i; j--)
        {
          dd[j] = (dd[j] - dd[j - 1]) / (za[j] - za[j - i]);
        }
    }

  return GSL_SUCCESS;
} /* gsl_poly_dd_hermite_init() */


#include "lah.h"

#ifdef HAVE_LAPACK /* Use a LAPACK package to do the heavy work */
#include <cblas.h>

lah_Return lah_matMul(lah_MatOp transA, lah_MatOp transB,
                      lah_value alpha, lah_value beta,
                      lah_mat *C, lah_mat const *A, lah_mat const *B)
{

    CBLAS_TRANSPOSE transA_cblas = CblasNoTrans;
    CBLAS_TRANSPOSE transB_cblas = CblasNoTrans;

    if (C == NULL || A == NULL || B == NULL )
        return lahReturnParameterError;

    /*
    order = LAH_CBLAS_LAYOUT(C);

    if (order == CblasColMajor)
    {
        lda = A->nR;
        ldb = B->nR;
        ldc = C->nR;
    }
    else
    {
        lda = A->nC;
        ldb = B->nC;
        ldc = C->nC;
    }
    */

    if (transA != lahNorm)
        transA_cblas = CblasTrans;
    if (transB != lahNorm)
        transB_cblas = CblasTrans;

    GEMM(LAH_CBLAS_LAYOUT, transA_cblas, transB_cblas, C->nR, C->nC, A->nC, beta,
         A->data, LAH_LEADING_DIM(A), B->data, LAH_LEADING_DIM(B), alpha, C->data, LAH_LEADING_DIM(C));

    return lahReturnOk;
}
#else /* fallback to primitive linear algebra subroutines */
#include <stdlib.h>
/* Computes the general Matrix mulitplikation of form:
 * C = alpha*C + beta*op(A)*op(B)
 * similar to the BLAS routine ?gemm.
 * op(A) = A for transA=lahNorm or op(A) = At for transA = lahTrans */
lah_Return lah_matMul(lah_MatOp transA, lah_MatOp transB,
                      lah_value alpha, lah_value beta,
                      lah_mat *C, const lah_mat *A, const lah_mat *B)
{
    lah_index c, r, i;
    lah_value *res, *value1, *value2;
    lah_value temp;

    if (C == NULL || A == NULL || B == NULL)
    {
        return lahReturnParameterError;
    }

    /* NOTE: lah_matMul calls itself with an transposed view of
     * the corresponding kop_mat and sets transA resp. transB
     * to lahNorm (else we would have an endless recursion)
    */
    if (transA == lahTrans)
    {
        /* Call lah_matMul again with transposed */
        lah_mat *At = lah_matTrans(A);
        lah_Return result = lah_matMul(lahNorm, transB, alpha,
                                         beta, C, At, B);
        free(At);
        return result;
    }
    if (transB == lahTrans)
    {
        /* Call lah_matMul again with transposed of B */
        lah_mat *Bt = lah_matTrans(B);
        lah_Return result = lah_matMul(transA, lahNorm, alpha,
                                       beta, C, A, Bt);
        free(Bt);
        return result;
    }

    if (C->nR != A->nR || C->nC != B->nC || A->nC != B->nR)
    {
        return lahReturnParameterError;
    }

    for (c = 0; c < C->nC; c++)
    {
        for (r = 0; r < C->nR; r++)
        {
            res = C->data + C->incRow * r + c * C->incCol;
            value1 = A->data + r * A->incRow;
            value2 = B->data + c * B->incCol;
            temp = 0;
            for (i = 0; i < A->nC; i++, value1 += A->incCol, value2 += B->incRow)
            {
                temp += *value1 * *value2;
            }
            *res = alpha * *res + beta * temp;
        }
    }

    return lahReturnOk;
}
#endif /* endif primitive implementation */


/*
 * Identity_matrix: test gsl_matrix_set_identity (make an identity matrix)
 *
 * Copyright (c) 2012 Jérémie Decock
 *
 * Required: GSL library (libgsl0-dev)
 * Usage: gcc identity_matrix.c -lgsl -lgslcblas -lm
 *    or: gcc identity_matrix.c $(pkg-config --libs gsl)
 */

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

#define I 3
#define J 3

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

    gsl_matrix * m = gsl_matrix_alloc(I, J);

    gsl_matrix_set_identity(m);

    //gsl_matrix_fprintf(stdout, m, "%f");

    int i, j;
    for(i=0 ; i<I ; i++) {
        for(j=0 ; j<J ; j++) {
            printf("%2.1f ", gsl_matrix_get(m, i, j));
        }
        printf("\n");
    }

    gsl_matrix_free(m);

    return 0;
}


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

/* Created: [GJ] Tue Apr 23 21:26:53 EDT 1996
 */
#include <config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <gsl/gsl_errno.h>
/* #include "interpolation.h" */
#include <gsl/gsl_integration.h>
#if 0

static char * info_string = 0;

void set_integ_info(const char * mess)
{
  if(info_string != 0) free(info_string);
  info_string = 0;
  if(mess != 0) {
    info_string = (char *)malloc((strlen(mess)+1) * sizeof(char));
    strcpy(info_string, mess);
  }
}

/* Integration function for use with open_romberg(). */
#define FUNC(x) ((*func)(x))
static double midpnt(double (*func)(double), double a, double b, int n)
{
  double x,tnm,sum,del,ddel;
  static double s;
  static int it;
  int j;

  if (n == 1) {
    it=1;
    return (s=(b-a)*FUNC(0.5*(a+b)));
  }
  else {
    tnm=it;
    del=(b-a)/(3.0*tnm);
    ddel=del+del;
    x=a+0.5*del;
    sum=0.0;
    for (j=1;j<=it;j++) {
      sum += FUNC(x);
      x += ddel;
      sum += FUNC(x);
      x += del;
    }
    it *= 3;
    s=(s+(b-a)*sum/tnm)/3.0;
    return s;
  }
}
#undef FUNC

#define JMAX 10
#define JMAXP JMAX+1
#define K 5
double open_romberg(double(*func)(double), double a, double b, double eps)
{
  int j;
  double ss,dss,h[JMAXP+1],s[JMAXP+1];

  h[1]=1.0;
  for (j=1;j<=JMAX;j++) {
    s[j]=midpnt(func,a,b,j);
    if (j >= K) {
      /* local_polint(&h[j-K],&s[j-K],K,0.0,&ss,&dss); */
      interp_poly(&h[j-K]+1,&s[j-K]+1,K,0.0,&ss,&dss);
      if (fabs(dss) < eps * fabs(ss)) return ss;
    }
    s[j+1]=s[j];
    h[j+1]=h[j]/9.0;
  }

  push_error("open_romberg: too many steps", Error_ConvFail_);
  push_generic_error("open_romberg:", info_string);

  return 0.;
}
#undef JMAX
#undef JMAXP
#undef K

double gauss_legendre_10(double (*func)(double), double a, double b)
{
  int j;
  static double x[] = {0.,
                         0.1488743389,
                         0.4333953941,
                         0.6794095682,
                         0.8650633666,
                         0.9739065285};
  static double w[] = {0.,
                         0.2955242247,
                         0.2692667193,
                         0.2190863625,
                         0.1494513491,
                         0.0666713443};

  double xm = 0.5 * (b + a);
  double xr = 0.5 * (b - a);

  double s = 0.;
  double dx;
  double result;

  for(j=1; j<=5; j++){
    dx = xr * x[j];
    s += w[j] * ((*func)(xm+dx) + (*func)(xm-dx));
  }
  result = s * xr;

  return result;
}

/************************************************************************
 *                                                                      *
 * Trapezoid rule.                                                      *
 *                                                                      *
 * The original trapzd() function from the Numerical Recipes worked     *
 * by side-effects; it tried to remember the value of the integral      *
 * at the current level of refinement. This is REAL BAD, because if     *
 * you try to use it to do a double integral you will get a surprise.   *
 * You cannot tell which "integral" it is remembering. This stems from  *
 * the stupid fact that there was only one, essentially global,         *
 * variable that was doing this memory job.                             *
 *                                                                      *
 * The solution is simple: pass the current refinement to the function  *
 * so that it can be remembered by an external environment, making it   *
 * easy to avoid confusion.                                             *
 *                                                                      *
 * So the new-method code-fragment for doing an integral looks like:    *
 *                                                                      *
 *       double answer;                                                 *
 *       for(j=1; j<=M+1; j++)                                          *
 *         trapezoid_rule(func, a, b, j, &answer);                      *
 *                                                                      *
 ************************************************************************/
void trapezoid_rule(double(*f)(double), double a, double b, int n, double *s)
{
  double x, tnm, sum, del;
  int it, j;

  if(n==1){
    *s = 0.5 * (b-a) * (f(b) + f(a));
  }
  else {
    for(it=1, j=1; j < n-1; j++)  it <<= 1;
    tnm = (double) it;
    del = (b-a) / tnm;
    x = a + 0.5 * del;

    for(sum=0., j=1; j<=it; j++, x+=del) { sum += f(x); }

    *s = 0.5 * (*s + del * sum);
  }
}

/************************************************************************
 *                                                                      *
 * Trapezoid rule.                                                      *
 * This version produces a tracing output.                              *
 *                                                                      *
 ************************************************************************/
#define FUNC(x) ((*func)(x))

void test_trapezoid_rule(double(*func)(double), double a, double b, int n,
                         double *s)
{
  double x, tnm, sum, del;
  int it, j;

  if(n==1){
    printf("t:  a= %g  b= %g   f(a)= %g  f(b)= %g\n",
           a, b, FUNC(a), FUNC(b));
  }

  if(n==1){
    *s = 0.5 * (b-a) * (FUNC(b) + FUNC(a));
    printf("s= %g\n", *s);
  }
  else {
    for(it=1, j=1; j < n-1; j++)  it <<= 1;
    tnm = (double) it;
    del = (b-a) / tnm;
    x = a + 0.5 * del;

    for(sum=0., j=1; j<=it; j++, x+=del) sum += FUNC(x);

    *s = 0.5 * (*s + del * sum);

    printf("sum= %g   tnm= %g  del= %g  s= %g\n", sum, tnm, del, *s);
  }
}
#undef FUNC

/* This is fixed to use the non-side-effecting version of the
 * trapezoidal rule, as implemented in trapezoid_rule() above.
 * See the discussion there for explanation of the original problem.
 */
#define JMAX 20
double gsl_integ_simpson(double (*func)(double), double a, double b, double eps)
{
  int j;
  double s, st, ost, os;

  ost = os = -1.e50;

  for(j=1; j<=JMAX; j++){

    trapezoid_rule(func, a, b, j, &st);
    s = (4.*st - ost) / 3.;

    if(fabs(s-os) < eps * fabs(os))
      return s;

    os = s;
    ost = st;
  }

  GSL_MESSAGE("simpson: too many steps");

  return 0.;
}
#undef JMAX

double gsl_integ_simpson_table(const double * x, const double * y, int n)
{
  int i;
  double result = 0.;
  for(i=0; i<n-1; i++) {
    result += 0.5 * (y[i+1] + y[i]) * (x[i+1] - x[i]);
  }
  return result;
}

/* apparatus for lorentzian variable change to remove pole */
static double (*dummy_f)(double);
static double dummy_x0;
static double dummy_w;
static inline double f_y(double y)
{ return dummy_f(dummy_x0 + dummy_w * tan(dummy_w * y)); }

double gsl_integ_lorenz(double (*f)(double),
                        double x0, double w,
                        double a, double b,
                        double eps)
{
  dummy_f = f;
  dummy_x0 = x0;
  dummy_w = fabs(w);

  if(dummy_w < 10.*sqrt(min_double)) {
    char buff[100];
    sprintf(buff,"lorenz_integ: width w= %g  too small", dummy_w);
    GSL_MESSAGE(buff);
    return 0.;
  }
  else {
    double lower_y = atan((a-x0)/dummy_w) / dummy_w;
    double upper_y = atan((b-x0)/dummy_w) / dummy_w;
    return gsl_integ_simpson(f_y, lower_y, upper_y, eps);
  }
}

#endif /* 0 */


/**
 * nn.c
 *
 * Created by mizu-bai
 *
 * This is a implementation of Tensors Warm-up: numpy in LEARNING PYTORCH WITH EXAMPLES
 * (https://pytorch.org/tutorials/beginner/pytorch_with_examples.html#) using C with GSL.
 */

#include <stdio.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_poly.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_rng.h>

int main(int argc, const char *argv[]) {
    // create random input and output data
    int size = 2000;
    gsl_vector *x = gsl_vector_alloc(size);
    gsl_vector *y = gsl_vector_alloc(size);
    for (int i = 0; i < size; i++) {
        double x_tmp = 2 * M_PI / (double)(size - 1) * (double)i - M_PI;
        gsl_vector_set(x, i, x_tmp);
        gsl_vector_set(y, i, sin(x_tmp));
    }

    // randomly init weights
    int w_size = 4;
    gsl_vector *w = gsl_vector_alloc(w_size);
    gsl_rng *r = gsl_rng_alloc(gsl_rng_default);
    for (int i = 0; i < w_size; i++) {
        gsl_vector_set(w, i, gsl_ran_gaussian(r, 1.0));
    }
    gsl_rng_free(r);

    double learning_rate = 1e-6;
    for(int t = 0; t < 2000; t++) {
        // forward pass, compute predicted y & compute loss
        gsl_vector *y_pred = gsl_vector_alloc(size);
        double loss = 0;
        for (int i = 0; i < size; i++) {
            double w_tmp = gsl_poly_eval(w->data, w_size, gsl_vector_get(x, i));
            gsl_vector_set(y_pred, i, w_tmp);
            loss += gsl_pow_2(gsl_vector_get(y_pred, i) - gsl_vector_get(y, i));
        }
        if (t % 100 == 99) {
            printf("t: %d, loss: %.4f\n", t, loss);
        }

        // compute gradients of w with respect to loss
        gsl_vector *grad_y_pred = gsl_vector_alloc(size);
        gsl_vector_memcpy(grad_y_pred, y_pred);
        gsl_vector_sub(grad_y_pred, y);
        gsl_vector_scale(grad_y_pred, 2);
        gsl_vector *grad_w = gsl_vector_alloc(w_size);
        gsl_vector_set_all(grad_w, 0.0);
        for(int i = 0; i < size; i++) {
            gsl_vector *grad_y_pred_tmp = gsl_vector_alloc(size);
            gsl_vector *x_tmp = gsl_vector_alloc(size);

            gsl_vector_set(grad_w, 0, gsl_vector_get(grad_w, 0) + gsl_vector_get(grad_y_pred, i));

            gsl_vector_memcpy(grad_y_pred_tmp, grad_y_pred);
            gsl_vector_mul(grad_y_pred_tmp, x);
            gsl_vector_set(grad_w, 1, gsl_vector_get(grad_w, 1) + gsl_vector_get(grad_y_pred_tmp, i));

            gsl_vector_memcpy(grad_y_pred_tmp, grad_y_pred);
            gsl_vector_memcpy(x_tmp, x);
            gsl_vector_mul(x_tmp, x);
            gsl_vector_mul(grad_y_pred_tmp, x_tmp);
            gsl_vector_set(grad_w, 2, gsl_vector_get(grad_w, 2) + gsl_vector_get(grad_y_pred_tmp, i));

            gsl_vector_memcpy(grad_y_pred_tmp, grad_y_pred);
            gsl_vector_memcpy(x_tmp, x);
            gsl_vector_mul(x_tmp, x);
            gsl_vector_mul(x_tmp, x);
            gsl_vector_mul(grad_y_pred_tmp, x_tmp);
            gsl_vector_set(grad_w, 3, gsl_vector_get(grad_w, 3) + gsl_vector_get(grad_y_pred_tmp, i));

            gsl_vector_free(grad_y_pred_tmp);
            gsl_vector_free(x_tmp);
        }

        gsl_vector_scale(grad_w, learning_rate);
        gsl_vector_sub(w, grad_w);

        // free
        gsl_vector_free(y_pred);
        gsl_vector_free(grad_y_pred);
        gsl_vector_free(grad_w);
    }

    printf("Result: y = %.4f + %.4f * x + %.4f * x^2 + %.4f * x^3\n", gsl_vector_get(w, 0), gsl_vector_get(w, 1), gsl_vector_get(w, 2), gsl_vector_get(w, 3));

    // free
    gsl_vector_free(x);
    gsl_vector_free(y);
    gsl_vector_free(w);

    return 0;
}


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

#include <config.h>
#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_cdf.h>

/* The exponential distribution has the form

   p(x) dx = exp(-x/mu) dx/mu

   for x = 0 ... +infty */

double
gsl_cdf_exponential_P (const double x, const double mu)
{
  if (x < 0)
    {
      return 0;
    }
  else
    {
      double P = -expm1 (-x / mu);

      return P;
    }
}

double
gsl_cdf_exponential_Q (const double x, const double mu)
{
  if (x < 0)
    {
      return 1;
    }
  else
    {
      double Q = exp (-x / mu);

      return Q;
    }
}


/* specfunc/gsl_sf_legendre.h
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2001, 2002, 2004 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 */

#ifndef __GSL_SF_LEGENDRE_H__
#define __GSL_SF_LEGENDRE_H__

#include <gsl/gsl_sf_result.h>

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

__BEGIN_DECLS

/* P_l(x)   l >= 0; |x| <= 1
 *
 * exceptions: GSL_EDOM
 */
int     gsl_sf_legendre_Pl_e(const int l, const double x, gsl_sf_result * result);
double  gsl_sf_legendre_Pl(const int l, const double x);

/* P_l(x) for l=0,...,lmax; |x| <= 1
 *
 * exceptions: GSL_EDOM
 */
int gsl_sf_legendre_Pl_array(
  const int lmax, const double x,
  double * result_array
  );

/* P_l(x) and P_l'(x) for l=0,...,lmax; |x| <= 1
 *
 * exceptions: GSL_EDOM
 */
int gsl_sf_legendre_Pl_deriv_array(
  const int lmax, const double x,
  double * result_array,
  double * result_deriv_array
  );

/* P_l(x), l=1,2,3
 *
 * exceptions: none
 */
int gsl_sf_legendre_P1_e(double x, gsl_sf_result * result);
int gsl_sf_legendre_P2_e(double x, gsl_sf_result * result);
int gsl_sf_legendre_P3_e(double x, gsl_sf_result * result);
double gsl_sf_legendre_P1(const double x);
double gsl_sf_legendre_P2(const double x);
double gsl_sf_legendre_P3(const double x);

/* Q_0(x), x > -1, x != 1
 *
 * exceptions: GSL_EDOM
 */
int gsl_sf_legendre_Q0_e(const double x, gsl_sf_result * result);
double gsl_sf_legendre_Q0(const double x);

/* Q_1(x), x > -1, x != 1
 *
 * exceptions: GSL_EDOM
 */
int gsl_sf_legendre_Q1_e(const double x, gsl_sf_result * result);
double gsl_sf_legendre_Q1(const double x);

/* Q_l(x), x > -1, x != 1, l >= 0
 *
 * exceptions: GSL_EDOM
 */
int gsl_sf_legendre_Ql_e(const int l, const double x, gsl_sf_result * result);
double gsl_sf_legendre_Ql(const int l, const double x);

/* P_l^m(x)  m >= 0; l >= m; |x| <= 1.0
 *
 * Note that this function grows combinatorially with l.
 * Therefore we can easily generate an overflow for l larger
 * than about 150.
 *
 * There is no trouble for small m, but when m and l are both large,
 * then there will be trouble. Rather than allow overflows, these
 * functions refuse to calculate when they can sense that l and m are
 * too big.
 *
 * If you really want to calculate a spherical harmonic, then DO NOT
 * use this. Instead use legendre_sphPlm() below, which  uses a similar
 * recursion, but with the normalized functions.
 *
 * exceptions: GSL_EDOM, GSL_EOVRFLW
 */
int     gsl_sf_legendre_Plm_e(const int l, const int m, const double x, gsl_sf_result * result);
double  gsl_sf_legendre_Plm(const int l, const int m, const double x);

/* P_l^m(x)  m >= 0; l >= m; |x| <= 1.0
 * l=|m|,...,lmax
 *
 * exceptions: GSL_EDOM, GSL_EOVRFLW
 */
int gsl_sf_legendre_Plm_array(
  const int lmax, const int m, const double x,
  double * result_array
  );

/* P_l^m(x)  and d(P_l^m(x))/dx;  m >= 0; lmax >= m; |x| <= 1.0
 * l=|m|,...,lmax
 *
 * exceptions: GSL_EDOM, GSL_EOVRFLW
 */
int gsl_sf_legendre_Plm_deriv_array(
  const int lmax, const int m, const double x,
  double * result_array,
  double * result_deriv_array
  );

/* P_l^m(x), normalized properly for use in spherical harmonics
 * m >= 0; l >= m; |x| <= 1.0
 *
 * There is no overflow problem, as there is for the
 * standard normalization of P_l^m(x).
 *
 * Specifically, it returns:
 *
 *        sqrt((2l+1)/(4pi)) sqrt((l-m)!/(l+m)!) P_l^m(x)
 *
 * exceptions: GSL_EDOM
 */
int     gsl_sf_legendre_sphPlm_e(const int l, int m, const double x, gsl_sf_result * result);
double  gsl_sf_legendre_sphPlm(const int l, const int m, const double x);

/* sphPlm(l,m,x) values
 * m >= 0; l >= m; |x| <= 1.0
 * l=|m|,...,lmax
 *
 * exceptions: GSL_EDOM
 */
int gsl_sf_legendre_sphPlm_array(
  const int lmax, int m, const double x,
  double * result_array
  );

/* sphPlm(l,m,x) and d(sphPlm(l,m,x))/dx values
 * m >= 0; l >= m; |x| <= 1.0
 * l=|m|,...,lmax
 *
 * exceptions: GSL_EDOM
 */
int gsl_sf_legendre_sphPlm_deriv_array(
  const int lmax, const int m, const double x,
  double * result_array,
  double * result_deriv_array
  );

/* size of result_array[] needed for the array versions of Plm
 * (lmax - m + 1)
 */
int gsl_sf_legendre_array_size(const int lmax, const int m);

/* Irregular Spherical Conical Function
 * P^{1/2}_{-1/2 + I lambda}(x)
 *
 * x > -1.0
 * exceptions: GSL_EDOM
 */
int gsl_sf_conicalP_half_e(const double lambda, const double x, gsl_sf_result * result);
double gsl_sf_conicalP_half(const double lambda, const double x);

/* Regular Spherical Conical Function
 * P^{-1/2}_{-1/2 + I lambda}(x)
 *
 * x > -1.0
 * exceptions: GSL_EDOM
 */
int gsl_sf_conicalP_mhalf_e(const double lambda, const double x, gsl_sf_result * result);
double gsl_sf_conicalP_mhalf(const double lambda, const double x);

/* Conical Function
 * P^{0}_{-1/2 + I lambda}(x)
 *
 * x > -1.0
 * exceptions: GSL_EDOM
 */
int gsl_sf_conicalP_0_e(const double lambda, const double x, gsl_sf_result * result);
double gsl_sf_conicalP_0(const double lambda, const double x);

/* Conical Function
 * P^{1}_{-1/2 + I lambda}(x)
 *
 * x > -1.0
 * exceptions: GSL_EDOM
 */
int gsl_sf_conicalP_1_e(const double lambda, const double x, gsl_sf_result * result);
double gsl_sf_conicalP_1(const double lambda, const double x);

/* Regular Spherical Conical Function
 * P^{-1/2-l}_{-1/2 + I lambda}(x)
 *
 * x > -1.0, l >= -1
 * exceptions: GSL_EDOM
 */
int gsl_sf_conicalP_sph_reg_e(const int l, const double lambda, const double x, gsl_sf_result * result);
double gsl_sf_conicalP_sph_reg(const int l, const double lambda, const double x);

/* Regular Cylindrical Conical Function
 * P^{-m}_{-1/2 + I lambda}(x)
 *
 * x > -1.0, m >= -1
 * exceptions: GSL_EDOM
 */
int gsl_sf_conicalP_cyl_reg_e(const int m, const double lambda, const double x, gsl_sf_result * result);
double gsl_sf_conicalP_cyl_reg(const int m, const double lambda, const double x);

/* The following spherical functions are specializations
 * of Legendre functions which give the regular eigenfunctions
 * of the Laplacian on a 3-dimensional hyperbolic space.
 * Of particular interest is the flat limit, which is
 * Flat-Lim := {lambda->Inf, eta->0, lambda*eta fixed}.
 */

/* Zeroth radial eigenfunction of the Laplacian on the
 * 3-dimensional hyperbolic space.
 *
 * legendre_H3d_0(lambda,eta) := sin(lambda*eta)/(lambda*sinh(eta))
 *
 * Normalization:
 * Flat-Lim legendre_H3d_0(lambda,eta) = j_0(lambda*eta)
 *
 * eta >= 0.0
 * exceptions: GSL_EDOM
 */
int gsl_sf_legendre_H3d_0_e(const double lambda, const double eta, gsl_sf_result * result);
double gsl_sf_legendre_H3d_0(const double lambda, const double eta);

/* First radial eigenfunction of the Laplacian on the
 * 3-dimensional hyperbolic space.
 *
 * legendre_H3d_1(lambda,eta) :=
 *    1/sqrt(lambda^2 + 1) sin(lam eta)/(lam sinh(eta))
 *    (coth(eta) - lambda cot(lambda*eta))
 *
 * Normalization:
 * Flat-Lim legendre_H3d_1(lambda,eta) = j_1(lambda*eta)
 *
 * eta >= 0.0
 * exceptions: GSL_EDOM
 */
int gsl_sf_legendre_H3d_1_e(const double lambda, const double eta, gsl_sf_result * result);
double gsl_sf_legendre_H3d_1(const double lambda, const double eta);

/* l'th radial eigenfunction of the Laplacian on the
 * 3-dimensional hyperbolic space.
 *
 * Normalization:
 * Flat-Lim legendre_H3d_l(l,lambda,eta) = j_l(lambda*eta)
 *
 * eta >= 0.0, l >= 0
 * exceptions: GSL_EDOM
 */
int gsl_sf_legendre_H3d_e(const int l, const double lambda, const double eta, gsl_sf_result * result);
double gsl_sf_legendre_H3d(const int l, const double lambda, const double eta);

/* Array of H3d(ell),  0 <= ell <= lmax
 */
int gsl_sf_legendre_H3d_array(const int lmax, const double lambda, const double eta, double * result_array);

/* associated legendre P_{lm} routines */

typedef enum
{
  GSL_SF_LEGENDRE_SCHMIDT,
  GSL_SF_LEGENDRE_SPHARM,
  GSL_SF_LEGENDRE_FULL,
  GSL_SF_LEGENDRE_NONE
} gsl_sf_legendre_t;

int gsl_sf_legendre_array(const gsl_sf_legendre_t norm,
                          const size_t lmax, const double x,
                          double result_array[]);
int gsl_sf_legendre_array_e(const gsl_sf_legendre_t norm,
                            const size_t lmax, const double x,
                            const double csphase,
                            double result_array[]);
int gsl_sf_legendre_deriv_array(const gsl_sf_legendre_t norm,
                                const size_t lmax, const double x,
                                double result_array[],
                                double result_deriv_array[]);
int gsl_sf_legendre_deriv_array_e(const gsl_sf_legendre_t norm,
                                  const size_t lmax, const double x,
                                  const double csphase,
                                  double result_array[],
                                  double result_deriv_array[]);
int gsl_sf_legendre_deriv_alt_array(const gsl_sf_legendre_t norm,
                                    const size_t lmax, const double x,
                                    double result_array[],
                                    double result_deriv_array[]);
int gsl_sf_legendre_deriv_alt_array_e(const gsl_sf_legendre_t norm,
                                      const size_t lmax, const double x,
                                      const double csphase,
                                      double result_array[],
                                      double result_deriv_array[]);
int gsl_sf_legendre_deriv2_array(const gsl_sf_legendre_t norm,
                                 const size_t lmax, const double x,
                                 double result_array[],
                                 double result_deriv_array[],
                                 double result_deriv2_array[]);
int gsl_sf_legendre_deriv2_array_e(const gsl_sf_legendre_t norm,
                                   const size_t lmax, const double x,
                                   const double csphase,
                                   double result_array[],
                                   double result_deriv_array[],
                                   double result_deriv2_array[]);
int gsl_sf_legendre_deriv2_alt_array(const gsl_sf_legendre_t norm,
                                     const size_t lmax, const double x,
                                     double result_array[],
                                     double result_deriv_array[],
                                     double result_deriv2_array[]);
int gsl_sf_legendre_deriv2_alt_array_e(const gsl_sf_legendre_t norm,
                                       const size_t lmax, const double x,
                                       const double csphase,
                                       double result_array[],
                                       double result_deriv_array[],
                                       double result_deriv2_array[]);
size_t gsl_sf_legendre_array_n(const size_t lmax);
size_t gsl_sf_legendre_array_index(const size_t l, const size_t m);
size_t gsl_sf_legendre_nlm(const size_t lmax);

__END_DECLS

#endif /* __GSL_SF_LEGENDRE_H__ */


#include <stdio.h>
#include <gsl/gsl_blas.h>
#include <time.h>
#include <stdlib.h>
#include <limits.h>
//gcc program.c -lgsl -lgslcblas -lm -o program

void saveTime(FILE *fp, char *type, int vSize, clock_t start, clock_t end) {
	//save time and size to file
	fprintf(fp, "%s, %d, %f\n", type, vSize, ((double) (end - start)) / CLOCKS_PER_SEC);
}

void ddot(FILE *fp, int vSize, gsl_vector *A, gsl_vector *B) {
	clock_t start, end;
	double result = 0;

	start = clock();
	gsl_blas_ddot(A, B, &result);
	end = clock();

	//save time and size to file
	saveTime(fp, "ddot", vSize, start, end);
}

void dgemv(FILE *fp, int vSize, gsl_vector *A) {
	clock_t start, end;
	gsl_matrix *M = gsl_matrix_alloc(vSize, vSize);
	gsl_vector *B = gsl_vector_alloc(vSize);

	for(int i = 0; i < vSize; i++) {
		for(int j = 0; j < vSize; j++) {
			gsl_matrix_set(M, i, j, (double) rand() * i / (double) (rand() + 1));
		}
		gsl_vector_set(B, i, rand() * i / (rand() + 1));
	}

	start = clock();
	gsl_blas_dgemv(CblasNoTrans, 1.0, M, A, 0.0, B);
	end = clock();
	saveTime(fp, "dgemv", vSize, start, end);

	gsl_matrix_free(M);
	gsl_vector_free(B);

}

void measure(FILE *fp, int size) {
	gsl_vector *x = gsl_vector_alloc(size);
	gsl_vector *y = gsl_vector_alloc(size);
	for(int t = 0; t < 10; t++) {
		for(int i = 0; i < size; i++) {
			gsl_vector_set(x, i, rand() * i / (rand() + 1));
			gsl_vector_set(y, i, rand() * i / (rand() + 1));
		}
		ddot(fp, size, x, y);
		dgemv(fp, size, x);
	}

	gsl_vector_free(x);
	gsl_vector_free(y);
}

int main (void) {
	//uruchomić i zmierzyć czasy działania obydwu funkcji dla różnych rozmiarów wektorów -> 10 pomiarów dla każdego rozmiaru wektora
	FILE *fp;
	srand(time(NULL));
	fp = fopen("wyniki.csv", "w+");
	fprintf(fp, "typ, rozmiar, czas\n"); //make headline

	for(int i = 100; i < 1000; i += 100) {
		int n = i;
			measure(fp, n);
			n += i;
	}

	fclose(fp);
	return 0;
}