/* poly/gsl_poly.h
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2004, 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.
 */

#ifndef __GSL_POLY_H__
#define __GSL_POLY_H__

#include <stdlib.h>
#include <gsl/gsl_inline.h>
#include <gsl/gsl_complex.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

/* Evaluate polynomial
 *
 * c[0] + c[1] x + c[2] x^2 + ... + c[len-1] x^(len-1)
 *
 * exceptions: none
 */

/* real polynomial, real x */
INLINE_DECL double gsl_poly_eval(const double c[], const int len, const double x);

/* real polynomial, complex x */
INLINE_DECL gsl_complex gsl_poly_complex_eval (const double c [], const int len, const gsl_complex z);

/* complex polynomial, complex x */
INLINE_DECL gsl_complex gsl_complex_poly_complex_eval (const gsl_complex c [], const int len, const gsl_complex z);

int gsl_poly_eval_derivs(const double c[], const size_t lenc, const double x, double res[], const size_t lenres);

#ifdef HAVE_INLINE
INLINE_FUN
double
gsl_poly_eval(const double c[], const int len, const double x)
{
  int i;
  double ans = c[len-1];
  for(i=len-1; i>0; i--) ans = c[i-1] + x * ans;
  return ans;
}

INLINE_FUN
gsl_complex
gsl_poly_complex_eval(const double c[], const int len, const gsl_complex z)
{
  int i;
  gsl_complex ans;
  GSL_SET_COMPLEX (&ans, c[len-1], 0.0);
  for(i=len-1; i>0; i--) {
    /* The following three lines are equivalent to
       ans = gsl_complex_add_real (gsl_complex_mul (z, ans), c[i-1]);
       but faster */
    double tmp = c[i-1] + GSL_REAL (z) * GSL_REAL (ans) - GSL_IMAG (z) * GSL_IMAG (ans);
    GSL_SET_IMAG (&ans, GSL_IMAG (z) * GSL_REAL (ans) + GSL_REAL (z) * GSL_IMAG (ans));
    GSL_SET_REAL (&ans, tmp);
  }
  return ans;
}

INLINE_FUN
gsl_complex
gsl_complex_poly_complex_eval(const gsl_complex c[], const int len, const gsl_complex z)
{
  int i;
  gsl_complex ans = c[len-1];
  for(i=len-1; i>0; i--) {
    /* The following three lines are equivalent to
       ans = gsl_complex_add (c[i-1], gsl_complex_mul (x, ans));
       but faster */
    double tmp = GSL_REAL (c[i-1]) + GSL_REAL (z) * GSL_REAL (ans) - GSL_IMAG (z) * GSL_IMAG (ans);
    GSL_SET_IMAG (&ans, GSL_IMAG (c[i-1]) + GSL_IMAG (z) * GSL_REAL (ans) + GSL_REAL (z) * GSL_IMAG (ans));
    GSL_SET_REAL (&ans, tmp);
  }
  return ans;
}
#endif /* HAVE_INLINE */

/* Work with divided-difference polynomials, Abramowitz & Stegun 25.2.26 */

int
gsl_poly_dd_init (double dd[], const double x[], const double y[],
                  size_t size);

INLINE_DECL double
gsl_poly_dd_eval (const double dd[], const double xa[], const size_t size, const double x);

#ifdef HAVE_INLINE
INLINE_FUN
double
gsl_poly_dd_eval(const double dd[], const double xa[], const size_t size, const double x)
{
  size_t i;
  double y = dd[size - 1];
  for (i = size - 1; i--;) y = dd[i] + (x - xa[i]) * y;
  return y;
}
#endif /* HAVE_INLINE */

int
gsl_poly_dd_taylor (double c[], double xp,
                    const double dd[], const double x[], size_t size,
                    double w[]);

/* Solve for real or complex roots of the standard quadratic equation,
 * returning the number of real roots.
 *
 * Roots are returned ordered.
 */
int gsl_poly_solve_quadratic (double a, double b, double c,
                              double * x0, double * x1);

int
gsl_poly_complex_solve_quadratic (double a, double b, double c,
                                  gsl_complex * z0, gsl_complex * z1);

/* Solve for real roots of the cubic equation
 * x^3 + a x^2 + b x + c = 0, returning the
 * number of real roots.
 *
 * Roots are returned ordered.
 */
int gsl_poly_solve_cubic (double a, double b, double c,
                          double * x0, double * x1, double * x2);

int
gsl_poly_complex_solve_cubic (double a, double b, double c,
                              gsl_complex * z0, gsl_complex * z1,
                              gsl_complex * z2);

/* Solve for the complex roots of a general real polynomial */

typedef struct
{
  size_t nc ;
  double * matrix ;
}
gsl_poly_complex_workspace ;

gsl_poly_complex_workspace * gsl_poly_complex_workspace_alloc (size_t n);
void gsl_poly_complex_workspace_free (gsl_poly_complex_workspace * w);

int
gsl_poly_complex_solve (const double * a, size_t n,
                        gsl_poly_complex_workspace * w,
                        gsl_complex_packed_ptr z);

__END_DECLS

#endif /* __GSL_POLY_H__ */


/*This program evaluates the differential equations for a photon's geodesic in Minkowski spacetime.
The equations are written in the form $\frac{d(x or p)^{\alpha}}{d\lambda}=f(x^{\alpha},p^{\alpha})$.
Where $p^{\alpha}={\dot{x}}^{\alpha}$*/

#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <gsl/gsl_errno.h>          //GSL error management module

#define C 299792.458     //Speed of light in velocity units
#define DLAMBDA 0.001   //Geodesics parameter step

/*Function for solving the differential equations of Minkowski geodesics.*/
void euler1(double *x0, double *x1, double *x2, double *x3, double *p0, double *p1, double *p2, double *p3, double *lambda)
{
  /*Increment in the variables of the differential equation we want to solve*/
  double dx0, dx1, dx2, dx3, dp0, dp1, dp2, dp3;

  /*Calculation of the increments*/
  dx0 = *p0*DLAMBDA; dx1 = *p1*DLAMBDA; dx2 = *p2*DLAMBDA; dx3 = *p3*DLAMBDA;
  dp0 = 0.0; dp1 = 0.0; dp2 = 0.0; dp3 = 0.0;

  /*New values of the variables of the differential equation. Since we are using pointers, when called the routine the value of variable change.*/
  *x0 = *x0 + dx0; *x1 = *x1 + dx1; *x2 = *x2 + dx2; *x3 = *x3 + dx3;
  *p0 = *p0 + dp0; *p1 = *p1 + dp1; *p2 = *p2 + dp2; *p3 = *p3 + dp3;

  /*Increment of parameter of geodesics*/
  *lambda = *lambda + DLAMBDA;
}

int main(void)
{
  int i;            //For array manipulation

  /*Initial conditions*/
  double x0 = 0.0, x1 = 0.0, x2 = 0.0, x3 = 0.0, p1 = 0.1*C, p2 = 0.9*C, p3 = 0.0, lambda = 0.0;
  double p0 = sqrt(pow(p1,2) + pow(p2,2) + pow(p3,2))/C;

  /*Pointer to file where solution of differential equation will be saved.*/
  FILE *geodesic;
  geodesic = fopen("geodesic_solution.dat","w");

  /*Write line of initial values in file*/
  fprintf(geodesic, "%.12lf %.12lf %.12lf %.12lf %.12lf %.12lf %.12lf %.12lf %.12lf\n", lambda, x0, x1, x2, x3, p0, p1, p2, p3);

  /*Solution of the differential equation*/
  for(i=0; i<1000; i++)
    {
      euler1(&x0, &x1, &x2, &x3, &p0, &p1, &p2, &p3, &lambda);
      fprintf(geodesic, "%12lf %12lf %12lf %12lf %12lf %12lf %12lf %12lf %12lf\n", lambda, x0, x1, x2, x3, p0, p1, p2, p3);
    }

  /** Releasing all used space in memory **/
  fclose(geodesic); //Close file storing the results

  /*GNUPLOT*/

  FILE *plot;
  plot = popen("gnuplot -persist","w");
  fprintf(plot, "set terminal x11 0\n");
  fprintf(plot, "set multiplot layout 1,3\n");
  fprintf(plot, "plot 'geodesic_solution.dat' using 1:2 not\n");
  fprintf(plot, "plot 'geodesic_solution.dat' using 1:3 not\n");
  fprintf(plot, "plot 'geodesic_solution.dat' using 1:4 not\n");
  fprintf(plot, "unset multiplot\n");
  fprintf(plot, "reset\n");
  fprintf(plot, "set terminal x11 1\n");
  fprintf(plot, "set multiplot layout 1,3\n");
  fprintf(plot, "plot 'geodesic_solution.dat' using 1:6 not\n");
  fprintf(plot, "plot 'geodesic_solution.dat' using 1:7 not\n");
  fprintf(plot, "plot 'geodesic_solution.dat' using 1:8 not\n");
  fprintf(plot, "unset multiplot\n");
  fprintf(plot, "set terminal x11 2\n");
  fprintf(plot, "splot 'geodesic_solution.dat' using 2:3:1\n");

  pclose(plot);
  //system("rm geodesic_solution.dat");
}


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

#ifdef ACCELERATE
#include <Accelerate/Accelerate.h>
#else
#include <cblas.h>
#endif

#include "neural.h"

void
neural_feed_forward(struct neural_net_layer *head, float *x, int batch_size)
{
	/* point "current" layer to head and set input as feature array x */
	struct neural_net_layer *curr = head;
	struct neural_net_layer *next;
	curr->act = x;

	int in_nodes;
	int out_nodes;

	while ((next = curr->next) != NULL) {
		in_nodes = curr->in_nodes;
		out_nodes = curr->out_nodes;

		/* act = X * Theta^T + bias */
		for (int i = 0; i < batch_size; i++) {
			memcpy(next->act + i * out_nodes, curr->bias, out_nodes * sizeof(float));
		}
		cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasTrans, batch_size, out_nodes, in_nodes,
			1.0, curr->act, in_nodes, curr->theta, in_nodes, 1.0, next->act, out_nodes);

		/* apply sigmoid function */
		for (int i = 0; i < out_nodes * batch_size; i++) {
			next->act[i] = 1.0 / (1 + expf(-next->act[i]));
		}

		/* transition to next layer */
		curr = next;
	}
}

void
neural_sgd_feed_forward(struct neural_net_layer *head, float *x)
{
	/* point "current" layer to head and set input as feature array x */
	struct neural_net_layer *curr = head;
	struct neural_net_layer *next;
	curr->act = x;

	int rows;
	int cols;

	while ((next = curr->next) != NULL) {
		rows = curr->out_nodes;
		cols = curr->in_nodes;

		/* act = theta * x + act */
		memcpy(next->act, curr->bias, rows * sizeof(float));
		cblas_sgemv(CblasRowMajor, CblasNoTrans, rows, cols, 1.0, curr->theta,
			cols, curr->act, 1, 1.0, next->act, 1);

		/* apply sigmoid function */
		for (int i = 0; i < rows; i++) {
			next->act[i] = 1.0 / (1 + expf(-next->act[i]));
		}

		/* transition to next layer */
		curr = next;
	}
}

void
neural_back_prop(struct neural_net_layer *tail, float *y, const int batch_size,
		const float alpha, const float lambda)
{
	/* get delta from final predictions vs. actual; delta = a - y */
	cblas_scopy(tail->in_nodes * batch_size, tail->act, 1, tail->delta, 1);
	cblas_saxpy(tail->in_nodes * batch_size, -1.0, y, 1, tail->delta, 1);

	struct neural_net_layer *curr;
	struct neural_net_layer *prev;
	int in_nodes;
	int out_nodes;

	for (curr = tail; curr->prev != NULL; curr = prev) {
		prev = curr->prev;
		in_nodes = prev->in_nodes;
		out_nodes = prev->out_nodes;

		/* Delta^(n-1) = Delta^(n) * Theta^(n-1) .* A^(n-1) (1 - A^(n-1)) */
		cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, batch_size, in_nodes, out_nodes,
				1.0, curr->delta, out_nodes, prev->theta, in_nodes, 0.0, prev->delta, in_nodes);
		for (int j = 0; j < in_nodes * batch_size; j++) {
			prev->delta[j] *= (prev->act[j] * (1 - prev->act[j]));
		}
	}

	for (curr = tail; curr->prev != NULL; curr = prev) {
		prev = curr->prev;
		in_nodes = prev->in_nodes;
		out_nodes = prev->out_nodes;

		/* bias -= alpha * delta */
		for (int i = 0; i < batch_size; i++) {
			cblas_saxpy(out_nodes, -alpha, curr->delta + i * out_nodes, 1, prev->bias, 1);
		}

		/* Theta -= alpha * (Delta^T * Act + lambda * Theta) */
		cblas_sgemm(CblasRowMajor, CblasTrans, CblasNoTrans, out_nodes, in_nodes, batch_size,
				-alpha, curr->delta, out_nodes, prev->act, in_nodes, 1 - alpha * lambda,
				prev->theta, in_nodes);
	}
}

