#include <malloc.h>
#include <math.h>
#include <memory.h>
#include <stdio.h>
#include <string.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_matrix_double.h>
#include "tc_mat.h"

struct tc_mat *tc_mat_ctr(uint32_t nr_, uint32_t nc_)
{
	struct tc_mat *A = (struct tc_mat *) malloc(sizeof(struct tc_mat));
	memset(A, 0, sizeof(struct tc_mat));
	tc_mat_resize(A, nr_, nc_);
	return A;
}

void tc_mat_dtr(struct tc_mat *A)
{
	if (!A)
		return;
	tc_mat_clear(A);
	free(A);
}

void tc_mat_clear(struct tc_mat *A)
{
	if (!A)
		return;
	if (A->a) {
		for(uint32_t r=0; r < A->nr; r++)
			if (A->a[r])
				free(A->a[r]);
		free(A->a);
	}
	A->a = NULL;
	A->nr = 0;
	A->nc = 0;
}

void tc_mat_print(const struct tc_mat *A)
{
	if (!A)
		return;
	for(uint32_t i=0; i < A->nr; i++) {
		for (uint32_t j=0; j < A->nc; j++)
			printf(" %13.10f", A->a[i][j]);
		printf("\n");
	}
}

void tc_mat_resize(struct tc_mat *A, uint32_t nr_, uint32_t nc_)
{
	if ((A->nr == nr_) && (A->nc == nc_))
		return;
	tc_mat_clear(A);
	A->nr = nr_;
	A->nc = nc_;
	A->a = (double **) malloc(sizeof(double *) * nr_);
	for(uint32_t r=0; r < nr_; r++)
		A->a[r] = (double *) malloc(sizeof(double) * nc_);
}

void tc_mat_copy(struct tc_mat *B, const struct tc_mat *A)
{
	if (A == B)
		return;
	if ((B->nr != A->nr) || (B->nc != A->nc))
		tc_mat_resize(B, A->nr, A->nc);
	for(uint32_t i=0; i < A->nr; i++)
		for(uint32_t j=0; j < A->nc; j++)
			B->a[i][j] = A->a[i][j];
}

void tc_mat_identity(struct tc_mat *A)
{
	for(uint32_t i=0; i < A->nr; i++)
		for(uint32_t j=0; j < A->nc; j++)
			A->a[i][j] = (i==j)? 1.0: 0.0;
}

void tc_mat_transpose(struct tc_mat *B, const struct tc_mat *A)
{
	if (A == B) {
		struct tc_mat *b = tc_mat_ctr(A->nc, A->nr);
		for(uint32_t i=0; i < A->nr; i++)
			for(uint32_t j=0; j < A->nc; j++)
				b->a[j][i] = A->a[i][j];
		tc_mat_copy(B, b);
		tc_mat_dtr(b);
		return;
	}
	if ((B->nr != A->nc) || (B->nc != A->nr))
		tc_mat_resize(B, A->nc, A->nr);
	for(uint32_t i=0; i < A->nr; i++)
		for(uint32_t j=0; j < A->nc; j++)
			B->a[j][i] = A->a[i][j];
}

int tc_mat_add(struct tc_mat *C, const struct tc_mat *A, const struct tc_mat *B)
{
	if (!C || !A || !B || (A->nr != B->nr) || (A->nc != B->nc))
		return -1;
	if ((C->nr != A->nr) || (C->nc != A->nc))
		tc_mat_resize(C, A->nr, B->nc);
	for(uint32_t i=0; i < C->nr; i++)
		for(uint32_t j=0; j < C->nc; j++)
			C->a[i][j] = A->a[i][j] + B->a[i][j];
	return 0;
}

int tc_mat_mult(struct tc_mat *C, const struct tc_mat *A, const struct tc_mat *B)
{
	if (!C || !A || !B || (A->nc != B->nr))
		return -1;
	if ((C == A) || (C == B)) {
		struct tc_mat *c = tc_mat_ctr(A->nr, B->nr);
		tc_mat_mult(c, A, B);
		tc_mat_copy(C, c);
		tc_mat_dtr(c);
		return 0;
	}
	if ((C->nc != B->nc) || (C->nr != A->nr))
		tc_mat_resize(C, A->nr, B->nc);
	for(uint32_t i=0; i < C->nr; i++) {
		for(uint32_t j=0; j < C->nc; j++) {
			C->a[i][j] = 0.0;
			for(uint32_t k=0; k < A->nc; k++)
				C->a[i][j] += A->a[i][k] * B->a[k][j];
		}
	}
	return 0;
}

int tc_mat_mult_scalar(struct tc_mat *C, double a, const struct tc_mat *B)
{
	if (!C || !B)
		return -1;
	if ((C->nr != B->nr) || (C->nc != B->nc))
		tc_mat_resize(C, B->nr, B->nc);
	for(uint32_t i=0; i < C->nr; i++)
		for(uint32_t j=0; j < C->nc; j++)
			C->a[i][j] = a * B->a[i][j];
	return 0;
}

/* Householder Reduction to Bidiagonal Form
 * Input m x n matrix A
 * Output: m x n matrix  U, products of Householder matrices
 *         m x n matrix  B, bidiagonal
 *         m x n matrix  V, products of Householder matrices
 *   such that A = U B V^T
 *
 * A Householder matrix P is a reflection transformation of the form
 * I - tau u u^T where tau = 2 / (u^T u). P is orthonormal since
 *        P^T P = (I - tau u u^T)^T (I - tau u u^T)
 *              = (I - tau u u^T)   (I - tau u u^T)
 *              =  I - 2 tau u u^T + tau^2 u (u^T u) u^T
 *              =  I - 2 tau u u^T + tau^2 u (2 / tau) u^T
 *              =  I
 * Products of orthonomal matrices are orthonormal so U and V will be
 * orthonormal.
 *
 * B is constructed via the algorithm
 *   1.  B <-- A
 *   2.  U <-- I  m x n
 *   3.  V <-- I  n x n
 *   4.  For k = 1, ..., n
 *       a. construct a Householder matrix P such that
 *             i. P is the identity on the first k-1 components, and
 *            ii. the k-th column of P B is zero below the diagonal.
 *       b.  B <--- P B
 *       c.  U <--- U P
 *       d.  If k < n - 2, increment k and repeat steps a-c for the
 *              operation from the right (the row of B P will be zero
 *              to the right of the superdiagonal).
 *
 *  Step 4a(i) is equivalent to setting the first k-1 components of u
 *  to be zero. Let v be the k-th column of P B. Since
 *      P v = (I - tau u u^T) v
 *          = (I - tau u u^T) v
 *          = v - tau (u^T v) u   (Eq. 1)
 *  we have that step 4a(ii) is equivalent to setting u to be an appropriate
 *  scalar multiple of v for all components above k.  The previous steps
 *  would have zeroed out the first k-2 components of v. Let
 *     b^2 = Sum_{i>=k} (v_i)^2
 *  and choose the sign of b to ensure s = v_k - b is nonzero. Set u = v/s
 *  on all components larger than k and set u_k = 1. Then
 *    tau = 2 / (u^T u)
 *        = 2 / (1 + (b^2 - (v_k)^2) / s^2 )
 *        = (2 s) / (s + (b^2 - (v_k)^2) / s)
 *        = (2 s) / (s - (b + v_k))
 *        = (2 s) / ((v_k - b) - (b + v_k))
 *        = (2 s) / (- 2 b)
 *        =  -s / b
 *  This leads to the zeroing out of the lower components of P v since
 *    u^T v = v_k + (b^2 - (v_k)^2) / s
 *          = v_k - (b   +  v_k)
 *          = b
 *  meaning that Eq 1 is simply
 *      P v = v - (-s/b) (b) u
 *          = v + s u
 *  and so P v is zero on all components larger than k.
 *
 *  Side note on efficiency 1 (not implemented):
 *    Step c produces U and V as products of Housholder matrices. The actual
 *    computation of these products is not the most computationally efficient
 *    method. A Housholder matrix acts on a vector v as
 *          P v = (I - tau u u^T) v
 *              =  v - tau u (v^T u)^T  (Eq. 2)
 *    Thus if we've recorded all u's and tau's, we can find U, the product of
 *    all the P's, by sequentially applying Eq 2 to the components e1, e2, etc.
 *
 *  Side note on efficiency 2 (not implemented):
 *    Since each iteration of B leads to known places where its terms have zeros,
 *    all matrix multiplications involving B can be restricted to the nonzero
 *    terms.
 *
 *  Side note on efficiency 3 (not implemented):
 *    Since the u's have more zeros in each iteration, the collection of u's
 *    can be stored in the original matrix A (and hence B) so long as the
 *    Side note on efficiency #2 was implemented.
 */
int tc_mat_bidiag_decomp(
	const struct tc_mat *A,
	struct tc_mat *U,
	struct tc_mat *B,
	struct tc_mat *V)
{
	if (!A || !U || !B || !V)
		return -1;
	if ((U->nr != A->nr) || (U->nc != A->nr))
		tc_mat_resize(U, A->nr, A->nr);
	if ((V->nr != A->nc) || (V->nc != A->nc))
		tc_mat_resize(V, A->nc, A->nc);
	if ((B->nr != A->nr) || (B->nc != A->nc))
		tc_mat_resize(B, A->nr, A->nc);
	tc_mat_copy(B, A);
	tc_mat_identity(U);
	tc_mat_identity(V);
	struct tc_mat *Ic = tc_mat_ctr(A->nc, A->nc);
	tc_mat_identity(Ic);
	struct tc_mat *Ir = tc_mat_ctr(A->nr, A->nr);
	tc_mat_identity(Ir);
	struct tc_mat *u = tc_mat_ctr(0, 0);
	struct tc_mat *uT = tc_mat_ctr(0, 0);
	struct tc_mat *u_uT = tc_mat_ctr(0, 0);
	struct tc_mat *P = tc_mat_ctr(0, 0);
	for(uint32_t k=0; (k < A->nc) && (k+1 < A->nr); k++) {
		/* from the left */
		double b_k = B->a[k][k];
		double b = 0.0;
		for(uint32_t i=k; i < A->nr; i++)
			b += B->a[i][k] * B->a[i][k];
		if (b != 0.0) { /* if b = 0, there's nothing to do */
			b = ((b_k > 0)? -1.0: 1.0) * sqrt(b);
			double s = b_k - b;
			double tau = - s / b;
			tc_mat_resize(u, A->nr, 1);
			for(uint32_t i=0; i < A->nr; i++)
				u->a[i][0] = (i > k)? B->a[i][k]/s: ((i==k)? 1.0: 0.0);
			/*  P = I - tau u u^T */
			tc_mat_transpose(uT, u);
			tc_mat_mult(u_uT, u, uT);
			tc_mat_mult_scalar(P, -tau, u_uT);
			tc_mat_add(P, Ir, P);
			/*  U B = (U P) (P B) */
			tc_mat_mult(B, P, B);
			tc_mat_mult(U, U, P);
		}
		if (k + 2 < A->nc) {
			/* from the right */
			double b_k = B->a[k][k+1];
			double b = 0.0;
			for(uint32_t j=k+1; j < A->nc; j++)
				b += B->a[k][j] * B->a[k][j];
			b = ((b_k > 0)? -1.0: 1.0) * sqrt(b);
			if (b != 0.0) { /* if b = 0, there's nothing to do */
				double s = b_k - b;
				double tau = - s / b;
				tc_mat_resize(u, A->nc, 1);
				for(uint32_t i=0; i < A->nc; i++)
					u->a[i][0] = (i > k+1)? B->a[k][i]/s: ((i==k+1)? 1.0: 0.0);
				/*  P = I - tau u u^T */
				tc_mat_transpose(uT, u);
				tc_mat_mult(u_uT, u, uT);
				tc_mat_mult_scalar(P, -tau, u_uT);
				tc_mat_add(P, Ic, P);
				/*  B V = (B P) (P V) */
				tc_mat_mult(B, B, P);
				tc_mat_mult(V, P, V);
			}
		}
	}
	tc_mat_dtr(P);
	tc_mat_dtr(u);
	tc_mat_dtr(uT);
	tc_mat_dtr(u_uT);
	tc_mat_dtr(Ic);
	tc_mat_dtr(Ir);
	return 0;
}

/* tc_mat_eigenvalues
 * only implmented for 2x2 matrices
 * Input 2 x 2 matrix A
 * Output: 2 x 1 matrix E of eigenvalues
 *
 * If we set A = [ a b ]
 *               [ c d ]
 * Then
 *  0 = det(A - x I)
 *    = (a - x)(d - x) - bc
 *    = x^2 - (a + d) x + (ad - bc)
 */
int tc_mat_eigenvalues(struct tc_mat *E, const struct tc_mat *A)
{
	if (!A || !E)
		return -1;
	if ((A->nr != 2) || (A->nc != 2))
		return -1;
	if (E->nr != A->nr)
		tc_mat_resize(E, A->nr, 1);
	double a = A->a[0][0];
	double b = A->a[1][0];
	double c = A->a[0][1];
	double d = A->a[1][1];
	double ad2 = (a + d)/2.0;
	double det = ad2*ad2 - (a*d - b*c);
	double s = (det <= 0.0)? 0.0: sqrt(det);
	E->a[0][0] = ad2 + s;
	E->a[1][0] = ad2 - s;
	return 0;
}

/* Wilkinson Shift
 * Input: m x 2 matrix A
 * Output: the Wilkinson shift
 *
 * ei= trailing 2x2 matrix of D^T D */
double tc_mat_wilkinson_shift(const struct tc_mat *A)
{
	if (!A)
		return 0.0;
	if (A->nc != 2)
		return 0.0;
	struct tc_mat *AT = tc_mat_ctr(2, A->nr);
	tc_mat_transpose(AT, A);
	struct tc_mat *AT_A = tc_mat_ctr(2, 2);
	tc_mat_mult(AT_A, AT, A);
	struct tc_mat *E = tc_mat_ctr(2, 1);
	tc_mat_eigenvalues(E, AT_A);
	double l0 = E->a[0][0];
	double l1 = E->a[1][0];
	double t22 = AT_A->a[1][1];
	tc_mat_dtr(AT);
	tc_mat_dtr(AT_A);
	tc_mat_dtr(E);
	return (fabs(l0 - t22) < fabs(l1 - t22))? l0: l1;
}

