/*
 * 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;
}


#include <r.h>

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

#include <gsl/gsl_cdf.h>

static void uniformly_distributed(uint64_t (*f)(void))
{
    const size_t B = 10, N = 100000;
    size_t boxes[B]; memset(boxes, 0, sizeof(boxes));

    for(size_t i = 0; i < N; i++) {
        boxes[f() % B] += 1;
    }

    double chi2 = 0;
    for(size_t i = 0; i < B; i++) {
        const double E = (double)N/B;
        const double err = (double)boxes[i] - E;
        chi2 += err*err/E;
    }

    assert(chi2 <= gsl_cdf_chisq_Pinv(0.95, B-1));
}

static void normally_distributed(float (*f)(void))
{
    const size_t B = 5, N = 1000;
    size_t boxes[B+1]; memset(boxes, 0, sizeof(boxes));

    float stddev = 2;
    for(size_t i = 0; i < N; i++) {
        size_t j = floor(fabsf(f()) * stddev);
        boxes[MIN(j, B)] += 1;
    }

    double E = 2*N*gsl_cdf_gaussian_Q(B, stddev);
    double err = (double)boxes[B] - E;
    double chi2 = err*err/E;
    for(size_t i = 0; i < B; i++) {
        E = N * 2 * (gsl_cdf_gaussian_P(i+1, stddev) - gsl_cdf_gaussian_P(i, stddev));
        err = (double)boxes[i] - E;
        chi2 += err*err/E;
    }

    assert(chi2 <= gsl_cdf_chisq_Pinv(0.95, B));
}

void xorshift_tests(void)
{
    xorshift_state_initialize();

    TEST(xorshift64_is_uniform, {
         uniformly_distributed(xorshift64_i);
    });

    TEST(xorshift128plus_is_uniform, {
          uniformly_distributed(xorshift128plus_i);
    });

    TEST(normal_dist_is_normal, {
          normally_distributed(normal_dist_i);
    });

    TEST_ABORT(xorshift64_is_not_normal, {
        float f() { return uniform_float(xorshift64_i()); }
        normally_distributed(f);
    });

    TEST_ABORT(normal_dist_is_not_uniform, {
        uint64_t f() { return llrint(fabsf(normal_dist_i())); }
        uniformly_distributed(f);
    });

    TEST_ABORT(normal_dist_test_should_respect_stddev, {
        float f() { return 2*normal_dist_i(); }
        normally_distributed(f);
    });
}


#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


#ifndef IMAT_MATRIX_H
#define IMAT_MATRIX_H

#include <cstdarg>
#include <cstdlib>

#include <fstream>
#include <sstream>

#ifdef GSL
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_sort_double.h>
#endif // GSL

//#include "split_str.h"
#include "vector.h"
#include "base.h"

namespace imat {

enum Dim { kByRow = 0, kByCol = 1 };  // Choose dimention for Min, Max, Mean
                                      // Sum, Covariance and Sort

// Matrix Class:
// Provide a C++ Matrix class, whoes data can be exposed to C numeric libraries
// (eg. GSL lib).
template <class T>
class Matrix {
  // Following functions are not as efficient as operate_and_assign (+=, -= and
  // *=). Try to use operate_and_assign instead of these functions whenever
  // possible.
  template <class TT>
  friend Matrix<TT> operator- (const Matrix<TT> &lfs, const Matrix<TT> &rhs);
  template <class TT>
  friend Matrix<TT> operator+ (const Matrix<TT> &lfs, const Matrix<TT> &rhs);
  template <class TT>
  friend Matrix<TT> operator* (const Matrix<TT> &lfs, const Matrix<TT> &rhs);
  template <class TT>
  friend Matrix<TT> operator* (const Matrix<TT> &mat, const TT &x);
  template <class TT>
  friend Matrix<TT> operator* (const TT &x, const Matrix<TT> &mat);
  //
  // Cross Product
  template <class TT>
  friend void Cross(Matrix<TT> *res, const Matrix<TT> &a, const Matrix<TT> &b);

 public:
  Matrix() : rows_(0), cols_(0), tda_(0), data_(NULL), owner_(false) {}
  ~Matrix()  { if (owner_) delete []data_; }
  Matrix(isize_t rows, isize_t cols);
  Matrix(const Matrix <T> &source);
  Matrix<T> & operator= (const Matrix<T> &source);
  ListInitializer<T> operator= (const T &value);
  void Allocate(isize_t rows, isize_t cols); // for empty matrix only
  // Matrix views: can only be created by mat.View(...)
  // View raw data
  void View(T *src, isize_t rows, isize_t cols, isize_t tda=kDefault);
  // View submatrix: const and non-const version
  void View(Matrix<T> *src,
            isize_t row_0,         isize_t col_0,
            isize_t rows=kDefault, isize_t cols=kDefault);
  void View(const Matrix<T> &src,
            isize_t row_0,         isize_t col_0,
            isize_t rows=kDefault, isize_t cols=kDefault);
  // Copy elements from a submatrix of src
  void SubCopy(const Matrix<T> &src,
               isize_t row_0=kDefault, isize_t col_0=kDefault,
               isize_t rows=kDefault,  isize_t cols=kDefault);

  T &       operator() (isize_t i, isize_t j);
  const T & operator() (isize_t i, isize_t j) const;
  Matrix<T> operator-() const;
  bool operator== (const Matrix<T> &other) const;
  void operator+= (const Matrix<T> &other);
  void operator-= (const Matrix<T> &other);
  void operator*= (const Matrix<T> &other);
  void operator+= (const T &x);
  void operator-= (const T &x);
  void operator*= (const T &x);

  void SetZero();           // Set all elements as 0
  void SetAll(const T &x);  // Set all elements as x
  void SetUnit();           // Set square matrix as unit matrix
  T Sum() const;            // Sumation of all elements
  T Min() const;            // Minimum of all elements
  T Max() const;            // Maximum of all elements
  double Mean() const;      // Mean value of all elements
  void GetRow(Vector<T> *row_view, isize_t i);
  void GetCol(Vector<T> *col_view, isize_t i);
  void Trans();

  // Following functions are designed to operate on rows or cols:
  // eg. A = [[ 1,  2,  3],
  //          [ 4,  5,  6],
  //          [ 7,  8,  9],
  //          [10, 11, 12]]
  // A.Sum(kByRow) ==> [6, 15, 24, 33]
  // A.Sum(kByCol) ==> [22, 26, 30]
  //
  Vector<T> Sum(Dim dimention) const;
  Vector<T> Min(Dim dimention) const;
  Vector<T> Max(Dim dimention) const;
  Vector<double> Mean(Dim dimention) const;
  void Norm(Dim dimention=kByRow);            // Only for T==double

#ifdef GSL
  // Get covariance matrix of data seris stored in cols or rows
  Matrix<T> Cov(Dim dimention=kByCol) const;  // Only for T==double
  void Sort(Dim dimention=kByRow, int key=0); // Only for T==double
  void Eigen(Vector<T> *eigen_value, Matrix<T> *eigen_vector); // Only for T==double
#endif // GSL

  void Print() const;
  void Readf(const char *filename);
  void Printf(const char *filename) const;

  bool     empty() const { return (rows_ == 0 or cols_ == 0); }
  isize_t  rows()  const { return rows_;                      }
  isize_t  cols()  const { return cols_;                      }
  isize_t  tda()   const { return tda_;                       }
  T*       data()        { return data_;                      }
  const T* data()  const { return data_;                      }
  void     clear();

 protected:
  bool is_view_() const { return (data_ != NULL and owner_ == false); }
  void copy_(const Matrix<T> &source);
  void allocate_(isize_t rows, isize_t cols);

  isize_t rows_;
  isize_t cols_;
  isize_t tda_;
  T *data_;
  bool owner_;
};

typedef Matrix<int> MatrixI;
typedef Matrix<double> MatrixD;

#include "matrix_inl.h"

} // namespace imat
#endif // IMAT_MATRIX_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;
}


#ifndef VEC3_H
#define VEC3_H

#include <cmath>
#include <iostream>

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
// you need to add the following libraries to your project : gsl, gslcblas

class Vec3 {
private:
    float mVals[3];
public:
    Vec3() {}
    Vec3( float x , float y , float z ) {
       mVals[0] = x; mVals[1] = y; mVals[2] = z;
    }
    float & operator [] (unsigned int c) { return mVals[c]; }
    float operator [] (unsigned int c) const { return mVals[c]; }
    void operator = (Vec3 const & other) {
       mVals[0] = other[0] ; mVals[1] = other[1]; mVals[2] = other[2];
    }
    float squareLength() const {
       return mVals[0]*mVals[0] + mVals[1]*mVals[1] + mVals[2]*mVals[2];
    }
    float length() const { return sqrt( squareLength() ); }
    void normalize() { float L = length(); mVals[0] /= L; mVals[1] /= L; mVals[2] /= L; }
    static float dot( Vec3 const & a , Vec3 const & b ) {
       return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
    }
    static Vec3 cross( Vec3 const & a , Vec3 const & b ) {
       return Vec3( a[1]*b[2] - a[2]*b[1] ,
                    a[2]*b[0] - a[0]*b[2] ,
                    a[0]*b[1] - a[1]*b[0] );
    }
    void operator += (Vec3 const & other) {
        mVals[0] += other[0];
        mVals[1] += other[1];
        mVals[2] += other[2];
    }
    void operator -= (Vec3 const & other) {
        mVals[0] -= other[0];
        mVals[1] -= other[1];
        mVals[2] -= other[2];
    }
    void operator *= (float s) {
        mVals[0] *= s;
        mVals[1] *= s;
        mVals[2] *= s;
    }
    void operator /= (float s) {
        mVals[0] /= s;
        mVals[1] /= s;
        mVals[2] /= s;
    }

    static Vec3 Rand(float magnitude = 1.f) {
        return Vec3( magnitude * (-1.f + 2.f * rand() / (float)(RAND_MAX)) , magnitude * (-1.f + 2.f * rand() / (float)(RAND_MAX)) , magnitude * (-1.f + 2.f * rand() / (float)(RAND_MAX)) );
    }

    Vec3 getOrthogonal() const {
        // CAREFUL ! THE NORM IS NOT PRESERVED !!!!
        if( mVals[0] == 0 ) {
            return Vec3( 0 , mVals[2] , -mVals[1] );
        }
        else if( mVals[1] == 0 ) {
            return Vec3( mVals[2] , 0 , -mVals[0] );
        }
        return Vec3( mVals[1] , -mVals[0] , 0 );
    }
};

static inline Vec3 operator + (Vec3 const & a , Vec3 const & b) {
   return Vec3(a[0]+b[0] , a[1]+b[1] , a[2]+b[2]);
}
static inline Vec3 operator - (Vec3 const & a , Vec3 const & b) {
   return Vec3(a[0]-b[0] , a[1]-b[1] , a[2]-b[2]);
}
static inline Vec3 operator * (float a , Vec3 const & b) {
   return Vec3(a*b[0] , a*b[1] , a*b[2]);
}
static inline Vec3 operator / (Vec3 const &  a , float b) {
   return Vec3(a[0]/b , a[1]/b , a[2]/b);
}
static inline std::ostream & operator << (std::ostream & s , Vec3 const & p) {
    s << p[0] << " " << p[1] << " " << p[2];
    return s;
}
static inline std::istream & operator >> (std::istream & s , Vec3 & p) {
    s >> p[0] >> p[1] >> p[2];
    return s;
}

class Mat3
{
public:
    ////////////         CONSTRUCTORS          //////////////
    Mat3()
    {
        vals[0] = 0;
        vals[1] = 0;
        vals[2] = 0;
        vals[3] = 0;
        vals[4] = 0;
        vals[5] = 0;
        vals[6] = 0;
        vals[7] = 0;
        vals[8] = 0;
    }
    Mat3( float v1 , float v2 , float v3 , float v4 , float v5 , float v6 , float v7 , float v8 , float v9)
    {
        vals[0] = v1;
        vals[1] = v2;
        vals[2] = v3;
        vals[3] = v4;
        vals[4] = v5;
        vals[5] = v6;
        vals[6] = v7;
        vals[7] = v8;
        vals[8] = v9;
    }
    Mat3( const Mat3 & m )
    {
        for(int i = 0 ; i < 3 ; ++i )
            for(int j = 0 ; j < 3 ; ++j )
                (*this)(i,j) = m(i,j);
    }

    bool isnan() const {
        return std::isnan(vals[0]) || std::isnan(vals[1]) || std::isnan(vals[2])
                 || std::isnan(vals[3]) || std::isnan(vals[4]) || std::isnan(vals[5])
                 || std::isnan(vals[6]) || std::isnan(vals[7]) || std::isnan(vals[8]);
    }

    void operator = (const Mat3 & m)
    {
        for(int i = 0 ; i < 3 ; ++i )
            for(int j = 0 ; j < 3 ; ++j )
                (*this)(i,j) = m(i,j);
    }

    void operator += (const Mat3 & m)
    {
        for(int i = 0 ; i < 3 ; ++i )
            for(int j = 0 ; j < 3 ; ++j )
                (*this)(i,j) += m(i,j);
    }
    void operator -= (const Mat3 & m)
    {
        for(int i = 0 ; i < 3 ; ++i )
            for(int j = 0 ; j < 3 ; ++j )
                (*this)(i,j) -= m(i,j);
    }
    void operator /= (double s)
    {
        for( unsigned int c = 0 ; c < 9 ; ++c )
            vals[c] /= s;
    }

    Mat3 operator - (const Mat3 & m2)
    {
        return Mat3( (*this)(0,0)-m2(0,0) , (*this)(0,1)-m2(0,1) , (*this)(0,2)-m2(0,2) , (*this)(1,0)-m2(1,0) , (*this)(1,1)-m2(1,1) , (*this)(1,2)-m2(1,2) , (*this)(2,0)-m2(2,0) , (*this)(2,1)-m2(2,1) , (*this)(2,2)-m2(2,2) );
    }
    Mat3 operator + (const Mat3 & m2)
    {
        return Mat3( (*this)(0,0)+m2(0,0) , (*this)(0,1)+m2(0,1) , (*this)(0,2)+m2(0,2) , (*this)(1,0)+m2(1,0) , (*this)(1,1)+m2(1,1) , (*this)(1,2)+m2(1,2) , (*this)(2,0)+m2(2,0) , (*this)(2,1)+m2(2,1) , (*this)(2,2)+m2(2,2) );
    }

    Mat3 operator / (float s)
    {
        return Mat3( (*this)(0,0)/s , (*this)(0,1)/s , (*this)(0,2)/s , (*this)(1,0)/s , (*this)(1,1)/s , (*this)(1,2)/s , (*this)(2,0)/s , (*this)(2,1)/s , (*this)(2,2)/s );
    }
    Mat3 operator * (float s)
    {
        return Mat3( (*this)(0,0)*s , (*this)(0,1)*s , (*this)(0,2)*s , (*this)(1,0)*s , (*this)(1,1)*s , (*this)(1,2)*s , (*this)(2,0)*s , (*this)(2,1)*s , (*this)(2,2)*s );
    }

    Vec3 operator * (const Vec3 & p) // computes m.p
    {
        return Vec3(
                    (*this)(0,0)*p[0] + (*this)(0,1)*p[1] + (*this)(0,2)*p[2],
                    (*this)(1,0)*p[0] + (*this)(1,1)*p[1] + (*this)(1,2)*p[2],
                    (*this)(2,0)*p[0] + (*this)(2,1)*p[1] + (*this)(2,2)*p[2]);
    }

    Mat3 operator * (const Mat3 & m2)
    {
        return Mat3(
                    (*this)(0,0)*m2(0,0) + (*this)(0,1)*m2(1,0) + (*this)(0,2)*m2(2,0) ,
                    (*this)(0,0)*m2(0,1) + (*this)(0,1)*m2(1,1) + (*this)(0,2)*m2(2,1) ,
                    (*this)(0,0)*m2(0,2) + (*this)(0,1)*m2(1,2) + (*this)(0,2)*m2(2,2) ,
                    (*this)(1,0)*m2(0,0) + (*this)(1,1)*m2(1,0) + (*this)(1,2)*m2(2,0) ,
                    (*this)(1,0)*m2(0,1) + (*this)(1,1)*m2(1,1) + (*this)(1,2)*m2(2,1) ,
                    (*this)(1,0)*m2(0,2) + (*this)(1,1)*m2(1,2) + (*this)(1,2)*m2(2,2) ,
                    (*this)(2,0)*m2(0,0) + (*this)(2,1)*m2(1,0) + (*this)(2,2)*m2(2,0) ,
                    (*this)(2,0)*m2(0,1) + (*this)(2,1)*m2(1,1) + (*this)(2,2)*m2(2,1) ,
                    (*this)(2,0)*m2(0,2) + (*this)(2,1)*m2(1,2) + (*this)(2,2)*m2(2,2)
                    );
    }

    ////////        ACCESS TO COORDINATES      /////////
    float operator () (unsigned int i , unsigned int j) const
    { return vals[3*i + j]; }
    float & operator () (unsigned int i , unsigned int j)
    { return vals[3*i + j]; }

    ////////        BASICS       /////////
    inline float sqrnorm()
    {
        return vals[0]*vals[0] + vals[1]*vals[1] + vals[2]*vals[2]
                + vals[3]*vals[3] + vals[4]*vals[4] + vals[5]*vals[5]
                + vals[6]*vals[6] +  vals[7]*vals[7] + vals[8]*vals[8];
    }

    inline float norm()
    { return sqrt( sqrnorm() ); }

    inline float determinant() const
    {
        return vals[0] * ( vals[4] * vals[8] - vals[7] * vals[5] )
                - vals[1] * ( vals[3] * vals[8] - vals[6] * vals[5] )
                + vals[2] * ( vals[3] * vals[7] - vals[6] * vals[4] );
    }

    static
    Mat3 pseudoInverse( Mat3 const & m , bool & isRealInverse , double defaultValueForInverseSingularValue = 0.0 )
    {
        float det = m.determinant();
        if( fabs(det) != 0.0 )
        {
            isRealInverse = true;
            return Mat3( m(1,1)*m(2,2) - m(2,1)*m(1,2) , m(0,2)*m(2,1) - m(0,1)*m(2,2) , m(0,1)*m(1,2) - m(0,2)*m(1,1) ,
                             m(1,2)*m(2,0) - m(1,0)*m(2,2) , m(0,0)*m(2,2) - m(0,2)*m(2,0) , m(0,2)*m(1,0) - m(0,0)*m(1,2) ,
                             m(1,0)*m(2,1) - m(1,1)*m(2,0) , m(0,1)*m(2,0) - m(0,0)*m(2,1) , m(0,0)*m(1,1) - m(0,1)*m(1,0) ) / det ;
        }

        // otherwise:
        isRealInverse = false;
        Mat3 U ; float sx ; float sy ; float sz ; Mat3 Vt;
        m.SVD(U,sx,sy,sz,Vt);
        float sxInv = sx == 0.0 ? 1.0 / sx : defaultValueForInverseSingularValue;
        float syInv = sy == 0.0 ? 1.0 / sy : defaultValueForInverseSingularValue;
        float szInv = sz == 0.0 ? 1.0 / sz : defaultValueForInverseSingularValue;
        return Vt.getTranspose() * Mat3::diag(sxInv , syInv , szInv) * U.getTranspose();
    }

    inline float trace() const
    { return vals[0] + vals[4] + vals[8]; }

    ////////        TRANSPOSE       /////////
    inline
    void transpose()
    {
        float xy = vals[1] , xz = vals[2] , yz = vals[5];
        vals[1] = vals[3];
        vals[3] = xy;
        vals[2] = vals[6];
        vals[6] = xz;
        vals[5] = vals[7];
        vals[7] = yz;
    }
    Mat3 getTranspose() const
    {
        return Mat3(vals[0],vals[3],vals[6],vals[1],vals[4],vals[7],vals[2],vals[5],vals[8]);
    }

    // ---------- ROTATION <-> AXIS/ANGLE ---------- //
    template< class point_t >
    void getAxisAndAngleFromRotationMatrix( point_t & axis , float & angle )
    {
        angle = acos( (trace() - 1.f) / 2.f );
        axis[0] = vals[7] - vals[5];
        axis[1] = vals[2] - vals[6];
        axis[2] = vals[3] - vals[1];
        axis.normalize();
    }

    template< class point_t >
    inline static
    Mat3 getRotationMatrixFromAxisAndAngle( const point_t & axis , float angle )
    {
        Mat3 w = vectorial(axis);
        return Identity() +  w * std::sin(angle) +  w * w * ((1.0) - std::cos(angle));
    }

    // ---------- STATIC STANDARD MATRICES ---------- //
    inline static Mat3 Identity()
    {  return Mat3(1,0,0  ,  0,1,0  ,  0,0,1);  }

    inline static Mat3 Zero()
    {  return Mat3(0,0,0  ,  0,0,0  ,  0,0,0);  }

    template< typename T2 >
    inline static Mat3 diag( T2 x , T2 y ,T2 z )
    {  return Mat3(x,0,0  ,  0,y,0  ,  0,0,z);  }

    template< class point_t >
    inline static Mat3 getFromCols(const point_t & c1 , const point_t & c2 , const point_t & c3)
    {
        // 0 1 2
        // 3 4 5
        // 6 7 8
        return Mat3( c1[0] , c2[0] , c3[0] ,
                         c1[1] , c2[1] , c3[1] ,
                         c1[2] , c2[2] , c3[2] );
    }
    template< class point_t >
    inline static Mat3 getFromRows(const point_t & r1 , const point_t & r2 , const point_t & r3)
    {
        // 0 1 2
        // 3 4 5
        // 6 7 8
        return Mat3( r1[0] , r1[1] , r1[2] ,
                         r2[0] , r2[1] , r2[2] ,
                         r3[0] , r3[1] , r3[2] );
    }

    inline static Mat3 RandRotation()
    {
        Vec3 axis(-1.0 + 2.0* (float)(rand()) / (float)( RAND_MAX )  ,
                       -1.0 + 2.0* (float)(rand()) / (float)( RAND_MAX )  ,
                       -1.0 + 2.0* (float)(rand()) / (float)( RAND_MAX )  );
        axis.normalize();
        float angle = 2.0 * M_PI * ((float)(rand()) / (float)( RAND_MAX )   - 0.5 );

        return Mat3::getRotationMatrixFromAxisAndAngle( axis , angle );
    }

    inline static Mat3 RandRotation( float maxAngle )
    {
        Vec3 axis(-1.0 + 2.0* (float)(rand()) / (float)( RAND_MAX )  ,
                       -1.0 + 2.0* (float)(rand()) / (float)( RAND_MAX )  ,
                       -1.0 + 2.0* (float)(rand()) / (float)( RAND_MAX )  );
        axis.normalize();
        float angle =  maxAngle * ((float)(rand()) / (float)( RAND_MAX )   - 0.5 );

        return Mat3::getRotationMatrixFromAxisAndAngle( axis , angle );
    }

    inline static Mat3 RandRotation( Vec3 twistAxis , double maxTwist , double maxRotation )
    {
        twistAxis.normalize();
        Vec3 u = twistAxis.getOrthogonal();
        u.normalize();
        const Vec3 & v = Vec3::cross(u,twistAxis);

        double uv_axis_angle = 2.0*M_PI * (double)(rand()) / (double)(RAND_MAX);
        const Vec3 & uv = cos(uv_axis_angle)*u + sin(uv_axis_angle)*v;

        double rotation_angle = maxRotation * ( ( 2.0 *(double)(rand()) / (double)(RAND_MAX) ) - 1.0 );
        double twist_angle = maxTwist * ( ( 2.0 *(double)(rand()) / (double)(RAND_MAX) ) - 1.0 );

        return Mat3::getRotationMatrixFromAxisAndAngle(uv , rotation_angle) * Mat3::getRotationMatrixFromAxisAndAngle(twistAxis , twist_angle);
    }

    void SVD( Mat3 & U , float & sx , float & sy , float & sz , Mat3 & Vt ) const
    {
        gsl_matrix * u = gsl_matrix_alloc(3,3);
        for(unsigned int i = 0 ; i < 3; ++i)
            for(unsigned int j = 0 ; j < 3; ++j)
                gsl_matrix_set( u , i , j , (*this)(i,j) );

        gsl_matrix * v = gsl_matrix_alloc(3,3);
        gsl_vector * s = gsl_vector_alloc(3);
        gsl_vector * work = gsl_vector_alloc(3);

        gsl_linalg_SV_decomp (u,
                              v,
                              s,
                              work);

        sx = s->data[0];
        sy = s->data[1];
        sz = s->data[2];
        for(unsigned int i = 0 ; i < 3; ++i)
        {
            for(unsigned int j = 0 ; j < 3; ++j)
            {
                U(i,j) = gsl_matrix_get( u , i , j );
                Vt(i,j) = gsl_matrix_get( v , j , i );
            }
        }

        gsl_matrix_free(u);
        gsl_matrix_free(v);
        gsl_vector_free(s);
        gsl_vector_free(work);

        // a transformation float is given as R.B.S.Bt, R = rotation , B = local basis (rotation matrix), S = scales in the basis B
        // it can be obtained from the svd decomposition of float = U Sigma Vt :
        // B = V
        // S = Sigma
        // R = U.Vt
    }

    // ---------- Projections onto Rotations ----------- //

    void setRotation()
    {
        Mat3 U,Vt;
        float sx,sy,sz;
        SVD(U,sx,sy,sz,Vt);
        const Mat3 & res = U*Vt;
        if( res.determinant() < 0 )
        {
            U(0,2) = -U(0,2);
            U(1,2) = -U(1,2);
            U(2,2) = -U(2,2);
            *this = (U*Vt);
            return;
        }
        // else
        *this = (res);
    }

    template< class point_t >
    inline static
    Mat3 tensor( const point_t & p1 , const point_t & p2 )
    {
        return Mat3(
                    p1[0]*p2[0] , p1[0]*p2[1] , p1[0]*p2[2],
                    p1[1]*p2[0] , p1[1]*p2[1] , p1[1]*p2[2],
                    p1[2]*p2[0] , p1[2]*p2[1] , p1[2]*p2[2]);
    }

    template< class point_t >
    inline static
    Mat3 vectorial( const point_t & p )
    {
        return Mat3(
                    0      , -p[2]  , p[1]     ,
                    p[2]   , 0      , - p[0]   ,
                    - p[1] , p[0]   , 0
                    );
    }

    Mat3 operator - () const
    {
        return Mat3( - vals[0],- vals[1],- vals[2],- vals[3],- vals[4],- vals[5],- vals[6],- vals[7],- vals[8] );
    }

private:
    float vals[9];
    // will be noted as :
    // 0 1 2
    // 3 4 5
    // 6 7 8
};

inline static
Mat3 operator * (float s , const Mat3 & m)
{
    return Mat3( m(0,0)*s , m(0,1)*s , m(0,2)*s , m(1,0)*s , m(1,1)*s , m(1,2)*s , m(2,0)*s , m(2,1)*s , m(2,2)*s );
}

inline static std::ostream & operator << (std::ostream & s , Mat3 const & m)
{
    s << m(0,0) << " \t" << m(0,1) << " \t" << m(0,2) << std::endl << m(1,0) << " \t" << m(1,1) << " \t" << m(1,2) << std::endl << m(2,0) << " \t" << m(2,1) << " \t" << m(2,2) << std::endl;
    return s;
}

#endif