void
neural_sgd_back_prop(struct neural_net_layer *tail, float *y, const float alpha,
		const float lambda)
{
	/* get delta from final predictions vs. actual; delta = a - y */
	cblas_scopy(tail->in_nodes, tail->act, 1, tail->delta, 1);
	cblas_saxpy(tail->in_nodes, -1.0, y, 1, tail->delta, 1);

	struct neural_net_layer *curr;
	struct neural_net_layer *prev;
	int rows;
	int cols;

	for (curr = tail; curr->prev != NULL; curr = prev) {
		prev = curr->prev;
		rows = prev->out_nodes;
		cols = prev->in_nodes;

		/* delta^(n-1) = theta^(n-1)T * delta^(n) .* a^(n-1) (1 - a^(n-1)) */
		cblas_sgemv(CblasRowMajor, CblasTrans, rows, cols, 1.0, prev->theta,
				cols, curr->delta, 1, 0.0, prev->delta, 1);
		for (int j = 0; j < cols; j++) {
			prev->delta[j] *= (prev->act[j] * (1 - prev->act[j]));
		}
	}

	for (curr = tail; curr->prev != NULL; curr = prev) {
		prev = curr->prev;
		rows = prev->out_nodes;
		cols = prev->in_nodes;

		/* bias -= alpha * delta */
		cblas_saxpy(rows, -alpha, curr->delta, 1, prev->bias, 1);

		/* theta -= alpha * (delta * actT + lambda * theta) */
		cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, rows, cols, 1, -alpha,
				curr->delta, 1, prev->act, cols, 1 - alpha * lambda, prev->theta, cols);
	}
}

void
neural_train_iteration(struct neural_net_layer *head, struct neural_net_layer *tail,
		float *features, float *labels, const int n_samples, const int batch_size,
		const float alpha, const float lambda)
{
	int n_features = head->in_nodes;
	int n_labels = tail->in_nodes;
	int i;

	if (batch_size == 1) {
		for (i = 0; i < n_samples; i++) {
			neural_sgd_feed_forward(head, features + i * n_features);
			neural_sgd_back_prop(tail, labels + i * n_labels, alpha, lambda);
		}
	} else {
		for (i = 0; i < n_samples / batch_size; i++) {
			neural_feed_forward(head, features + i * n_features * batch_size, batch_size);
			neural_back_prop(tail, labels + i * n_labels * batch_size, batch_size, alpha, lambda);
		}

		/* take care of remaining examples */
		int remainder = n_samples % batch_size;
		if (remainder > 0) {
			neural_feed_forward(head, features + i * n_features * batch_size, remainder);
			neural_back_prop(tail, labels + i * n_labels * batch_size, remainder, alpha, lambda);
		}
	}
}

void
neural_predict_prob(struct neural_net_layer *head, struct neural_net_layer *tail,
		float *features, float *preds, const int n_samples, const int batch_size)
{
	int n_features = head->in_nodes;
	int n_labels = tail->in_nodes;
	int i;

	if (batch_size == 1) {
		for (i = 0; i < n_samples; i++) {
			neural_sgd_feed_forward(head, features + i * n_features);
			memcpy(preds + i * n_labels, tail->act, n_labels * sizeof(float));
		}
	} else {
		for (i = 0; i < n_samples / batch_size; i++) {
			neural_feed_forward(head, features + i * n_features * batch_size, batch_size);
			memcpy(preds + i * n_labels * batch_size, tail->act,
					n_labels * batch_size * sizeof(float));
		}

		/* take care of remaining examples */
		int remainder = n_samples % batch_size;
		if (remainder > 0) {
			neural_feed_forward(head, features + i * n_features * batch_size, remainder);
			memcpy(preds + i * n_labels * batch_size, tail->act,
					n_labels * remainder * sizeof(float));
		}
	}
}


#include "matrix_support.h"
#include <cblas.h>

int main(int argc, char *argv[])
{
    /* enum CBLAS_ORDER {CblasRowMajor=101, CblasColMajor=102}; */
    /* enum CBLAS_TRANSPOSE {CblasNoTrans=111, CblasTrans=112, CblasConjTrans=113}; */
    /* enum CBLAS_UPLO {CblasUpper=121, CblasLower=122}; */
    /* enum CBLAS_DIAG {CblasNonUnit=131, CblasUnit=132}; */
    /* enum CBLAS_SIDE {CblasLeft=141, CblasRight=142}; */

    CBLAS_ORDER order = CblasRowMajor;
    CBLAS_TRANSPOSE transA = CblasNoTrans;
    CBLAS_TRANSPOSE transB = CblasNoTrans;
    int m = 2;
    int k = 3;
    int n = 4;
    double alpha = 1;
    double beta = 1;
    double *A = create_matrix(m, k);
    double *B = create_matrix(k, n);
    double *C = create_matrix(m, n);
    double *D = create_matrix(n, k);
    double *x = create_vector(k);
    double *d = create_vector(m);
    printf("A\n");
    init_matrix_vector(A, m * k, 0.0, 1.0, 2.0, 3.0, 4.0, 5.0);
    print_matrix(A, m, k);
    printf("\nB\n");
    init_matrix_vector(B, k * n, 0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0,
            10.0, 11.0);
    print_matrix(B, k, n);
    printf("\nD\n");
    init_matrix_vector(D, n * k, 0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0,
            10.0, 11.0);
    print_matrix(D, n, k);
    printf("\nx\n");
    init_matrix_vector(x, k * 1, -2.0, 2.0, 10.0);
    print_vector(x, k);

    /* void cblas_dgemm(const enum CBLAS_ORDER Order, const enum CBLAS_TRANSPOSE TransA, */
    /*                  const enum CBLAS_TRANSPOSE TransB, const int M, const int N, */
    /*                  const int K, const double alpha, const double *A, */
    /*                  const int lda, const double *B, const int ldb, */
    /*                  const double beta, double *C, const int ldc); */

    cblas_dgemm(order,
            transA,
            transB,
            m,
            n,
            k,
            alpha,
            A,
            k,
            B,
            n,
            beta,
            C,
            n);
    printf("\nC = A.dot(B)\n");
    print_matrix(C, m, n);

    printf("\nd = A.dot(x)\n");

    /*
     * dasum takes the sum of the absolute values
     */

    /**
     * ddot dot product between two vectors
     */

    /*
     * dnrm2 Eucledian norm of a vector
     */

    /*
     * dscal scales a vector by a constant
     */

    /*
     * daxpy constant times a vector plus a vector. uses unrolled loops for
     * increments equal to one.
     */

    /* void cblas_dgemv(const enum CBLAS_ORDER order, */
    /*              const enum CBLAS_TRANSPOSE TransA, const int M, const int N, */
    /*              const double alpha, const double *A, const int lda, */
    /*              const double *X, const int incX, const double beta, */
    /*              double *Y, const int incY); */
    cblas_dgemv(order,
            transA, // not transposed
            m,   // rows of a
            k,   // cols of a
            alpha, // scalar to multiply A with
            A,   //matrix A
            k, // lda: leading dimension of A
            x, // vector x
            1, // incx
            beta, // beta: scalar to multiply y with (0 having y as output)
            d, // vector d (Y in formula)
            1); // incy

    print_vector(d, m);
    return 0;
}


static char help[] =
"ODE system solver example using TS.  Solves N-dimensional system\n"
"    dy/dt = G(t,y)\n"
"with y(t0) = y0 to compute y(tf).  Serial only.\n"
"Sets TS type to Runge-Kutta.  The implemented example has\n"
"G_0 = y_1, G_1 = - y_0 + t, y_0(0) = 0, y_1(0) = 0.  The exact solution is\n"
"y_0(t) = t - sin(t), y_1(t) = 1 - cos(t).\n\n";

#include <petsc.h>

extern PetscErrorCode ExactSolution(double, Vec);
extern PetscErrorCode FormRHSFunction(TS, double, Vec, Vec, void*);

//STARTMAIN
int main(int argc,char **argv) {
  PetscErrorCode ierr;
  int       steps;
  double    t0 = 0.0, tf = 20.0, dt = 0.1, err;
  Vec       y, yexact;
  TS        ts;

  PetscInitialize(&argc,&argv,(char*)0,help);

  ierr = VecCreate(PETSC_COMM_WORLD,&y); CHKERRQ(ierr);
  ierr = VecSetSizes(y,PETSC_DECIDE,2); CHKERRQ(ierr);
  ierr = VecSetFromOptions(y); CHKERRQ(ierr);
  ierr = VecDuplicate(y,&yexact); CHKERRQ(ierr);

  ierr = TSCreate(PETSC_COMM_WORLD,&ts); CHKERRQ(ierr);
  ierr = TSSetProblemType(ts,TS_NONLINEAR); CHKERRQ(ierr);
  ierr = TSSetRHSFunction(ts,NULL,FormRHSFunction,NULL); CHKERRQ(ierr);
  ierr = TSSetType(ts,TSRK); CHKERRQ(ierr);

  // set time axis
  ierr = TSSetTime(ts,t0); CHKERRQ(ierr);
  ierr = TSSetMaxTime(ts,tf); CHKERRQ(ierr);
  ierr = TSSetTimeStep(ts,dt); CHKERRQ(ierr);
  ierr = TSSetExactFinalTime(ts,TS_EXACTFINALTIME_MATCHSTEP); CHKERRQ(ierr);
  ierr = TSSetFromOptions(ts); CHKERRQ(ierr);

  // set initial values and solve
  ierr = TSGetTime(ts,&t0); CHKERRQ(ierr);
  ierr = ExactSolution(t0,y); CHKERRQ(ierr);
  ierr = TSSolve(ts,y); CHKERRQ(ierr);

  // compute error and report
  ierr = TSGetStepNumber(ts,&steps); CHKERRQ(ierr);
  ierr = TSGetTime(ts,&tf); CHKERRQ(ierr);
  ierr = ExactSolution(tf,yexact); CHKERRQ(ierr);
  ierr = VecAXPY(y,-1.0,yexact); CHKERRQ(ierr);    // y <- y - yexact
  ierr = VecNorm(y,NORM_INFINITY,&err); CHKERRQ(ierr);
  ierr = PetscPrintf(PETSC_COMM_WORLD,
              "error at tf = %.3f with %d steps:  |y-y_exact|_inf = %g\n",
              tf,steps,err); CHKERRQ(ierr);

  VecDestroy(&y);  VecDestroy(&yexact);  TSDestroy(&ts);
  return PetscFinalize();
}
//ENDMAIN

//STARTCALLBACKS
PetscErrorCode ExactSolution(double t, Vec y) {
    double *ay;
    VecGetArray(y,&ay);
    ay[0] = t - sin(t);
    ay[1] = 1.0 - cos(t);
    VecRestoreArray(y,&ay);
    return 0;
}

PetscErrorCode FormRHSFunction(TS ts, double t, Vec y, Vec g, void *ptr) {
    const double *ay;
    double       *ag;
    VecGetArrayRead(y,&ay);
    VecGetArray(g,&ag);
    ag[0] = ay[1];            // = G_1(t,y)
    ag[1] = - ay[0] + t;      // = G_2(t,y)
    VecRestoreArrayRead(y,&ay);
    VecRestoreArray(g,&ag);
    return 0;
}
//ENDCALLBACKS


/*
** t-tests
**
** G.Lohmann, Jan 2017
*/

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

#include <gsl/gsl_cdf.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

#define TINY 1.0e-10
#define ABS(x) ((x) > 0 ? (x) : -(x))
#define SQR(x) ((x)*(x))