int tc_mat_svd(
	const struct tc_mat *A,
	struct tc_mat *U,
	struct tc_mat *D,
	struct tc_mat *V)
{
	if (!U || !D || !V || !A)
		return -1;
	if ((U->nr != A->nr) || (U->nc != A->nc))
		tc_mat_resize(U, A->nr, A->nc);
	if ((V->nr != A->nc) || (V->nc != A->nc))
		tc_mat_resize(V, A->nc, A->nc);
	if ((D->nr != A->nc) || (D->nc != A->nc))
		tc_mat_resize(D, A->nc, A->nc);
	if (A->nr < A->nc)
		return -2;
	/* case nc = 1 */
	if (A->nc == 1) {
		V->a[0][0] = 1.0;
		double normsq = 0.0;
		for(uint32_t i=0; i < A->nr; i++)
			normsq += A->a[i][0] * A->a[i][0];
		D->a[0][0] = sqrt(normsq);
		if (D->a[0][0] == 0.0)
			tc_mat_copy(U, A);
		else {
			double inv = 1.0 / D->a[0][0];
			for(uint32_t i=0; i < A->nr; i++)
				U->a[i][0] = A->a[i][0] * inv;
		}
		return 0;
	}
	/* Step1: A = U D V where D is bi-diagonal
	 *            A is nr x nc
	 *            U is nr x nr
	 *            D is nr x nc
	 *            V is nc x nc
	 *
	 *    If A was not a square matrix, change U and D
	 *            U  nr x nc
	 *            D  nc x nc
	 */
	tc_mat_bidiag_decomp(A, U, D, V);
	if (D->nr != D->nc) {
		struct tc_mat *U0 = tc_mat_ctr(A->nr, A->nc);
		for(uint32_t i=0; i < A->nr; i++)
			for(uint32_t j=0; j < A->nc; j++)
				U0->a[i][j] = U->a[i][j];
		tc_mat_resize(U, A->nr, A->nc);
		tc_mat_copy(U, U0);
		tc_mat_dtr(U0);

		struct tc_mat *D0 = tc_mat_ctr(A->nc, A->nc);
		for(uint32_t i=0; i < A->nc; i++)
			for(uint32_t j=0; j < A->nc; j++)
				D0->a[i][j] = D->a[i][j];
		tc_mat_resize(D, A->nc, A->nc);
		tc_mat_copy(D, D0);
		tc_mat_dtr(D0);
	}
	/* iterate until either max_iterations has been hit
	 * or the largest superdiagonal entry is below the
	 * threshold. Set threshold = 1-e8 * largest element.
	 * Any term less than 0.1 threshold will be considered
	 * to be zero.
	 */
	const uint32_t max_iterations = 100 * D->nc;
	double threshold = D->a[0][0];
	for(uint32_t i=0; i < D->nc; i++)
		if (fabs(D->a[i][i]) > threshold)
			threshold = fabs(D->a[i][i]);
	for(uint32_t i=0; i+1 < D->nc; i++)
		if (fabs(D->a[i][i+1]) > threshold)
			threshold = fabs(D->a[i][i+1]);
	threshold *= 1e-12;
	const double zero_threshold = 0.1 * threshold;
	/* Always reindex the components so that
     * D so that has the form
	 *     [D1, 0] where D1 is diagonal and
	 *     [ 0,D2]       D2 is bidiagonal
	 * Let i0 be the starting index of D2
	 */
	uint32_t i0 = 0;
	for(uint32_t I=0; I < max_iterations; I++)
	{
		/* For any zeros on the diagonal, apply a series of
		 * Givens rotations to also zero out its off-diagonal
		 * term.  Then move this component into D1.
		 */
		for(uint32_t i1=i0; i1 < D->nr; i1++) {
			if (fabs(D->a[i1][i1]) > zero_threshold)
				continue;
			if ((i1+1 == D->nr) && fabs(D->a[i1-1][i1]) > zero_threshold)
				continue;
			if ((i1+1 < D->nr) && (fabs(D->a[i1][i1+1]) > zero_threshold)) {
				for(uint32_t i=i1; i+1 < D->nr; i++) {
					/* U D = (U G) (G^T D)  */
					double alpha = D->a[i1][i+1];
					if (fabs(alpha) < zero_threshold)
						break;
					double beta = D->a[i+1][i+1];
					double gamma = sqrt(alpha*alpha + beta*beta);
					double c = beta / gamma;
					double s = alpha / gamma;
					for(uint32_t j=0; j < D->nr; j++) {
						double a = D->a[i1][j];
						double b = D->a[i+1][j];
						D->a[i1][j] = a*c - b*s;
						D->a[i+1][j] = a*s + b*c;
					}
					for(uint32_t j=0; j < U->nr; j++) {
						double a = U->a[j][i1];
						double b = U->a[j][i+1];
						U->a[j][i1] = a*c - b*s;
						U->a[j][i+1] = a*s + b*c;
					}
				}
				i1++;
			}
			/* move (i0,i1-1) down, i1 -> i0  */
			for(uint32_t j=0; j < D->nr; j++) {
				double tmp = V->a[i1][j];
				for(uint32_t k=i1; k > i0; k--)
					V->a[k][j] = V->a[k-1][j];
				V->a[i0][j] = tmp;
			}
			for(uint32_t j=0; j < D->nr; j++) {
				double tmp = U->a[j][i1];
				for(uint32_t k=i1; k > i0; k--)
					U->a[j][k] = U->a[j][k-1];
				U->a[j][i0] = tmp;
			}
			double tmp = D->a[i1][i1];
			double tmp1 = D->a[i1][i1+1];
			for(uint32_t k=i1; k > i0; k--) {
				D->a[k][k] = D->a[k-1][k-1];
				if (k+1 < D->nc)
					D->a[k][k+1] = D->a[k-1][k];
			}
			D->a[i0][i0] = tmp;
			D->a[i0][i0+1] = tmp1;
			i0++;
		}
		/* For any zeros on the superdiagonal, move the
		 * component to D1.
		 */
		for(uint32_t i=i0; i+1 < D->nr; i++) {
			if (fabs(D->a[i][i+1]) >= zero_threshold)
				continue;
			if (i == i0) {
				i0++;
				continue;
			}
			if (i+2 != D->nr)
				continue;
			/* move (i0,i) down, i+1 -> i0    */
			for(uint32_t j=0; j < D->nr; j++) {
				double tmp = V->a[i+1][j];
				for(uint32_t k=i+1; k > i0; k--)
					V->a[k][j] = V->a[k-1][j];
				V->a[i0][j] = tmp;
			}
			for(uint32_t j=0; j < D->nr; j++) {
				double tmp = U->a[j][i+1];
				for(uint32_t k=i+1; k > i0; k--)
					U->a[j][k] = U->a[j][k-1];
				U->a[j][i0] = tmp;
			}
			double tmp = D->a[i+1][i+1];
			double tmp1 = D->a[i][i+1];
			for(uint32_t k=i+1; k > i0; k--) {
				D->a[k][k] = D->a[k-1][k-1];
				D->a[k-1][k] = (k-1==i0)? tmp1: D->a[k-2][k-1];
			}
			D->a[i0][i0] = tmp;
			i0++;
			if ((i+2 == D->nr) && (i0 != i))
				i--; /* retry last term */
		}
		if (i0 >= D->nr - 1)
			break;
		/* Find largest element on super diagonal.*/
		/* Break if less than threshold.          */
		double largest_off_diag = 0.0;
		uint32_t n_zeros_on_off_diagonal = 0;
		for(uint32_t i=i0; i+1 < D->nr; i++) {
			if (fabs(D->a[i][i+1]) > largest_off_diag)
				largest_off_diag = fabs(D->a[i][i+1]);
			if (fabs(D->a[i][i+1]) < zero_threshold)
				n_zeros_on_off_diagonal++;
		}
		if (largest_off_diag < threshold)
			break;
		/* Find the next zero on the uper diagonal*/
		/* Treat [i0,i1] as a block.              */
		uint32_t i1 = i0;
		for(  ; i1+1 < D->nr; i1++)
			if (fabs(D->a[i1][i1+1]) < zero_threshold)
				break;
		/* Find Wilkerson shift.                  */
		struct tc_mat *t = tc_mat_ctr(3, 2);
		for(uint32_t i=0; i < 3; i++)
			for(uint32_t j=0; j < 2; j++)
				t->a[i][j] = (i1+i>2)? D->a[i1+i-2][i1+j-1]: 0.0;
		double mu = tc_mat_wilkinson_shift(t);
		tc_mat_dtr(t);
		double alpha = D->a[i0][i0] * D->a[i0][i0] - mu;
		double beta = D->a[i0][i0] * D->a[i0][i0+1];
		/* Apply Givens rotations G from i0 to the bottom,
		 * chasing the nonzero element until off the matrix
		 */
		for(uint32_t i=i0; i < i1; i++) {
			/* D V = (D G) (G^T V)  */
			double gamma = sqrt(alpha*alpha + beta*beta);
			double c = alpha / gamma;
			double s = -beta / gamma;
			for(uint32_t j=0; j < D->nr; j++) {
				double a = D->a[j][i+0];
				double b = D->a[j][i+1];
				D->a[j][i+0] = a*c - b*s;
				D->a[j][i+1] = a*s + b*c;
			}
			for(uint32_t j=0; j < D->nc; j++) {
				double a = V->a[i+0][j];
				double b = V->a[i+1][j];
				V->a[i+0][j] = a*c - b*s;
				V->a[i+1][j] = a*s + b*c;
			}
			/* U D = (U G) (G^T D) */
			alpha = D->a[i+0][i];
			beta = D->a[i+1][i];
			gamma = sqrt(alpha*alpha + beta*beta);
			if (fabs(gamma) > 0.0) {
				c = alpha / gamma;
				s = -beta / gamma;
				for(uint32_t j=0; j < D->nc; j++) {
					double a = D->a[i+0][j];
					double b = D->a[i+1][j];
					D->a[i+0][j] = a*c - b*s;
					D->a[i+1][j] = a*s + b*c;
				}
				for(uint32_t j=0; j < U->nr; j++) {
					double a = U->a[j][i+0];
					double b = U->a[j][i+1];
					U->a[j][i+0] = a*c - b*s;
					U->a[j][i+1] = a*s + b*c;
				}
			}
			if (i + 2 < D->nr) {
				alpha = D->a[i][i+1];
				beta = D->a[i][i+2];
			}
		}
	}
	/* now swap components to order the diagonal terms
	 * from largest to smallest
	 */
	for(uint32_t i=0; i+1 < D->nr; i++) {
		double largest = fabs(D->a[i][i]);
		uint32_t largest_i = i;
		for(uint32_t j=i+1; j < D->nr; j++) {
			if (fabs(D->a[j][j]) > largest) {
				largest_i = j;
				largest = fabs(D->a[j][j]);
			}
		}
		if (largest_i != i) {
			for(uint32_t j=0; j < D->nr; j++) {
				double tmp = V->a[i][j];
				V->a[i][j] = V->a[largest_i][j];
				V->a[largest_i][j] = tmp;
				tmp = U->a[j][i];
				U->a[j][i] = U->a[j][largest_i];
				U->a[j][largest_i] = tmp;
			}
			double tmp = D->a[i][i];
			D->a[i][i] = D->a[largest_i][largest_i];
			D->a[largest_i][largest_i] = tmp;
		}
		if (D->a[i][i] < 0) {
			D->a[i][i] = -D->a[i][i];
			for(uint32_t j=0; j < D->nr; j++)
				V->a[i][j] = -V->a[i][j];
		}
	}
	/* just to be sure, zero out all off-diagonal terms of D */
	for(uint32_t i=0; i < D->nr; i++)
		for(uint32_t j=0; j < D->nc; j++)
			if (i != j)
				D->a[i][j] = 0.0;
	/* transpose V */
	tc_mat_transpose(V, V);
	return 0;
}


/*
** compute histogram density function
**
** G.Lohmann, Nov 2012
*/

#include <viaio/Vlib.h>

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

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_histogram.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_cdf.h>
#include <gsl/gsl_math.h>

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

/* kernel density estimation */
double GaussKernel(double x)
{
  return exp(-0.5*x*x)/sqrt(2.0*M_PI);
}

double VKernelDensity(double x,double h,double nh,gsl_histogram *histogram)
{
  size_t i;
  double sum,upper,lower;

  sum = 0;
  for (i=0; i<gsl_histogram_bins(histogram); i++) {
    double nx = gsl_histogram_get (histogram,i);
    gsl_histogram_get_range (histogram,i,&lower,&upper);
    double xi = lower + (upper-lower)*0.5;
    double u = (x-xi)/h;
    sum += nx * GaussKernel(u);
  }
  return sum/nh;
}

void VPrintHistogram(gsl_histogram *histogram,int numperm,VString filename)
{
  size_t i;
  double z=0,mx=0,lower=0,upper=0,fz=0;
  extern double GaussKernel(double x);
  extern double VKernelDensity(double,double,double,gsl_histogram *);

  gsl_histogram_get_range (histogram,0,&lower,&upper);
  double alpha = 1.06;  /* silverman's rule */
  double sig = gsl_histogram_sigma(histogram);
  double kx = gsl_histogram_sum(histogram);
  double h  = alpha * sig * pow(kx,-0.2);
  double nh = h*kx;

  FILE *fph = fopen(filename,"w");
  if (!fph) VError(" err opening hist file");
  for (i=0; i<gsl_histogram_bins(histogram); i++) {
    gsl_histogram_get_range (histogram,i,&lower,&upper);
    z  = lower;
    fz = VKernelDensity(z,h,nh,histogram);
    mx = gsl_histogram_get (histogram,i);
    fprintf(fph,"%lf %lf %.2lf\n",lower,fz,mx);
  }
  fclose(fph);
}