/*
** convert t to z values
*/
double t2z(double t,double df)
{
  double p=0,z=0;
  double a,b,x;
  extern double gsl_sf_beta_inc(double,double,double);

  /* t to p */
  x = df/(df+(t*t));
  if (x <= 0 || x > 1) return 0;

  a = 0.5*df;
  b = 0.5;
  p = 0.5 * gsl_sf_beta_inc(a,b,x);

  /* p to z */
  z = gsl_cdf_ugaussian_Qinv(p);
  return z;
}

void avevar(double *data,int n,double *a,double *v)
{
  int j;
  double ave,var,nx,s,u;

  nx = (double)n;
  ave = 0;
  for (j=0; j<n; j++) ave += data[j];
  ave /= nx;

  var = u = 0;
  for (j=0; j<n; j++) {
    s = data[j]-ave;
    u   += s;
    var += s*s;
  }
  var=(var-u*u/nx)/(nx-1);

  *v = var;
  *a = ave;
}

/* welch test, unequal variance */
double welchtest(double *data1,double *data2,int n1,int n2)
{
  double tiny=TINY;
  double ave1,ave2,var1,var2;
  double z=0,t=0,df=0;
  double nx1 = (double)n1;
  double nx2 = (double)n2;

  avevar(data1,n1,&ave1,&var1);
  if (var1 < tiny) return 0;
  avevar(data2,n2,&ave2,&var2);
  if (var2 < tiny) return 0;
  t = (ave1 - ave2)/sqrt(var1/nx1 + var2/nx2);
  df = SQR(var1/nx1+var2/nx2)/(SQR(var1/nx1)/(nx1-1)+SQR(var2/nx2)/(nx2-1));
  z  = t2z((double) t,(double) df);
  if (t < 0) z = -z;
  return z;
}

/* paired twosample test */
double paired_ttest(double *data1,double *data2,int n)
{
  int j;
  double ave,var,tiny=TINY;
  for (j=0; j<n; j++) {
    data1[j] -= data2[j];
  }
  avevar(data1,n,&ave,&var);
  if (var < tiny) return 0;
  double nx = (double)n;
  double df = nx-1.0;
  double t = sqrt(nx) * ave/sqrt(var);
  double z = t2z(t,df);
  if (t < 0) z = -z;
  return z;
}

/* paired twosample test */
double xtest2(double *data1,double *data2,int n)
{
  int j;
  double nx,ave1,ave2,var1,var2,sd,u,df,cov;
  double tiny=TINY;
  double t=0,z=0;

  avevar(data1,n,&ave1,&var1);
  avevar(data2,n,&ave2,&var2);
  if (var1 < tiny || var2 < tiny) return 0;

  nx = (double)n;
  df = nx-1;
  cov = 0;
  for (j=0; j<n; j++)
    cov += (data1[j]-ave1)*(data2[j]-ave2);
  cov /= df;

  t = 0;
  u = (var1+var2-2.0*cov);
  if (u < tiny) return 0;
  sd = sqrt(u/nx);
  if (sd < tiny) return 0;
  t = (double)(ave1-ave2)/sd;
  z = t2z(t,df);
  if (t < 0) z = -z;
  return z;
}

/* twosample t-test, pooled variance  */
double ttest2(double *data1,double *data2,int n1,int n2)
{
  int j;
  double nx1,nx2,ave1,ave2,sd,s1,s2,df;
  double tiny=TINY;
  double t=0,z=0;

  nx1 = (double)n1;
  nx2 = (double)n2;

  ave1 = ave2 = 0;
  for (j=0; j<n1; j++) ave1 += data1[j];
  for (j=0; j<n2; j++) ave2 += data2[j];
  if (ABS(ave1) < tiny) return 0;
  if (ABS(ave2) < tiny) return 0;
  ave1 /= nx1;
  ave2 /= nx2;

  s1 = 0;
  for (j=0; j<n1; j++) s1 += SQR(data1[j]-ave1);

  s2 = 0;
  for (j=0; j<n2; j++) s2 += SQR(data2[j]-ave2);

  df = nx1 + nx2 - 2.0;
  sd = sqrt((1.0/nx1 + 1.0/nx2) * (s1+s2)/df);
  if (sd < tiny) return 0;

  t = (double)(ave1-ave2)/sd;
  z = t2z(t,df);
  if (t < 0) z = -z;
  return z;
}

/* onesample t-test */
double ttest1(double *data,int n)
{
  double ave=0,var=0,tiny=TINY;
  avevar(data,n,&ave,&var);
  if (var < tiny) return 0;
  double nx = (double)n;
  double df = nx-1.0;
  double t  = sqrt(nx) * ave/sqrt(var);
  double z  = t2z(t,df);
  if (t < 0) z = -z;
  return z;
}


#ifndef BIO_MATH_H_
#define BIO_MATH_H_

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

#include <boost/io/ios_state.hpp>

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

#include <numeric>

BIO_NS_START

/** Calculates the log of n choose the observed counts.  */
template < typename ObsIt >
double
calc_ln_n_choose(
	ObsIt obs_begin,
	ObsIt obs_end)
{
	double result = 0.0;

	unsigned n = 0;
	while (obs_end != obs_begin)
	{
		result -= gsl_sf_lnfact(*obs_begin);
		n += *obs_begin;

		++obs_begin;
	}

	result += gsl_sf_lnfact(n);

	return result;
}

/** Calculates a factor involved in the likelihood of a multinomial distribution with a dirichlet prior. */
template < typename ObsIt, typename AlphaIt >
double
calc_ln_gamma_factor(
	ObsIt obs_begin,
	ObsIt obs_end,
	AlphaIt alpha_begin)
{
	double ln_pi_gamma_alpha_obs = 0.0; //work in logarithms
	double ln_pi_gamma_alpha = 0.0; //work in logarithms
	double sum_alpha = 0.0;
	double sum_obs = 0.0;
	while (obs_end != obs_begin)
	{
		sum_alpha += *alpha_begin;
		sum_obs += *obs_begin;
		ln_pi_gamma_alpha += gsl_sf_lngamma(*alpha_begin);
		ln_pi_gamma_alpha_obs += gsl_sf_lngamma(*alpha_begin + *obs_begin);

		++obs_begin;
		++alpha_begin;
	}

	double ln_result =
		gsl_sf_lngamma(sum_alpha)
		- ln_pi_gamma_alpha
		+ ln_pi_gamma_alpha_obs
		- gsl_sf_lngamma(sum_alpha + sum_obs);

	return ln_result;
}

/** Calculates the log of the likelihood of seeing the observed counts under a dirichlet prior. */
template < typename ObsIt, typename AlphaIt >
double
calc_multinomial_ln_likelihood_dirichlet_prior(
	ObsIt obs_begin,
	ObsIt obs_end,
	AlphaIt alpha_begin)
{
	return calc_ln_gamma_factor(obs_begin, obs_end, alpha_begin) + calc_ln_n_choose(obs_begin, obs_end);
}

/** Calculates the log of the likelihood of seeing the observed counts under a uniform multinomial distribution. */
template < typename ObsIt >
double
calc_multinomial_ln_likelihood_uniform_dist(
	ObsIt obs_begin,
	ObsIt obs_end)
{
	const unsigned n = std::accumulate(obs_begin, obs_end, unsigned(0));
	if (0 == n)
	{
		return 0.0;
	}

	const double ln_n_choose = calc_ln_n_choose(obs_begin, obs_end);
	return ln_n_choose - n * gsl_sf_log(double(obs_end - obs_begin));
}

/** Calculates the log of the likelihood of seeing the observed counts under a multinomial distribution with given p's. */
template < typename ObsIt, typename PIt >
double
calc_multinomial_ln_likelihood(
	ObsIt obs_begin,
	ObsIt obs_end,
	PIt p_begin)
{
	const unsigned n = std::accumulate(obs_begin, obs_end, unsigned(0));
	if (0 == n)
	{
		return 0.0;
	}

	double result = calc_ln_n_choose(obs_begin, obs_end);
	while (obs_end != obs_begin)
	{
		result += *obs_begin * gsl_sf_log(*p_begin);

		++obs_begin;
		++p_begin;
	}

	return result;
}

/** Calculates the ratio of evidence in favour of a uniform distribution over one with a dirichlet prior
defined by the alphas. */
template < typename ObsIt, typename AlphaIt >
double
calc_ln_multinomial_uniform_evidence(
	ObsIt obs_begin,
	ObsIt obs_end,
	AlphaIt alpha_begin)
{
	const double ln_likelihood_uniform_dist = calc_multinomial_ln_likelihood_uniform_dist(obs_begin, obs_end);
	const double ln_likelihood_dirichlet_prior = calc_multinomial_ln_likelihood_dirichlet_prior(obs_begin, obs_end, alpha_begin);
	return ln_likelihood_uniform_dist - ln_likelihood_dirichlet_prior;
}

/** Calculates the ratio of evidence in favour of a particular multinomial distribution (defined by the p's)
over one with a dirichlet prior defined by the alpha's. */
template < typename ObsIt, typename AlphaIt, typename PIt >
double
calc_ln_multinomial_evidence(
	ObsIt obs_begin,
	ObsIt obs_end,
	AlphaIt alpha_begin,
	PIt p_begin)
{
	return
		calc_multinomial_ln_likelihood(obs_begin, obs_end, p_begin)
		- calc_multinomial_ln_likelihood_dirichlet_prior(obs_begin, obs_end, alpha_begin);
}

BIO_NS_END

#endif //BIO_MATH_H_


/* randist/weibull.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>

/* The Weibull distribution has the form,

   p(x) dx = (b/a) (x/a)^(b-1) exp(-(x/a)^b) dx

 */

double
gsl_ran_weibull (const gsl_rng * r, const double a, const double b)
{
  double x = gsl_rng_uniform_pos (r);

  double z = pow (-log (x), 1 / b);

  return a * z;
}

double
gsl_ran_weibull_pdf (const double x, const double a, const double b)
{
  if (x < 0)
    {
      return 0 ;
    }
  else if (x == 0)
    {
      if (b == 1)
        return 1/a ;
      else
        return 0 ;
    }
  else if (b == 1)
    {
      return exp(-x/a)/a ;
    }
  else
    {
      double p = (b/a) * exp (-pow (x/a, b) + (b - 1) * log (x/a));
      return p;
    }
}


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

int main() {
  double a = 1.0, b = -3.0, c = 2.0;
  double x0 = 0.0, x1 = 0.0;
  gsl_poly_solve_quadratic(a, b, c, &x0, &x1);
  printf("%4.1fx^2 + %4.1fx + %4.1f = 0 -> x = %4.1f, %4.1f\n",
      a, b, c, x0, x1);
  return 0;
}


/* randist/multinomial.c
 *
 * Copyright (C) 2002 Gavin E. Crooks <gec@compbio.berkeley.edu>
 *
 * 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 <math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_sf_gamma.h>

/* The multinomial distribution has the form

                                      N!           n_1  n_2      n_K
   prob(n_1, n_2, ... n_K) = -------------------- p_1  p_2  ... p_K
                             (n_1! n_2! ... n_K!)

   where n_1, n_2, ... n_K are nonnegative integers, sum_{k=1,K} n_k = N,
   and p = (p_1, p_2, ..., p_K) is a probability distribution.

   Random variates are generated using the conditional binomial method.
   This scales well with N and does not require a setup step.

   Ref:
   C.S. David, The computer generation of multinomial random variates,
   Comp. Stat. Data Anal. 16 (1993) 205-217
*/

void
gsl_ran_multinomial (const gsl_rng * r, const size_t K,
                     const unsigned int N, const double p[], unsigned int n[])
{
  size_t k;
  double norm = 0.0;
  double sum_p = 0.0;

  unsigned int sum_n = 0;

  /* p[k] may contain non-negative weights that do not sum to 1.0.
   * Even a probability distribution will not exactly sum to 1.0
   * due to rounding errors.
   */

  for (k = 0; k < K; k++)
    {
      norm += p[k];
    }

  for (k = 0; k < K; k++)
    {
      if (p[k] > 0.0)
        {
          n[k] = gsl_ran_binomial (r, p[k] / (norm - sum_p), N - sum_n);
        }
      else
        {
          n[k] = 0;
        }

      sum_p += p[k];
      sum_n += n[k];
    }

}

double
gsl_ran_multinomial_pdf (const size_t K,
                         const double p[], const unsigned int n[])
{
  return exp (gsl_ran_multinomial_lnpdf (K, p, n));
}

double
gsl_ran_multinomial_lnpdf (const size_t K,
                           const double p[], const unsigned int n[])
{
  size_t k;
  unsigned int N = 0;
  double log_pdf = 0.0;
  double norm = 0.0;

  for (k = 0; k < K; k++)
    {
      N += n[k];
    }

  for (k = 0; k < K; k++)
    {
      norm += p[k];
    }

  log_pdf = gsl_sf_lnfact (N);

  for (k = 0; k < K; k++)
    {
      log_pdf -= gsl_sf_lnfact (n[k]);
    }

  for (k = 0; k < K; k++)
    {
      log_pdf += log (p[k] / norm) * n[k];
    }

  return log_pdf;
}


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

#include "cosmocalc.h"

gsl_spline *cosmocalc_aexpn2age_spline = NULL;
gsl_interp_accel *cosmocalc_aexpn2age_acc = NULL;
double AGE = 0;

/* function for integration using gsl integration */
static double age_integ_funct(double a, void *p)
{
  return 1.0/a/hubble_noscale(a);
}

/* init function  - some help from Gadget-2 applied here */
void init_cosmocalc_age_table(void)
{
#define WORKSPACE_NUM 100000
#define ABSERR 0.0
#define RELERR 1e-8

  static int initFlag = 1;
  static int currCosmoNum;

  gsl_integration_workspace *workspace;
  gsl_function F;
  long i;
  double result,abserr,afact;
  double age_table[COSMOCALC_AGE_TABLE_LENGTH];
  double aexpn_table[COSMOCALC_AGE_TABLE_LENGTH];

  if(initFlag == 1 || currCosmoNum != cosmoData.cosmoNum)
    {
      initFlag = 0;
      currCosmoNum = cosmoData.cosmoNum;

      workspace = gsl_integration_workspace_alloc((size_t) WORKSPACE_NUM);

      aexpn_table[0] = 0.0;
      age_table[0] = 0.0;
      for(i=1;i<COSMOCALC_AGE_TABLE_LENGTH;++i)
	{
	  afact = (1.0 - 0.0)/(COSMOCALC_AGE_TABLE_LENGTH-1.0)*((double) i);
	  F.function = &age_integ_funct;
	  gsl_integration_qag(&F,1e-12,afact,ABSERR,RELERR,(size_t) WORKSPACE_NUM,GSL_INTEG_GAUSS51,workspace,&result,&abserr);
	  aexpn_table[i] = afact;
	  age_table[i] = result*TH/cosmoData.h;
	}

      gsl_integration_workspace_free(workspace);
#undef ABSERR
#undef RELERR
#undef WORKSPACE_NUM

      //init the spline and accelerators
      if(cosmocalc_aexpn2age_spline != NULL)
	gsl_spline_free(cosmocalc_aexpn2age_spline);
      cosmocalc_aexpn2age_spline = gsl_spline_alloc(GSL_SPLINE_TYPE,(size_t) (COSMOCALC_AGE_TABLE_LENGTH));
      gsl_spline_init(cosmocalc_aexpn2age_spline,aexpn_table,age_table,(size_t) (COSMOCALC_AGE_TABLE_LENGTH));
      if(cosmocalc_aexpn2age_acc != NULL)
	gsl_interp_accel_reset(cosmocalc_aexpn2age_acc);
      else
	cosmocalc_aexpn2age_acc = gsl_interp_accel_alloc();

      AGE = gsl_spline_eval(cosmocalc_aexpn2age_spline,1.0,cosmocalc_aexpn2age_acc);
    }
}

double age(double a)
{
  static int initFlag = 1;
  static int currCosmoNum;

  if(initFlag == 1 || currCosmoNum != cosmoData.cosmoNum)
    {
      initFlag = 0;
      currCosmoNum = cosmoData.cosmoNum;
      init_cosmocalc_age_table();
    }

  return gsl_spline_eval(cosmocalc_aexpn2age_spline,a,cosmocalc_aexpn2age_acc);
}

//call age first to set AGE
double lookback(double a)
{
  double age_a = age(a);
  return AGE - age_a;
}


#pragma once

#include <gsl/gsl_sf_zeta.h>
#include <cmath>

double real_trigamma_cmplxarg(double x, double y, int m, int l){
    //compute the real part of the trigamma function at z = x + iy using
    //my accelerated series formula
    //argument should have positive real part (x > 0)
    //m gives the number of zeta function terms to use
    //l gives the number of terms to use in the residual series
    double result = 0;

    //if x is small, the result will be large and thus, inaccurate.  Use the
    //polygamma shift formula to give a larger value of x for the computation
    if(x < 1000){
        return ((x*x - y*y)/(x*x + y*y)/(x*x + y*y)) + real_trigamma_cmplxarg(x + 1, y, m, l);
    }
    else{
        double phase, ypow = 1.0, xpow, denom;
        //compute finite sum of Hurwitz zeta functions
        for (int i = 1; i < m; ++i){
            phase = 2*(i/2) == i ? -1.0 : 1.0;
            result += phase*(2*i - 1)*ypow*gsl_sf_hzeta(2*i, x);
            ypow = ypow*y*y;
        }
        //compute the infinite sum of residuals
        phase = 2*(m/2) == m ? 1.0 : -1.0;
        for (int n = 0; n < l; ++n){
            xpow = pow(x + n, 2*m);
            denom = ((x + n)*(x + n) + y*y)*((x + n)*(x + n) + y*y);
            result += phase*ypow*((2*m - 1)*y*y + (2*m + 1)*(x + n)*(x + n)) / xpow / denom;
        }
        return result;
    }
}


/*
 * bspline.c: Routines for calculating values of B-splines and their
 *  derivatives, as well as efficient computation of the values of
 *  N-dimensional B-spline surfaces.
 */

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

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

#include "photospline/bspline.h"

/*
 * Compute the value of the ith nth-order basis spline of a set
 * defined by knots at the point x.
 *
 * This is implemented using the De Boor algorithm, as outlined on
 * Wikipedia.
 */

double
bspline(const double *knots, double x, int i, int n)
{
	double result;

	if (n == 0) {
		/*
		 * Special case the 0th order case, where B-Splines
		 * are constant functions from one knot to the next.
		 */

		if (x >= knots[i] && x < knots[i+1])
			return 1.0;
		else
			return 0.0;
	}

	result = (x - knots[i])*bspline(knots, x, i, n-1) /
	    (knots[i+n] - knots[i]);
	result += (knots[i+n+1] - x)*bspline(knots, x, i+1, n-1) /
	    (knots[i+n+1] - knots[i+1]);

	return result;
}

/*
 * A brain-dead reimplementation of de Boor's BSPLVB, which generates
 * the values of the non-zero B-splines at x from the bottom up without
 * unnecessarily recalculating terms.
 *
 * NB: for bsplvb_simple(), bspline_nonzero(), and bspline_deriv_nonzero(),
 * `left' must be the index of the nearest fully-supported knot
 * span for splines of order n, i.e. n <= left <= nknots-n-2. For bsplvb(),
 * `left' must be the index of the nonzero 0-th order spline, i.e.
 * knots[left] <= x < knots[left+1].
 *
 * See Chapter X in:
 *
 * Carl de Boor. A Practical Guide to Splines, volume 27 of Applied
 *     Mathematical Sciences. Springer-Verlag, 1978.
 */

void
bsplvb_simple(const double *knots, const unsigned nknots,
    double x, int left, int degree, float *restrict biatx)
{
	assert(degree>0);
	int i, j;
	double saved, term;
	double delta_l[degree], delta_r[degree];

	biatx[0] = 1.0;

	/*
	 * Handle the (rare) cases where x is outside the full
	 * support of the spline surface.
	 */
	if (left == degree-1)
		while (left >= 0 && x < knots[left])
			left--;
	else if (left == nknots-degree-1)
		while (left < nknots-1 && x > knots[left+1])
			left++;

	/*
	 * NB: if left < degree-1 or left > nknots-degree-1,
	 * the following loop will dereference addresses ouside
	 * of knots[0:nknots]. While terms involving invalid knot
	 * indices will be discarded, it is important that `knots'
	 * have (maxdegree-1)*sizeof(double) bytes of padding
	 * before and after its valid range to prevent segfaults
	 * (see parsefitstable()).
	 */
	for (j = 0; j < degree-1; j++) {
		delta_r[j] = knots[left+j+1] - x;
		delta_l[j] = x - knots[left-j];

		saved = 0.0;

		for (i = 0; i < j+1; i++) {
			term = biatx[i] / (delta_r[i] + delta_l[j-i]);
			biatx[i] = saved + delta_r[i]*term;
			saved = delta_l[j-i]*term;
		}

		biatx[j+1] = saved;
	}

	/*
	 * If left < (spline order), only the first (left+1)
	 * splines are valid; the remainder are utter nonsense.
	 */
	if ((i = degree-1-left) > 0) {
		for (j = 0; j < left+1; j++)
			biatx[j] = biatx[j+i]; /* Move valid splines over. */
		for ( ; j < degree; j++)
			biatx[j] = 0.0; /* The rest are zero by construction. */
	} else if ((i = left+degree+1-nknots) > 0) {
		for (j = degree-1; j > i-1; j--)
			biatx[j] = biatx[j-i];
		for ( ; j >= 0; j--)
			biatx[j] = 0.0;
	}
}

void
bsplvb(const double *knots, const double x, const int left, const int jlow,
    const int jhigh, float *restrict biatx,
    double *restrict delta_l, double *restrict delta_r)
{
	int i, j;
	double saved, term;

	if (jlow == 0)
		biatx[0] = 1.0;

	for (j = jlow; j < jhigh-1; j++) {
		delta_r[j] = knots[left+j+1] - x;
		delta_l[j] = x - knots[left-j];

		saved = 0.0;

		for (i = 0; i < j+1; i++) {
			term = biatx[i] / (delta_r[i] + delta_l[j-i]);
			biatx[i] = saved + delta_r[i]*term;
			saved = delta_l[j-i]*term;
		}

		biatx[j+1] = saved;
	}
}

void
bspline_deriv_nonzero(const double *knots, const unsigned nknots,
    const double x, int left, const int n, float *restrict biatx)
{
	/* NB: it might be tempting to use unsigned integers *left* and *n* here,
	   but indices into the knot vector may be negative (up to -order) before
	   the first fully-supported knot. */
	assert(n>0);
	int i, j;
	double temp, a;
	double delta_l[n], delta_r[n];

	/* Special case for constant splines */
	if (n == 0)
		return;

	/*
	 * Handle the (rare) cases where x is outside the full
	 * support of the spline surface.
	 */
	if (left == n)
		while (left >= 0 && x < knots[left])
			left--;
	else if (left == nknots-n-2)
		while (left < nknots-1 && x > knots[left+1])
			left++;

	/* Get the non-zero n-1th order B-splines at x */
	bsplvb(knots, x, left, 0 /* jlow */, n /* jhigh */,
	    biatx, delta_l, delta_r);

	/*
	 * Now, form the derivatives of the nth order B-splines from
	 * linear combinations of the lower-order splines.
	 */

	/*
	 * On the last supported segment of the ith nth order spline,
	 * only the i+1th n-1th order spline is nonzero.
	 */
	temp = biatx[0];
	biatx[0] =  - n*temp / ((knots[left+1] - knots[left+1-n]));

	/* On the middle segments, both the ith and i+1th splines contribute. */
	for (i = 1; i < n; i++) {
		a = n*temp/((knots[left+i] - knots[left+i-n]));
		temp = biatx[i];
		biatx[i] = a - n*temp/(knots[left+i+1] - knots[left+i+1-n]);
	}
	/*
	 * On the first supported segment of the i+nth nth order spline,
	 * only the ith n-1th order spline is nonzero.
	 */
	biatx[n] = n*temp/((knots[left+n] - knots[left]));

	/* Rearrange for partially-supported points. */
	if ((i = n-left) > 0) {
		for (j = 0; j < left+1; j++)
			biatx[j] = biatx[j+i]; /* Move valid splines over. */
		for ( ; j < n+1; j++)
			biatx[j] = 0.0; /* The rest are zero by construction. */
	} else if ((i = left+n+2-nknots) > 0) {
		for (j = n; j > i-1; j--)
			biatx[j] = biatx[j-i];
		for ( ; j >= 0; j--)
			biatx[j] = 0.0;
	}

}