/* update histogram */
void HistoUpdate(float *A,size_t nvox,gsl_histogram *hist)
{
  double tiny = 1.0e-8;
  size_t i,j;
  double hmin = gsl_histogram_min(hist);
  double hmax = gsl_histogram_max(hist);

  for (i=0; i<nvox; i++) {
    for (j=0; j<i; j++) {
      const size_t k=j+i*(i+1)/2;
      double u = (double)A[k];
      if (ABS(u) < tiny) continue;
      if (u < hmin) u = hmin;
      if (u > hmax-tiny) u = hmax-tiny;
      gsl_histogram_increment (hist,u);
    }
  }
}


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

void integral_gen (int nmin, int nmax, double vals[]);
double f (double x, void *params);

double f (double x, void *params)
{
    double n = (double) *(int *) params;
    double f = exp (-x) * pow (x, n);

    return f;
}

void integral_gen (int nmin, int nmax, double vals[])
{

    double a = 0., b = 1.;      // limits of integration
    double abserr = 0., relerr = 1.e-8; // requested errors
    double result;              // the integral value
    double error;               // the error estimate
    int n;

    size_t np = 1000;           // work area size
    gsl_integration_workspace *w = gsl_integration_workspace_alloc (np);

    gsl_function F;

    F.function = &f;
    F.params = &n;

    for (n = nmin; n <= nmax; n++)
    {
        gsl_integration_qag (&F, a, b, abserr, relerr, np, GSL_INTEG_GAUSS15,
            w, &result, &error);
        vals[n] = result;
    }

    gsl_integration_workspace_free (w);
}


/* sample.c
 * written by Sanghyuk Moon
 * Last update: 2020 July 24
 *
 * DESCRIPTION:
 *   Generate 3D particle distribution that is consistent with prescribed density.
 *   Density is assumed to be axisymmetric and normalized such that rho(R,z) ~ [0,1].
 *   R, z coordinates are generated by rejection sampling using the Halton's pseudo-random
 *   sequence, while phi coordinates are uniformly spaced between [0,2pi].
 *
 * RETURN:
 *   Particle positions in Cartesian coordinates (x,y,z).
 *
 * REQUIREMENTS:
 *   GNU Scientific Library ($sudo apt install libgsl-dev)
 *
 * USAGE:
 *   $gcc sample.c -lgsl -lm
 *   $a.out > out.txt
 */

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

const unsigned int dim=2;

double density(double R, double z) {
  double rho, scaleh, Rmin;
  Rmin = 0.1;
  scaleh = 0.1; // Gaussian scale height of the disk
  rho = Rmin/R*exp(-z*z/scaleh/scaleh/2);
  return rho;
}

void sample_particles(long int ngas, double x1min, double x1max, double x3min, double x3max) {
  long id=0;
  double pos[dim], u4;
  double x1,x2,x3; // particle position in cylindrical coordinates
  double den; // desired gas density
  gsl_qrng *qrng;
  gsl_rng *rng;

  // GSL random number generator setup
  qrng = gsl_qrng_alloc(gsl_qrng_halton, dim);
  rng = gsl_rng_alloc(gsl_rng_ranlxd2);

  while (id < ngas) {
    gsl_qrng_get(qrng, pos);
    x1 = x1min + (x1max-x1min)*pos[0];
    x3 = x3min + (x3max-x3min)*pos[1];
    u4 = x1max*gsl_rng_uniform(rng); // uniform random number in [0, x1max]
    den = density(x1, x3);

    // accept if u4 < R*density.
    // additional weighting by R compensates the R*d\phi volume factor.
    if (u4 <= x1*den) {
      x2 = 2.0*M_PI*(double)id/(double)ngas;
      printf("%le %le %le\n", x1*cos(x2), x1*sin(x2), x3);
      id++;
    }
  }
  gsl_qrng_free(qrng);
  gsl_rng_free(rng);
}

int main() {
  long int ngas; // number of particles
  double x1min, x1max, x3min, x3max;

  // set domain info
  ngas = pow(10,6);
  x1min = 0.1;
  x1max = 1.0;
  x3min = -0.5;
  x3max = 0.5;

  // run rejection samplig
  sample_particles(ngas, x1min, x1max, x3min, x3max);
  return 0;
}


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#define COMPEARTH_PRIVATE_GEM3 1
#include "compearth.h"
#ifdef DEBUG
#ifdef COMPEARTH_USE_MKL
#ifdef __clang__
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wreserved-id-macro"
#pragma clang diagnostic ignored "-Wstrict-prototypes"
#endif
#include <mkl_cblas.h>
#ifdef __clang__
#pragma clang diagnostic pop
#endif
#else
#include <cblas.h>
#endif
#endif

/*!
 * @brief Compute a rotation matrix given an axis and angle
 *
 * @param[in] n     Number of matrices to generate.
 * @param[in] v     Rotation axis.  This is an array of dimension [3 x n]
 *                  with leading dimension 3.
 * @param[in] xi    Rotation angles (degrees).  This is an array of
 *                  dimension [n].
 *
 * @param[in] U     Rotation matrices.  This is an array of dimension
 *                  [3 x 3 x n] where each leading [3 x 3] matrix is
 *                  in column major format.
 *
 * @result 0 indicates success.
 *
 * @date 2016 - Ben Baker translated Carl Tape's rotmat_gen.m to C
 *
 * @copyright MIT
 *
 */
int compearth_eulerUtil_rotmatGen(const int n,
                                  const double *__restrict__ v,
                                  const double *__restrict__ xi,
                                  double *__restrict__ U)
{
    double R1[9], R2[9], R3[9], R4[9], R5[9], R54[9], R543[9], R5432[9];
    double ele, nvph_deg, nvth_deg, rho, vph, vth, vph_deg, vth_deg;
    int i, ierr;
    const double pi180i = 180.0/M_PI;
    ierr = 0;
    for (i=0; i<n; i++)
    {
        compearth_matlab_cart2sph(1, &v[3*i+0], &v[3*i+1], &v[3*i+2],
                                  &vph, &ele, &rho);
        vth = M_PI_2 - ele;
        vth_deg = vth*pi180i;
        vph_deg = vph*pi180i;
        nvph_deg =-vph_deg;
        nvth_deg =-vth_deg;
        ierr = ierr + compearth_eulerUtil_rotmat(1, &nvph_deg, 3, R1);
        ierr = ierr + compearth_eulerUtil_rotmat(1, &nvth_deg, 2, R2);
        ierr = ierr + compearth_eulerUtil_rotmat(1, &xi[i],    3, R3);
        ierr = ierr + compearth_eulerUtil_rotmat(1, &vth_deg,  2, R4);
        ierr = ierr + compearth_eulerUtil_rotmat(1, &vph_deg,  3, R5);
        if (ierr != 0)
        {
            fprintf(stderr, "%s: Error computing rotation matrices %d\n",
                    __func__, i);
        }
        gemm3_colMajorNoTransNoTrans(R5, R4, R54);
        gemm3_colMajorNoTransNoTrans(R54, R3, R543);
        gemm3_colMajorNoTransNoTrans(R543, R2, R5432);
        gemm3_colMajorNoTransNoTrans(R5432, R1, &U[9*i]);
#ifdef DEBUG
        double U9[9];
        cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans,
                    3, 3, 3, 1.0, R5, 3, R4, 3, 0.0, R54, 3);
        cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans,
                    3, 3, 3, 1.0, R54, 3, R3, 3, 0.0, R543, 3);
        cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans,
                    3, 3, 3, 1.0, R543, 3, R2, 3, 0.0, R5432, 3);
        cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans,
                    3, 3, 3, 1.0, R5432, 3, R1, 3, 0.0, U9, 3);
        for (int k=0; k<9; k++)
        {
            if (fabs(U9[k] - U[9*i+k]) > 1.e-14)
            {
                printf("Computation failed: %f %f\n", U9[k], U[9*i+k]);
                ierr = ierr + 1;
            }
        }
#endif
    }
    return ierr;
}


/*
   The following program solves the second-order nonlinear
   Van der Pol oscillator equation (see Landau/Paez 14.12, part 1),
   x"(t) + \mu x'(t) (x(t)^2 - 1) + x(t) = 0
   This can be converted into a first order system suitable for
   use with the library by introducing a separate variable for
   the velocity, v = x'(t).  We assign x --> y[0] and v --> y[1].
   So the equations are:
   x' = v                  ==>  dy[0]/dt = f[0] = y[1]
   v' = -x + \mu v (1-x^2) ==>  dy[1]/dt = f[1] = -y[0] + mu*y[1]*(1-y[0]*y[0])
   x"(t) = - \mu x'(t) (x(t)^2 - 1) - x(t)

   x''(t) + x'(t) + x(t) = 0
   x''(t) = -x'(t) - x(t)

   x' = v ===> dy[0]/dt = f[0] = y[1]
   v' = -v - x
*/

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

/* function prototypes */
int rhs (double t, const double y[], double f[], void *params_ptr);
//int jacobian (double t, const double y[], double *dfdy, double dfdt[], void *params_ptr);

/*************************** main program ****************************/

int *jac;

int
main (void)
{
    int dimension = 2;		/* number of differential equations */

    double eps_abs = 1.e-8;	/* absolute error requested */
    double eps_rel = 1.e-10;	/* relative error requested */

    /* define the type of routine for making steps: */
    const gsl_odeiv_step_type *type_ptr = gsl_odeiv_step_rkf45;
    /* some other possibilities (see GSL manual):
       = gsl_odeiv_step_rk4;
       = gsl_odeiv_step_rkck;
       = gsl_odeiv_step_rk8pd;
       = gsl_odeiv_step_rk4imp;
       = gsl_odeiv_step_bsimp;
       = gsl_odeiv_step_gear1;
       = gsl_odeiv_step_gear2;
    */

    /*
       allocate/initialize the stepper, the control function, and the
       evolution function.
    */
	// 룬게쿠타 방식의 2계 미분 방정식으로 stepping을 진행합니다.
    gsl_odeiv_step *step_ptr = gsl_odeiv_step_alloc (type_ptr, dimension);
	// 허용 오차
    gsl_odeiv_control *control_ptr = gsl_odeiv_control_y_new (eps_abs, eps_rel);
	// 계산하는 차원
    gsl_odeiv_evolve *evolve_ptr = gsl_odeiv_evolve_alloc (dimension);

    gsl_odeiv_system my_system;	/* structure with the rhs function, etc. */

    double mu = 10;		/* parameter for the diffeq */
    double y[2];			/* current solution vector */

    double t, t_next;		/* current and next independent variable */
    double tmin, tmax, delta_t;	/* range of t and step size for output */

    double h = 1e-6;		/* starting step size for ode solver */

    /* load values into the my_system structure */
    my_system.function = rhs;	/* the right-hand-side functions dy[i]/dt */
    //my_system.jacobian = jacobian;	/* the Jacobian df[i]/dy[j] */
    my_system.jacobian = NULL;
    my_system.dimension = dimension;	/* number of diffeq's */
    //my_system.params = μ	/* parameters to pass to rhs and jacobian */
    my_system.params = NULL;

    tmin = 0.;			/* starting t value */
    tmax = 10.;			/* final t value */
    delta_t = 1.;

	// 주의점이라면 2계 미분 방정식은 최소 2개의 초기값이 필요하다는 부분을 주의!
    y[0] = 1.;			/* initial x value */
    y[1] = 0.;			/* initial v value */

    t = tmin;             /* initialize t */
    printf ("%.5e %.5e %.5e\n", t, y[0], y[1]);	/* initial values */

    /* step to tmax from tmin */
    for (t_next = tmin + delta_t; t_next <= tmax; t_next += delta_t)
    {
        while (t < t_next)	/* evolve from t to t_next */
        {
            gsl_odeiv_evolve_apply (evolve_ptr, control_ptr, step_ptr,
                                    &my_system, &t, t_next, &h, y);
        }
		// y[0], y[1] 결과 확인이 필요함
        printf ("%.5e %.5e %.5e\n", t, y[0], y[1]); /* print at t=t_next */
    }

    /* all done; free up the gsl_odeiv stuff */
    gsl_odeiv_evolve_free (evolve_ptr);
    gsl_odeiv_control_free (control_ptr);
    gsl_odeiv_step_free (step_ptr);

    return 0;
}

/*************************** rhs ****************************/
/*
   x''(t) + x'(t) + x(t) = 0
   x''(t) = -x'(t) - x(t)

   x' = v            ===> dy[0]/dt = f[0] = y[1]
   v' = -v - x       ===> dy[1]/dt = f[1] = -y[1] - y[0]
*/

// https://www.wolframalpha.com/input/?i=y%27%27+%2B+y%27+%2B+y+%3D+0%2C+y%280%29+%3D+1%2C+y%27%280%29+%3D+0, y(0) = 1
// https://www.wolframalpha.com/input/?i=%28sqrt%283%29+*+sin%28sqrt%283%29+%2F+2%29+%2B+3+*+cos%28sqrt%283%29+%2F+2%29%29+%2F+%283+*+sqrt%28e%29%29, y(1)
// (sqrt(3) * sin(sqrt(3) / 2) + 3 * cos(sqrt(3) / 2)) / (3 * sqrt(e)), y(1) = 0.6597001...
int
rhs (double t, const double v[], double f[], void *params_ptr)
//rhs (double t, const double y[], double f[], void *params_ptr)
{
	/*
    f[0] = y[1];
    f[1] = -y[0] + mu * y[1] * (1. - y[0] * y[0]);
	*/
	// * 코드 작성 팁: 미분항은 f에 배치합니다.
	// 1계와 마찬가지로 반드시 표준형이어야 합니다!
	// 첫번째 1차 미분이 무엇(치환)입니다 ~
	f[0] = v[1];
	// 두번째 2차 미분을 치환을 기반으로 어떻게 표현해야 하는지를 기록합니다.
	f[1] = -v[1] - v[0];
	/* Tip: 배열의 인덱스는 결국 미분의 횟수라고 생각해도 좋습니다 */
	// 현재 초기값이 있으니까 두 개의 근이 나오지 않고 한 개의 근이 나옴!

    return GSL_SUCCESS;		/* GSL_SUCCESS defined in gsl/errno.h as 0 */
}