double
bspline_deriv(const double *knots, double x, int i, int n, unsigned order)
{
	double result;

	if (n == 0) {
		/*
		 * Special case the 0th order case, where B-Splines
		 * are constant functions from one knot to the next.
		 */

		return 0.0;
	}

	if (order <= 1) {
		result = n * bspline(knots, x, i, n-1) / (knots[i+n] - knots[i]);
		result -= n * bspline(knots, x, i+1, n-1) / (knots[i+n+1] - knots[i+1]);
	} else {
		result = n * bspline_deriv(knots, x, i, n-1, order-1) / (knots[i+n] - knots[i]);
		result -= n * bspline_deriv(knots, x, i+1, n-1, order-1) / (knots[i+n+1] - knots[i+1]);
	}
	return result;
}

double
bspline_deriv_2(const double *knots, double x, int i, int n)
{
	double result;

	if (n <= 1) {
		/*
		 * Special case the 1st order case, where B-Splines
		 * are linear functions from one knot to the next.
		 */

		return 0.0;
	}

	result = bspline(knots, x, i, n-2) /
	    ((knots[i+n] - knots[i])*(knots[i+n-1] - knots[i]));
	result -= bspline(knots, x, i+1, n-2) *
	    (1./(knots[i+n] - knots[i]) + 1./(knots[i+n+1] - knots[i+1])) /
	    (knots[i+n] - knots[i+1]);
	result += bspline(knots, x, i+2, n-2) /
	    ((knots[i+n+1] - knots[i+1])*(knots[i+n+1] - knots[i+2]));

	result *= n*(n-1);

	return result;
}

/*
 * Evaluates the results of a full spline basis given a set of knots,
 * a position, an order, and a central spline for the position (or -1).
 * The central spline should be the index of the 0th order basis spline
 * that is non-zero at the position x.
 */

double
splineeval(const double *knots, const double *weights, int nknots, double x, int order,
    int center)
{
	double work = 0.0;
	int i;

	if (center < 0) {
		/* XXX: should be a binary search */
		for (center = 0; center+1 < nknots; center++) {
			if (x > knots[center] && x < knots[center+1])
				break;
		}

		if (center+1 >= nknots)
			return 0.0;
	}

	i = center - order;
	if (i < 0)
		i = 0;

	while (i < nknots-order-1 && i <= center) {
		work += weights[i]*bspline(knots, x, i, order);
		i++;
	}

	return work;
}

int
tablesearchcenters(const struct splinetable *table, const double *x, int *centers)
{
	int i, min, max;

	for (i = 0; i < table->ndim; i++) {

		/* Ensure we are actually inside the table. */
		if (x[i] <= table->knots[i][0] ||
		    x[i] > table->knots[i][table->nknots[i]-1])
			return (-1);

		/*
		 * If we're only a few knots in, take the center to be
		 * the nearest fully-supported knot.
		 */
		if (x[i] < table->knots[i][table->order[i]]) {
			centers[i] = table->order[i];
			continue;
		} else if (x[i] >= table->knots[i][table->naxes[i]]) {
			centers[i] = table->naxes[i]-1;
			continue;
		}

		min = table->order[i];
		max = table->nknots[i]-2;
		do {
			centers[i] = (max+min)/2;

			if (x[i] < table->knots[i][centers[i]])
				max = centers[i]-1;
			else
				min = centers[i]+1;
		} while (x[i] < table->knots[i][centers[i]] ||
		    x[i] >= table->knots[i][centers[i]+1]);

		/*
		 * B-splines are defined on a half-open interval. For the
		 * last point of the interval, move center one point to the
		 * left to get the limit of the sum without evaluating
		 * absent basis functions.
		 */
		if (centers[i] == table->naxes[i])
			centers[i]--;
	}

	return (0);
}

static int
maxorder(int *order, int ndim)
{
	int i, max = 0;

	for (i = 0; i < ndim; i++)
		if (order[i] > max)
			max = order[i];

	return (max);
}

/*
 * The N-Dimensional tensor product basis version of splineeval.
 * Evaluates the results of a full spline basis given a set of knots,
 * a position, an order, and a central spline for the position (or -1).
 * The central spline should be the index of the 0th order basis spline
 * that is non-zero at the position x.
 *
 * x is the vector at which we will evaluate the space
 */

static double
ndsplineeval_core(const struct splinetable *table, const int *centers, int maxdegree,
    float localbasis[table->ndim][maxdegree])
{
	assert(table->ndim>0);
	int i, j, n, tablepos;
	float result;
	float basis_tree[table->ndim+1];
	int nchunks;
	int decomposedposition[table->ndim];

	tablepos = 0;
	for (n = 0; n < table->ndim; n++) {
		decomposedposition[n] = 0;
		tablepos += (centers[n] - table->order[n])*table->strides[n];
	}

	basis_tree[0] = 1;
	for (n = 0; n < table->ndim; n++)
		basis_tree[n+1] = basis_tree[n]*localbasis[n][0];
	nchunks = 1;
	for (n = 0; n < table->ndim - 1; n++)
		nchunks *= (table->order[n] + 1);

	result = 0;
	n = 0;
	while (1) {
		for (i = 0; __builtin_expect(i < table->order[table->ndim-1] +
		    1, 1); i++) {

			result += basis_tree[table->ndim-1]*
			    localbasis[table->ndim-1][i]*
			    table->coefficients[tablepos + i];
		}

		if (__builtin_expect(++n == nchunks, 0))
			break;

		tablepos += table->strides[table->ndim-2];
		decomposedposition[table->ndim-2]++;

		/* Carry to higher dimensions */
		for (i = table->ndim-2;
		    decomposedposition[i] > table->order[i]; i--) {
			decomposedposition[i-1]++;
			tablepos += (table->strides[i-1]
			    - decomposedposition[i]*table->strides[i]);
			decomposedposition[i] = 0;
		}
		for (j = i; __builtin_expect(j < table->ndim-1, 1); j++)
			basis_tree[j+1] = basis_tree[j]*
			    localbasis[j][decomposedposition[j]];
	}

	return result;
}

/* This function returns bspline coefficients along a given dimension, fixing the values+coefficients for the
other dimensions. Used to obtain a 1-d spline representation that can be easily further convolved. */

void
ndsplineeval_slice_coeffs(const struct splinetable *table, const double *x, const int *centers, double *results,int slice_dimension, int derivative, int area_norm )
{
	assert(table->ndim>0);
	int n;
	int maxdegree = maxorder(table->order, table->ndim) + 1;
	float localbasis[table->ndim][maxdegree];

	if(slice_dimension==table->ndim-1)
	{
		printf("ERROR!!! slice dimension cannot be ndim-1 in this implementation!");
	}

	for (n = 0; n < table->ndim; n++) {

			bsplvb_simple(table->knots[n], table->nknots[n],
			    x[n], centers[n], table->order[n] + 1,
			    localbasis[n]);
	}

	int i, j, tablepos, slice_dimension_stride, num_coeffs;
	//double result;
	num_coeffs=table->nknots[slice_dimension]-table->order[slice_dimension]-1;

	memset(results, 0, sizeof(double)*num_coeffs);
	// temp_result is stored to teomporarily store coefficients for derivative calculation
	double temp_result[num_coeffs];

	float basis_tree[table->ndim+1]; // the last basis_tree dimension is unused ..
	int nchunks;
	int decomposedposition[table->ndim];

	//int derivative_correction[table->ndim];

	nchunks = 1;
	for (n = 0; n < table->ndim - 1; n++)
	{
		//derivative_correction[n]=0;

		if(n==slice_dimension)
		{
			continue;
		}
		nchunks *= (table->order[n] + 1);

	}

	// initialize overall table position
	tablepos = 0;
	for (n = 0; n < table->ndim; n++) {
		decomposedposition[n]=0;
		if(n!=slice_dimension)
		{

			tablepos += (centers[n] - table->order[n])*table->strides[n];
		}
		else
		{
			slice_dimension_stride=table->strides[n];
		}
	}

	// initialize local basis
	basis_tree[0] = 1;
	for (n = 0; n < table->ndim; n++)
	{
		if(n==slice_dimension)
		{
			basis_tree[n+1] = basis_tree[n];
			continue;

		}
		basis_tree[n+1] = basis_tree[n]*localbasis[n][0];
	}

	double temp_base_eval=0.0;
	n=0;
	while (1) {

		for (i = 0; __builtin_expect(i < table->order[table->ndim-1] +
		    1, 1); i++) {

			// in contrast to ndsplineeval_core, save a value for each coefficient, separated by the slice dimension stride
			temp_base_eval=basis_tree[table->ndim-1]*localbasis[table->ndim-1][i];
			for(int nc=0; __builtin_expect(nc<num_coeffs,1) ;nc++)
			{
				results[nc] += temp_base_eval*table->coefficients[tablepos + i + nc*slice_dimension_stride];
			}

		}

		if (__builtin_expect(++n == nchunks, 0))
			break;

		// special case when the slicing dimension is ndim-2 .. skip over this dimension immediately by pushing tablepos forward by (order+1)*strides
		if(slice_dimension==table->ndim-2)
		{
			tablepos += table->strides[table->ndim-2]*(table->order[table->ndim-2]+1);
			decomposedposition[table->ndim-2]+=(table->order[table->ndim-2]+1);
		}
		else
		{
			// otherwise just add one stride
			tablepos += table->strides[table->ndim-2];
			decomposedposition[table->ndim-2]++;
		}

		// Carry to higher dimensions
		for (i = table->ndim-2;
		    decomposedposition[i] > table->order[i]; i--) {

			if(i==slice_dimension+1)
			{
				//printf("i=slicedimsino +1 .. \n");
				decomposedposition[i-2]++;
				tablepos += (table->strides[i-2]
			    - decomposedposition[i]*table->strides[i]);
				decomposedposition[i] = 0;
				// add one extra -1, since we want to skip the slicing dimension
				i=i-1;
			}
			else
			{
				decomposedposition[i-1]++;
				tablepos += (table->strides[i-1]
			    - decomposedposition[i]*table->strides[i]);
				decomposedposition[i] = 0;
			}

		}

		// stacks the tree basis up .. never include the dimension of interest, ie.e the slice dimension
		for (j = i; __builtin_expect(j < table->ndim-1, 1); j++)
		 {
			//printf("last loop index .. %d\n", j);
			if(j==slice_dimension)
			{
				basis_tree[j+1] = basis_tree[j];
			}
			else
			{
				basis_tree[j+1] = basis_tree[j]*
			    localbasis[j][decomposedposition[j]];
			}
		}

	}

	for(int nc=0; __builtin_expect(nc<num_coeffs,1) ;nc++)
	{

		if(derivative>0)
		{
			double y_diff;
			double x_diff;
			int deriv_order=table->order[slice_dimension];

			if(nc==0)
			{
				// first one is special
				y_diff=deriv_order*results[0];
			}
			else
			{
				// first form derivatives
				// requires two coefficient results usually ... so only start once we know >=2 coefficients
				// since table->order is really the degree, the new derivative order is just the degree of the original spline

				y_diff=deriv_order*(results[nc]-results[nc-1]);

			}
			x_diff=table->knots[slice_dimension][deriv_order+nc]-table->knots[slice_dimension][nc];

			temp_result[nc]=y_diff/((double)x_diff);

			if(area_norm)
			{
				double norm_factor= ((double)deriv_order)/(   table->knots[slice_dimension][deriv_order+nc] - table->knots[slice_dimension][nc]);
				temp_result[nc]/=norm_factor;
			}

			// check again for area normalization
		}
		else
		{
			if(area_norm)
			{
				int real_order=table->order[slice_dimension]+1;
				double norm_factor= ((double)(real_order))/ (   table->knots[slice_dimension][real_order+nc] - table->knots[slice_dimension][nc]);
				results[nc]/=norm_factor;
			}

			//printf("SLICE: abs res: %d %.10f\n", nc, results[nc]);
		}

	}

	// calculated the derivate .. copy temp_result into result
	if(derivative>0)
	{
		memcpy(results, temp_result, sizeof(temp_result));
	}

}

double
ndsplineeval(const struct splinetable *table, const double *x, const int *centers,
    int derivatives)
{
	assert(table->ndim>0);
	int n;
	int maxdegree = maxorder(table->order, table->ndim) + 1;
	float localbasis[table->ndim][maxdegree];

	for (n = 0; n < table->ndim; n++) {
		if (derivatives & (1 << n)) {
			bspline_deriv_nonzero(table->knots[n],
			    table->nknots[n], x[n], centers[n],
			    table->order[n], localbasis[n]);
		} else {
			bsplvb_simple(table->knots[n], table->nknots[n],
			    x[n], centers[n], table->order[n] + 1,
			    localbasis[n]);
		}
	}

	return ndsplineeval_core(table, centers, maxdegree, localbasis);
}

double
ndsplineeval_deriv(const struct splinetable *table, const double *x,
    const int *centers, const unsigned *derivatives)
{

	assert(table->ndim>0);
	int i, n;
	int maxdegree = maxorder(table->order, table->ndim) + 1;
	float localbasis[table->ndim][maxdegree];

	for (n = 0; n < table->ndim; n++) {
		if (derivatives == NULL || derivatives[n] == 0) {
			bsplvb_simple(table->knots[n], table->nknots[n],
			    x[n], centers[n], table->order[n] + 1,
			    localbasis[n]);
		} else if (derivatives[n] == 1) {
			bspline_deriv_nonzero(table->knots[n],
			    table->nknots[n], x[n], centers[n],
			    table->order[n], localbasis[n]);
		} else {
			for (i = 0; i <= table->order[n]; i++)
				localbasis[n][i] = bspline_deriv(
				    table->knots[n], x[n],
				    centers[n] - table->order[n] + i,
				    table->order[n], derivatives[n]);
		}
	}

	return ndsplineeval_core(table, centers, maxdegree, localbasis);
}

double
ndsplineeval_linalg(const struct splinetable *table, const double *x,
    const int *centers, int derivatives)
{
	assert(table->ndim>0);
	int totalcoeff, n;
	int coeffstrides[table->ndim];
	gsl_matrix_float *basis1, *basis2, *basis_elem;

	assert(table->ndim > 0);
	coeffstrides[table->ndim - 1] = totalcoeff = 1;
        for (n = table->ndim-1; n >= 0; n--) {
                totalcoeff *= (table->order[n] + 1);
                if (n > 0)
                        coeffstrides[n-1] = totalcoeff;
        }

	float basis1_data[totalcoeff], basis2_data[totalcoeff],
	    elem_data[maxorder(table->order, table->ndim) + 1];
	gsl_matrix_float b1, b2, be;
	basis1 = &b1; basis2 = &b2; basis_elem = &be;
	basis1->data = basis1_data;
	basis2->data = basis2_data;
	basis_elem->data = elem_data;

	/*
	 * Form outer product basis1 = basis2 x basis_elem, filling basis_elem
	 * every time with the non-zero basis functions on each axis and
	 * swapping basis1 and basis2 via tmp_basis.
	 */
	basis2->size1 = 1;
	basis2->size2 = 1;
	basis2->data[0] = 1.0;

	for (n = table->ndim-1; n >= 0; n--) {
		gsl_matrix_float *tmp_basis;
		if (derivatives & (1 << n)) {
			bspline_deriv_nonzero(table->knots[n],
			    table->nknots[n], x[n], centers[n],
			    table->order[n], basis_elem->data);
		} else {
			bsplvb_simple(table->knots[n], table->nknots[n],
			    x[n], centers[n], table->order[n] + 1,
			    basis_elem->data);
		}

		basis_elem->size1 = table->order[n] + 1;
		basis_elem->size2 = 1;

		basis1->size2 = basis2->size2;
		basis1->size1 = basis_elem->size1;
		cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans,
		    basis1->size1, basis1->size2, basis_elem->size2, 1.0,
		    basis_elem->data, 1, basis2->data, basis2->size2, 0,
		    basis1->data, basis1->size2);
		basis1->size2 = basis1->size1 * basis1->size2;
		basis1->size1 = 1;

		tmp_basis = basis1;
		basis1 = basis2;
		basis2 = tmp_basis;
	}

	/* Now basis1 is free, so fill it with the spline coefficients */
	int i, tablepos;
	int decomposedposition[table->ndim];
	tablepos = 0;
	for (n = 0; n < table->ndim; n++) {
		decomposedposition[n] = 0;
		tablepos += (centers[n] - table->order[n])*table->strides[n];
	}

	for (i = 0; i < table->order[table->ndim-1] + 1; i++)
		basis1->data[i] = table->coefficients[tablepos + i];
	for (n = 1; n < coeffstrides[0] /* number of chunks */; n++) {
		tablepos += table->strides[table->ndim-2];
		decomposedposition[table->ndim-2]++;
		/* Carry to higher dimensions */
		for (i = table->ndim-2; __builtin_expect(i > 0 &&
		    decomposedposition[i] > table->order[i], 0); i--) {
			decomposedposition[i-1]++;
			tablepos += (table->strides[i-1]
			    - decomposedposition[i]*table->strides[i]);
			decomposedposition[i] = 0;
		}

		for (i = 0; i < table->order[table->ndim-1] + 1; i++)
			basis1->data[n*(table->order[table->ndim-1] + 1) + i] =
			    table->coefficients[tablepos + i];
	}

	/* Take the dot product */
	__builtin_prefetch(basis1->data);
	__builtin_prefetch(basis2->data);
	return cblas_sdot(totalcoeff, basis1->data, 1, basis2->data, 1);
}


#include <stdio.h>
#include <gsl/gsl_rng.h>
#include <math.h>

// #include <stdlib.h>
// #include <time.h>

double circle_overlap(double *rA, double radiiA, double *rB, double radiiB)
{

    double rAB[2];
    rAB[0] = rB[0] - rA[0];
    rAB[1] = rB[1] - rA[1];

    double F;
    F = (pow(rAB[0], 2) + pow(rAB[1], 2)) / pow(radiiA + radiiB, 2);

    return F;

}

size_t gen_pts_rsa_2d(double *x, double *y,
    size_t npoints, double radius, int step_limit,
    unsigned long randSeed)
{

    // Setup GSL random number generator
    const gsl_rng_type * T;
    gsl_rng * r;
    T = gsl_rng_default;
    r = gsl_rng_alloc (T);

    // Set the seed
    // srand ( time(NULL) );
    // unsigned long randSeed = rand();
    gsl_rng_set(r, randSeed);

    // Set the initial position
    double xn = gsl_rng_uniform (r) * (1 - 2 * radius) + radius;
    double yn = gsl_rng_uniform (r) * (1 - 2 * radius) + radius;
    x[0] = xn;
    y[0] = yn;

    double diameter = 2 * radius;

    size_t valid_pts;
    double F;
    int k, flag, step;

    step = 0;
    valid_pts = 1;

    double rA[2];
    double rB[2];

    while ((valid_pts < npoints) & (step < step_limit))
    {

        xn = gsl_rng_uniform (r) * (1 - 2 * radius) + radius;
        yn = gsl_rng_uniform (r) * (1 - 2 * radius) + radius;

        flag = 1;
        for (k = 0; k < valid_pts; k++)
        {

            rA[0] = x[k];
            rA[1] = y[k];
            rB[0] = xn;
            rB[1] = yn;

            F = circle_overlap(&rA[0], radius, &rB[0], radius);

            if (F < 1.0)
            {

                flag = 0;
                break;

            }
        }
        if (flag == 1)
        {

           x[valid_pts] = xn;
           y[valid_pts] = yn;
           valid_pts += 1;

        }

        step += 1;

    }

    gsl_rng_free (r);

    return valid_pts;

}

unsigned int metro_md_2d(double *x, double *y,
    double radius, size_t npoints, unsigned int step_limit,
    unsigned long randSeed)
{

    /* Setup GSL random number generator */
    const gsl_rng_type * T;
    gsl_rng * r;
    T = gsl_rng_default;
    r = gsl_rng_alloc (T);

    /* Set the seed */
    // srand ( time(NULL) );
    // unsigned long randSeed = rand();
    gsl_rng_set(r, randSeed);

    double diameter = 2 * radius;

    double F, dx, dy, xn, yn;
    unsigned int step, i, k, flag, success_steps;

    double rA[2];
    double rB[2];

    step = 0;
    success_steps = 0;
    while (step < step_limit)
    {

        i = step % npoints;

        /* Generate new position */
        while (1)
        {

            dx = diameter * (gsl_rng_uniform (r) - 0.5);
            xn = x[i] + dx;

            dy = diameter * (gsl_rng_uniform (r) - 0.5);
            yn = y[i] + dy;

            if (((xn > radius) & (xn < 1 - radius)) & ((yn > radius) & (yn < 1 - radius)) )
            {
               break;
            }

        }

        /* Determine if new position overlaps with other positions */
        flag = 1;
        for (k = 0; k < npoints; k++)
        {

            rA[0] = x[k];
            rA[1] = y[k];
            rB[0] = xn;
            rB[1] = yn;

            F = circle_overlap(&rA[0], radius, &rB[0], radius);

            if ((F < 1.0) & (i != k))
            {
                flag = 0;
                break;
            }

        }

        if (flag == 1)
        {

            x[i] = xn;
            y[i] = yn;
            success_steps = success_steps + 1;

        }

        step = step + 1;

    }

    gsl_rng_free (r);

    return success_steps;

}

unsigned int metro_pd_2d(double *x, double *y,
    double *radius, size_t npoints, int step_limit,
    unsigned long randSeed)
{

    /* Setup GSL random number generator */
    const gsl_rng_type * T;
    gsl_rng * r;
    T = gsl_rng_default;
    r = gsl_rng_alloc (T);

    /* Set the seed */
    // srand ( time(NULL) );
    // unsigned long randSeed = rand();
    gsl_rng_set(r, randSeed);

    double F, dx, dy, xn, yn, diameter;
    unsigned int step, i, k, flag, success_steps;

    double rA[2];
    double rB[2];

    step = 0;
    success_steps = 0;
    while (step < step_limit)
    {

        i = step % npoints;

        /* Generate new position */
        while (1)
        {

            diameter = 2 * radius[i];

            dx = diameter * (gsl_rng_uniform (r) - 0.5);
            xn = x[i] + dx;

            dy = diameter * (gsl_rng_uniform (r) - 0.5);
            yn = y[i] + dy;

            if (((xn > radius[i]) & (xn < 1 - radius[i])) & ((yn > radius[i]) & (yn < 1 - radius[i])) )
            {
               break;
            }

        }

        /* Determine if new position overlaps with other positions */
        flag = 1;
        for (k = 0; k < npoints; k++)
        {

            rA[0] = x[k];
            rA[1] = y[k];
            rB[0] = xn;
            rB[1] = yn;

            F = circle_overlap(&rA[0], radius[i], &rB[0], radius[k]);

            if ((F < 1.0) & (i != k))
            {
                flag = 0;
                break;
            }

        }

        if (flag == 1)
        {

            x[i] = xn;
            y[i] = yn;
            success_steps += 1;

        }

        step = step + 1;

    }

    gsl_rng_free (r);

    return success_steps;

}


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <gbpLib.h>
#include <gbpMath.h>
#include <gbpHalos.h>
#include <gsl/gsl_sf_zeta.h>

#define READ_MATCHES_GOODNESS_A 1.4421650782960167E+01
#define READ_MATCHES_GOODNESS_B 2.7010890377990435E+01
#define READ_MATCHES_GOODNESS_C ONE_THIRD
#define READ_MATCHES_GOODNESS_D -2.4490239433470165E+00

#define MAXIMUM_SCORE_COEFF 2.61      // Only if MATCH_SCORE_RANK_INDEX=-1.5.  This comes from Wolfram Alpha.  Query "sum k^(-1.5) from k = 1 to N"
#define EULER_MASCHERONI_CONST 0.5772 // Used if MATCH_SCORE_RANK_INDEX=-1

float maximum_match_score(double n_particles) {
    if(n_particles < 1)
        return (0);
    if(MATCH_SCORE_RANK_INDEX == (-1.5))
        return (MAXIMUM_SCORE_COEFF - gsl_sf_hzeta(-MATCH_SCORE_RANK_INDEX, n_particles));
    else if(MATCH_SCORE_RANK_INDEX == (-1.))
        return (EULER_MASCHERONI_CONST + take_ln(n_particles));
    else if(MATCH_SCORE_RANK_INDEX == (-TWO_THIRDS))
        return ((float)pow(READ_MATCHES_GOODNESS_A + READ_MATCHES_GOODNESS_B * n_particles, READ_MATCHES_GOODNESS_C) + READ_MATCHES_GOODNESS_D);
    else
        SID_exit_error(
                "Invalid MATCH_SCORE_RANK_INDEX passed to maximum_match_score().  Please update the code to accomodate MATCH_SCORE_RANK_INDEX=%lf.",
                SID_ERROR_LOGIC, MATCH_SCORE_RANK_INDEX);
}