/*************************** Jacobian ****************************/
/*
  Define the Jacobian matrix using GSL matrix routines.
  (see the GSL manual under "Ordinary Differential Equations")

  * params is a void pointer that is used in many GSL routines
  to pass parameters to a function
*/

#if 0
int
jacobian (double t, const double y[], double *dfdy,
          double dfdt[], void *params_ptr)
{
    /* get parameter(s) from params_ptr; here, just a double */
    double mu = *(double *) params_ptr;

    gsl_matrix_view dfdy_mat = gsl_matrix_view_array (dfdy, 2, 2);

    gsl_matrix *m_ptr = &dfdy_mat.matrix;	/* m_ptr points to the matrix */

    /* fill the Jacobian matrix as shown */
    gsl_matrix_set (m_ptr, 0, 0, 0.0);	/* df[0]/dy[0] = 0 */
    gsl_matrix_set (m_ptr, 0, 1, 1.0);	/* df[0]/dy[1] = 1 */
    gsl_matrix_set (m_ptr, 1, 0, -2.0 * mu * y[0] * y[1] - 1.0); /* df[1]/dy[0] */
    gsl_matrix_set (m_ptr, 1, 1, -mu * (y[0] * y[0] - 1.0));     /* df[1]/dy[1] */

    /* set explicit t dependence of f[i] */
    dfdt[0] = 0.0;
    dfdt[1] = 0.0;

    return GSL_SUCCESS;		/* GSL_SUCCESS defined in gsl/errno.h as 0 */
}
#endif


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

/* Author:  G. Jungman, modified by C. Krueger */

#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>

/* Factorise a general complex-valued M x N matrix A into
 *
 *   A = Q R
 *
 * where Q is unitary (M x M) and R is upper triangular (M x N).
 *
 * Q is stored as a packed set of Householder transformations in the
 * strict lower triangular part of the input matrix.
 *
 * R is stored in the diagonal and upper triangle of the input matrix.
 *
 * The full matrix for Q can be obtained as the product
 *
 *       Q = Q_k .. Q_2 Q_1
 *
 * where k = MIN(M,N) and
 *
 *       Q_i = (I - tau_i * v_i * v_i')
 *
 * and where v_i is a Householder vector
 *
 *       v_i = [1, m(i+1,i), m(i+2,i), ... , m(M,i)]
 *
 * This storage scheme is the same as in LAPACK.  */

int
gsl_linalg_complex_QR_decomp (gsl_matrix_complex * A, gsl_vector_complex * tau)
{
  const size_t M = A->size1;
  const size_t N = A->size2;

  if (tau->size != N)
    {
      GSL_ERROR ("size of tau must be N", GSL_EBADLEN);
    }
  else
    {
      size_t i;

      for (i = 0; i < GSL_MIN (M, N); i++)
        {
          /* Compute the Householder transformation to reduce the j-th
             column of the matrix to a multiple of the j-th unit vector */

          gsl_vector_complex_view c = gsl_matrix_complex_subcolumn (A, i, i, M - i);
          gsl_complex tau_i = gsl_linalg_complex_householder_transform (&(c.vector));

          gsl_vector_complex_set (tau, i, tau_i);

          /* apply the transformation to the remaining columns and update the norms */

          if (i + 1 < N)
            {
              gsl_matrix_complex_view m = gsl_matrix_complex_submatrix (A, i, i + 1, M - i, N - i - 1);
              gsl_complex tau_i_conj = gsl_complex_conjugate(tau_i);
              gsl_vector_complex_view work = gsl_vector_complex_subvector(tau, i + 1, N - i - 1);

              gsl_linalg_complex_householder_left(tau_i_conj, &(c.vector), &(m.matrix), &(work.vector));
            }
        }

      return GSL_SUCCESS;
    }
}

/* Solves the system A x = b using the QR factorisation,

 *  R x = Q^H b
 *
 * to obtain x.
 */