#include <stdio.h>
#include <stdlib.h>

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

int
main(void)
{
  const size_t N = 500;                        /* length of time series */
  const size_t K = 51;                         /* window size */
  const double alpha[3] = { 0.5, 3.0, 10.0 };  /* alpha values */
  gsl_vector *x = gsl_vector_alloc(N);         /* input vector */
  gsl_vector *y1 = gsl_vector_alloc(N);        /* filtered output vector for alpha1 */
  gsl_vector *y2 = gsl_vector_alloc(N);        /* filtered output vector for alpha2 */
  gsl_vector *y3 = gsl_vector_alloc(N);        /* filtered output vector for alpha3 */
  gsl_vector *k1 = gsl_vector_alloc(K);        /* Gaussian kernel for alpha1 */
  gsl_vector *k2 = gsl_vector_alloc(K);        /* Gaussian kernel for alpha2 */
  gsl_vector *k3 = gsl_vector_alloc(K);        /* Gaussian kernel for alpha3 */
  gsl_rng *r = gsl_rng_alloc(gsl_rng_default);
  gsl_filter_gaussian_workspace *gauss_p = gsl_filter_gaussian_alloc(K);
  size_t i;
  double sum = 0.0;

  /* generate input signal */
  for (i = 0; i < N; ++i)
    {
      double ui = gsl_ran_gaussian(r, 1.0);
      sum += ui;
      gsl_vector_set(x, i, sum);
    }

  /* compute kernels without normalization */
  gsl_filter_gaussian_kernel(alpha[0], 0, 0, k1);
  gsl_filter_gaussian_kernel(alpha[1], 0, 0, k2);
  gsl_filter_gaussian_kernel(alpha[2], 0, 0, k3);

  /* apply filters */
  gsl_filter_gaussian(GSL_FILTER_END_PADVALUE, alpha[0], 0, x, y1, gauss_p);
  gsl_filter_gaussian(GSL_FILTER_END_PADVALUE, alpha[1], 0, x, y2, gauss_p);
  gsl_filter_gaussian(GSL_FILTER_END_PADVALUE, alpha[2], 0, x, y3, gauss_p);

  /* print kernels */
  for (i = 0; i < K; ++i)
    {
      double k1i = gsl_vector_get(k1, i);
      double k2i = gsl_vector_get(k2, i);
      double k3i = gsl_vector_get(k3, i);

      printf("%e %e %e\n", k1i, k2i, k3i);
    }

  printf("\n\n");

  /* print filter results */
  for (i = 0; i < N; ++i)
    {
      double xi = gsl_vector_get(x, i);
      double y1i = gsl_vector_get(y1, i);
      double y2i = gsl_vector_get(y2, i);
      double y3i = gsl_vector_get(y3, i);

      printf("%.12e %.12e %.12e %.12e\n", xi, y1i, y2i, y3i);
    }

  gsl_vector_free(x);
  gsl_vector_free(y1);
  gsl_vector_free(y2);
  gsl_vector_free(y3);
  gsl_vector_free(k1);
  gsl_vector_free(k2);
  gsl_vector_free(k3);
  gsl_rng_free(r);
  gsl_filter_gaussian_free(gauss_p);

  return 0;
}


#ifndef UTILITIES_H
#define UTILITIES_H
#include <vector>
#include <math.h>
#include <random>
#include <iostream>
#include <fstream>
#include <eigen3/Eigen/Dense>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/math/distributions/normal.hpp>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <opencv2/opencv.hpp>
#include <opencv2/ml/ml.hpp>
#include  "Structures.h"
#include <map>

using namespace std;
using namespace boost::math;
using namespace Eigen;
using namespace cv;

namespace Eigen {
    namespace internal {
        template<typename Scalar>
        struct scalar_normal_dist_op
        {
          static boost::mt19937 rng;    // The uniform pseudo-random algorithm
          mutable boost::normal_distribution<Scalar> norm;  // The gaussian combinator
          EIGEN_EMPTY_STRUCT_CTOR(scalar_normal_dist_op)
          template<typename Index>
          inline const Scalar operator() (Index, Index = 0) const { return norm(rng); }
        };
        template<typename Scalar> boost::mt19937 scalar_normal_dist_op<Scalar>::rng;
        template<typename Scalar>
        struct functor_traits<scalar_normal_dist_op<Scalar> >{
            enum{
                Cost = 50 * NumTraits<Scalar>::MulCost, PacketAccess = false, IsRepeatable = false
            };
        };
    }
}
class Utilities{
    public:
        Utilities();
        virtual ~Utilities();
        //Read a file that has a specific number of dimensions for each attribute
        virtual vector < vector < vector<double> > >  readFile(string CloudSeperator, string filepath);
        double multivariateNormalPDF(Vector3d instance, Vector3d mu, Matrix3d covar, int dimensionality);
        Eigen::MatrixXd sampleMultivariateNormal( Eigen::VectorXd mean, Eigen::MatrixXd covar, int numOfSamples, int dimensionality);
        Eigen::MatrixXd sampleMultinomial(vector<double> probabilities, int samples);
        int randcat( vector<double> * vec);
        Eigen::Matrix3d iwishrnd( Matrix3d tau, double nu, int dimensionality, int df);
        Eigen::VectorXd exprnd(double rate, int samples);
        double exppdf(double x , double lambda);
        double gammarnd(double alpha , double beta);
        RowVectorXd dirrnd(RowVectorXd q0);
        double catpdf(int index , vector<double>  probabilities);
        // Exponential distribution distances
        double Expsquaredhellinger(double lambda1, double lambda2);
        double ExpKLDivergence(double lambda1, double lambda2);
        // Gaussian distribution distances
        double GaussKLDivergence(std::vector<double> mean1, Matrix3d covar1, std::vector<double> mean2, Matrix3d covar2 );
        double Wasserstein(std::vector<double> mean1, Matrix3d covar1, std::vector<double> mean2, Matrix3d covar2 );

        vector<float>  categoricalhistogramCompare( float histA[], float histB[] , int N);
        float categoricalKLDivergence( cv::Mat * mat1, cv::Mat * mat2);
    protected:
    private:
};
#endif // UTILITIES_H


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

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

#include "complex_internal.h"
#include "bitreverse.h"

static int
FUNCTION(fft_complex,bitreverse_order) (BASE data[],
                                        const size_t stride,
                                        const size_t n,
                                        size_t logn)
{
  /* This is the Goldrader bit-reversal algorithm */

  size_t i;
  size_t j = 0;

  logn = 0 ; /* not needed for this algorithm */

  for (i = 0; i < n - 1; i++)
    {
      size_t k = n / 2 ;

      if (i < j)
        {
          const BASE tmp_real = REAL(data,stride,i);
          const BASE tmp_imag = IMAG(data,stride,i);
          REAL(data,stride,i) = REAL(data,stride,j);
          IMAG(data,stride,i) = IMAG(data,stride,j);
          REAL(data,stride,j) = tmp_real;
          IMAG(data,stride,j) = tmp_imag;
        }

      while (k <= j)
        {
          j = j - k ;
          k = k / 2 ;
        }

      j += k ;
    }

  return 0;
}

static int
FUNCTION(fft_real,bitreverse_order) (BASE data[],
                                const size_t stride,
                                const size_t n,
                                size_t logn)
{
  /* This is the Goldrader bit-reversal algorithm */

  size_t i;
  size_t j = 0;

  logn = 0 ; /* not needed for this algorithm */

  for (i = 0; i < n - 1; i++)
    {
      size_t k = n / 2 ;

      if (i < j)
        {
          const BASE tmp = VECTOR(data,stride,i);
          VECTOR(data,stride,i) = VECTOR(data,stride,j);
          VECTOR(data,stride,j) = tmp;
        }

      while (k <= j)
        {
          j = j - k ;
          k = k / 2 ;
        }

      j += k ;
    }

  return 0;
}


/* cdf/poisson.c
 *
 * Copyright (C) 2004 Jason H. Stover.
 *
 * 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307, USA.
 */

/*
 * Computes the cumulative distribution function for a Poisson
 * random variable. For a Poisson random variable X with parameter
 * mu,
 *
 *          Pr( X <= k ) = Pr( Y >= p )
 *
 * where Y is a gamma random variable with parameters k+1 and 1.
 *
 * Reference:
 *
 * W. Feller, "An Introduction to Probability and Its
 * Applications," volume 1. Wiley, 1968. Exercise 46, page 173,
 * chapter 6.
 */

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

#include "error.h"

/*
 * Pr (X <= k) for a Poisson random variable X.
 */

double
gsl_cdf_poisson_P (const unsigned int k, const double mu)
{
  double P;
  double a;

  if (mu <= 0.0)
    {
      CDF_ERROR ("mu <= 0", GSL_EDOM);
    }

  a = (double) k + 1.0;
  P = gsl_cdf_gamma_Q (mu, a, 1.0);

  return P;
}

/*
 * Pr ( X > k ) for a Possion random variable X.
 */

double
gsl_cdf_poisson_Q (const unsigned int k, const double mu)
{
  double Q;
  double a;

  if (mu <= 0.0)
    {
      CDF_ERROR ("mu <= 0", GSL_EDOM);
    }

  a = (double) k + 1.0;
  Q = gsl_cdf_gamma_P (mu, a, 1.0);

  return Q;
}


/////////////////////////////////////////////////////////////////////////////////
//
//  Levenberg - Marquardt non-linear minimization algorithm
//  Modified and simplified by Ted Laurence to use for MLE of Poisson-distributed data; Used only for
//		double precision, without constraints
//  Copyright (C) 2004  Manolis Lourakis (lourakis at ics forth gr)
//  Institute of Computer Science, Foundation for Research & Technology - Hellas
//  Heraklion, Crete, Greece.
//
//  Modifications Copyright (c) 2010, Lawrence Livermore National Security, LLC. Produced at the Lawrence Livermore National Laboratory. Written by Ted Laurence (laurence2@llnl.gov)
//  LLNL-CODE-424602 All rights reserved.
//  This file is part of dlevmar_mle_der
//
//  Please also read Our Notice and GNU General Public License.
//
//  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.
//
/////////////////////////////////////////////////////////////////////////////////
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_vector.h>
#include <math.h>

#include "levmar_mle.h"
#include "lm_mle_compiler.h"

#define EPSILON       1E-12
#define ONE_THIRD     0.3333333334 /* 1.0/3.0 */
#define LM_REAL_MAX FLT_MAX
#define LM_REAL_MIN -FLT_MAX

double compute_chisq_measure(double *e, double *x, double *hx, int n, int fitType)
{
	int i;
	double chisq=0.0;

	switch (fitType)
	{
	case LM_CHISQ_MLE:
		for (i=0; i<n; i++)
		{
			if ( hx[i] > 0)
			{
				if (x[i]==0)
					chisq += 2 * hx[i];
				else
					chisq += 2 * (hx[i]-x[i]-x[i]*log(hx[i]/x[i]));
				e[i]=x[i]/hx[i]-1.0;
			}
			else
				e[i]=0.0;
		}
		break;
	case LM_CHISQ_NEYMAN:
		for (i=0; i<n; i++)
		{
			if (x[i]==0)
			{
				chisq += (x[i]-hx[i])*(x[i]-hx[i]);
				e[i]=x[i]-hx[i];
			}
			else
			{
				chisq += (x[i]-hx[i])*(x[i]-hx[i])/x[i];
				e[i]=(x[i]-hx[i])/x[i];
			}
		}
		break;
	case LM_CHISQ_EQUAL_WT:
		for (i=0; i<n; i++)
		{
			e[i]=x[i]-hx[i];
			chisq += e[i]*e[i];
		}
		break;
	}
	return chisq;
}

/*
 * This function seeks the parameter vector p that best describes the measurements vector x.
 * More precisely, given a vector function  func : R^m --> R^n with n>=m,
 * it finds p s.t. func(p) ~= x, i.e. the squared second order (i.e. L2) norm of
 * e=x-func(p) is minimized.
 *
 * This function requires an analytic Jacobian.
 *
 * Returns the number of iterations (>=0) if successful, LM_ERROR if failed
 *
 * For more details, see K. Madsen, H.B. Nielsen and O. Tingleff's lecture notes on
 * non-linear least squares at http://www.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf
 */