int
gsl_linalg_complex_QR_solve (const gsl_matrix_complex * QR, const gsl_vector_complex * tau,
                             const gsl_vector_complex * b, gsl_vector_complex * x)
{
  if (QR->size1 != QR->size2)
    {
      GSL_ERROR ("QR matrix must be square", GSL_ENOTSQR);
    }
  else if (QR->size1 != b->size)
    {
      GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
    }
  else if (QR->size2 != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else
    {
      /* copy x <- b */
      gsl_vector_complex_memcpy (x, b);

      /* solve for x */
      gsl_linalg_complex_QR_svx (QR, tau, x);

      return GSL_SUCCESS;
    }
}

/* Solves the system A x = b in place using the QR factorisation,

 *  R x = Q^H b
 *
 * to obtain x.
 */

int
gsl_linalg_complex_QR_svx (const gsl_matrix_complex * QR, const gsl_vector_complex * tau, gsl_vector_complex * x)
{

  if (QR->size1 != QR->size2)
    {
      GSL_ERROR ("QR matrix must be square", GSL_ENOTSQR);
    }
  else if (QR->size1 != x->size)
    {
      GSL_ERROR ("matrix size must match x/rhs size", GSL_EBADLEN);
    }
  else
    {
      /* compute rhs = Q^H b */
      gsl_linalg_complex_QR_QHvec (QR, tau, x);

      /* solve R x = rhs, storing x in-place */
      gsl_blas_ztrsv (CblasUpper, CblasNoTrans, CblasNonUnit, QR, x);

      return GSL_SUCCESS;
    }
}

/* Find the least squares solution to the overdetermined system
 *
 *   A x = b
 *
 * for M >= N using the QR factorization A = Q R.
 */

int
gsl_linalg_complex_QR_lssolve (const gsl_matrix_complex * QR, const gsl_vector_complex * tau,
                               const gsl_vector_complex * b, gsl_vector_complex * x,
                               gsl_vector_complex * residual)
{
  const size_t M = QR->size1;
  const size_t N = QR->size2;

  if (M < N)
    {
      GSL_ERROR ("QR matrix must have M>=N", GSL_EBADLEN);
    }
  else if (M != b->size)
    {
      GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
    }
  else if (N != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else if (M != residual->size)
    {
      GSL_ERROR ("matrix size must match residual size", GSL_EBADLEN);
    }
  else
    {
      gsl_matrix_complex_const_view R = gsl_matrix_complex_const_submatrix (QR, 0, 0, N, N);
      gsl_vector_complex_view c = gsl_vector_complex_subvector(residual, 0, N);

      gsl_vector_complex_memcpy(residual, b);

      /* compute rhs = Q^H b */
      gsl_linalg_complex_QR_QHvec (QR, tau, residual);

      /* solve R x = rhs */
      gsl_vector_complex_memcpy(x, &(c.vector));
      gsl_blas_ztrsv (CblasUpper, CblasNoTrans, CblasNonUnit, &(R.matrix), x);

      /* compute residual = b - A x = Q (Q^H b - R x) */
      gsl_vector_complex_set_zero(&(c.vector));
      gsl_linalg_complex_QR_Qvec(QR, tau, residual);

      return GSL_SUCCESS;
    }
}

/* form the product v := Q^H v from a QR factorized matrix */

int
gsl_linalg_complex_QR_QHvec (const gsl_matrix_complex * QR, const gsl_vector_complex * tau, gsl_vector_complex * v)
{
  const size_t M = QR->size1;
  const size_t N = QR->size2;

  if (tau->size != N)
    {
      GSL_ERROR ("size of tau must be N", GSL_EBADLEN);
    }
  else if (v->size != M)
    {
      GSL_ERROR ("vector size must be M", GSL_EBADLEN);
    }
  else
    {
      size_t i;

      /* compute Q^H v */

      for (i = 0; i < GSL_MIN (M, N); i++)
        {
          gsl_vector_complex_const_view h = gsl_matrix_complex_const_subcolumn (QR, i, i, M - i);
          gsl_vector_complex_view w = gsl_vector_complex_subvector (v, i, M - i);
          gsl_complex ti = gsl_vector_complex_get (tau, i);
          gsl_complex ti_conj = gsl_complex_conjugate(ti);
          gsl_linalg_complex_householder_hv (ti_conj, &(h.vector), &(w.vector));
        }

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_complex_QR_Qvec (const gsl_matrix_complex * QR, const gsl_vector_complex * tau, gsl_vector_complex * v)
{
  const size_t M = QR->size1;
  const size_t N = QR->size2;

  if (tau->size != GSL_MIN (M, N))
    {
      GSL_ERROR ("size of tau must be MIN(M,N)", GSL_EBADLEN);
    }
  else if (v->size != M)
    {
      GSL_ERROR ("vector size must be M", GSL_EBADLEN);
    }
  else
    {
      size_t i;

      /* compute Q v */

      for (i = GSL_MIN (M, N); i-- > 0;)
        {
          gsl_vector_complex_const_view c = gsl_matrix_complex_const_column (QR, i);
          gsl_vector_complex_const_view h = gsl_vector_complex_const_subvector (&(c.vector),
                                                                i, M - i);
          gsl_vector_complex_view w = gsl_vector_complex_subvector (v, i, M - i);
          gsl_complex ti = gsl_vector_complex_get (tau, i);
          /* we do not need the conjugate of ti here */
          gsl_linalg_complex_householder_hv (ti, &h.vector, &w.vector);
        }
      return GSL_SUCCESS;
    }
}

/* form the unitary matrix Q from the packed QR matrix */
int
gsl_linalg_complex_QR_unpack (const gsl_matrix_complex * QR, const gsl_vector_complex * tau,
                              gsl_matrix_complex * Q, gsl_matrix_complex * R)
{
  const size_t M = QR->size1;
  const size_t N = QR->size2;

  if (Q->size1 != M || Q->size2 != M)
    {
      GSL_ERROR ("Q matrix must be M x M", GSL_ENOTSQR);
    }
  else if (R->size1 != M || R->size2 != N)
    {
      GSL_ERROR ("R matrix must be M x N", GSL_ENOTSQR);
    }
  else if (tau->size != N)
    {
      GSL_ERROR ("size of tau must be N", GSL_EBADLEN);
    }
  else
    {
      size_t i, j;

      /* initialize Q to the identity */
      gsl_matrix_complex_set_identity (Q);

      for (i = GSL_MIN (M, N); i-- > 0;)
        {
          gsl_vector_complex_const_view c = gsl_matrix_complex_const_column (QR, i);
          gsl_vector_complex_const_view h = gsl_vector_complex_const_subvector (&c.vector, i, M - i);
          gsl_matrix_complex_view m = gsl_matrix_complex_submatrix (Q, i, i, M - i, M - i);
          gsl_complex ti = gsl_vector_complex_get (tau, i);
          gsl_vector_complex_view work = gsl_matrix_complex_subcolumn(R, 0, 0, M - i);

          /* we do not need the conjugate of ti here */
          gsl_linalg_complex_householder_left (ti, &h.vector, &m.matrix, &work.vector);
        }

      /* form the right triangular matrix R from a packed QR matrix */

      for (i = 0; i < M; i++)
        {
          for (j = 0; j < i && j < N; j++)
            gsl_matrix_complex_set (R, i, j, GSL_COMPLEX_ZERO);

          for (j = i; j < N; j++)
            gsl_matrix_complex_set (R, i, j, gsl_matrix_complex_get (QR, i, j));
        }

      return GSL_SUCCESS;
    }
}


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

double f (double x, void *params);
void integral_gen (int nmin, int nmax, double vals[]);

double f (double x, void *params)
{
       double n = *(double *) params;
       double f = exp (-x) *pow (x, n);

       return f;
}

void integral_gen (int nmin, int nmax, double vals[])
{
       double result;
       double error;
       size_t np = 1000;
       gsl_integration_workspace *w = gsl_integration_workspace_alloc (np);
       gsl_function F;

       for (double n = nmin; n <= nmax; n++)
       {
            F.function = &f;
            F.params = &n;

            gsl_integration_qag( &F, 0., 1., 0., 1.e-7, np, GSL_INTEG_GAUSS15, w, &result, &error);
            vals[(int) n] = result;
       }
}


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "compearth.h"
#ifdef COMPEARTH_USE_MKL
#ifdef __clang__
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wreserved-id-macro"
#pragma clang diagnostic ignored "-Wstrict-prototypes"
#endif
#include <mkl_cblas.h>
#ifdef __clang__
#pragma clang diagnostic pop
#endif
#else
#include <cblas.h>
#endif

/*!
 * @brief Converts v-w coordinates to lune coordinates.
 *
 * @param[in] nv      Number of v coordinates.
 * @param[in] v       v coordinates s.t. \f$ v \in [-1/3, 1/3] \f$.  This
 *                    an array of dimension [nv].
 * @param[in] nw      Number of w coordinates.
 * @param[in] w       w coordinates (similar to lune latitudes) s.t.
 *                    \f$ w \in [-3\pi/8, 3/pi/8] \f$.  This is an
 *                    array of dimension [nw].
 *
 * @param[out] gamma  Longitude (degrees) \f$ \gamma \in [-30,30] \f$.
 *                    This is an array of dimension [nv].
 * @param[out] delta  lune latitude (degrees) \f$ \delta \in [-90,90] \f$.
 *                    This is an array of dimension [nw].
 *
 * @result 0 indicates success.
 *
 * @author Carl Tape and converted to C by Ben Baker
 *
 * @date 2016
 *
 * @copyright MIT
 *
 */
int compearth_rect2lune(const int nv, const double *__restrict__ v,
                        const int nw, const double *__restrict__ w,
                        double *__restrict__ gamma,
                        double *__restrict__ delta)

{
    double u[CE_CHUNKSIZE] __attribute__((aligned(64)));
    int i, j, ierr, nuLoc;
/*
    const int maxit = 20;
    const int useHalley = 2;
    const double tol = 1.e-12;
*/
    const double pi38 = 3.0*M_PI/8.0;
    const double pi180i = 180.0/M_PI;
    ierr = 0;
    // convert v -> gamma
    compearth_v2gamma(nv, v, gamma);
    // convert latitude to colatitude
    for (i=0; i<nw; i=i+CE_CHUNKSIZE)
    {
        nuLoc = MIN(CE_CHUNKSIZE, nw - i);
        for (j=0; j<nuLoc; j++)
        {
            u[j] = pi38 - w[i+j];
        }
        // convert u -> beta
        ierr = compearth_u2beta(nuLoc, u, &delta[i]);
        //ierr = compearth_u2beta(nuLoc, maxit, useHalley, u, tol, &delta[i]);
        if (ierr != 0)
        {
            fprintf(stderr, "%s: Computing u2beta\n", __func__);
            return -1;
        }
    }
    // convert to gamma to degrees
    cblas_dscal(nv, pi180i, gamma, 1);
    // convert delta from colatitude to latitude and radians to degrees
    #pragma omp simd
    for (i=0; i<nw; i++)
    {
        delta[i] = 90.0 - pi180i*delta[i];
    }
    return ierr;
}


static char help[] = "Solves a 1D Poisson problem with DMDA and KSP.\n\n";

#include <petsc.h>

extern PetscErrorCode formdirichletlaplacian(DM, Mat);
extern PetscErrorCode formExactAndRHS(DM, Vec, Vec);

int main(int argc,char **args) {
  PetscErrorCode ierr;
  DM             da;
  KSP            ksp;
  Mat            A;
  Vec            b,u,uexact;
  PetscReal      errnorm;
  DMDALocalInfo  info;

  PetscInitialize(&argc,&args,(char*)0,help);
  ierr = DMDACreate1d(PETSC_COMM_WORLD,
               DM_BOUNDARY_NONE,
               9,1,1,NULL,
               &da); CHKERRQ(ierr);
  ierr = DMSetFromOptions(da); CHKERRQ(ierr);
  ierr = DMSetUp(da); CHKERRQ(ierr);
  ierr = DMDASetUniformCoordinates(da,0.0,1.0,-1.0,-1.0,-1.0,-1.0); CHKERRQ(ierr);
  ierr = DMCreateMatrix(da,&A);CHKERRQ(ierr);
  ierr = MatSetOptionsPrefix(A,"a_"); CHKERRQ(ierr);
  ierr = MatSetFromOptions(A); CHKERRQ(ierr);
  ierr = DMCreateGlobalVector(da,&b);CHKERRQ(ierr);
  ierr = VecDuplicate(b,&u); CHKERRQ(ierr);
  ierr = VecDuplicate(b,&uexact); CHKERRQ(ierr);
  ierr = formExactAndRHS(da,uexact,b); CHKERRQ(ierr);
  ierr = formdirichletlaplacian(da,A); CHKERRQ(ierr);
  ierr = KSPCreate(PETSC_COMM_WORLD,&ksp); CHKERRQ(ierr);
  ierr = KSPSetOperators(ksp,A,A); CHKERRQ(ierr);
  ierr = KSPSetFromOptions(ksp); CHKERRQ(ierr);
  ierr = KSPSolve(ksp,b,u); CHKERRQ(ierr);
  ierr = VecAXPY(u,-1.0,uexact); CHKERRQ(ierr);    // u <- u + (-1.0) uxact
  ierr = VecNorm(u,NORM_INFINITY,&errnorm); CHKERRQ(ierr);
  ierr = DMDAGetLocalInfo(da,&info);CHKERRQ(ierr);
  ierr = PetscPrintf(PETSC_COMM_WORLD,
             "on %d point grid:  error |u-uexact|_inf = %g\n",
             info.mx,errnorm); CHKERRQ(ierr);
  VecDestroy(&u);  VecDestroy(&uexact);  VecDestroy(&b);
  MatDestroy(&A);  KSPDestroy(&ksp);  DMDestroy(&da);
  return PetscFinalize();
}

PetscErrorCode formdirichletlaplacian(DM da, Mat A) {
    PetscErrorCode ierr;
    DMDALocalInfo  info;
    PetscInt       i;

    ierr = DMDAGetLocalInfo(da,&info); CHKERRQ(ierr);
    for (i=info.xs; i<info.xs+info.xm; i++) {
      PetscReal   v[3];
      PetscInt    row = i, col[3];
      PetscInt    ncols=0;
      if ( (i==0) || (i==info.mx-1) ) {
        col[ncols] = i;  v[ncols++] = 1.0;
      } else {
        col[ncols] = i;  v[ncols++] = 2.0;
        if (i-1>0) {
          col[ncols] = i-1;  v[ncols++] = -1.0;  }
        if (i+1<info.mx-1) {
          col[ncols] = i+1;  v[ncols++] = -1.0;  }
      }
      ierr = MatSetValues(A,1,&row,ncols,col,v,INSERT_VALUES); CHKERRQ(ierr);
    }
    ierr = MatAssemblyBegin(A,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    ierr = MatAssemblyEnd(A,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    return 0;
}

PetscErrorCode formExactAndRHS(DM da, Vec uexact, Vec b) {
  PetscErrorCode ierr;
  DMDALocalInfo  info;
  PetscInt       i;
  PetscReal      h, x, *ab, *auexact;

  ierr = DMDAGetLocalInfo(da,&info); CHKERRQ(ierr);
  h = 1.0/(info.mx-1);
  ierr = DMDAVecGetArray(da, b, &ab);CHKERRQ(ierr);
  ierr = DMDAVecGetArray(da, uexact, &auexact);CHKERRQ(ierr);
  for (i=info.xs; i<info.xs+info.xm; i++) {
    x = i * h;
    auexact[i] = x*x * (1.0 - x*x);
    if ( (i>0) && (i<info.mx-1) )
      ab[i] = h*h * (12.0 * x*x - 2.0);
    else
      ab[i] = 0.0;
  }
  ierr = DMDAVecRestoreArray(da, uexact, &auexact);CHKERRQ(ierr);
  ierr = DMDAVecRestoreArray(da, b, &ab); CHKERRQ(ierr);
  ierr = VecAssemblyBegin(b); CHKERRQ(ierr);
  ierr = VecAssemblyEnd(b); CHKERRQ(ierr);
  ierr = VecAssemblyBegin(uexact); CHKERRQ(ierr);
  ierr = VecAssemblyEnd(uexact); CHKERRQ(ierr);
  return 0;
}


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

#include <gsl/gsl_vector.h>
#include <gsl/gsl_multimin.h>

typedef struct {
    gsl_vector *alpha, *center;
    double offset;
} Params;

/*
 * Function implements a 2-D parabola centered on center, with minimum
 * at offset.
 * */
double parabola(const gsl_vector *v, void *params) {
    double x = gsl_vector_get(v, 0);
    double y = gsl_vector_get(v, 1);
    Params *p = (Params *) params;
    double a = gsl_vector_get(p->alpha, 0);
    double b = gsl_vector_get(p->alpha, 1);
    double c = p->offset;
    x -= gsl_vector_get(p->center, 0);
    y -= gsl_vector_get(p->center, 1);
    return a*x*x + b*y*y + c;
}

int main(int argc, char *argv[]) {
    const int nr_dims = 2, nr_iters = 100;
    /* Use Nelder & Mead simplex method */
    const gsl_multimin_fminimizer_type *T = gsl_multimin_fminimizer_nmsimplex2;
    gsl_multimin_fminimizer *multimin;
    gsl_multimin_function f;
    gsl_vector *x, *step_size;
    Params params;
    int iter_nr = 0, status;
    double size;

    /* Initialize the parameters */
    params.alpha = gsl_vector_alloc(nr_dims);
    gsl_vector_set(params.alpha, 0, 1.0);
    gsl_vector_set(params.alpha, 1, 2.0);
    params.center = gsl_vector_alloc(nr_dims);
    gsl_vector_set(params.center, 0, 1.5);
    gsl_vector_set(params.center, 1, -0.5);
    params.offset = 3.4;
    f.n = nr_dims;
    f.f = parabola;
    f.params = &params;

    /* Initiallize the initial position */
    x = gsl_vector_alloc(nr_dims);
    gsl_vector_set(x, 0, 2.3);
    gsl_vector_set(x, 1, -1.5);

    /* Initialize the simplex step sizes */
    step_size = gsl_vector_alloc(nr_dims);
    gsl_vector_set_all(step_size, 1.0);

    /* Initialize the minimizer */
    multimin = gsl_multimin_fminimizer_alloc(T, 2);
    gsl_multimin_fminimizer_set(multimin, &f, x, step_size);

    /* Do the iterations until success, or the maximum number of iterations
     * is exceeded. */
    do {
        iter_nr++;
        /* Do an iteration step */
        status = gsl_multimin_fminimizer_iterate(multimin);
        if (status)
            break;
        /* Determine the step size to use a a stopping criterion */
        size = gsl_multimin_fminimizer_size(multimin);
        status = gsl_multimin_test_size(size, 1.0e-4);
        if (status == GSL_SUCCESS) {
            printf("Minimum:\n");
            printf("%4d: f(%.5f, %.5f) = %.5f, last step size = %.5f\n",
                   iter_nr,
                   gsl_vector_get(multimin->x, 0),
                   gsl_vector_get(multimin->x, 1),
                   multimin->fval,
                   size);
            printf("Expected:\n");
            printf("      f(%.5f, %.5f) = %.5f\n",
                   gsl_vector_get(params.center, 0),
                   gsl_vector_get(params.center, 1),
                   params.offset);
        } else {
            fprintf(stderr, "%4d: f(%.5f, %.5f) = %.5f, step size = %.5f\n",
                    iter_nr,
                    gsl_vector_get(multimin->x, 0),
                    gsl_vector_get(multimin->x, 1),
                    multimin->fval,
                    size);
        }
    } while (status == GSL_CONTINUE && iter_nr < nr_iters);

    /* Free everything that was allocated */
    gsl_multimin_fminimizer_free(multimin);
    gsl_vector_free(x);
    gsl_vector_free(step_size);
    gsl_vector_free(params.alpha);
    gsl_vector_free(params.center);
    return EXIT_SUCCESS;
}


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

/* Find a minimum in x=[0,1] of the interpolating quadratic through
 * (0,f0) (1,f1) with derivative fp0 at x=0.  The interpolating
 * polynomial is q(x) = f0 + fp0 * z + (f1-f0-fp0) * z^2
 */

static double
interp_quad (double f0, double fp0, double f1, double zl, double zh)
{
  double fl = f0 + zl*(fp0 + zl*(f1 - f0 -fp0));
  double fh = f0 + zh*(fp0 + zh*(f1 - f0 -fp0));
  double c = 2 * (f1 - f0 - fp0);       /* curvature */

  double zmin = zl, fmin = fl;

  if (fh < fmin) { zmin = zh; fmin = fh; }

  if (c > 0)  /* positive curvature required for a minimum */
    {
      double z = -fp0 / c;      /* location of minimum */
      if (z > zl && z < zh) {
        double f = f0 + z*(fp0 + z*(f1 - f0 -fp0));
        if (f < fmin) { zmin = z; fmin = f; };
      }
    }

  return zmin;
}

/* Find a minimum in x=[0,1] of the interpolating cubic through
 * (0,f0) (1,f1) with derivatives fp0 at x=0 and fp1 at x=1.
 *
 * The interpolating polynomial is:
 *
 * c(x) = f0 + fp0 * z + eta * z^2 + xi * z^3
 *
 * where eta=3*(f1-f0)-2*fp0-fp1, xi=fp0+fp1-2*(f1-f0).
 */

static double
cubic (double c0, double c1, double c2, double c3, double z)
{
  return c0 + z * (c1 + z * (c2 + z * c3));
}

static void
check_extremum (double c0, double c1, double c2, double c3, double z,
                double *zmin, double *fmin)
{
  /* could make an early return by testing curvature >0 for minimum */

  double y = cubic (c0, c1, c2, c3, z);

  if (y < *fmin)
    {
      *zmin = z;  /* accepted new point*/
      *fmin = y;
    }
}

static double
interp_cubic (double f0, double fp0, double f1, double fp1, double zl, double zh)
{
  double eta = 3 * (f1 - f0) - 2 * fp0 - fp1;
  double xi = fp0 + fp1 - 2 * (f1 - f0);
  double c0 = f0, c1 = fp0, c2 = eta, c3 = xi;
  double zmin, fmin;
  double z0, z1;

  zmin = zl; fmin = cubic(c0, c1, c2, c3, zl);
  check_extremum (c0, c1, c2, c3, zh, &zmin, &fmin);

  {
    int n = gsl_poly_solve_quadratic (3 * c3, 2 * c2, c1, &z0, &z1);

    if (n == 2)  /* found 2 roots */
      {
        if (z0 > zl && z0 < zh)
          check_extremum (c0, c1, c2, c3, z0, &zmin, &fmin);
        if (z1 > zl && z1 < zh)
          check_extremum (c0, c1, c2, c3, z1, &zmin, &fmin);
      }
    else if (n == 1)  /* found 1 root */
      {
        if (z0 > zl && z0 < zh)
          check_extremum (c0, c1, c2, c3, z0, &zmin, &fmin);
      }
  }

  return zmin;
}

static double
interpolate (double a, double fa, double fpa,
             double b, double fb, double fpb, double xmin, double xmax,
             int order)
{
  /* Map [a,b] to [0,1] */
  double z, alpha, zmin, zmax;

  zmin = (xmin - a) / (b - a);
  zmax = (xmax - a) / (b - a);

  if (zmin > zmax)
    {
      double tmp = zmin;
      zmin = zmax;
      zmax = tmp;
    };

  if (order > 2 && GSL_IS_REAL(fpb)) {
    z = interp_cubic (fa, fpa * (b - a), fb, fpb * (b - a), zmin, zmax);
  } else {
    z = interp_quad (fa, fpa * (b - a), fb, zmin, zmax);
  }

  alpha = a + z * (b - a);

  return alpha;
}

/* recommended values from Fletcher are
   rho = 0.01, sigma = 0.1, tau1 = 9, tau2 = 0.05, tau3 = 0.5 */

static int
minimize (gsl_function_fdf * fn, double rho, double sigma,
          double tau1, double tau2, double tau3,
          int order, double alpha1, double *alpha_new)
{
  double f0, fp0, falpha, falpha_prev, fpalpha, fpalpha_prev, delta,
    alpha_next;
  double alpha = alpha1, alpha_prev = 0.0;
  double a, b, fa, fb, fpa, fpb;
  const size_t bracket_iters = 100, section_iters = 100;
  size_t i = 0;

  GSL_FN_FDF_EVAL_F_DF (fn, 0.0, &f0, &fp0);
  falpha_prev = f0;
  fpalpha_prev = fp0;

  /* Avoid uninitialized variables morning */
  a = 0.0; b = alpha;
  fa = f0; fb = 0.0;
  fpa = fp0; fpb = 0.0;

  /* Begin bracketing */

  while (i++ < bracket_iters)
    {
      falpha = GSL_FN_FDF_EVAL_F (fn, alpha);

      /* Fletcher's rho test */

      if (falpha > f0 + alpha * rho * fp0 || falpha >= falpha_prev)
        {
          a = alpha_prev; fa = falpha_prev; fpa = fpalpha_prev;
          b = alpha; fb = falpha; fpb = GSL_NAN;
          break;                /* goto sectioning */
        }

      fpalpha = GSL_FN_FDF_EVAL_DF (fn, alpha);

      /* Fletcher's sigma test */

      if (fabs (fpalpha) <= -sigma * fp0)
        {
          *alpha_new = alpha;
          return GSL_SUCCESS;
        }

      if (fpalpha >= 0)
        {
          a = alpha; fa = falpha; fpa = fpalpha;
          b = alpha_prev; fb = falpha_prev; fpb = fpalpha_prev;
          break;                /* goto sectioning */
        }

      delta = alpha - alpha_prev;

      {
        double lower = alpha + delta;
        double upper = alpha + tau1 * delta;

        alpha_next = interpolate (alpha_prev, falpha_prev, fpalpha_prev,
                             alpha, falpha, fpalpha, lower, upper, order);

      }

      alpha_prev = alpha;
      falpha_prev = falpha;
      fpalpha_prev = fpalpha;
      alpha = alpha_next;
    }

  /*  Sectioning of bracket [a,b] */

  while (i++ < section_iters)
    {
      delta = b - a;

      {
        double lower = a + tau2 * delta;
        double upper = b - tau3 * delta;

        alpha = interpolate (a, fa, fpa, b, fb, fpb, lower, upper, order);
      }

      falpha = GSL_FN_FDF_EVAL_F (fn, alpha);

      if ((a-alpha)*fpa <= GSL_DBL_EPSILON) {
        /* roundoff prevents progress */
        return GSL_ENOPROG;
      };

      if (falpha > f0 + rho * alpha * fp0 || falpha >= fa)
        {
          /*  a_next = a; */
          b = alpha; fb = falpha; fpb = GSL_NAN;
        }
      else
        {
          fpalpha = GSL_FN_FDF_EVAL_DF (fn, alpha);

          if (fabs(fpalpha) <= -sigma * fp0)
            {
              *alpha_new = alpha;
              return GSL_SUCCESS;  /* terminate */
            }

          if ( ((b-a) >= 0 && fpalpha >= 0) || ((b-a) <=0 && fpalpha <= 0))
            {
              b = a; fb = fa; fpb = fpa;
              a = alpha; fa = falpha; fpa = fpalpha;
            }
          else
            {
              a = alpha; fa = falpha; fpa = fpalpha;
            }
        }
    }

  return GSL_SUCCESS;
}


#pragma once

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

#include <MathLib/MathLibDll.h>

#include <MathLib/MathLib.h>
#include <MathLib/ThreeTuple.h>

#include <gsl/matrix/gsl_matrix.h>

#define MATRIX_AT(m, i, j) (*((m->data + ((i) * m->tda + (j)))))

class Vector3d;

/*====================================================================================================================================================================*
 | This class will be used to represent matrices of arbitrary sizes (m rows by n columns) that have elements of type double. The underlying data strucutre used by    |
 | this class is gsl's (Gnu Scientific Library) matrix class. This class also makes use of the ATLAS implementation of BLAS for some operations such as matrix-matrix |
 | multiplication. This class is meant to improve performance, not necessarily ease of use.                                                                           |
 *====================================================================================================================================================================*/

class MATHLIB_DECLSPEC Matrix {
friend class Vector;
friend class ParticleEngine;
protected:

	//this data structure holds all the matrix data
	gsl_matrix *matrix;

public:
	/**
		constructor - creates an m rows by n columns matrix that is not initialized to any particular values
	*/
	Matrix(int m, int n);

	/**
		default constructor
	*/
	Matrix();

	/**
		copy constructor - performs a deep copy of the matrix passed in as a parameter.
	*/
	Matrix(const Matrix& other);

	/**
		destructor.
	*/
	~Matrix();

	/**
		loads the matrix with all zero values.
	*/
	void loadZero();

	/**
		loads the matrix with 1's on the diagonal, 0's everywhere else - note: the matrix doesn't have to be square.
	*/
	void loadIdentity();

	/**
		this method sets the current matrix to a 3x3 matrix that is equal to the outer product of the vectors a and b
	*/
	void setToOuterproduct(const Vector3d& a, const Vector3d& b);

	/**
		this method resizes the current matrix to have m rows and n cols. If not enough space is allocated for it, then a new matrix of
		correct dimensions is allocated. There is no guarantee with regards to the data that is contained in the matrix after a resize operation.
	*/
	void resizeTo(int m, int n);

	/**
		copy operator - performs a deep copy of the matrix passed in as a parameter.
	*/
	Matrix& operator=(const Matrix &other);

	/**
		this method performs a shallow copy of the matrix that is passed in as a parameter.
	*/
	void shallowCopy(const Matrix& other, int startRow = 0, int startCol = 0, int endRow = -1, int endCol = -1);

	/**
		this method performs a deep copy of the matrix that is passed in as a paramerer.
	*/
	void deepCopy(const Matrix& other);

	/**
		Returns the number of columns
	*/
	int getColumnCount() const;

	/**
		Returns the number of rows
	*/
	int getRowCount()const;

	/**
		Multiplies each element in the current matrix by a constant
	*/
	void multiplyBy(const double val);

	/**
		This method sets the current matrix to be equal to one of the products: a * b, a'*b, a*b' or a'*b'.
		The values of transA and transB indicate which of the matrices are tranposed and which ones are not.
	*/
	void setToProductOf(const Matrix& a, const Matrix& b, bool transA = false, bool transB = false);

	/**
		This method computes the inverse of the matrix a and writes it over the current matrix. The implementation
		for the inverse of a matrix was obtained from Graphite. The parameter t is used as a threshold value for
		determinants, etc so that we still get a result for the inverse of our matrix even if it is very poorly conditioned.
	*/
	void setToInverseOf(const Matrix &a, double t = 0);

	/**
		This method prints the contents of the matrix - testing purpose only.
	*/
	void printMatrix() const;

	/**
		This method sets the current matrix to be a sub-matrix (starting at (i,j) and ending at (i+rows, j+cols)
		of the one that is passed in as a parameter - shallow copy only.
	*/
	void setToSubmatrix(const Matrix &a, int i, int j, int rows, int cols);

	/**
		This method returns a copy of the value of the matrix at (i,j)
	*/
	double get(int i, int j) const;

	/**
		This method sets the value of the matrix at (i,j) to newVal.
	*/
	void set(int i, int j, double newVal);

	/**
		This method is used to set the values in the matrix to the ones that are passed in the array of doubles.
		It is assumed that the array contains the right number of elements and that there is no space between consecutive rows (tda == nrCols).
	*/
	void setValues(double* vals);

	/**
		this method returns a pointer to its internal matrix data structure
	*/
	gsl_matrix* getMatrixPointer() const;

	/**
		Implement this operator to have a quick way of multiplying 3x3 matrices by vectors - used for dynamics for instance
	*/
	Vector3d operator * (const Vector3d &other);

	/**
		Implement a potentially faster function for 3x3 matrix - vector3d multiplication. Result and v need to be different!!
	*/
	void postMultiplyVector(const Vector3d& v, Vector3d& result);

	/**
		Still need to do (if need be): get row/col vector. Initialize a matrix from a vector so that we can create a 1xn matrix easily.
		make sure dgemv works properly, implement vector class, etc, swap, add, scale matrices, etc
	*/

	/**
		this method adds the matrix that is passed in as a parameter to the current matrix. In addition, it scales, both matrices by the two
		numbers that are passed in as parameters:
		*this = a * *this + b * other.
	*/
	void add(const Matrix& other, double scaleA = 1.0, double scaleB = 1.0);

	/**
		this method subtracts the matrix that is passed in as a parameter from the current matrix. In addition, it scales, both matrices by the two
		numbers that are passed in as parameters:
		*this = a * *this - b * other.
	*/
	void sub(const Matrix& other, double scaleA = 1.0, double scaleB = 1.0);

};

MATHLIB_DECLSPEC void testMatrixClass();


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

#include <stdlib.h>
#include <math.h>
#include <cblas.h>
#include <core/color-space.h>
#include <imgproc/histogram-equalization.h>

// ref: https://www.math.uci.edu/icamp/courses/math77c/demos/hist_eq.pdf
static void histeq(double const *src, double *dst, size_t area, unsigned range) {
    unsigned *cdf;
    size_t i;
    cdf = calloc(range, sizeof(unsigned));
    for (i = 0; i < area; ++i) {
        cdf[(int) (src[i] * range)] += 1;
    }
    for (i = 1; i < range; ++i) {
        cdf[i] += cdf[i - 1];
    }
    for (i = 0; i < area; ++i) {
        dst[i] = floor((double) (range - 1) * cdf[(int) (src[i] * range)] / area) / range;
    }
    free(cdf);
}

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

    /* TODO: impl. for multiple channels */
    channel = core_image_get_channel(src);
    g_return_val_if_fail(channel == 1, FALSE);

    src = g_object_ref(src);
    src_data = core_image_get_data(src);
    size = core_image_get_size(src);
    area = core_size_get_area(size);
    color_space = core_image_get_color_space(src);
    pixel_type = core_image_get_pixel_type(src);
    range = core_pixel_get_range(pixel_type);
    inv_range = 1.0 / range;

    /* cast to double and normalize pixel data to [0, 1] */
    if (core_pixel_is_double(pixel_type)) {
        src_data_cast = src_data;
    } else if (core_pixel_is_uint8(pixel_type)) {
        src_data_cast = g_malloc(sizeof(gdouble) * area);
        for (i = 0; i < area; ++i) {
            src_data_cast[i] = ((guint8 *) src_data)[i] * inv_range;
        }
    } else {
        g_return_val_if_fail(FALSE, FALSE);
    }
    dst_data_cast = g_malloc(sizeof(gdouble) * area);

    /* run the algorithm */
    histeq(src_data_cast, dst_data_cast, area, 256);

    /* cast back to uchar */
    if (core_pixel_is_double(pixel_type)) {
        /* exactly do nothing */
    } else if (core_pixel_is_uint8(pixel_type)) {
        /* reuse allocated memory */
        for (i = 0; i < area; ++i) {
            ((guint8 *) dst_data_cast)[i] = dst_data_cast[i] * 255.0;
        }
    } else {
        g_return_val_if_fail(FALSE, FALSE);
    }
    dst_data = dst_data_cast;
    if (core_pixel_is_uint8(pixel_type)) {
        /* release useless memory, can be removed for speed */
        dst_data = g_realloc(dst_data, sizeof(guint8) * area);
    }

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

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


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

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

#include <gsl/gsl_complex.h>
#include <gsl/gsl_dft_complex.h>
#include <gsl/gsl_errno.h>

#include "complex_internal.h"

int
gsl_dft_complex_forward (const double data[],
                         const size_t stride, const size_t n,
                         double result[])
{
  gsl_fft_direction sign = forward;
  int status = gsl_dft_complex (data, stride, n, result, sign);
  return status;
}

int
gsl_dft_complex_backward (const double data[],
                         const size_t stride, const size_t n,
                         double result[])
{
  gsl_fft_direction sign = backward;
  int status = gsl_dft_complex (data, stride, n, result, sign);
  return status;
}

int
gsl_dft_complex_inverse (const double data[],
                         const size_t stride, const size_t n,
                         double result[])
{
  gsl_fft_direction sign = backward;
  int status = gsl_dft_complex (data, stride, n, result, sign);

  /* normalize inverse fft with 1/n */

  {
    const ATOMIC norm = 1.0 / n;
    size_t i;
    for (i = 0; i < n; i++)
      {
        REAL(result,stride,i) *= norm;
        IMAG(result,stride,i) *= norm;
      }
  }
  return status;
}

int
gsl_dft_complex (const double data[],
                 const size_t stride, const size_t n,
                 double result[],
                 const gsl_fft_direction sign)
{

  size_t i, j, exponent;

  const double d_theta = 2.0 * ((int) sign) * M_PI / (double) n;

  /* FIXME: check that input length == output length and give error */

  for (i = 0; i < n; i++)
    {
      ATOMIC sum_real = 0;
      ATOMIC sum_imag = 0;

      exponent = 0;

      for (j = 0; j < n; j++)
        {
          double theta = d_theta * (double) exponent;
          /* sum = exp(i theta) * data[j] */

          ATOMIC w_real = cos (theta);
          ATOMIC w_imag = sin (theta);

          ATOMIC data_real = REAL(data,stride,j);
          ATOMIC data_imag = IMAG(data,stride,j);

          sum_real += w_real * data_real - w_imag * data_imag;
          sum_imag += w_real * data_imag + w_imag * data_real;

          exponent = (exponent + i) % n;
        }
      REAL(result,stride,i) = sum_real;
      IMAG(result,stride,i) = sum_imag;
    }

  return 0;
}


/* linalg/lu.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman, 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., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/* Author:  G. Jungman */

#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_permute_vector.h>
#include <gsl/gsl_blas.h>

#include "gsl_linalg.h"

#define REAL double

/* Factorise a general N x N matrix A into,
 *
 *   P A = L U
 *
 * where P is a permutation matrix, L is unit lower triangular and U
 * is upper triangular.
 *
 * L is stored in the strict lower triangular part of the input
 * matrix. The diagonal elements of L are unity and are not stored.
 *
 * U is stored in the diagonal and upper triangular part of the
 * input matrix.
 *
 * P is stored in the permutation p. Column j of P is column k of the
 * identity matrix, where k = permutation->data[j]
 *
 * signum gives the sign of the permutation, (-1)^n, where n is the
 * number of interchanges in the permutation.
 *
 * See Golub & Van Loan, Matrix Computations, Algorithm 3.4.1 (Gauss
 * Elimination with Partial Pivoting).
 */

int
gsl_linalg_LU_decomp (gsl_matrix * A, gsl_permutation * p, int *signum)
{
  if (A->size1 != A->size2)
    {
      GSL_ERROR ("LU decomposition requires square matrix", GSL_ENOTSQR);
    }
  else if (p->size != A->size1)
    {
      GSL_ERROR ("permutation length must match matrix size", GSL_EBADLEN);
    }
  else
    {
      const size_t N = A->size1;
      size_t i, j, k;

      *signum = 1;
      gsl_permutation_init (p);

      for (j = 0; j < N - 1; j++)
	{
	  /* Find maximum in the j-th column */

	  REAL ajj, max = fabs (gsl_matrix_get (A, j, j));
	  size_t i_pivot = j;

	  for (i = j + 1; i < N; i++)
	    {
	      REAL aij = fabs (gsl_matrix_get (A, i, j));

	      if (aij > max)
		{
		  max = aij;
		  i_pivot = i;
		}
	    }

	  if (i_pivot != j)
	    {
	      gsl_matrix_swap_rows (A, j, i_pivot);
	      gsl_permutation_swap (p, j, i_pivot);
	      *signum = -(*signum);
	    }

	  ajj = gsl_matrix_get (A, j, j);

	  if (ajj != 0.0)
	    {
	      for (i = j + 1; i < N; i++)
		{
		  REAL aij = gsl_matrix_get (A, i, j) / ajj;
		  gsl_matrix_set (A, i, j, aij);

		  for (k = j + 1; k < N; k++)
		    {
		      REAL aik = gsl_matrix_get (A, i, k);
		      REAL ajk = gsl_matrix_get (A, j, k);
		      gsl_matrix_set (A, i, k, aik - aij * ajk);
		    }
		}
	    }
	}

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_LU_solve (const gsl_matrix * LU, const gsl_permutation * p, const gsl_vector * b, gsl_vector * x)
{
  if (LU->size1 != LU->size2)
    {
      GSL_ERROR ("LU matrix must be square", GSL_ENOTSQR);
    }
  else if (LU->size1 != p->size)
    {
      GSL_ERROR ("permutation length must match matrix size", GSL_EBADLEN);
    }
  else if (LU->size1 != b->size)
    {
      GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
    }
  else if (LU->size2 != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else
    {
      /* Copy x <- b */

      gsl_vector_memcpy (x, b);

      /* Solve for x */

      gsl_linalg_LU_svx (LU, p, x);

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_LU_svx (const gsl_matrix * LU, const gsl_permutation * p, gsl_vector * x)
{
  if (LU->size1 != LU->size2)
    {
      GSL_ERROR ("LU matrix must be square", GSL_ENOTSQR);
    }
  else if (LU->size1 != p->size)
    {
      GSL_ERROR ("permutation length must match matrix size", GSL_EBADLEN);
    }
  else if (LU->size1 != x->size)
    {
      GSL_ERROR ("matrix size must match solution/rhs size", GSL_EBADLEN);
    }
  else
    {
      /* Apply permutation to RHS */

      gsl_permute_vector (p, x);

      /* Solve for c using forward-substitution, L c = P b */

      gsl_blas_dtrsv (CblasLower, CblasNoTrans, CblasUnit, LU, x);

      /* Perform back-substitution, U x = c */

      gsl_blas_dtrsv (CblasUpper, CblasNoTrans, CblasNonUnit, LU, x);

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_LU_refine (const gsl_matrix * A, const gsl_matrix * LU, const gsl_permutation * p, const gsl_vector * b, gsl_vector * x, gsl_vector * residual)
{
  if (A->size1 != A->size2)
    {
      GSL_ERROR ("matrix a must be square", GSL_ENOTSQR);
    }
  if (LU->size1 != LU->size2)
    {
      GSL_ERROR ("LU matrix must be square", GSL_ENOTSQR);
    }
  else if (A->size1 != LU->size2)
    {
      GSL_ERROR ("LU matrix must be decomposition of a", GSL_ENOTSQR);
    }
  else if (LU->size1 != p->size)
    {
      GSL_ERROR ("permutation length must match matrix size", GSL_EBADLEN);
    }
  else if (LU->size1 != b->size)
    {
      GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
    }
  else if (LU->size1 != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else
    {
      /* Compute residual, residual = (A * x  - b) */

      gsl_vector_memcpy (residual, b);
      gsl_blas_dgemv (CblasNoTrans, 1.0, A, x, -1.0, residual);

      /* Find correction, delta = - (A^-1) * residual, and apply it */

      gsl_linalg_LU_svx (LU, p, residual);
      gsl_blas_daxpy (-1.0, residual, x);

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_LU_invert (const gsl_matrix * LU, const gsl_permutation * p, gsl_matrix * inverse)
{
  size_t i, n = LU->size1;

  int status = GSL_SUCCESS;

  gsl_matrix_set_identity (inverse);

  for (i = 0; i < n; i++)
    {
      gsl_vector_view c = gsl_matrix_column (inverse, i);
      int status_i = gsl_linalg_LU_svx (LU, p, &(c.vector));

      if (status_i)
	status = status_i;
    }

  return status;
}

double
gsl_linalg_LU_det (gsl_matrix * LU, int signum)
{
  size_t i, n = LU->size1;

  double det = (double) signum;

  for (i = 0; i < n; i++)
    {
      det *= gsl_matrix_get (LU, i, i);
    }

  return det;
}

double
gsl_linalg_LU_lndet (gsl_matrix * LU)
{
  size_t i, n = LU->size1;

  double lndet = 0.0;

  for (i = 0; i < n; i++)
    {
      lndet += log (fabs (gsl_matrix_get (LU, i, i)));
    }

  return lndet;
}

int
gsl_linalg_LU_sgndet (gsl_matrix * LU, int signum)
{
  size_t i, n = LU->size1;

  int s = signum;

  for (i = 0; i < n; i++)
    {
      double u = gsl_matrix_get (LU, i, i);

      if (u < 0)
	{
	  s *= -1;
	}
      else if (u == 0)
	{
	  s = 0;
	  break;
	}
    }

  return s;
}


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

#include <gsl/gsl_errno.h>

#include "ccl.h"

/* ------- ROUTINE: ccl_linear spacing ------
INPUTS: [xmin,xmax] of the interval to be divided in N bins
OUTPUT: bin edges in range [xmin,xmax]
*/

double * ccl_linear_spacing(double xmin, double xmax, int N)
{
  double dx = (xmax-xmin)/(N -1.);

  double * x = malloc(sizeof(double)*N);
  if (x==NULL) {
    ccl_raise_warning(
      CCL_ERROR_MEMORY,
      "ERROR: Could not allocate memory for linear-spaced array (N=%d)\n", N);
    return x;
  }

  for (int i=0; i<N; i++) {
    x[i] = xmin + dx*i;
  }
  x[0]=xmin; //Make sure roundoff errors don't spoil edges
  x[N-1]=xmax; //Make sure roundoff errors don't spoil edges

  return x;
}

/* ------- ROUTINE: ccl_linlog spacing ------
 * INPUTS: [xminlog,xmax] of the interval to be divided in bins
 *         xmin when linear spacing starts
 *         Nlog number of logarithmically spaced bins
 *         Nlin number of linearly spaced bins
 * OUTPUT: bin edges in range [xminlog,xmax]
 * */

double * ccl_linlog_spacing(double xminlog, double xmin, double xmax, int Nlog, int Nlin)
{
  if (Nlog<2) {
    ccl_raise_warning(
      CCL_ERROR_LINLOGSPACE,
      "ERROR: Cannot make log-spaced array with %d points - need at least 2\n", Nlog);
    return NULL;
  }

  if (!(xminlog>0 && xmin>0)) {
    ccl_raise_warning(
      CCL_ERROR_LINLOGSPACE,
      "ERROR: Cannot make log-spaced array xminlog or xmin non-positive (had %le, %le)\n", xminlog, xmin);
    return NULL;
  }

  if (xminlog>xmin){
    ccl_raise_warning(CCL_ERROR_LINLOGSPACE, "ERROR: xminlog must be smaller as xmin");
    return NULL;
  }

  if (xmin>xmax){
    ccl_raise_warning(CCL_ERROR_LINLOGSPACE, "ERROR: xmin must be smaller as xmax");
    return NULL;
  }

  double * x = malloc(sizeof(double)*(Nlin+Nlog-1));
  if (x==NULL) {
    ccl_raise_warning(
      CCL_ERROR_MEMORY,
      "ERROR: Could not allocate memory for array of size (Nlin+Nlog-1)=%d)\n", (Nlin+Nlog-1));
    return x;
  }

  double dx = (xmax-xmin)/(Nlin -1.);
  double log_xchange = log(xmin);
  double log_xmin = log(xminlog);
  double dlog_x = (log_xchange - log_xmin) /  (Nlog-1.);

  for (int i=0; i<Nlin+Nlog-1; i++) {
    if (i<Nlog)
        x[i] = exp(log_xmin + dlog_x*i);
    if (i>=Nlog)
        x[i] = xmin + dx*(i-Nlog+1);
  }

  x[0]=xminlog; //Make sure roundoff errors don't spoil edges
  x[Nlog-1]=xmin; //Make sure roundoff errors don't spoil edges
  x[Nlin+Nlog-2]=xmax; //Make sure roundoff errors don't spoil edges

  return x;
}

/* ------- ROUTINE: ccl_log spacing ------
INPUTS: [xmin,xmax] of the interval to be divided logarithmically in N bins
TASK: divide an interval in N logarithmic bins
OUTPUT: bin edges in range [xmin,xmax]
*/

double * ccl_log_spacing(double xmin, double xmax, int N)
{
  if (N<2) {
    ccl_raise_warning(
      CCL_ERROR_LOGSPACE,
      "ERROR: Cannot make log-spaced array with %d points - need at least 2\n", N);
    return NULL;
  }

  if (!(xmin>0 && xmax>0)) {
    ccl_raise_warning(
      CCL_ERROR_LOGSPACE,
      "ERROR: Cannot make log-spaced array xmax or xmax non-positive (had %le, %le)\n", xmin, xmax);
    return NULL;
  }

  double log_xmax = log(xmax);
  double log_xmin = log(xmin);
  double dlog_x = (log_xmax - log_xmin) /  (N-1.);

  double * x = malloc(sizeof(double)*N);
  if (x==NULL) {
    ccl_raise_warning(
      CCL_ERROR_MEMORY,
      "ERROR: Could not allocate memory for log-spaced array (N=%d)\n", N);
    return x;
  }

  double xratio = exp(dlog_x);
  x[0] = xmin; //Make sure roundoff errors don't spoil edges
  for (int i=1; i<N-1; i++) {
    x[i] = x[i-1] * xratio;
  }
  x[N-1]=xmax; //Make sure roundoff errors don't spoil edges

  return x;
}

//Spline creator
//n     -> number of points
//x     -> x-axis
//y     -> f(x)-axis
//y0,yf -> values of f(x) to use beyond the interpolation range
SplPar *ccl_spline_init(int n,double *x,double *y,double y0,double yf)
{
  SplPar *spl=malloc(sizeof(SplPar));
  if(spl==NULL)
    return NULL;

  spl->intacc=gsl_interp_accel_alloc();
  spl->spline=gsl_spline_alloc(gsl_interp_cspline,n);
  int parstatus=gsl_spline_init(spl->spline,x,y,n);
  if(parstatus) {
    gsl_interp_accel_free(spl->intacc);
    gsl_spline_free(spl->spline);
    return NULL;
  }

  spl->x0=x[0];
  spl->xf=x[n-1];
  spl->y0=y0;
  spl->yf=yf;

  return spl;
}

//Evaluates spline at x checking for bound errors
double ccl_spline_eval(double x,SplPar *spl)
{
  if(x<=spl->x0)
    return spl->y0;
  else if(x>=spl->xf)
    return spl->yf;
  else {
    double y;
    int stat=gsl_spline_eval_e(spl->spline,x,spl->intacc,&y);
    if (stat!=GSL_SUCCESS) {
      ccl_raise_gsl_warning(stat, "ccl_utils.c: ccl_splin_eval():");
      return NAN;
    }
    return y;
  }
}

#define CCL_GAMMA1 2.6789385347077476336556 //Gamma(1/3)
#define CCL_GAMMA2 1.3541179394264004169452 //Gamma(2/3)
#define CCL_ROOTPI12 21.269446210866192327578 //12*sqrt(pi)
double ccl_j_bessel(int l,double x)
{
  double jl;
  double ax=fabs(x);
  double ax2=x*x;
  if(l<0) {
    fprintf(stderr,"CosmoMas: l>0 for Bessel function");
    exit(1);
  }

  if(l<7) {
    if(l==0) {
      if(ax<0.1) jl=1-ax2*(1-ax2/20.)/6.;
      else jl=sin(x)/x;
    }
    else if(l==1) {
      if(ax<0.2) jl=ax*(1-ax2*(1-ax2/28)/10)/3;
      else jl=(sin(x)/ax-cos(x))/ax;
    }
    else if(l==2) {
      if(ax<0.3) jl=ax2*(1-ax2*(1-ax2/36)/14)/15;
      else jl=(-3*cos(x)/ax-sin(x)*(1-3/ax2))/ax;
    }
    else if(l==3) {
      if(ax<0.4)
	jl=ax*ax2*(1-ax2*(1-ax2/44)/18)/105;
      else
	jl=(cos(x)*(1-15/ax2)-sin(x)*(6-15/ax2)/ax)/ax;
    }
    else if(l==4) {
      if(ax<0.6)
	jl=ax2*ax2*(1-ax2*(1-ax2/52)/22)/945;
      else
	jl=(sin(x)*(1-(45-105/ax2)/ax2)+cos(x)*(10-105/ax2)/ax)/ax;
    }
    else if(l==5) {
      if(ax<1.0)
	jl=ax2*ax2*ax*(1-ax2*(1-ax2/60)/26)/10395;
      else {
	jl=(sin(x)*(15-(420-945/ax2)/ax2)/ax-
	    cos(x)*(1-(105-945/ax2)/ax2))/ax;
      }
    }
    else {
      if(ax<1.0)
	jl=ax2*ax2*ax2*(1-ax2*(1-ax2/68)/30)/135135;
      else {
	jl=(sin(x)*(-1+(210-(4725-10395/ax2)/ax2)/ax2)+
	    cos(x)*(-21+(1260-10395/ax2)/ax2)/ax)/ax;
      }
    }
  }
  else {
    double nu=l+0.5;
    double nu2=nu*nu;

    if(ax<1.0E-40) jl=0;
    else if((ax2/l)<0.5) {
      jl=(exp(l*log(ax/nu)-M_LN2+nu*(1-M_LN2)-(1-(1-3.5/nu2)/(30*nu2))/(12*nu))/nu)*
	(1-ax2/(4*nu+4)*(1-ax2/(8*nu+16)*(1-ax2/(12*nu+36))));
    }
    else if((l*l/ax)<0.5) {
      double beta=ax-0.5*M_PI*(l+1);
      jl=(cos(beta)*(1-(nu2-0.25)*(nu2-2.25)/(8*ax2)*(1-(nu2-6.25)*(nu2-12.25)/(48*ax2)))-
	  sin(beta)*(nu2-0.25)/(2*ax)*(1-(nu2-2.25)*(nu2-6.25)/(24*ax2)*
				       (1-(nu2-12.25)*(nu2-20.25)/(80*ax2))))/ax;
    }
    else {
      double l3=pow(nu,0.325);
      if(ax<nu-1.31*l3) {
	double cosb=nu/ax;
	double sx=sqrt(nu2-ax2);
	double cotb=nu/sx;
	double secb=ax/nu;
	double beta=log(cosb+sx/ax);
	double cot3b=cotb*cotb*cotb;
	double cot6b=cot3b*cot3b;
	double sec2b=secb*secb;
	double expterm=((2+3*sec2b)*cot3b/24
			-((4+sec2b)*sec2b*cot6b/16
			  +((16-(1512+(3654+375*sec2b)*sec2b)*sec2b)*cot3b/5760
			    +(32+(288+(232+13*sec2b)*sec2b)*sec2b)*sec2b*cot6b/(128*nu))*
			  cot6b/nu)/nu)/nu;
	jl=sqrt(cotb*cosb)/(2*nu)*exp(-nu*beta+nu/cotb-expterm);
      }
      else if(ax>nu+1.48*l3) {
	double cosb=nu/ax;
	double sx=sqrt(ax2-nu2);
	double cotb=nu/sx;
	double secb=ax/nu;
	double beta=acos(cosb);
	double cot3b=cotb*cotb*cotb;
	double cot6b=cot3b*cot3b;
	double sec2b=secb*secb;
	double trigarg=nu/cotb-nu*beta-0.25*M_PI-
	  ((2+3*sec2b)*cot3b/24+(16-(1512+(3654+375*sec2b)*sec2b)*sec2b)*
	   cot3b*cot6b/(5760*nu2))/nu;
	double expterm=((4+sec2b)*sec2b*cot6b/16-
			(32+(288+(232+13*sec2b)*sec2b)*sec2b)*
			sec2b*cot6b*cot6b/(128*nu2))/nu2;
	jl=sqrt(cotb*cosb)/nu*exp(-expterm)*cos(trigarg);
      }
      else {
	double beta=ax-nu;
	double beta2=beta*beta;
	double sx=6/ax;
	double sx2=sx*sx;
	double secb=pow(sx,0.3333333333333333333333);
	double sec2b=secb*secb;

	jl=(CCL_GAMMA1*secb+beta*CCL_GAMMA2*sec2b
	    -(beta2/18-1.0/45.0)*beta*sx*secb*CCL_GAMMA1
	    -((beta2-1)*beta2/36+1.0/420.0)*sx*sec2b*CCL_GAMMA2
	    +(((beta2/1620-7.0/3240.0)*beta2+1.0/648.0)*beta2-1.0/8100.0)*sx2*secb*CCL_GAMMA1
	    +(((beta2/4536-1.0/810.0)*beta2+19.0/11340.0)*beta2-13.0/28350.0)*beta*sx2*sec2b*CCL_GAMMA2
	    -((((beta2/349920-1.0/29160.0)*beta2+71.0/583200.0)*beta2-121.0/874800.0)*
	      beta2+7939.0/224532000.0)*beta*sx2*sx*secb*CCL_GAMMA1)*sqrt(sx)/CCL_ROOTPI12;
      }
    }
  }
  if((x<0)&&(l%2!=0)) jl=-jl;

  return jl;
}

//Spline destructor
void ccl_spline_free(SplPar *spl)
{
  gsl_spline_free(spl->spline);
  gsl_interp_accel_free(spl->intacc);
  free(spl);
}


//STARTWHOLE
static char help[] = "Newton's method for a two-variable system.\n"
    "No analytical Jacobian.  Run with -snes_fd or -snes_mf.\n\n";

#include <petsc.h>

extern PetscErrorCode FormFunction(SNES, Vec, Vec, void*);

int main(int argc,char **argv) {
    PetscErrorCode ierr;
    SNES  snes;          // nonlinear solver context
    Vec   x, r;          // solution, residual vectors

    PetscInitialize(&argc,&argv,NULL,help);
    ierr = VecCreate(PETSC_COMM_WORLD,&x); CHKERRQ(ierr);
    ierr = VecSetSizes(x,PETSC_DECIDE,2); CHKERRQ(ierr);
    ierr = VecSetFromOptions(x); CHKERRQ(ierr);
    ierr = VecSet(x,1.0); CHKERRQ(ierr);
    ierr = VecDuplicate(x,&r); CHKERRQ(ierr);

    ierr = SNESCreate(PETSC_COMM_WORLD,&snes); CHKERRQ(ierr);
    ierr = SNESSetFunction(snes,r,FormFunction,NULL); CHKERRQ(ierr);
    ierr = SNESSetFromOptions(snes); CHKERRQ(ierr);
    ierr = SNESSolve(snes,NULL,x); CHKERRQ(ierr);
    ierr = VecView(x,PETSC_VIEWER_STDOUT_WORLD); CHKERRQ(ierr);

    VecDestroy(&x);  VecDestroy(&r);  SNESDestroy(&snes);
    return PetscFinalize();
}

PetscErrorCode FormFunction(SNES snes, Vec x, Vec F, void *ctx) {
    PetscErrorCode ierr;
    const double b = 2.0, *ax;
    double       *aF;

    ierr = VecGetArrayRead(x,&ax);CHKERRQ(ierr);
    ierr = VecGetArray(F,&aF);CHKERRQ(ierr);
    aF[0] = (1.0 / b) * PetscExpReal(b * ax[0]) - ax[1];
    aF[1] = ax[0] * ax[0] + ax[1] * ax[1] - 1.0;
    ierr = VecRestoreArrayRead(x,&ax);CHKERRQ(ierr);
    ierr = VecRestoreArray(F,&aF);CHKERRQ(ierr);
    return 0;
}
//ENDWHOLE


/* randist/erlang.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 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 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_sf_gamma.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

/* The sum of N samples from an exponential distribution gives an
   Erlang distribution

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

   for x = 0 ... +infty */

double
gsl_ran_erlang (const gsl_rng * r, const double a, const double n)
{
  return gsl_ran_gamma (r, n, a);
}

double
gsl_ran_erlang_pdf (const double x, const double a, const double n)
{
  if (x <= 0)
    {
      return 0 ;
    }
  else
    {
      double p;
      double lngamma = gsl_sf_lngamma (n);

      p = exp ((n - 1) * log (x/a) - x/a - lngamma) / a;
      return p;
    }
}


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

#include "demo_fn.h"
#include "demo_fn.c"

int
main (void)
{
  int status;
  int iter = 0, max_iter = 100;
  const gsl_root_fdfsolver_type *T;
  gsl_root_fdfsolver *s;
  double x0, x = 5.0, r_expected = sqrt (5.0);
  gsl_function_fdf FDF;
  struct quadratic_params params = {1.0, 0.0, -5.0};

  FDF.f = &quadratic;
  FDF.df = &quadratic_deriv;
  FDF.fdf = &quadratic_fdf;
  FDF.params = &params;

  T = gsl_root_fdfsolver_newton;
  s = gsl_root_fdfsolver_alloc (T);
  gsl_root_fdfsolver_set (s, &FDF, x);

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

  printf ("%-5s %10s %10s %10s\n",
          "iter", "root", "err", "err(est)");
  do
    {
      iter++;
      status = gsl_root_fdfsolver_iterate (s);
      x0 = x;
      x = gsl_root_fdfsolver_root (s);
      status = gsl_root_test_delta (x, x0, 0, 1e-3);

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

      printf ("%5d %10.7f %+10.7f %10.7f\n",
              iter, x, x - r_expected, x - x0);
    }
  while (status == GSL_CONTINUE && iter < max_iter);

  gsl_root_fdfsolver_free (s);
  return status;
}


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

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

int main() {
	gsl_matrix* A;
	gsl_matrix* U;
    gsl_matrix* V;
    gsl_vector* S;
    gsl_vector* work;
    int i, j;
    int M, N;

    M = N = 2;

    work = gsl_vector_alloc(N);
    S = gsl_vector_alloc(N);
    A = gsl_matrix_alloc(M, N);
    V = gsl_matrix_alloc(N, N);

    gsl_matrix_set(A, 0, 0, -0.93);
    gsl_matrix_set(A, 0, 1,  1.80);
    gsl_matrix_set(A, 1, 0,  0);
    gsl_matrix_set(A, 1, 1,  0);

    gsl_linalg_SV_decomp(A, V, S, work);
    U = A;

    printf("S = [");
    for (i=0; i<N; i++)
        printf(" %g", gsl_vector_get(S, i));
    printf(" ]\n");

    printf("U = [");
    for (j=0; j<M; j++) {
        if (j)
            printf("    [");
        for (i=0; i<N; i++)
            printf(" %g", gsl_matrix_get(U, j, i));
        printf(" ]\n");
    }

    printf("V = [");
    for (j=0; j<N; j++) {
        if (j)
            printf("    [");
        for (i=0; i<N; i++)
            printf(" %g", gsl_matrix_get(V, j, i));
        printf(" ]\n");
    }

    gsl_matrix_set(A, 0, 0, -0.93);
    gsl_matrix_set(A, 0, 1,  1.80);
    gsl_matrix_set(A, 1, 0,  0);
    gsl_matrix_set(A, 1, 1,  0);

    gsl_linalg_SV_decomp_jacobi(A, V, S);

    printf("S = [");
    for (i=0; i<N; i++)
        printf(" %g", gsl_vector_get(S, i));
    printf(" ]\n");

    printf("U = [");
    for (j=0; j<M; j++) {
        if (j)
            printf("    [");
        for (i=0; i<N; i++)
            printf(" %g", gsl_matrix_get(U, j, i));
        printf(" ]\n");
    }

    printf("V = [");
    for (j=0; j<N; j++) {
        if (j)
            printf("    [");
        for (i=0; i<N; i++)
            printf(" %g", gsl_matrix_get(V, j, i));
        printf(" ]\n");
    }

	return 0;
}


#include <stdlib.h>
#include <stdio.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_multiroots.h>
int rosenbrock_f (const gsl_vector * x, void *params,
		  gsl_vector * f);
int print_state (size_t iter, gsl_multiroot_fsolver * s);

struct rparams
  {
    double a;
    double b;
  };

int rosenbrock_f (const gsl_vector * x, void *params,
              gsl_vector * f)
{
  double a = ((struct rparams *) params)->a;
  double b = ((struct rparams *) params)->b;

  const double x0 = gsl_vector_get (x, 0);
  const double x1 = gsl_vector_get (x, 1);

  const double y0 = a * (1 - x0);
  const double y1 = b * (x1 - x0 * x0);

  gsl_vector_set (f, 0, y0);
  gsl_vector_set (f, 1, y1);

  return GSL_SUCCESS;
}

int print_state (size_t iter, gsl_multiroot_fsolver * s)
{
  printf ("iter = %3u x = % .3f % .3f "
          "f(x) = % .3e % .3e\n",
          iter,
          gsl_vector_get (s->x, 0),
          gsl_vector_get (s->x, 1),
          gsl_vector_get (s->f, 0),
          gsl_vector_get (s->f, 1));
  return 0;
}

int main (void)
{
  const gsl_multiroot_fsolver_type *T;
  gsl_multiroot_fsolver *s;

  int status;
  size_t iter = 0;

  const size_t n = 2;
  struct rparams p = {1.0, 10.0};
  gsl_multiroot_function f = {&rosenbrock_f, n, &p};

  double x_init[2] = {-10.0, -5.0};
  gsl_vector *x = gsl_vector_alloc (n);

  gsl_vector_set (x, 0, x_init[0]);
  gsl_vector_set (x, 1, x_init[1]);

  T = gsl_multiroot_fsolver_hybrids;
  s = gsl_multiroot_fsolver_alloc (T, 2);
  gsl_multiroot_fsolver_set (s, &f, x);

  print_state (iter, s);

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

      print_state (iter, s);

      if (status)   /* check if solver is stuck */
        break;

      status =
        gsl_multiroot_test_residual (s->f, 1e-7);
    }
  while (status == GSL_CONTINUE && iter < 1000);

  printf ("status = %s\n", gsl_strerror (status));

  gsl_multiroot_fsolver_free (s);
  gsl_vector_free (x);
  return 0;
}