int dlevmar_mle_der(
  void (*func)(double *p, double *hx, int m, int n, void *adata), /* functional relation describing measurements. A p \in R^m yields a \hat{x} \in  R^n */
  void (*jacf)(double *p, double *j, int m, int n, void *adata),  /* function to evaluate the Jacobian \part x / \part p */
  double *p,         /* I/O: initial parameter estimates. On output has the estimated solution */
  double *x,         /* I: measurement vector. NULL implies a zero vector */
  int m,              /* I: parameter vector dimension (i.e. #unknowns) */
  int n,              /* I: measurement vector dimension */
  int itmax,          /* I: maximum number of iterations */
  double opts[5],    /* I: minim. options [\mu, \epsilon1, \epsilon2, \epsilon3,\epsilon4]. Respectively the scale factor for initial \mu,
                       * stopping thresholds for ||J^T e||_inf, ||Dp||_2, chisq, and delta_chisq. Set to NULL for defaults to be used
                       */
  double info[LM_INFO_SZ],
					           /* O: information regarding the minimization. Set to NULL if don't care
                      * info[0]= ||e||_2 at initial p.
                      * info[1-4]=[ ||e||_2, ||J^T e||_inf,  ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.
                      * info[5]= # iterations,
                      * info[6]=reason for terminating: 1 - stopped by small gradient J^T e
                      *                                 2 - stopped by small Dp
                      *                                 3 - stopped by itmax
                      *                                 4 - singular matrix. Restart from current p with increased mu
                      *                                 5 - no further error reduction is possible. Restart with increased mu
                      *                                 6 - stopped by small ||e||_2
                      *                                 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error
					  *									8 - stopped by small change in chisq on successful iteration (dF<eps4)
                      * info[7]= # function evaluations
                      * info[8]= # Jacobian evaluations
                      * info[9]= # linear systems solved, i.e. # attempts for reducing error
                      */
  double *work,     /* working memory at least LM_DER_WORKSZ() reals large, allocated if NULL */
  double *covar,    /* O: Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed. */
  void *adata,		/* pointer to possibly additional data, passed uninterpreted to func & jacf.
                      * Set to NULL if not needed
                      */
  int fitType)      /* Which type of fit to perform
					*	LM_CHISQ_MLE is MLE for Poisson distribution
					*	LM_CHISQ_NEYMAN	is least squares where sigma^2 is set to max(x,1)
					*	LM_CHISQ_EQUAL_WT is least squares where sigma^2 is set to 1 */
{
register int i, j, k, l;
int worksz, freework=0, issolved;
/* temp work arrays */
double *e,          /* nx1 */
	   *e_test,     /* nx1 */
       *hx,         /* \hat{x}_i, nx1 */
       *jacTe,      /* J^T e_i mx1 */
       *jac,        /* nxm */
       *jacTjac,    /* mxm */
	   *LUdecomp,   /* mxm */
       *Dp,         /* mx1 */
   *diag_jacTjac,   /* diagonal of J^T J, mx1 */
       *pDp;        /* p + Dp, mx1 */

register double mu,  /* damping constant */
                tmp; /* mainly used in matrix & vector multiplications */
double p_chisq, jacTe_inf, pDp_chisq; /* ||e(p)||_2, ||J^T e||_inf, ||e(p+Dp)||_2 */
double p_L2, Dp_L2=LM_REAL_MAX, dF, dL;
double tau, eps1, eps2, eps2_sq, eps3, eps4;
double init_p_chisq;
int nu=2, nu2, stop=0, nfev, njev=0, nlss=0;
const int nm=n*m;

gsl_matrix_view LUdecomp_view;
gsl_vector_view jacTe_view, Dp_view;
gsl_permutation * perm = gsl_permutation_alloc (m);
int signum;

  mu=jacTe_inf=0.0; /* -Wall */

  if(n<m){
    fprintf(stderr, "(): cannot solve a problem with fewer measurements [%d] than unknowns [%d]\n", n, m);
    return LM_ERROR;
  }

  if(!jacf){
    fprintf(stderr, "No function specified for computing the Jacobian");
    return LM_ERROR;
  }

  if(opts){
	  tau=opts[0];
	  eps1=opts[1];
	  eps2=opts[2];
	  eps2_sq=opts[2]*opts[2];
    eps3=opts[3];
	eps4=opts[4];
  }
  else{ // use default values
	  tau=LM_INIT_MU;
	  eps1=LM_STOP_THRESH;
	  eps2=LM_STOP_THRESH;
	  eps2_sq=LM_STOP_THRESH*LM_STOP_THRESH;
	  eps3=LM_STOP_THRESH;
	  eps4=LM_STOP_THRESH;
  }

  if(!work){
    worksz=LM_MLE_WORKSZ(m, n); //2*n+4*m + n*m + m*m;
    work=(double *)malloc(worksz*sizeof(double)); /* allocate a big chunk in one step */
    if(!work){
      fprintf(stderr, "(): memory allocation request failed\n");
      return LM_ERROR;
    }
    freework=1;
  }

  /* set up work arrays */
  e=work;
  e_test=e + n;
  hx=e_test + n;
  jacTe=hx + n;
  jac=jacTe + m;
  jacTjac=jac + nm;
  LUdecomp=jacTjac + m*m;
  Dp=LUdecomp + m*m;
  diag_jacTjac=Dp + m;
  pDp=diag_jacTjac + m;

  LUdecomp_view=gsl_matrix_view_array(LUdecomp,m,m);
  jacTe_view=gsl_vector_view_array(jacTe,m);
  Dp_view=gsl_vector_view_array(Dp,m);

  /* compute e=x - f(p) and its chisq measure*/
  (*func)(p, hx, m, n, adata); nfev=1;
  /* ### e=x-hx, p_eL2=||e|| */
  p_chisq=compute_chisq_measure(e, x, hx, n, fitType);

  init_p_chisq=p_chisq;
  if(!LM_FINITE(p_chisq)) stop=7;

  for(k=0; k<itmax && !stop; ++k){
	register int l, im;
	register double alpha, wt, *jaclm;

    /* Note that p and e have been updated at a previous iteration */

    if(p_chisq<=eps3){ /* error is small */
      stop=6;
      break;
    }

    /* Compute the Jacobian J at p,  J^T J,  J^T e,  ||J^T e||_inf and ||p||^2.
     * Since J^T J is symmetric, its computation can be sped up by computing
     * only its upper triangular part and copying it to the lower part
     */

    (*jacf)(p, jac, m, n, adata); ++njev;

    /* J^T J, J^T e */
	 for(i=m*m; i-->0; )
		jacTjac[i]=0.0;
	 for(i=m; i-->0; )
		jacTe[i]=0.0;

	 for(l=n; l-->0; ){
		jaclm=jac+l*m;
		switch (fitType)
		{
		case LM_CHISQ_MLE:
			if (hx[l]>0)
				wt=x[l]/(hx[l]*hx[l]);
			else
				wt=0.0;
			break;
		case LM_CHISQ_NEYMAN:
			if (x[l]==0)
				wt=1.0;
			else
				wt=1.0/x[l];
			break;
		case LM_CHISQ_EQUAL_WT:
			wt=1.0;
			break;
		}
		for(i=m; i-->0; ){
		  im=i*m;
		  alpha=jaclm[i]; //jac[l*m+i];
		  for(j=i+1; j-->0; ) /* j<=i computes lower triangular part only */
			jacTjac[im+j]+=jaclm[j]*alpha*wt; //jac[l*m+j]

		  /* J^T e */
		  jacTe[i]+=alpha*e[l];
		}
	 }

	 for(i=m; i-->0; ) /* copy to upper part */
		for(j=i+1; j<m; ++j)
		  jacTjac[i*m+j]=jacTjac[j*m+i];

	  /* Compute ||J^T e||_inf and ||p||^2 */
    for(i=0, p_L2=jacTe_inf=0.0; i<m; ++i){
      if(jacTe_inf < (tmp=FABS(jacTe[i]))) jacTe_inf=tmp;

      diag_jacTjac[i]=jacTjac[i*m+i]; /* save diagonal entries so that augmentation can be later canceled */
      p_L2+=p[i]*p[i];
    }

    /* check for convergence */
    if((jacTe_inf <= eps1)){
      Dp_L2=0.0; /* no increment for p in this case */
      stop=1;
      break;
    }

   /* compute initial damping factor */
    if(k==0){
      for(i=0, tmp=LM_REAL_MIN; i<m; ++i)
        if(diag_jacTjac[i]>tmp) tmp=diag_jacTjac[i]; /* find max diagonal element */
      mu=tau*tmp;
    }

    /* determine increment using adaptive damping */
    while(1){
      /* augment normal equations */
      for(i=0; i<m; ++i)
        jacTjac[i*m+i]+=mu;

      /* solve augmented equations */
      /* use the LU included with GSL*/
	  for (i=0; i<m*m; i++)
		LUdecomp[i]=jacTjac[i];

	  gsl_linalg_LU_decomp(&LUdecomp_view.matrix, perm, &signum);
	  issolved = !gsl_linalg_LU_solve (&LUdecomp_view.matrix, perm, &jacTe_view.vector, &Dp_view.vector);

	  if(issolved){
        /* compute p's new estimate and ||Dp||^2 */
        for(i=0, Dp_L2=0.0; i<m; ++i){
          pDp[i]=p[i] + (tmp=Dp[i]);
          Dp_L2+=tmp*tmp;
        }
        //Dp_L2=sqrt(Dp_L2);

        if(Dp_L2<=eps2_sq*p_L2){ /* relative change in p is small, stop */
          stop=2;
          break;
        }

//       if(Dp_L2>=(p_L2+eps2)/EPSILON*EPSILON){ /* almost singular */
//         stop=4;
//         break;
//      }

        (*func)(pDp, hx, m, n, adata); ++nfev; /* evaluate function at p + Dp */
        /* compute ||e(pDp)||_2 */
        /* ### hx=x-hx, pDp_chisq=||hx|| */
        pDp_chisq= compute_chisq_measure(e_test, x, hx, n, fitType);

		if(!LM_FINITE(pDp_chisq)){ /* chisq is not finite, most probably due to a user error.
                                  * This check makes sure that the inner loop does not run indefinitely.
                                  * Thanks to Steve Danauskas for reporting such cases
                                  */
          stop=7;
          break;
        }

        for(i=0, dL=0.0; i<m; ++i)
          dL+=Dp[i]*(mu*Dp[i]+jacTe[i]);

        dF=p_chisq-pDp_chisq;

        if(dL>0.0 && dF>0.0){ /* reduction in error, increment is accepted */
          tmp=(2.0*dF/dL-1.0);
          tmp=1.0-tmp*tmp*tmp;
          mu=mu*( (tmp>=ONE_THIRD)? tmp : ONE_THIRD );
          nu=2;

          for(i=0 ; i<m; ++i) /* update p's estimate */
            p[i]=pDp[i];

          for(i=0; i<n; ++i) /* update e and ||e||_2 */
            e[i]=e_test[i];
          p_chisq=pDp_chisq;
		  if (dF<eps4)
			  stop = 8;
          break;
        }
	  }

      /* if this point is reached, either the linear system could not be solved or
       * the error did not reduce; in any case, the increment must be rejected
       */

      mu*=nu;
      nu2=nu<<1; // 2*nu;
      if(nu2<=nu){ /* nu has wrapped around (overflown). Thanks to Frank Jordan for spotting this case */
        stop=5;
        break;
	  }
      nu=nu2;

      for(i=0; i<m; ++i) /* restore diagonal J^T J entries */
        jacTjac[i*m+i]=diag_jacTjac[i];
	} /* inner loop */
  }

  if(k>=itmax) stop=3;

  for(i=0; i<m; ++i) /* restore diagonal J^T J entries */
    jacTjac[i*m+i]=diag_jacTjac[i];

  if(info){
    info[0]=init_p_chisq;
    info[1]=p_chisq;
    info[2]=jacTe_inf;
    info[3]=Dp_L2;
    for(i=0, tmp=LM_REAL_MIN; i<m; ++i)
      if(tmp<jacTjac[i*m+i]) tmp=jacTjac[i*m+i];
    info[4]=mu/tmp;
    info[5]=(double)k;
    info[6]=(double)stop;
    info[7]=(double)nfev;
    info[8]=(double)njev;
    info[9]=(double)nlss;
  }

  if(freework) free(work);

  gsl_permutation_free(perm);

  return (stop!=4 && stop!=7)?  k : LM_ERROR;
}