/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2012-2017 by Synge Todo <wistaria@phys.s.u-tokyo.ac.jp>
*
* Distributed under the Boost Software License, Version 1.0. (See accompanying
* file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
*
*****************************************************************************/

#include <cblas.h>
#include <lapacke.h>
#include <rokko/cmatrix.h>

int imax(int x, int y) { return (x > y) ? x : y; }

int main(int argc, char** argv) {
  int n = 5;
  int i, j, info;
  double norm;
  double *w;
  double **a, **u, **t;

  if (argc > 1) n = atoi(argv[1]);

  /* generate matrix */
  a = alloc_dmatrix(n, n);
  for (j = 0; j < n; ++j) {
    for (i = 0; i < n; ++i) {
      mat_elem(a, i, j) = n - imax(i, j);
    }
  }
  printf("Matrix A: ");
  fprint_dmatrix(stdout, n, n, a);

  /* diagonalization */
  u = alloc_dmatrix(n, n);
  cblas_dcopy(n * n, mat_ptr(a), 1, mat_ptr(u), 1);
  w = alloc_dvector(n);
  info = LAPACKE_dsyev(LAPACK_COL_MAJOR, 'V', 'U', n, mat_ptr(u), n, vec_ptr(w));
  if (info != 0) {
    fprintf(stderr, "Error: dsyev fails with error code %d\n", info);
    exit(255);
  }
  printf("Eigenvalues: ");
  fprint_dvector(stdout, n, w);
  printf("Eigenvectors: ");
  fprint_dmatrix(stdout, n, n, u);

  /* orthogonality check */
  t = alloc_dmatrix(n, n);
  cblas_dgemm(CblasColMajor, CblasTrans, CblasNoTrans, n, n, n, 1, mat_ptr(u), n,
              mat_ptr(u), n, 0, mat_ptr(t), n);
  for (i = 0; i < n; ++i) mat_elem(t, i, i) -= 1;
  norm = LAPACKE_dlange(LAPACK_COL_MAJOR, 'F', n, n, mat_ptr(t), n);
  printf("|| U^t U - I || = %e\n", norm);
  if (norm > 1e-10) {
    fprintf(stderr, "Error: orthogonality check\n");
    exit(255);
  }

  /* eigenvalue check */
  cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, n, n, n, 1, mat_ptr(a), n,
              mat_ptr(u), n, 0, mat_ptr(t), n);
  cblas_dgemm(CblasColMajor, CblasTrans, CblasNoTrans, n, n, n, 1, mat_ptr(u), n,
              mat_ptr(t), n, 0, mat_ptr(a), n);
  for (i = 0; i < n; ++i) mat_elem(a, i, i) -= w[i];
  norm = LAPACKE_dlange(LAPACK_COL_MAJOR, 'F', n, n, mat_ptr(a), n);
  printf("|| U^t A U - diag(w) || = %e\n", norm);
  if (norm > 1e-10) {
    fprintf(stderr, "Error: eigenvalue check\n");
    exit(255);
  }

  free_dmatrix(a);
  free_dmatrix(u);
  free_dvector(w);
  free_dmatrix(t);
  return 0;
}


/*
 * Copyright (C) 2015  University of Oregon
 *
 * You may distribute under the terms of either the GNU General Public
 * License or the Apache License, as specified in the LICENSE file.
 *
 * For more information, see the LICENSE file.
 */

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

void vnmr_svd(double **a, int irow, int jcol, double w[], double **v)
{
   gsl_matrix *gA;
   gsl_matrix *gV;
   gsl_vector *gS;
   int i,j;
   int res;
   gA = gsl_matrix_alloc(irow,jcol);
   for (i=0; i<irow; i++)
      for (j=0; j<jcol; j++)
         gsl_matrix_set(gA, i, j, a[i+1][j+1]);

   gV = gsl_matrix_alloc(jcol,jcol);
   gS = gsl_vector_alloc(jcol);

   res = gsl_linalg_SV_decomp_jacobi(gA, gV, gS);

   for (i=0; i<irow; i++)
      for (j=0; j<jcol; j++)
         a[i+1][j+1] = gsl_matrix_get( gA, i, j);
   for (i=0; i<jcol; i++)
      for (j=0; j<jcol; j++)
         v[i+1][j+1] = gsl_matrix_get( gV, i, j);
   for (i=0; i<jcol; i++)
      w[i+1] = gsl_vector_get( gS, i);

   gsl_matrix_free(gA);
   gsl_matrix_free(gV);
   gsl_vector_free(gS);
}

void vnmr_svd_solve(double **u, double w[], double **v, int irow, int jcol, double b[], double x[])
{
   gsl_matrix *gU;
   gsl_matrix *gV;
   gsl_vector *gS;
   gsl_vector *gB;
   gsl_vector *gX;
   int i,j;
   int res;
   gU = gsl_matrix_alloc(irow,jcol);
   for (i=0; i<irow; i++)
      for (j=0; j<jcol; j++)
         gsl_matrix_set(gU, i, j, u[i+1][j+1]);

   gV = gsl_matrix_alloc(jcol,jcol);
   for (i=0; i<jcol; i++)
      for (j=0; j<jcol; j++)
         gsl_matrix_set( gV, i, j, v[i+1][j+1]);

   gS = gsl_vector_alloc(jcol);
   for (i=0; i<jcol; i++)
      gsl_vector_set( gS, i, w[i+1]);

   gB = gsl_vector_alloc(irow);
   for (i=0; i<irow; i++)
      gsl_vector_set( gB, i, b[i+1]);

   gX = gsl_vector_alloc(jcol);
   res = gsl_linalg_SV_solve(gU, gV, gS, gB, gX);

   for (i=0; i<jcol; i++)
      x[i+1] = gsl_vector_get( gX, i);

   gsl_matrix_free(gU);
   gsl_matrix_free(gV);
   gsl_vector_free(gS);
   gsl_vector_free(gB);
   gsl_vector_free(gX);
fprintf(stderr,"solve m=%d n=%d\n",irow, jcol);
for (i=1; i<=jcol; i++)
     fprintf(stderr," w[%d]: %g\n",i,w[i]);
for (i=0; i<jcol; i++)
     fprintf(stderr," x[%d]: %g\n",i+1,x[i+1]);
}


/* roots/bisection.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 Reid Priedhorsky, Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 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.
 */

/* bisection.c -- bisection root finding algorithm */

#include <config.h>

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

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

#include "roots.h"

typedef struct
  {
    double f_lower, f_upper;
  }
bisection_state_t;

static int bisection_init (void * vstate, gsl_function * f, double * root, double x_lower, double x_upper);
static int bisection_iterate (void * vstate, gsl_function * f, double * root, double * x_lower, double * x_upper);

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

  double f_lower, f_upper ;

  *root = 0.5 * (x_lower + x_upper) ;

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

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

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

  return GSL_SUCCESS;

}

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

  double x_bisect, f_bisect;

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

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

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

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

  x_bisect = (x_left + x_right) / 2.0;

  SAFE_FUNC_CALL (f, x_bisect, &f_bisect);

  if (f_bisect == 0.0)
    {
      *root = x_bisect;
      *x_lower = x_bisect;
      *x_upper = x_bisect;
      return GSL_SUCCESS;
    }

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

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

  return GSL_SUCCESS;
}

static const gsl_root_fsolver_type bisection_type =
{"bisection",                           /* name */
 sizeof (bisection_state_t),
 &bisection_init,
 &bisection_iterate};

const gsl_root_fsolver_type  * gsl_root_fsolver_bisection = &bisection_type;


/*
 * Simple example of NLopt usage from:
 * http://nlopt.readthedocs.io/en/latest/NLopt_Tutorial/
 */
#include <stdio.h>
#include <math.h>
#include <nlopt.h>

int count = 0;

//
// Objective function.
//
double myfunc(unsigned n, const double *x, double *grad, void *my_func_data)
{
    count++;
    if (grad) {
        grad[0] = 0.0;
        grad[1] = 0.5 / sqrt(x[1]);
    }
    return sqrt(x[1]);
}

typedef struct {
    double a, b;
} my_constraint_data;

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

int main()
{
    // Select algorithm.

    //
    // Global optimization
    //
//  nlopt_algorithm algorithm = NLOPT_GN_DIRECT;        // DIviding RECTangles algorithm for global optimization
//  nlopt_algorithm algorithm = NLOPT_GN_DIRECT_L;      // locally biased variant of DIRECT
//  nlopt_algorithm algorithm = NLOPT_GN_DIRECT_L_RAND; // slightly randomized variant of DIRECT-L
//  nlopt_algorithm algorithm = NLOPT_GN_CRS2_LM;       // Controlled Random Search with local mutation
//  nlopt_algorithm algorithm = NLOPT_G_MLSL;           // Multi-Level Single-Linkage
//  nlopt_algorithm algorithm = NLOPT_G_MLSL_LDS;       // modification of MLSL with low-discrepancy sequence
//  nlopt_algorithm algorithm = NLOPT_GD_STOGO;         // StoGO
//  nlopt_algorithm algorithm = NLOPT_GD_STOGO_RAND;    // randomized variant of StoGO
//  nlopt_algorithm algorithm = NLOPT_GN_ISRES;         // Improved Stochastic Ranking Evolution Strategy
//  nlopt_algorithm algorithm = NLOPT_GN_ESCH;          // evolutionary algorithm

    //
    // Local derivative-free optimization
    //
//  nlopt_algorithm algorithm = NLOPT_LN_COBYLA;        // Constrained Optimization BY Linear Approximations
//  nlopt_algorithm algorithm = NLOPT_LN_BOBYQA;        // BOBYQA, iterative quadratic approximation
//  nlopt_algorithm algorithm = NLOPT_LN_NEWUOA;        // NEWUOA, quadratic approximation
//  nlopt_algorithm algorithm = NLOPT_LN_NEWUOA_BOUND;  // variant of NEWUOA with bound constraints
//  nlopt_algorithm algorithm = NLOPT_LN_PRAXIS;        // PRincipal AXIS
//  nlopt_algorithm algorithm = NLOPT_LN_NELDERMEAD;    // Nelder-Mead Simplex
//  nlopt_algorithm algorithm = NLOPT_LN_SBPLX;         // re-implementation of Subplex algorithm

    //
    // Local gradient-based optimization
    //
    nlopt_algorithm algorithm = NLOPT_LD_MMA;           // Method of Moving Asymptotes
//  nlopt_algorithm algorithm = NLOPT_LD_CCSAQ;         // conservative convex separable approximation, quadratic
//  nlopt_algorithm algorithm = NLOPT_LD_SLSQP;         // Sequential Least-Squares Quadratic Programming
//  nlopt_algorithm algorithm = NLOPT_LD_LBFGS;         // Low-storage BFGS
//  nlopt_algorithm algorithm = NLOPT_LD_TNEWTON;       // Truncated Newton
//  nlopt_algorithm algorithm = NLOPT_LD_VAR1;          // shifted limited-memory variable-metric algorithm, rank-1
//  nlopt_algorithm algorithm = NLOPT_LD_VAR2;          // shifted limited-memory variable-metric algorithm, rank-2

    // Create optimizer object.
    // Select dimensionality.
    nlopt_opt opt = nlopt_create(algorithm, 2);

    // Set objective function.
    nlopt_set_min_objective(opt, myfunc, NULL);

    // Specify bounds.
    double lb[2] = { -HUGE_VAL, 0 }; // lower bounds
    nlopt_set_lower_bounds(opt, lb);
    //TODO: nlopt_set_upper_bounds();

    // Add constraints.
    my_constraint_data data[2] = { {2, 0}, {-1, 1} };
    nlopt_add_inequality_constraint(opt, myconstraint, &data[0], 1e-8);
    nlopt_add_inequality_constraint(opt, myconstraint, &data[1], 1e-8);

    // Stopping criteria, or. a relative tolerance on the optimization parameters.
    nlopt_set_xtol_rel(opt, 1e-4);

    // Perform the optimization, starting with some initial guess.
    double x[2] = { 1.234, 5.678 }; // initial guess
    double minf;                    // the minimum objective value upon return

    if (nlopt_optimize(opt, x, &minf) < 0) {
        printf("nlopt failed!\n");
    } else {
        printf("found minimum after %d evaluations\n", count);
        printf("found minimum at f(%g, %g) = %0.10g\n", x[0], x[1], minf);
    }

    nlopt_destroy(opt);
    return 0;
}


#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>
#include <gsl/vector/gsl_vector.h>

#include <MathLib/Matrix.h>

#define VECTOR_AT(v, i) (*((v->data + (i * v->tda ))))

/*====================================================================================================================================================================*
 | 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 Vector : public Matrix{
public:
	/**
		constructor - creates an n row vector that is not initialized to any particular values
	*/
	Vector(int n);

	/**
		default constructor
	*/
	Vector();

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

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

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

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

	/**
		this method performs a shallow copy of the Vector that is passed in as a parameter.
	*/
	void shallowCopy(const Matrix& other);

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

	/**
		This method sets the current vector to be equal to one of the products: A * b or A'*b.
		The value of transA indicates if A is transposed or not
	*/
	void setToProductOf(const Matrix& A, const Matrix& B, bool transA = false, bool transB = false);

	/**
		This method sets the current vector to be equal to one of the rows of A - shallow column only!
	*/
	void setToRow(const Matrix& A, int row, int start = 0, int howMany = -1);

	/**
		This method sets the current vector to be equal to one of the cols of A - shallow column only!
	*/
	void setToCol(const Matrix& A, int col, int start = 0, int howMany = -1);

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

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

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

	/**
		Computes the 2-norm squared for the current vector.
	*/
	inline double normSquared(){
		int r = getRowCount();
		double result = 0;
		for (int i=0;i<r;i++){
			double n = MATRIX_AT(matrix, i, 0) ;
			result += n*n;
		}
		return result;
	}

};

MATHLIB_DECLSPEC
void testVectorClass();


#ifndef VECTORBOOST
#define VECTORBOOST

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/qvm/mat_operations.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <lapacke.h>

#include <random>

#include <iostream>

class matrixBoost{
        private:

                boost::numeric::ublas::matrix<double> value;
		boost::numeric::ublas::range r1;
		boost::numeric::ublas::range r2;
		boost::numeric::ublas::matrix<double>valBack;
		bool subMatrixCopy = false;

		//http://www.crystalclearsoftware.com/cgi-bin/boost_wiki/wiki.pl?LU_Matrix_Inversion
		bool InvertMatrix(const boost::numeric::ublas::matrix<double>& input, boost::numeric::ublas::matrix<double>& inverse) {
			using namespace boost::numeric::ublas;
			typedef permutation_matrix<std::size_t> pmatrix;
			// create a working copy of the input
			matrix<double> A(input);
			// create a permutation matrix for the LU-factorization
			pmatrix pm(A.size1());
			// perform LU-factorization
			int res = lu_factorize(A,pm);
			if( res != 0 ) return false;
			// create identity matrix of "inverse"
			inverse.assign(boost::numeric::ublas::identity_matrix<double>(A.size1()));
			// backsubstitute to get the inverse
			lu_substitute(A, pm, inverse);
			return true;
		}

		void setSystemValue(boost::numeric::ublas::matrix<double> v){
			value = v;
		}

        public:
                matrixBoost(){}

		matrixBoost(int rowCount, int colCount){
			value = boost::numeric::ublas::zero_matrix<double>(rowCount,colCount);
		}

                matrixBoost(boost::numeric::ublas::matrix<double> value):value(value){}

                static matrixBoost Identity(int dimension,int dimension2){
                        return matrixBoost(boost::numeric::ublas::identity_matrix<double>(dimension,dimension2));
                }

                matrixBoost transpose(){
                        matrixBoost t(boost::numeric::ublas::trans(value));
                        return t;
                }

                matrixBoost inverse(){
			boost::numeric::ublas::matrix<double> inverse(value.size1(),value.size2());
			InvertMatrix(value,inverse);
                        matrixBoost t(inverse);
                        return t;
                }

                boost::numeric::ublas::matrix<double> getSystemValue() const{
                        return value;
                }

		matrixBoost & block(int i1, int i2, int l1, int l2){
			if(subMatrixCopy){
				std::swap(valBack,value);
			}
			subMatrixCopy = true;
			r1 = boost::numeric::ublas::range(i1,i1+l1);
			r2 = boost::numeric::ublas::range(i2,i2+l2);
			valBack = boost::numeric::ublas::project(value,r1,r2);
			std::swap(valBack,value);
			return *this;
		}

		matrixBoost &operator=( const matrixBoost other){
			if(subMatrixCopy){
				subMatrixCopy = false;
				boost::numeric::ublas::project(valBack,r1,r2) = other.getSystemValue();
				std::swap(valBack,value);
			}else{
				value = other.getSystemValue();
			}
			return *this;//better be no chaining of equal signs
		}

		static void lowerTriangulate(matrixBoost & val){
			boost::numeric::ublas::matrix<double> in= val.getSystemValue();
			int rows = in.size1();
			int cols = in.size2();
			for(int i = 0; i < rows; i++){
				int length = cols - i;
				boost::numeric::ublas::vector<double> v = (row(in,i));
				v = boost::numeric::ublas::subrange(v,i,cols);
				double mew = boost::numeric::ublas::norm_2(v);
				if(mew!=0){
					double beta  = v(0)  + (-2*(v(0) < 0)+1)*mew;
					v = v/beta;
				}
				v(0) = 1;
				boost::numeric::ublas::range t1(i,rows);
				boost::numeric::ublas::range t2(i,cols);
				boost::numeric::ublas::vector<double> w = -2.0/
					(boost::numeric::ublas::norm_2(v)*boost::numeric::ublas::norm_2(v))*
					(prod(project(in,t1,t2),v));

				boost::numeric::ublas::matrix<double> vM(v.size(),1);
				for(int i = 0; i < v.size(); i++){
					vM(i,0) = v(i);
				}
				boost::numeric::ublas::matrix<double> wM(w.size(),1);
				for(int i = 0; i < w.size(); i++){
					wM(i,0) = w(i);
				}
				project(in,t1,t2) = project(in,t1,t2) +  prod(wM,boost::numeric::ublas::trans(vM));
			}
			val.setSystemValue(in);
		}
};

class vectorBoost{
        private:
		boost::numeric::ublas::vector<double> value;
        public:
                vectorBoost(){
                }

                vectorBoost(boost::numeric::ublas::vector<double> value):value(value){}

                boost::numeric::ublas::vector<double> getSystemValue() const{
                        return value;
                }

		static vectorBoost randomVector(int dimension,uint32_t seed){
			std::mt19937 generator(seed);
			std::normal_distribution<double> distribution(0.0,1.0);
			boost::numeric::ublas::vector<double> tmp(dimension);
			for(int i = 0; i < dimension; i++){
				tmp(i) = distribution(generator);
			}
			return vectorBoost(tmp);
		}

		static vectorBoost solve(matrixBoost A, vectorBoost b){
			boost::numeric::ublas::matrix<double> Alocal = A.getSystemValue();
			boost::numeric::ublas::vector<double> blocal = b.getSystemValue();
			boost::numeric::ublas::permutation_matrix<size_t> pm(Alocal.size1());

			boost::numeric::ublas::lu_factorize(Alocal,pm);
			boost::numeric::ublas::lu_substitute(Alocal, pm, blocal);
			return vectorBoost(blocal);
		}
};

vectorBoost operator-(const vectorBoost &lhs,const vectorBoost &rhs){
        vectorBoost res(lhs.getSystemValue() - rhs.getSystemValue());
        return res;
};

vectorBoost operator+(const vectorBoost &lhs,const vectorBoost &rhs){
        vectorBoost res(lhs.getSystemValue() + rhs.getSystemValue());
        return res;
};

std::ostream& operator<<(std::ostream& stream, const vectorBoost & vec) {
	boost::numeric::ublas::vector<double> v = vec.getSystemValue();
	for(int i = 0; i < v.size()-1; i++){
		stream<<double(v(i))<<'\n';
	}
	stream<<v(v.size()-1);
	return stream;
}

matrixBoost operator*(const matrixBoost &lhs, const matrixBoost &rhs){
        matrixBoost res(boost::numeric::ublas::prod(lhs.getSystemValue(),rhs.getSystemValue()));
        return res;
}

matrixBoost operator+(const matrixBoost &lhs,const matrixBoost &rhs){
        matrixBoost res(lhs.getSystemValue() + rhs.getSystemValue());
        return res;
};

matrixBoost operator-(const matrixBoost &lhs,const matrixBoost &rhs){
        matrixBoost res(lhs.getSystemValue() - rhs.getSystemValue());
        return res;
};

std::ostream& operator<<(std::ostream& stream, const matrixBoost & mat) {
	boost::numeric::ublas::matrix<double> m = mat.getSystemValue();
	for(int i = 0; i < m.size1(); i++){
		for(int j = 0; j < m.size2(); j++){
			stream<<m(i,j)<<" ";
		}stream<<'\n';
	}
	return stream;
}

vectorBoost operator*(const matrixBoost &lhs, const vectorBoost &rhs){
        vectorBoost res(boost::numeric::ublas::prod(lhs.getSystemValue(),rhs.getSystemValue()));
        return res;
}

vectorBoost operator*(const vectorBoost &lhs, const matrixBoost &rhs){
        vectorBoost res(boost::numeric::ublas::prod(lhs.getSystemValue(),rhs.getSystemValue()));
        return res;
}
#endif


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

int main(void)
{
  double data[5] = {17.2, 18.1, 16.5, 18.3, 12.6};
  double mean, variance, largest, smallest;

  mean     = gsl_stats_mean(data, 1, 5);
  variance = gsl_stats_variance(data, 1, 5);
  largest  = gsl_stats_max(data, 1, 5);
  smallest = gsl_stats_min(data, 1, 5);

  printf ("The dataset is %g, %g, %g, %g, %g\n",
         data[0], data[1], data[2], data[3], data[4]);

  printf ("The sample mean is %g\n", mean);
  printf ("The estimated variance is %g\n", variance);
  printf ("The largest value is %g\n", largest);
  printf ("The smallest value is %g\n", smallest);
  return 0;
}


/* vectorviews.c */

// 8.4.13 Example programs for matrices of gsl-ref.pdf
// GNU GSL GNU Scientific Library reference book
// pp. 99,100

/*
  Compiling advice:
  gcc vectorviews.c -lgsl -lgslcblas -lm

 */

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

int main (void)
{
  size_t i,j;

  gsl_matrix *m = gsl_matrix_alloc(10,10);

  for (i = 0; i < 10; i++)
    for (j = 0; j < 10; j++)
      gsl_matrix_set (m, i, j, sin(i) + cos(j));

  for (j = 0; j < 10; j++)
    {
      gsl_vector_view column = gsl_matrix_column(m, j);
      double d;

      d = gsl_blas_dnrm2 (&column.vector);

      printf("matrix column %zu, norm = %g\n", j, d);
    }

  gsl_matrix_free(m);

  return 0;
}


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

int main(int argc, char *argv[])
{
    /* 0 + 2x - 3x^2 + 1x^3 */
    double p[] = {0, 2, -3, 1};
    double z[6];
    gsl_poly_complex_workspace *w = gsl_poly_complex_workspace_alloc(4);
    gsl_poly_complex_solve(p, 4, w, z);
    gsl_poly_complex_workspace_free(w);

    for(int i = 0; i < 3; ++i)
        printf("%.12f\n", z[2 * i]);

    return 0;
}


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

/* Author:  G. Jungman */

#ifndef __GSL_SF_COUPLING_H__
#define __GSL_SF_COUPLING_H__

#include <gsl/gsl_sf_result.h>

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

__BEGIN_DECLS

/* 3j Symbols:  / ja jb jc \
 *              \ ma mb mc /
 *
 * exceptions: GSL_EDOM, GSL_EOVRFLW
 */
int gsl_sf_coupling_3j_e(int two_ja, int two_jb, int two_jc,
                            int two_ma, int two_mb, int two_mc,
                            gsl_sf_result * result
                            );
double gsl_sf_coupling_3j(int two_ja, int two_jb, int two_jc,
                          int two_ma, int two_mb, int two_mc
                          );

/* 6j Symbols:  / ja jb jc \
 *              \ jd je jf /
 *
 * exceptions: GSL_EDOM, GSL_EOVRFLW
 */
int gsl_sf_coupling_6j_e(int two_ja, int two_jb, int two_jc,
                         int two_jd, int two_je, int two_jf,
                         gsl_sf_result * result
                         );
double gsl_sf_coupling_6j(int two_ja, int two_jb, int two_jc,
                          int two_jd, int two_je, int two_jf
                          );

/* Racah W coefficients:
 *
 *   W(a b c d; e f) = (-1)^{a+b+c+d} / a b e \
 *                                    \ d c f /
 *
 * exceptions: GSL_EDOM, GSL_EOVRFLW
 */
int gsl_sf_coupling_RacahW_e(int two_ja, int two_jb, int two_jc,
                             int two_jd, int two_je, int two_jf,
                             gsl_sf_result * result
                             );
double gsl_sf_coupling_RacahW(int two_ja, int two_jb, int two_jc,
                              int two_jd, int two_je, int two_jf
                              );

/* 9j Symbols:  / ja jb jc \
 *              | jd je jf |
 *              \ jg jh ji /
 *
 * exceptions: GSL_EDOM, GSL_EOVRFLW
 */
int gsl_sf_coupling_9j_e(int two_ja, int two_jb, int two_jc,
                         int two_jd, int two_je, int two_jf,
                         int two_jg, int two_jh, int two_ji,
                         gsl_sf_result * result
                         );
double gsl_sf_coupling_9j(int two_ja, int two_jb, int two_jc,
                          int two_jd, int two_je, int two_jf,
                          int two_jg, int two_jh, int two_ji
                          );

/* INCORRECT version of 6j Symbols:
 * This function actually calculates
 *              / ja jb je \
 *              \ jd jc jf /
 * It represents the original implementation,
 * which had the above permutation of the
 * arguments. This was wrong and confusing,
 * and I had to fix it. Sorry for the trouble.
 * [GJ] Tue Nov 26 12:53:39 MST 2002
 *
 * exceptions: GSL_EDOM, GSL_EOVRFLW
 */
#ifndef GSL_DISABLE_DEPRECATED
int gsl_sf_coupling_6j_INCORRECT_e(int two_ja, int two_jb, int two_jc,
                                   int two_jd, int two_je, int two_jf,
                                   gsl_sf_result * result
                                   );
double gsl_sf_coupling_6j_INCORRECT(int two_ja, int two_jb, int two_jc,
                                    int two_jd, int two_je, int two_jf
                                    );
#endif /* !GSL_DISABLE_DEPRECATED */

__END_DECLS

#endif /* __GSL_SF_COUPLING_H__ */


#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <lapacke.h>
#define ABS(a)     (((a) < 0) ? -(a) : (a))

int main()
{
  /*
   *	hbar^2/2m = 1
  */

  static double pi = M_PI;
  int n, npw;
  double v0, a, b, ecut, k;
  double *g, *e, *h, *wrk;
  int i, j, ij, m, lwork, info, p;
  char *V = "V";
  char *U = "U";
  FILE *out;

  /*  Input data
      Potential: V(x)=-V_0 for |x|<b/2, V(x)=0 for |x|>b/2
      Periodicity:    V(x+a)=V(x)
  */
  fprintf(stdout, "Parameters for potential well: V_0, a, b > ");
  scanf("%lf %lf %lf",&v0, &a, &b);
  if ( v0 <= 0 ||  a <= 0 || b <= 0 || a <= b )  {
    fprintf(stderr, "wrong input parameters\n");
    exit(1);
  }
  fprintf (stdout,"   V_0=%f, a0=%f, b=%f\n",v0,a,b);
  /*
    Plane waves basis set: G_n=n*2pi/a, \hbar^2/2m*G^2 < Ecut
  */
  fprintf(stdout, "Cutoff for plane waves: ecut > ");
  scanf("%lf",&ecut);
  if ( ecut <= 0) {
    fprintf(stderr, "wrong input parameter\n");
    exit(2);
  }
  /*
    Number of plane waves
  */
  npw = (int) ( sqrt ( ecut/pow( 2.0*pi/a, 2) ) + 0.5 );
  npw = 2*npw+1;
  fprintf (stdout,"   ecut = %f,  # of PWs=%d\n",ecut, npw);
  /*
    Assign values of  G_n: n=0,+1,-1,+2,-2, etc
  */
  n=100;
  g  = (double *) malloc ( npw * sizeof (double) );
  e  = (double *) malloc ( npw * sizeof (double) );
  h  = (double *) malloc ( n*n * sizeof (double) );
  wrk= (double *) malloc (3*npw* sizeof (double) );

  g[0] = 0.0;
  for ( i = 1; i < npw; i+=2 ) {
    g[i  ] = (i+1)*pi/a;
    g[i+1] =-(i+1)*pi/a;
  }
  for ( ij = 0; ij < n*n; ++ij ) {
    h[ij] = 0.0;
  }
  /*
    Loop on k-vectors: k runs from -pi/a to pi/a
  */
  out = fopen("bands.out", "w");
  for ( m =-n; m <= n; m++ ) {
    k = m*pi/n/a;
    /*
      Assign values of the matrix elements of the hamiltonian
      on the plane wave basis
    */
    ij = 0;
    for ( i = 0; i < npw; ++i ) {
      for ( j = 0; j < npw; ++j ) {
      /* NOTA BENE: the matrix h is a vector in the calling program,
         while dsyev expects a (pointer to a) fortran matrix.
         A fortran matrix is "simulated" in the following way:
         if h[ij] is the C array and  h(i,j) is a N*M Fortran matrix,
         h[ij] == h(i,j), where  ij = (j-1)*N + (i-1)  (i=1,N, j=1,M)
      */

	if ( i == j ) {
	  h[ij++] = (k+g[i])*(k+g[i]) - v0/a*b;
	} else {
	  h[ij++] = -v0/a * sin( (g[j]-g[i])*b/2.0 ) / (g[j]-g[i])*2.0;
	}
      }
    }
    /*
      Solution [expansion coefficients are stored into h(i,j)
      j=basis function index, i= eigenvalue index]
      (beware fortran-C reversed index convention!)
    */
    lwork = 3*npw;
    dsyev_ ( V, U, &npw, h, &npw, e, wrk, &lwork, &info );

    if ( info != 0) {
      fprintf(stderr, "H-matrix diagonalization failed\n");
      exit(3);
    }

    /*printf("%f ",k);
    for(p=0; p<npw; p++) printf("%f ",e[p]);printf("\n");*/

    fprintf(out,"%12.6f ",k);
    for(p=0; p<npw; p++) fprintf(out,"%10.6f ",e[p]);
    fprintf(out,"\n");

  }
  fclose(out);

  return 0;

}


#include "ccl_utils.h"
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include "ccl_params.h"
#include "ccl_error.h"
#include <gsl/gsl_errno.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) {
    fprintf(stderr, "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_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) {
    fprintf(stderr, "ERROR: Cannot make log-spaced array with %d points - need at least 2\n", N);
    return NULL;
  }

  if (!(xmin>0 && xmax>0)) {
    fprintf(stderr, "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) {
    fprintf(stderr, "ERROR: Could not allocate memory for log-spaced array (N=%d)\n", N);
    return x;
  }

  for (int i=0; i<N; i++) {
    x[i] = exp(log_xmin + dlog_x*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;
}

//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_exception(stat,"ccl_utils.c: ccl_splin_eval(): gsl error\n");
      return NAN;
    }
    return y;
  }
}

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


/* Compile with:
export LDLIBS="`pkg-config --libs gsl`"
export CFLAGS="`pkg-config --cflags gsl` -g -Wall -std=gnu11 -O3"
make gsl_erf
*/
#include <gsl/gsl_cdf.h>
#include <stdio.h>

int main(){
    double bottom_tail = gsl_cdf_gaussian_P(-1.96, 1);
    printf("Area between [-1.96, 1.96]: %g\n", 1-2*bottom_tail);
}


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

#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_spmatrix.h>
#include <gsl/gsl_splinalg.h>

int
main()
{
  const size_t N = 100;                       /* number of grid points */
  const size_t n = N - 2;                     /* subtract 2 to exclude boundaries */
  const double h = 1.0 / (N - 1.0);           /* grid spacing */
  gsl_spmatrix *A = gsl_spmatrix_alloc(n ,n); /* triplet format */
  gsl_spmatrix *C;                            /* compressed format */
  gsl_vector *f = gsl_vector_alloc(n);        /* right hand side vector */
  gsl_vector *u = gsl_vector_alloc(n);        /* solution vector */
  size_t i;

  /* construct the sparse matrix for the finite difference equation */

  /* construct first row */
  gsl_spmatrix_set(A, 0, 0, -2.0);
  gsl_spmatrix_set(A, 0, 1, 1.0);

  /* construct rows [1:n-2] */
  for (i = 1; i < n - 1; ++i)
    {
      gsl_spmatrix_set(A, i, i + 1, 1.0);
      gsl_spmatrix_set(A, i, i, -2.0);
      gsl_spmatrix_set(A, i, i - 1, 1.0);
    }

  /* construct last row */
  gsl_spmatrix_set(A, n - 1, n - 1, -2.0);
  gsl_spmatrix_set(A, n - 1, n - 2, 1.0);

  /* scale by h^2 */
  gsl_spmatrix_scale(A, 1.0 / (h * h));

  /* construct right hand side vector */
  for (i = 0; i < n; ++i)
    {
      double xi = (i + 1) * h;
      double fi = -M_PI * M_PI * sin(M_PI * xi);
      gsl_vector_set(f, i, fi);
    }

  /* convert to compressed column format */
  C = gsl_spmatrix_ccs(A);

  /* now solve the system with the GMRES iterative solver */
  {
    const double tol = 1.0e-6;  /* solution relative tolerance */
    const size_t max_iter = 10; /* maximum iterations */
    const gsl_splinalg_itersolve_type *T = gsl_splinalg_itersolve_gmres;
    gsl_splinalg_itersolve *work =
      gsl_splinalg_itersolve_alloc(T, n, 0);
    size_t iter = 0;
    double residual;
    int status;

    /* initial guess u = 0 */
    gsl_vector_set_zero(u);

    /* solve the system A u = f */
    do
      {
        status = gsl_splinalg_itersolve_iterate(C, f, tol, u, work);

        /* print out residual norm ||A*u - f|| */
        residual = gsl_splinalg_itersolve_normr(work);
        fprintf(stderr, "iter %zu residual = %.12e\n", iter, residual);

        if (status == GSL_SUCCESS)
          fprintf(stderr, "Converged\n");
      }
    while (status == GSL_CONTINUE && ++iter < max_iter);

    /* output solution */
    for (i = 0; i < n; ++i)
      {
        double xi = (i + 1) * h;
        double u_exact = sin(M_PI * xi);
        double u_gsl = gsl_vector_get(u, i);

        printf("%f %.12e %.12e\n", xi, u_gsl, u_exact);
      }

    gsl_splinalg_itersolve_free(work);
  }

  gsl_spmatrix_free(A);
  gsl_spmatrix_free(C);
  gsl_vector_free(f);
  gsl_vector_free(u);

  return 0;
} /* main() */


#include "linearwaves.h"
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>

#define FREAD_CHECK(i,j) ((i) >= (j)) ? (void) 0 : printf("fread returned read fewer items than expected (%zu < %zu) at line %d in %s of %s\n",i,j,__LINE__,__func__,__FILE__)

void interpolation(double *xd, double *yd, int nd, double *x, double *y, double *dy, double *d2y, int n) {
    gsl_interp_accel *acc = gsl_interp_accel_alloc ();
    gsl_spline *spline = gsl_spline_alloc (gsl_interp_cspline, nd);

    gsl_spline_init (spline, xd, yd, nd);

    int i;
    for(i=0;i<n;i++) {
        y[i] = gsl_spline_eval(spline,x[i],acc);
        dy[i] = gsl_spline_eval_deriv(spline,x[i],acc);
        d2y[i] = gsl_spline_eval_deriv2(spline,x[i],acc);
    }
    gsl_spline_free (spline);
    gsl_interp_accel_free (acc);

    return;
}

void interpolate_onto_grid(double *xd, double *yd, int ndata, double *lr, double *ynew, double *dlydlr, double *d2lydlr, int n,int add_flag) {
    int i;
    int jstart = 0;
    int jend = n-1;

    for(i=0;i<n;i++) {
        if (lr[i] >= xd[0]) {
            jstart = i;
            break;
        }
    }
    for(i=jstart;i<n;i++) {
        if (lr[i] >= xd[ndata-1]) {
            if (lr[i] == xd[ndata-1]) {
                jend = i;
            }
            else {
                jend = i-1;
            }
            break;
        }
    }
    int n_interp = jend-jstart +1;

    interpolation(xd,yd,ndata,&lr[jstart],&ynew[jstart],&dlydlr[jstart],&d2lydlr[jstart],n_interp);

    for(i=0;i<jstart;i++) {
        ynew[i] = ynew[jstart] + dlydlr[jstart]*(lr[i]-lr[jstart]);
        dlydlr[i] = dlydlr[jstart];
        d2lydlr[i] = 0;
    }
    for(i=jend+1;i<n;i++) {
        ynew[i] = ynew[jend] + dlydlr[jend]*(lr[i]-lr[jend]);
        dlydlr[i] = dlydlr[jend];
        d2lydlr[i] = 0;
    }

// Add in inner truncation and outer truncation
    if (add_flag) {
        double ri = exp(lr[0])*.99;
        double ro = exp(lr[n-1])/2.0;
        double x;
        for(i=0;i<n;i++) {
            x = exp(lr[i]);
            ynew[i] += log(1 - sqrt(ri/x)) - pow(x/ro,2);
            dlydlr[i] += .5*sqrt(ri/x)*pow(1-sqrt(ri/x),-1) - 2*pow(x/ro,2);
            d2lydlr[i] += -.25*sqrt(ri/x)*pow(1-sqrt(ri/x),-2) - 4*pow(x/ro,2);
        }
    }
    return;

}

void read_sigma(char *fname, double *lr, double *sigma, double *dlsdlr, double *d2lsdlr, double *omega, double *dlomdlr, double *d2lomdlr, int n,int readomega) {
    double temp;
    size_t ndata;
    double *xd,*yd;
    int i;
    size_t fres;

    FILE *f = fopen(fname,"r");
    ndata = 1;
    fres = fread(&temp,sizeof(double),ndata,f);
    FREAD_CHECK(fres,ndata);
    ndata=(size_t)temp;

    xd = (double *)malloc(sizeof(double)*ndata);
    yd = (double *)malloc(sizeof(double)*ndata);

    fres = fread(xd,sizeof(double),ndata,f);
    FREAD_CHECK(fres,ndata);
    fres =  fread(yd,sizeof(double),ndata,f);
    FREAD_CHECK(fres,ndata);

    interpolate_onto_grid(xd,yd,ndata,lr,sigma,dlsdlr,d2lsdlr,n,TRUE);

    if (readomega) {
        fres =  fread(yd,sizeof(double),ndata,f);
        FREAD_CHECK(fres,ndata);
        for(i=0;i<(int)ndata;i++) {
            yd[i] = log( exp(yd[i])/pow(exp(xd[i]),2));
        }
        interpolate_onto_grid(xd,yd,ndata,lr,omega,dlomdlr,d2lomdlr,n,FALSE);
    }

    fclose(f);
    SAFE_FREE(xd);
    SAFE_FREE(yd);
    return;
}
/*
int main(void) {
    double *x,*y,*dy,*d2y;
    int i;

    double rmin = .01;
    double rmax = 10;
    int n = 1000;
    x = (double *)malloc(sizeof(double)*n);
    y = (double *)malloc(sizeof(double)*n);
    dy = (double *)malloc(sizeof(double)*n);
    d2y = (double *)malloc(sizeof(double)*n);

    for(i=0;i<n;i++) {
        x[i] = log(rmin) + i*log(rmax/rmin)/n;
        y[i]  = 0;
        dy[i] = 0;
        d2y[i] = 0;
    }

    read_sigma("../profiles/avg_profs/prof_q1e-3_a1e-2.bin",x,y,dy,d2y,n);

    FILE *f = fopen("interp_res.dat","w");

    double temp = (double)n;
    fwrite(&temp,sizeof(double),1,f);
    fwrite(x,sizeof(double),n,f);
    fwrite(y,sizeof(double),n,f);
    fwrite(dy,sizeof(double),n,f);
    fwrite(d2y,sizeof(double),n,f);
    SAFE_FREE(x);
    SAFE_FREE(y);
    SAFE_FREE(dy);
    SAFE_FREE(d2y);
}
*/


#ifndef KABSCH_RMSD
#define KABSCH_RMSD

#include <valarray>

#include <cblas.h>
#include <lapacke.h>

namespace kabsch
{

template <class M>
double rmsd(
        const M P,
        const M Q)
{
    double rmsd {0.0};
    const unsigned int D {3};
    const unsigned int size = P.size();
    const unsigned int N = size / D;

    for(unsigned int i = 0; i < size; ++i) {
        rmsd += (P[i] - Q[i])*(P[i] - Q[i]);
    }

    return sqrt(rmsd/N);
}

template <class M>
M centroid(M coordinates)
{

    double x {0};
    double y {0};
    double z {0};
    unsigned int size = coordinates.size();
    unsigned int n_atoms = size / 3;

    unsigned int i = 0;
    while(i<size)
    {
        x += coordinates[i++];
        y += coordinates[i++];
        z += coordinates[i++];
    }

    x /= n_atoms;
    y /= n_atoms;
    z /= n_atoms;

    return M {x, y, z};
}

template <class Matrix>
Matrix multiply(Matrix A, Matrix B,
    const int M,
    const int N,
    const int K)
{
    double one = 1.0;

    Matrix C(M*N);

    cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans,
        M, N, K, one,
        begin(A), K,
        begin(B), N, 1.0,
        begin(C), N);

    return C;
}

template <class Matrix>
Matrix transpose_multiply(Matrix A, Matrix B,
    const int M,
    const int N,
    const int K)
{
    double one = 1.0;

    Matrix C(M*N);

    cblas_dgemm(CblasRowMajor, CblasTrans, CblasNoTrans,
        M, N, K, one,
        begin(A), M,
        begin(B), N, one,
        begin(C), N);

    return C;
}

template <class Matrix>
std::tuple<Matrix, Matrix, Matrix> matrix_svd(Matrix A, int rows, int cols)
{
    // lapack_int LAPACKE_dgesvd( int matrix_layout, char jobu, char jobvt,
    //     lapack_int m, lapack_int n,
    //     double* a, lapack_int lda,
    //     double* s, double* u, lapack_int ldu,
    //     double* vt, lapack_int ldvt,
    //     double* superb );

    Matrix U(cols*rows);
    Matrix S(rows);
    Matrix VT(cols*rows);
    Matrix superb(cols*rows);
    int info;

    info = LAPACKE_dgesvd(LAPACK_ROW_MAJOR, 'A', 'A',
        rows, cols,
        begin(A), rows,
        begin(S),
        begin(U), rows,
        begin(VT), rows,
        begin(superb));

    // TODO check if SVD is unconverged
    if(info > 0)
    {
        // Failed
    }

    return make_tuple(U, S, VT);
}

template <class M>
double determinant3x3(M A)
{
    // determinant of a square 3x3 matrix
    double det = A[0]*A[4]*A[8]
        +A[1]*A[5]*A[6]
        +A[2]*A[3]*A[7]
        -A[2]*A[4]*A[6]
        -A[1]*A[3]*A[8]
        -A[0]*A[5]*A[7];

    return det;
}

template <class M, class T>
M kabsch(M P, M Q, const T n_atoms)
{
    // const unsigned int L = P.size();
    // const unsigned int D = 3;

    M U;
    M V;
    M S;

    M C = transpose_multiply(P, Q, 3, 3, n_atoms);

    tie(U, S, V) = matrix_svd(C, 3, 3);

    // Getting the sign of the det(U)*(V) to decide whether we need to correct
    // our rotation matrix to ensure a right-handed coordinate system.
    if(determinant3x3(U)*determinant3x3(V) < 0.0)
    {
        // TODO More numpy'ish way to do this?
        U[std::slice( 2, 3, 3 )] = {-U[3*0+2], -U[3*1+2], -U[3*2+2]};
    }

    M rotation = multiply(U, V, 3, 3, 3);

    return rotation;
}

template <class M, class T>
M kabsch_rotate(
        const M P,
        const M Q,
        const T n_atoms)
{
    M U = kabsch(P, Q, n_atoms);
    M product = multiply(P, U, n_atoms, 3, 3);
    return product;
}

template <class M, class T>
double kabsch_rmsd(
        const M P,
        const M Q,
        const T n_atoms)
{
    M P_rotated = kabsch_rotate(P, Q, n_atoms);
    return rmsd(P_rotated, Q);
}

} // namespace rmsd

#endif


#ifndef CURVATURE_H_LSXONAFA
#define CURVATURE_H_LSXONAFA

#include <cmath>
#include <gsl/gsl>
#include <type_traits>

namespace sens_loc::math {

/// This function calculates the gaussian curvature for a function given its
/// derivatives.
/// \tparam Real precision of the calculation
/// \param f_u,f_v,f_uu,f_vv,f_uv partial derivatives of the depth values
/// \returns gaussian curvature for these derivatives
/// \sa conversion::depth_to_gaussian_curvature
/// \note check https://en.wikipedia.org/wiki/Gaussian_curvature for more info
template <typename Real>
inline Real gaussian_curvature(
    Real f_u, Real f_v, Real f_uu, Real f_vv, Real f_uv) noexcept {
    static_assert(std::is_floating_point_v<Real>);

    return (f_uu * f_vv - f_uv * f_uv) / (Real(1.) + f_u * f_u + f_v * f_v);
}

/// This function calculates the mean curvature for a function given its
/// derivatives.
/// \tparam Real precicions of the calculation
/// \param f_u,f_v,f_uu,f_vv,f_uv partial derivatives of the depth values
/// \returns the mean curvature for these derivatives
/// \sa conversion::depth_to_mean_curvature
/// \note check https://en.wikipedia.org/wiki/Mean_curvature for more info
template <typename Real>
inline Real
mean_curvature(Real f_u, Real f_v, Real f_uu, Real f_vv, Real f_uv) noexcept {
    static_assert(std::is_floating_point_v<Real>);

    using std::pow;
    using std::sqrt;
    // NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
    return ((Real(1.) + f_v * f_v) * f_uu - Real(2.) * f_u * f_v * f_uv +
            (Real(1.) + f_u * f_u) * f_vv) /
           // NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
           pow(Real(2.) * sqrt(Real(1.) + f_u * f_u + f_v * f_v), Real(3.));
}
}  // namespace sens_loc::math

#endif /* end of include guard: CURVATURE_H_LSXONAFA */


#include "ll.h"
#include "gslsupp.h"

#include <math.h>
#include <gsl/gsl_sf.h>

double log_fact(size_t n)
{
    gsl_sf_result res;
    if (n <= UINT_MAX)
    {
        if (gsl_sf_lnfact_e((unsigned) n, &res) == GSL_SUCCESS) return res.val;
    }
    else
    {
        if (gsl_sf_lngamma_e((double) n + 1., &res) == GSL_SUCCESS) return res.val;
    }
    return nan(__func__);
}

double log_choose(size_t n, size_t m)
{
    if (m > n) return nan(__func__);
    if (m == n || !m) return 0.;
    return m > (n >> 1) ? log_fact(n) - log_fact(n - m) - log_fact(m) : log_fact(n) - log_fact(m) - log_fact(n - m); // Addition here may be not associative
}

double pdf_hypergeom(size_t k, size_t n1, size_t n2, size_t t)
{
    size_t car, n12 = size_add(&car, n1, n2);
    if (car) return nan(__func__);
    if (t > n12) t = n12;
    if (k > n1 || k > t || (t > n2 && k < t - n2)) return 0.;
    return exp(log_choose(n1, k) + log_choose(n2, t - k) - log_choose(n12, t));
}

double gamma_inc_P(double a, double x)
{
    gsl_sf_result res;
    if (gsl_sf_gamma_inc_P_e(a, x, &res) == GSL_SUCCESS) return res.val;
    return nan(__func__);
}

double gamma_inc_Q(double a, double x)
{
    gsl_sf_result res;
    if (gsl_sf_gamma_inc_Q_e(a, x, &res) == GSL_SUCCESS) return res.val;
    return nan(__func__);
}

double cdf_gamma_Q(double x, double a, double b)
{
    if (x <= 0.) return 1.;
    double y = x / b;
    return y < a ? 1. - gamma_inc_P(a, y) : gamma_inc_Q(a, y);
}

double cdf_chisq_Q(double x, double df)
{
    return cdf_gamma_Q(x, df / 2., 2.);
}


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

double f (double x, void * params)
{
  return pow (x, 1.5);
}

int
main (void)
{
  gsl_function F;
  double result, abserr;

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

  printf ("f(x) = x^(3/2)\n");

  gsl_deriv_central (&F, 2.0, 1e-8, &result, &abserr);
  printf ("x = 2.0\n");
  printf ("f'(x) = %.10f +/- %.10f\n", result, abserr);
  printf ("exact = %.10f\n\n", 1.5 * sqrt(2.0));

  gsl_deriv_forward (&F, 0.0, 1e-8, &result, &abserr);
  printf ("x = 0.0\n");
  printf ("f'(x) = %.10f +/- %.10f\n", result, abserr);
  printf ("exact = %.10f\n", 0.0);

  return 0;
}


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

/* Author:  G. Jungman */

#ifndef __GSL_SF_BESSEL_H__
#define __GSL_SF_BESSEL_H__

#if !defined( GSL_FUN )
#  if !defined( GSL_DLL )
#    define GSL_FUN extern
#  elif defined( BUILD_GSL_DLL )
#    define GSL_FUN extern __declspec(dllexport)
#  else
#    define GSL_FUN extern __declspec(dllimport)
#  endif
#endif

#include <stdlib.h>
#include <gsl/gsl_mode.h>
#include <gsl/gsl_precision.h>
#include <gsl/gsl_sf_result.h>

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

__BEGIN_DECLS

/* Regular Bessel Function J_0(x)
 *
 * exceptions: none
 */
GSL_FUN int gsl_sf_bessel_J0_e(const double x,  gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_J0(const double x);

/* Regular Bessel Function J_1(x)
 *
 * exceptions: GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_J1_e(const double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_J1(const double x);

/* Regular Bessel Function J_n(x)
 *
 * exceptions: GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_Jn_e(int n, double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_Jn(const int n, const double x);

/* Regular Bessel Function J_n(x),  nmin <= n <= nmax
 *
 * exceptions: GSL_EDOM, GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_Jn_array(int nmin, int nmax, double x, double * result_array);

/* Irregular Bessel function Y_0(x)
 *
 * x > 0.0
 * exceptions: GSL_EDOM, GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_Y0_e(const double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_Y0(const double x);

/* Irregular Bessel function Y_1(x)
 *
 * x > 0.0
 * exceptions: GSL_EDOM, GSL_EOVRFLW, GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_Y1_e(const double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_Y1(const double x);

/* Irregular Bessel function Y_n(x)
 *
 * x > 0.0
 * exceptions: GSL_EDOM, GSL_EOVRFLW, GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_Yn_e(int n,const double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_Yn(const int n,const double x);

/* Irregular Bessel function Y_n(x), nmin <= n <= nmax
 *
 * x > 0.0
 * exceptions: GSL_EDOM, GSL_EOVRFLW, GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_Yn_array(const int nmin, const int nmax, const double x, double * result_array);

/* Regular modified Bessel function I_0(x)
 *
 * exceptions: GSL_EOVRFLW
 */
GSL_FUN int gsl_sf_bessel_I0_e(const double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_I0(const double x);

/* Regular modified Bessel function I_1(x)
 *
 * exceptions: GSL_EOVRFLW, GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_I1_e(const double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_I1(const double x);

/* Regular modified Bessel function I_n(x)
 *
 * exceptions: GSL_EOVRFLW, GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_In_e(const int n, const double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_In(const int n, const double x);

/* Regular modified Bessel function  I_n(x) for n=nmin,...,nmax
 *
 * nmin >=0, nmax >= nmin
 * exceptions: GSL_EDOM, GSL_EOVRFLW, GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_In_array(const int nmin, const int nmax, const double x, double * result_array);

/* Scaled regular modified Bessel function
 *  exp(-|x|) I_0(x)
 *
 * exceptions: none
 */
GSL_FUN int gsl_sf_bessel_I0_scaled_e(const double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_I0_scaled(const double x);

/* Scaled regular modified Bessel function
 *  exp(-|x|) I_1(x)
 *
 * exceptions: GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_I1_scaled_e(const double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_I1_scaled(const double x);

/* Scaled regular modified Bessel function
 *  exp(-|x|) I_n(x)
 *
 * exceptions: GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_In_scaled_e(int n, const double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_In_scaled(const int n, const double x);

/* Scaled regular modified Bessel function
 *  exp(-|x|) I_n(x)  for n=nmin,...,nmax
 *
 * nmin >=0, nmax >= nmin
 * exceptions: GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_In_scaled_array(const int nmin, const int nmax, const double x, double * result_array);

/* Irregular modified Bessel function K_0(x)
 *
 * x > 0.0
 * exceptions: GSL_EDOM, GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_K0_e(const double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_K0(const double x);

/* Irregular modified Bessel function K_1(x)
 *
 * x > 0.0
 * exceptions: GSL_EDOM, GSL_EOVRFLW, GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_K1_e(const double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_K1(const double x);

/* Irregular modified Bessel function K_n(x)
 *
 * x > 0.0
 * exceptions: GSL_EDOM, GSL_EOVRFLW, GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_Kn_e(const int n, const double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_Kn(const int n, const double x);

/* Irregular modified Bessel function  K_n(x)  for n=nmin,...,nmax
 *
 * x > 0.0, nmin >=0, nmax >= nmin
 * exceptions: GSL_EDOM, GSL_EOVRFLW, GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_Kn_array(const int nmin, const int nmax, const double x, double * result_array);

/* Scaled irregular modified Bessel function
 *  exp(x) K_0(x)
 *
 * x > 0.0
 * exceptions: GSL_EDOM
 */
GSL_FUN int gsl_sf_bessel_K0_scaled_e(const double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_K0_scaled(const double x);

/* Scaled irregular modified Bessel function
 *  exp(x) K_1(x)
 *
 * x > 0.0
 * exceptions: GSL_EDOM, GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_K1_scaled_e(const double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_K1_scaled(const double x);

/* Scaled irregular modified Bessel function
 *  exp(x) K_n(x)
 *
 * x > 0.0
 * exceptions: GSL_EDOM, GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_Kn_scaled_e(int n, const double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_Kn_scaled(const int n, const double x);

/* Scaled irregular modified Bessel function  exp(x) K_n(x)  for n=nmin,...,nmax
 *
 * x > 0.0, nmin >=0, nmax >= nmin
 * exceptions: GSL_EDOM, GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_Kn_scaled_array(const int nmin, const int nmax, const double x, double * result_array);

/* Regular spherical Bessel function j_0(x) = sin(x)/x
 *
 * exceptions: none
 */
GSL_FUN int gsl_sf_bessel_j0_e(const double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_j0(const double x);

/* Regular spherical Bessel function j_1(x) = (sin(x)/x - cos(x))/x
 *
 * exceptions: GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_j1_e(const double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_j1(const double x);

/* Regular spherical Bessel function j_2(x) = ((3/x^2 - 1)sin(x) - 3cos(x)/x)/x
 *
 * exceptions: GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_j2_e(const double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_j2(const double x);

/* Regular spherical Bessel function j_l(x)
 *
 * l >= 0, x >= 0.0
 * exceptions: GSL_EDOM, GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_jl_e(const int l, const double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_jl(const int l, const double x);

/* Regular spherical Bessel function j_l(x) for l=0,1,...,lmax
 *
 * exceptions: GSL_EDOM, GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_jl_array(const int lmax, const double x, double * result_array);

/* Regular spherical Bessel function j_l(x) for l=0,1,...,lmax
 * Uses Steed's method.
 *
 * exceptions: GSL_EDOM, GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_jl_steed_array(const int lmax, const double x, double * jl_x_array);

/* Irregular spherical Bessel function y_0(x)
 *
 * exceptions: none
 */
GSL_FUN int gsl_sf_bessel_y0_e(const double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_y0(const double x);

/* Irregular spherical Bessel function y_1(x)
 *
 * exceptions: GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_y1_e(const double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_y1(const double x);

/* Irregular spherical Bessel function y_2(x)
 *
 * exceptions: GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_y2_e(const double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_y2(const double x);

/* Irregular spherical Bessel function y_l(x)
 *
 * exceptions: GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_yl_e(int l, const double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_yl(const int l, const double x);

/* Irregular spherical Bessel function y_l(x) for l=0,1,...,lmax
 *
 * exceptions: GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_yl_array(const int lmax, const double x, double * result_array);

/* Regular scaled modified spherical Bessel function
 *
 * Exp[-|x|] i_0(x)
 *
 * exceptions: none
 */
GSL_FUN int gsl_sf_bessel_i0_scaled_e(const double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_i0_scaled(const double x);

/* Regular scaled modified spherical Bessel function
 *
 * Exp[-|x|] i_1(x)
 *
 * exceptions: GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_i1_scaled_e(const double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_i1_scaled(const double x);

/* Regular scaled modified spherical Bessel function
 *
 * Exp[-|x|] i_2(x)
 *
 * exceptions: GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_i2_scaled_e(const double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_i2_scaled(const double x);

/* Regular scaled modified spherical Bessel functions
 *
 * Exp[-|x|] i_l(x)
 *
 * i_l(x) = Sqrt[Pi/(2x)] BesselI[l+1/2,x]
 *
 * l >= 0
 * exceptions: GSL_EDOM, GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_il_scaled_e(const int l, double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_il_scaled(const int l, const double x);

/* Regular scaled modified spherical Bessel functions
 *
 * Exp[-|x|] i_l(x)
 * for l=0,1,...,lmax
 *
 * exceptions: GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_il_scaled_array(const int lmax, const double x, double * result_array);

/* Irregular scaled modified spherical Bessel function
 * Exp[x] k_0(x)
 *
 * x > 0.0
 * exceptions: GSL_EDOM, GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_k0_scaled_e(const double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_k0_scaled(const double x);

/* Irregular modified spherical Bessel function
 * Exp[x] k_1(x)
 *
 * x > 0.0
 * exceptions: GSL_EDOM, GSL_EUNDRFLW, GSL_EOVRFLW
 */
GSL_FUN int gsl_sf_bessel_k1_scaled_e(const double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_k1_scaled(const double x);

/* Irregular modified spherical Bessel function
 * Exp[x] k_2(x)
 *
 * x > 0.0
 * exceptions: GSL_EDOM, GSL_EUNDRFLW, GSL_EOVRFLW
 */
GSL_FUN int gsl_sf_bessel_k2_scaled_e(const double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_k2_scaled(const double x);

/* Irregular modified spherical Bessel function
 * Exp[x] k_l[x]
 *
 * k_l(x) = Sqrt[Pi/(2x)] BesselK[l+1/2,x]
 *
 * exceptions: GSL_EDOM, GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_kl_scaled_e(int l, const double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_kl_scaled(const int l, const double x);

/* Irregular scaled modified spherical Bessel function
 * Exp[x] k_l(x)
 *
 * for l=0,1,...,lmax
 * exceptions: GSL_EDOM, GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_kl_scaled_array(const int lmax, const double x, double * result_array);

/* Regular cylindrical Bessel function J_nu(x)
 *
 * exceptions: GSL_EDOM, GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_Jnu_e(const double nu, const double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_Jnu(const double nu, const double x);

/* Irregular cylindrical Bessel function Y_nu(x)
 *
 * exceptions:
 */
GSL_FUN int gsl_sf_bessel_Ynu_e(double nu, double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_Ynu(const double nu, const double x);

/* Regular cylindrical Bessel function J_nu(x)
 * evaluated at a series of x values. The array
 * contains the x values. They are assumed to be
 * strictly ordered and positive. The array is
 * over-written with the values of J_nu(x_i).
 *
 * exceptions: GSL_EDOM, GSL_EINVAL
 */
GSL_FUN int gsl_sf_bessel_sequence_Jnu_e(double nu, gsl_mode_t mode, size_t size, double * v);

/* Scaled modified cylindrical Bessel functions
 *
 * Exp[-|x|] BesselI[nu, x]
 * x >= 0, nu >= 0
 *
 * exceptions: GSL_EDOM
 */
GSL_FUN int gsl_sf_bessel_Inu_scaled_e(double nu, double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_Inu_scaled(double nu, double x);

/* Modified cylindrical Bessel functions
 *
 * BesselI[nu, x]
 * x >= 0, nu >= 0
 *
 * exceptions: GSL_EDOM, GSL_EOVRFLW
 */
GSL_FUN int gsl_sf_bessel_Inu_e(double nu, double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_Inu(double nu, double x);

/* Scaled modified cylindrical Bessel functions
 *
 * Exp[+|x|] BesselK[nu, x]
 * x > 0, nu >= 0
 *
 * exceptions: GSL_EDOM
 */
GSL_FUN int gsl_sf_bessel_Knu_scaled_e(const double nu, const double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_Knu_scaled(const double nu, const double x);

GSL_FUN int gsl_sf_bessel_Knu_scaled_e10_e(const double nu, const double x, gsl_sf_result_e10 * result);

/* Modified cylindrical Bessel functions
 *
 * BesselK[nu, x]
 * x > 0, nu >= 0
 *
 * exceptions: GSL_EDOM, GSL_EUNDRFLW
 */
GSL_FUN int gsl_sf_bessel_Knu_e(const double nu, const double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_Knu(const double nu, const double x);

/* Logarithm of modified cylindrical Bessel functions.
 *
 * Log[BesselK[nu, x]]
 * x > 0, nu >= 0
 *
 * exceptions: GSL_EDOM
 */
GSL_FUN int gsl_sf_bessel_lnKnu_e(const double nu, const double x, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_lnKnu(const double nu, const double x);

/* s'th positive zero of the Bessel function J_0(x).
 *
 * exceptions:
 */
GSL_FUN int gsl_sf_bessel_zero_J0_e(unsigned int s, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_zero_J0(unsigned int s);

/* s'th positive zero of the Bessel function J_1(x).
 *
 * exceptions:
 */
GSL_FUN int gsl_sf_bessel_zero_J1_e(unsigned int s, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_zero_J1(unsigned int s);

/* s'th positive zero of the Bessel function J_nu(x).
 *
 * exceptions:
 */
GSL_FUN int gsl_sf_bessel_zero_Jnu_e(double nu, unsigned int s, gsl_sf_result * result);
GSL_FUN double gsl_sf_bessel_zero_Jnu(double nu, unsigned int s);

__END_DECLS

#endif /* __GSL_SF_BESSEL_H__ */


/* cdf/hypergeometric.c
 *
 * Copyright (C) 2004 Jason H. Stover.
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307, USA.
 */

/*
 * Computes the cumulative distribution function for a hypergeometric
 * random variable. A hypergeometric random variable X is the number
 * of elements of type 1 in a sample of size t, drawn from a population
 * of size n1 + n2, in which n1 are of type 1 and n2 are of type 2.
 *
 * This algorithm computes Pr( X <= k ) by summing the terms from
 * the mass function, Pr( X = k ).
 *
 * References:
 *
 * T. Wu. An accurate computation of the hypergeometric distribution
 * function. ACM Transactions on Mathematical Software. Volume 19, number 1,
 * March 1993.
 *  This algorithm is not used, since it requires factoring the
 *  numerator and denominator, then cancelling. It is more accurate
 *  than the algorithm used here, but the cancellation requires more
 *  time than the algorithm used here.
 *
 * W. Feller. An Introduction to Probability Theory and Its Applications,
 * third edition. 1968. Chapter 2, section 6.
 */

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

#include "error.h"

static double
lower_tail (const unsigned int k, const unsigned int n1,
            const unsigned int n2, const unsigned int t)
{
  double relerr;
  int i = k;
  double s, P;

  s = gsl_ran_hypergeometric_pdf (i, n1, n2, t);
  P = s;

  while (i > 0)
    {
      double factor =
        (i / (n1 - i + 1.0)) * ((n2 + i - t) / (t - i + 1.0));
      s *= factor;
      P += s;
      relerr = s / P;
      if (relerr < GSL_DBL_EPSILON)
        break;
      i--;
    }

  return P;
}

static double
upper_tail (const unsigned int k, const unsigned int n1,
            const unsigned int n2, const unsigned int t)
{
  double relerr;
  unsigned int i = k + 1;
  double s, Q;

  s = gsl_ran_hypergeometric_pdf (i, n1, n2, t);
  Q = s;

  while (i < t)
    {
      double factor =
        ((n1 - i) / (i + 1.0)) * ((t - i) / (n2 + i + 1.0 - t));
      s *= factor;
      Q += s;
      relerr = s / Q;
      if (relerr < GSL_DBL_EPSILON)
        break;
      i++;
    }

  return Q;
}

/*
 * Pr (X <= k)
 */
double
gsl_cdf_hypergeometric_P (const unsigned int k,
                          const unsigned int n1,
                          const unsigned int n2, const unsigned int t)
{
  double P;

  if (t > (n1 + n2))
    {
      CDF_ERROR ("t larger than population size", GSL_EDOM);
    }
  else if (k >= n1 || k >= t)
    {
      P = 1.0;
    }
  else if (k < 0.0)
    {
      P = 0.0;
    }
  else
    {
      double midpoint = (int) (t * n1 / (n1 + n2));

      if (k >= midpoint)
        {
          P = 1 - upper_tail (k, n1, n2, t);
        }
      else
        {
          P = lower_tail (k, n1, n2, t);
        }
    }

  return P;
}

/*
 * Pr (X > k)
 */
double
gsl_cdf_hypergeometric_Q (const unsigned int k,
                          const unsigned int n1,
                          const unsigned int n2, const unsigned int t)
{
  double Q;

  if (t > (n1 + n2))
    {
      CDF_ERROR ("t larger than population size", GSL_EDOM);
    }
  else if (k >= n1 || k >= t)
    {
      Q = 0.0;
    }
  else if (k < 0.0)
    {
      Q = 1.0;
    }
  else
    {
      double midpoint = (int) (t * n1 / (n1 + n2));

      if (k < midpoint)
        {
          Q = 1 - lower_tail (k, n1, n2, t);
        }
      else
        {
          Q = upper_tail (k, n1, n2, t);
        }
    }

  return Q;
}


#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

static double compearth_norm64f(const int n, const double *__restrict__ x,
                                const enum ceNormType_enum norm,
                                const double p, int *ierr);
/*!
 * @brief Computes the matrix (Frobenius) norm for a matrix
 *
 * @param[in] n       number of matrices
 * @param[in] M       3 x 3 input matrix as a length 9 array [9*n]
 * @param[in] Lnorm   matrix norm
 *                      TWO_NORM (2)
 *                      ONE_NORM (1)
 *                      P_NORM (in this case must set p)
 *                      INFINITY_NORM
 *                      NEGATIVE_INFINITY_NORM
 * @param[in] p       if using a p-norm this is the value for p (> 0)
 *
 * @param[out] mnorm  matrix norms for each matrix
 *
 * @result 0 indicates success
 *
 */
int compearth_normMat(const int n,
                      const double *__restrict__ M,
                      const enum ceNormType_enum Lnorm,
                      const double p,
                      double *__restrict__ mnorm)
{
    int i, ierr;
    ierr = 0;
    for (i=0; i<n; i++)
    {
        mnorm[i] = compearth_norm64f(9, &M[9*i], Lnorm, p, &ierr);
        if (ierr != 0)
        {
            fprintf(stderr, "%s: Error computing matrix norm!\n", __func__);
            mnorm[i] = 0.0;
            break;
        }
    }
    return ierr;
}

/*!
 * @brief Computes the norm of a vector.  This is from ISTI's ISCL.
 *
 * @param[in] n      Length of array x.
 * @param[in] x      Array of dimension [n] of which to compute norm.
 * @param[in] norm   Type of norm to compute.
 * @param[in] p      If performing a P norm then this must be defined to
 *                   a real number greater than or equal to 1.  Otherwise,
 *                   it will not be accessed.
 *
 * @param[out] ierr  0 indicates success
 *
 * @result P-norm of array x.
 *
 * @author Ben Baker
 *
 * @date August 2017 - redefined variables so that this may exist in
 *       compearth without collisions with ISCL.
 *
 */
static double compearth_norm64f(const int n, const double *__restrict__ x,
                                const enum ceNormType_enum norm,
                                const double p, int *ierr)
{
    double xnorm;
    int i;
    *ierr = 0;
    xnorm = 0.0;
    if (n < 1 || x == NULL)
    {
        if (n < 1){fprintf(stderr, "%s: Error no data\n", __func__);}
        if (x == NULL){fprintf(stderr, "%s: Error x is NULL\n", __func__);}
        *ierr = 1;
        return xnorm;
    }
    // 2 norm
    if (norm == CE_TWO_NORM)
    {
        xnorm = cblas_dnrm2(n, x, 1);
    // 1 norm
    }
    else if (norm == CE_ONE_NORM)
    {
        xnorm = cblas_dasum(n, x, 1);
    }
    // p norm
    else if (norm == CE_P_NORM)
    {
        if (p <= 0.0)
        {
            fprintf(stderr, "%s: Invalid p value %f\n", __func__, p);
            *ierr = 1;
            return xnorm;
        }
        #pragma omp simd reduction(+:xnorm)
        for (i=0; i<n; i++)
        {
            xnorm = xnorm + pow(fabs(x[i]), p);
        }
        xnorm = pow(xnorm, 1./p);
    }
    // infinity norm
    else if (norm == CE_INFINITY_NORM)
    {
        xnorm = fabs(x[cblas_idamax(n, x, 1)]);
    }
    // negative infinity norm
    else if (norm == CE_NEGATIVE_INFINITY_NORM)
    {
        xnorm = fabs(x[0]);
        #pragma omp simd reduction(min:xnorm)
        for (i=1; i<n; i++)
        {
            xnorm = fmin(fabs(x[i]), xnorm);
        }
    }
    // not sure - default to 2-norm
    else
    {
        fprintf(stderr, "%s: Defaulting to 2-norm\n", __func__);
        xnorm = cblas_dnrm2(n, x, 1);
    }
    return xnorm;
}


#include <math.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_cdf.h>
#include "gauss.h"

#define SQRT_2_PI     2.506628274631      /* sqrt(2*PI) */
#define LOG_SQRT_2_PI 0.918938533204673   /* log(sqrt(2*PI)) */

gauss_t gauss_init_prec(double prec_mean, double prec){
	gauss_t r;
	r.prec_mean = prec_mean;
	r.prec      = prec;
	r.var       = 1.0 / prec;
	r.mean      = prec_mean / prec;
	r.stddev    = sqrt(r.var);
	return r;
}

gauss_t gauss_init_var(double mean, double var){
	gauss_t r;
	r.var       = var;
	r.mean      = mean;
	r.stddev    = sqrt(var);
	r.prec      = 1.0 / var;
	r.prec_mean = r.prec * r.mean;
	return r;
}

gauss_t gauss_init_std(double mean, double stddev){
	gauss_t r;
	r.var       = stddev * stddev;
	r.mean      = mean;
	r.stddev    = stddev;
	r.prec      = 1.0 / r.var;
	r.prec_mean = r.prec * r.mean;
	return r;
}

double gauss_log_product_normal(gauss_t a, gauss_t b){
	if(a.prec == 0 || b.prec == 0) return 0.0;

	double var_sum   = a.var + b.var;
	double mean_diff = a.mean - b.mean;

	return -LOG_SQRT_2_PI - (log(var_sum)/2.0) - ((mean_diff * mean_diff) / (2.0 * var_sum));
}

double gauss_log_ratio_normal(gauss_t a, gauss_t b){
	if(a.prec == 0 || b.prec == 0) return 0.0;

	double var_diff  = a.var - b.var;
	double mean_diff = a.mean - b.mean;

	return log(b.var) + LOG_SQRT_2_PI - (log(var_diff)/2.0) + ((mean_diff * mean_diff) / (2.0 * var_diff));
}

double gauss_cdf_P(gauss_t a, double x){
	return gsl_cdf_gaussian_P(x - a.mean, a.stddev);
}

double gauss_cdf_Q(gauss_t a, double x){
	return gsl_cdf_gaussian_Q(x - a.mean, a.stddev);
}

gauss_t gauss_add(gauss_t a, gauss_t b){
	gauss_t r;
	r.mean      = a.mean + b.mean;
	r.var       = a.var  + b.var;
	r.prec      = 1.0 / r.var;
	r.prec_mean = r.prec * r.mean;
	r.stddev    = sqrt(r.var);
	return r;
}

gauss_t gauss_sub(gauss_t a, gauss_t b){
	gauss_t temp = b;
	temp.mean = -temp.mean;
	return gauss_add(a, temp);
}

double gauss_abs_diff(gauss_t a, gauss_t b){
	double prec_mean_diff = fabs(a.prec_mean - b.prec_mean);
	double prec_diff = sqrt(fabs(a.prec - b.prec));
	return (prec_mean_diff > prec_diff) ? prec_mean_diff : prec_diff;
}

gauss_t gauss_div(gauss_t a, gauss_t b){
	double prec      = a.prec      - b.prec;
	double prec_mean = a.prec_mean - b.prec_mean;
	return gauss_init_prec(prec_mean, prec);
}

gauss_t gauss_mul(gauss_t a, gauss_t b){
	double prec      = a.prec      + b.prec;
	double prec_mean = a.prec_mean + b.prec_mean;
	return gauss_init_prec(prec_mean, prec);
}


#ifndef __SPATIAL_STATISTICS_H__
#define __SPATIAL_STATISTICS_H__

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

extern "C" double gsl_sf_gamma(const double);
extern "C" double gsl_sf_hyperg_1F1(double, double, double);

template <typename T> struct Spatial_Statistics
{
  private:
    T phi, nu;
    T con;
    int d;

  public:
    Spatial_Statistics(T phi, T nu, int dim)
    {
        this->phi = phi;
        this->nu = nu;
        // this->alpha = alpha;
        this->d = dim;

        con = pow(2, (nu - 1)) * tgamma(nu);
        con = 1.0 / con;

        // c1 = pow(h / sigma, d) / pow(sigma, alpha);
        // c2 = -pow(2.0, alpha) * gsl_sf_gamma((alpha + (T)d) / 2) / pow(M_PI, (T)d / 2) / gsl_sf_gamma((T)d / 2);
    }

    // T G(T x) const
    //{
    //    return c2 * gsl_sf_hyperg_1F1((alpha + (T)d) / 2, (T)d / 2, -x * x);
    //}

    T operator()(T *pt_x, T *pt_y) const
    {
        T diff = 0.0;
        for (int i = 0; i < d; i++)
            diff += (pt_x[i] - pt_y[i]) * (pt_x[i] - pt_y[i]);
        T dist = sqrt(diff);

        dist = 4.0 * sqrt(2.0 * nu) * (dist / phi);
        if (dist == 0.0)
        {
            return 1.0;
        }
        else
        {
            return con * pow(dist, nu) * gsl_sf_bessel_Knu(nu, dist);
        }
    }
};

template <typename T> struct MaternKernel
{
  private:
    T h, sigma, alpha;
    T c1, c2;
    int d;

  public:
    MaternKernel(T h, T sigma, T alpha, int dim)
    {
        this->h = h;
        this->sigma = sigma;
        this->alpha = alpha;
        this->d = dim;

        c1 = pow(h / sigma, d) / pow(sigma, alpha);
        c2 = -pow(2.0, alpha) * gsl_sf_gamma((alpha + (T)d) / 2.0) / pow(M_PI, (T)d / 2.0) / gsl_sf_gamma((T)d / 2.0);
    }

    T G(T x) const
    {
        return c2 * gsl_sf_hyperg_1F1((alpha + (T)d) / 2.0, (T)d / 2.0, -x * x);
    }

    T operator()(T *pt_x, T *pt_y) const
    {
        T diff = 0.0;
        for (int i = 0; i < d; i++)
            diff += (pt_x[i] - pt_y[i]) * (pt_x[i] - pt_y[i]);
        T dist = sqrt(diff);

        return c1 * G(dist / sigma);
    }
};

#endif


static char help[] =
"Solves the p-Helmholtz equation in 2D using Q_1 FEM.  Option prefix -ph_.\n"
"Problem is posed as minimizing this objective functional over W^{1,p}\n"
"for p>1:\n"
"    I[u] = int_Omega (1/p) |grad u|^p + (1/2) u^2 - f u.\n"
"The strong form equation, namely setting the gradient to zero, is a PDE\n"
"    - div( |grad u|^{p-2} grad u ) + u = f\n"
"subject to homogeneous Neumann boundary conditions.  Implements objective\n"
"and gradient (residual) but no Hessian (Jacobian).  Defaults to linear\n"
"problem (p=2) and quadrature degree 2.  Can be run with only an objective\n" "function; use -ph_no_gradient -snes_fd_function.\n\n";

#include <petsc.h>
#include "../interlude/quadrature.h"

typedef struct {
    PetscReal  p, eps;
    PetscInt   quadpts;
    PetscReal  (*f)(PetscReal x, PetscReal y, PetscReal p, PetscReal eps);
} PHelmCtx;

static PetscReal f_constant(PetscReal x, PetscReal y, PetscReal p, PetscReal eps) {
    return 1.0;
}

static PetscReal u_exact_cosines(PetscReal x, PetscReal y, PetscReal p, PetscReal eps) {
    return PetscCosReal(PETSC_PI * x) * PetscCosReal(PETSC_PI * y);
}

static PetscReal f_cosines(PetscReal x, PetscReal y, PetscReal p, PetscReal eps) {
    const PetscReal uu = u_exact_cosines(x,y,p,eps),
                    pi2 = PETSC_PI * PETSC_PI,
                    lapu = - 2 * pi2 * uu;
    if (p == 2.0) {
        return - lapu + uu;
    } else {
        const PetscReal
            ux = - PETSC_PI * PetscSinReal(PETSC_PI * x)
                 * PetscCosReal(PETSC_PI * y),
            uy = - PETSC_PI * PetscCosReal(PETSC_PI * x)
                 * PetscSinReal(PETSC_PI * y),
            // note regularization changes f(x,y) but not u(x,y):
            w = ux * ux + uy * uy + eps * eps,
            pi3 = pi2 * PETSC_PI,
            wx = pi3 * PetscSinReal(2 * PETSC_PI * x)
                 * PetscCosReal(2 * PETSC_PI * y),
            wy = pi3 * PetscCosReal(2 * PETSC_PI * x)
                 * PetscSinReal(2 * PETSC_PI * y);
        const PetscReal s = (p - 2) / 2;  //  -1/2 <= s <= 0
        return - s * PetscPowScalar(w,s-1) * (wx * ux + wy * uy)
               - PetscPowScalar(w,s) * lapu + uu;
    }
}

typedef enum {CONSTANT, COSINES} ProblemType;
static const char* ProblemTypes[] = {"constant","cosines",
                                     "ProblemType", "", NULL};

extern PetscErrorCode GetVecFromFunction(DMDALocalInfo*, Vec,
                         PetscReal (*)(PetscReal, PetscReal, PetscReal, PetscReal), PHelmCtx*);
extern PetscErrorCode FormObjectiveLocal(DMDALocalInfo*, PetscReal**, PetscReal*, PHelmCtx*);
extern PetscErrorCode FormFunctionLocal(DMDALocalInfo*, PetscReal**, PetscReal**, PHelmCtx*);

int main(int argc,char **argv) {
    PetscErrorCode ierr;
    DM             da;
    SNES           snes;
    Vec            u_initial, u, u_exact;
    PHelmCtx       user;
    DMDALocalInfo  info;
    ProblemType    problem = COSINES;
    PetscBool      no_objective = PETSC_FALSE,
                   no_gradient = PETSC_FALSE,
                   exact_init = PETSC_FALSE,
                   view_f = PETSC_FALSE;
    PetscReal      err;

    ierr = PetscInitialize(&argc,&argv,NULL,help); if (ierr) return ierr;

    user.p = 2.0;
    user.eps = 0.0;
    user.quadpts = 2;
    ierr = PetscOptionsBegin(PETSC_COMM_WORLD,"ph_",
                  "p-Helmholtz solver options",""); CHKERRQ(ierr);
    ierr = PetscOptionsReal("-eps",
                  "regularization parameter eps",
                  "phelm.c",user.eps,&(user.eps),NULL); CHKERRQ(ierr);
    ierr = PetscOptionsBool("-exact_init",
                  "use exact solution to initialize",
                  "phelm.c",exact_init,&(exact_init),NULL);CHKERRQ(ierr);
    ierr = PetscOptionsBool("-no_objective",
                  "do not set the objective evaluation function",
                  "phelm.c",no_objective,&(no_objective),NULL);CHKERRQ(ierr);
    ierr = PetscOptionsBool("-no_gradient",
                  "do not set the residual evaluation function",
                  "phelm.c",no_gradient,&(no_gradient),NULL);CHKERRQ(ierr);
    ierr = PetscOptionsReal("-p",
                  "exponent p > 1",
                  "phelm.c",user.p,&(user.p),NULL); CHKERRQ(ierr);
    if (user.p < 1.0) {
         SETERRQ(PETSC_COMM_SELF,1,"p >= 1 required");
    }
    if (user.p == 1.0) {
        ierr = PetscPrintf(PETSC_COMM_WORLD,
            "WARNING: well-posedness only known for p > 1\n"); CHKERRQ(ierr);
    }
    ierr = PetscOptionsEnum("-problem",
                  "problem type determines right side f(x,y)",
                  "phelm.c",ProblemTypes,(PetscEnum)problem,(PetscEnum*)&problem,
                  NULL); CHKERRQ(ierr);
    ierr = PetscOptionsInt("-quadpts",
                  "number n of quadrature points in each direction (= 1,2,3 only)",
                  "phelm.c",user.quadpts,&(user.quadpts),NULL); CHKERRQ(ierr);
    if ((user.quadpts < 1) || (user.quadpts > 3)) {
        SETERRQ(PETSC_COMM_SELF,3,"quadrature points n=1,2,3 only");
    }
    ierr = PetscOptionsBool("-view_f",
                  "view right-hand side to STDOUT",
                  "phelm.c",view_f,&(view_f),NULL);CHKERRQ(ierr);
    ierr = PetscOptionsEnd(); CHKERRQ(ierr);

    ierr = DMDACreate2d(PETSC_COMM_WORLD,
           DM_BOUNDARY_NONE, DM_BOUNDARY_NONE, DMDA_STENCIL_BOX,
           2,2,PETSC_DECIDE,PETSC_DECIDE,1,1,NULL,NULL,&da); CHKERRQ(ierr);
    ierr = DMSetFromOptions(da); CHKERRQ(ierr);
    ierr = DMSetUp(da); CHKERRQ(ierr);
    ierr = DMSetApplicationContext(da,&user);CHKERRQ(ierr);
    ierr = DMDASetUniformCoordinates(da,0.0,1.0,0.0,1.0,-1.0,-1.0); CHKERRQ(ierr);
    ierr = DMDAGetLocalInfo(da,&info); CHKERRQ(ierr);

    ierr = SNESCreate(PETSC_COMM_WORLD,&snes); CHKERRQ(ierr);
    ierr = SNESSetDM(snes,da); CHKERRQ(ierr);
    if (!no_objective) {
        ierr = DMDASNESSetObjectiveLocal(da,
             (DMDASNESObjective)FormObjectiveLocal,&user); CHKERRQ(ierr);
    }
    if (no_gradient) {
        // why isn't this the default?  why no programmatic way to set?
        ierr = PetscOptionsSetValue(NULL,"-snes_fd_function_eps","0.0"); CHKERRQ(ierr);
    } else {
        ierr = DMDASNESSetFunctionLocal(da,INSERT_VALUES,
             (DMDASNESFunction)FormFunctionLocal,&user); CHKERRQ(ierr);
    }
    ierr = SNESSetFromOptions(snes); CHKERRQ(ierr);

    // set initial iterate and right-hand side
    ierr = DMCreateGlobalVector(da,&u_initial);CHKERRQ(ierr);
    ierr = VecSet(u_initial,0.5); CHKERRQ(ierr);
    switch (problem) {
        case CONSTANT:
            if (exact_init) {
                ierr = VecSet(u_initial,1.0); CHKERRQ(ierr);
            }
            user.f = &f_constant;
            break;
        case COSINES:
            if (exact_init) {
                ierr = GetVecFromFunction(&info,u_initial,&u_exact_cosines,&user); CHKERRQ(ierr);
            }
            user.f = &f_cosines;
            break;
        default:
            SETERRQ(PETSC_COMM_SELF,4,"unknown problem type\n");
    }

    // optionally view right-hand-side on initial grid
    if (view_f) {
        Vec vf;
        ierr = VecDuplicate(u_initial,&vf); CHKERRQ(ierr);
        switch (problem) {
            case CONSTANT:
                ierr = VecSet(vf,1.0); CHKERRQ(ierr);
                break;
            case COSINES:
                ierr = GetVecFromFunction(&info,vf,&f_cosines,&user); CHKERRQ(ierr);
                break;
        }
        ierr = VecView(vf,PETSC_VIEWER_STDOUT_WORLD); CHKERRQ(ierr);
        VecDestroy(&vf);
    }

    // solve and clean up
    ierr = SNESSolve(snes,NULL,u_initial); CHKERRQ(ierr);
    ierr = VecDestroy(&u_initial); CHKERRQ(ierr);
    ierr = DMDestroy(&da); CHKERRQ(ierr);
    ierr = SNESGetSolution(snes,&u); CHKERRQ(ierr);
    ierr = SNESGetDM(snes,&da); CHKERRQ(ierr);
    ierr = DMDAGetLocalInfo(da,&info); CHKERRQ(ierr);

    // evaluate numerical error
    ierr = VecDuplicate(u,&u_exact); CHKERRQ(ierr);
    switch (problem) {
        case CONSTANT:
            ierr = VecSet(u_exact,1.0); CHKERRQ(ierr);
            break;
        case COSINES:
            ierr = GetVecFromFunction(&info,u_exact,&u_exact_cosines,&user); CHKERRQ(ierr);
            break;
    }
    ierr = VecAXPY(u,-1.0,u_exact); CHKERRQ(ierr);    // u <- u + (-1.0) uexact
    ierr = VecNorm(u,NORM_INFINITY,&err); CHKERRQ(ierr);
    ierr = PetscPrintf(PETSC_COMM_WORLD,
        "done on %d x %d grid with p=%.3f ...\n"
        "  numerical error:  |u-u_exact|_inf = %.3e\n",
        info.mx,info.my,user.p,err); CHKERRQ(ierr);

    VecDestroy(&u_exact);  SNESDestroy(&snes);
    return PetscFinalize();
}

PetscErrorCode GetVecFromFunction(DMDALocalInfo *info, Vec w,
         PetscReal (*fcn)(PetscReal, PetscReal, PetscReal, PetscReal),
         PHelmCtx *user) {
    PetscErrorCode  ierr;
    const PetscReal hx = 1.0 / (info->mx - 1), hy = 1.0 / (info->my - 1);
    PetscReal       x, y, **aw;
    PetscInt        i, j;
    ierr = DMDAVecGetArray(info->da,w,&aw); CHKERRQ(ierr);
    for (j = info->ys; j < info->ys + info->ym; j++) {
        y = j * hy;
        for (i = info->xs; i < info->xs + info->xm; i++) {
            x = i * hx;
            aw[j][i] = (*fcn)(x,y,user->p,user->eps);
        }
    }
    ierr = DMDAVecRestoreArray(info->da,w,&aw); CHKERRQ(ierr);
    return 0;
}

//STARTFEM
static PetscReal xiL[4]  = { 1.0, -1.0, -1.0,  1.0},
                 etaL[4] = { 1.0,  1.0, -1.0, -1.0};

static PetscReal chi(PetscInt L, PetscReal xi, PetscReal eta) {
    return 0.25 * (1.0 + xiL[L] * xi) * (1.0 + etaL[L] * eta);
}

// evaluate v(xi,eta) on reference element using local node numbering
static PetscReal eval(const PetscReal v[4], PetscReal xi, PetscReal eta) {
    return   v[0] * chi(0,xi,eta) + v[1] * chi(1,xi,eta)
           + v[2] * chi(2,xi,eta) + v[3] * chi(3,xi,eta);
}

typedef struct {
    PetscReal  xi, eta;
} gradRef;

static gradRef dchi(PetscInt L, PetscReal xi, PetscReal eta) {
    const gradRef result = {0.25 * xiL[L]  * (1.0 + etaL[L] * eta),
                            0.25 * etaL[L] * (1.0 + xiL[L]  * xi)};
    return result;
}

// evaluate partial derivs of v(xi,eta) on reference element
static gradRef deval(const PetscReal v[4], PetscReal xi, PetscReal eta) {
    gradRef   sum = {0.0,0.0}, tmp;
    PetscInt  L;
    for (L=0; L<4; L++) {
        tmp = dchi(L,xi,eta);
        sum.xi += v[L] * tmp.xi;  sum.eta += v[L] * tmp.eta;
    }
    return sum;
}

static PetscReal GradInnerProd(PetscReal hx, PetscReal hy,
                               gradRef du, gradRef dv) {
    const PetscReal cx = 4.0 / (hx * hx),  cy = 4.0 / (hy * hy);
    return cx * du.xi * dv.xi + cy * du.eta * dv.eta;
}

static PetscReal GradPow(PetscReal hx, PetscReal hy,
                         gradRef du, PetscReal P, PetscReal eps) {
    return PetscPowScalar(GradInnerProd(hx,hy,du,du) + eps*eps, P/2.0);
}
//ENDFEM

/* FLOPS:  (counting PetscPowScalar as 1)
     chi = 6
     eval = 4*6+7 = 31
     dchi = 8
     deval = 4*8+4 = 36
     GradInnerProd = 9
     GradPow = 9+4 = 13
     ObjIntegrandRef = deval + 2*eval + GradPow + 10 = 121
     FunIntegrandRef = chi + dchi + 2*eval + deval + GradPo + GradInnerProd + 9
                     = 143
*/

//STARTOBJECTIVE
static PetscReal ObjIntegrandRef(DMDALocalInfo *info,
                       const PetscReal ff[4], const PetscReal uu[4],
                       PetscReal xi, PetscReal eta, PHelmCtx *user) {
    const gradRef    du = deval(uu,xi,eta);
    const PetscReal  hx = 1.0 / (info->mx-1),  hy = 1.0 / (info->my-1),
                     u = eval(uu,xi,eta);
    return GradPow(hx,hy,du,user->p,0.0) / user->p + 0.5 * u * u
           - eval(ff,xi,eta) * u;
}

PetscErrorCode FormObjectiveLocal(DMDALocalInfo *info, PetscReal **au,
                                  PetscReal *obj, PHelmCtx *user) {
  PetscErrorCode  ierr;
  const PetscReal hx = 1.0 / (info->mx-1),  hy = 1.0 / (info->my-1);
  const Quad1D    q = gausslegendre[user->quadpts-1];
  PetscReal       x, y, lobj = 0.0;
  PetscInt        i,j,r,s;
  MPI_Comm        com;

  // loop over all elements
  for (j = info->ys; j < info->ys + info->ym; j++) {
      if (j == 0)
          continue;
      y = j * hy;
      for (i = info->xs; i < info->xs + info->xm; i++) {
          if (i == 0)
              continue;
          x = i * hx;
          const PetscReal ff[4] = {user->f(x,y,user->p,user->eps),
                                   user->f(x-hx,y,user->p,user->eps),
                                   user->f(x-hx,y-hy,user->p,user->eps),
                                   user->f(x,y-hy,user->p,user->eps)};
          const PetscReal uu[4] = {au[j][i],au[j][i-1],
                                   au[j-1][i-1],au[j-1][i]};
          // loop over quadrature points on this element
          for (r = 0; r < q.n; r++) {
              for (s = 0; s < q.n; s++) {
                  lobj += q.w[r] * q.w[s]
                          * ObjIntegrandRef(info,ff,uu,
                                            q.xi[r],q.xi[s],user);
              }
          }
      }
  }
  lobj *= hx * hy / 4.0;  // from change of variables formula
  ierr = PetscObjectGetComm((PetscObject)(info->da),&com); CHKERRQ(ierr);
  ierr = MPI_Allreduce(&lobj,obj,1,MPIU_REAL,MPIU_SUM,com); CHKERRQ(ierr);
  ierr = PetscLogFlops(129*info->xm*info->ym); CHKERRQ(ierr);
  return 0;
}
//ENDOBJECTIVE

//STARTFUNCTION
static PetscReal IntegrandRef(DMDALocalInfo *info, PetscInt L,
                     const PetscReal ff[4], const PetscReal uu[4],
                     PetscReal xi, PetscReal eta, PHelmCtx *user) {
  const gradRef    du    = deval(uu,xi,eta),
                   dchiL = dchi(L,xi,eta);
  const PetscReal  hx = 1.0 / (info->mx-1),  hy = 1.0 / (info->my-1);
  return GradPow(hx,hy,du,user->p - 2.0,user->eps)
           * GradInnerProd(hx,hy,du,dchiL)
         + (eval(uu,xi,eta) - eval(ff,xi,eta)) * chi(L,xi,eta);
}

PetscErrorCode FormFunctionLocal(DMDALocalInfo *info, PetscReal **au,
                                 PetscReal **FF, PHelmCtx *user) {
  PetscErrorCode ierr;
  const PetscReal hx = 1.0 / (info->mx-1),  hy = 1.0 / (info->my-1);
  const Quad1D    q = gausslegendre[user->quadpts-1];
  const PetscInt  li[4] = {0,-1,-1,0},  lj[4] = {0,0,-1,-1};
  PetscReal       x, y;
  PetscInt        i,j,l,r,s,PP,QQ;

  // clear residuals
  for (j = info->ys; j < info->ys + info->ym; j++)
      for (i = info->xs; i < info->xs + info->xm; i++)
          FF[j][i] = 0.0;

  // loop over all elements
  for (j = info->ys; j <= info->ys + info->ym; j++) {
      if ((j == 0) || (j > info->my-1))
          continue;
      y = j * hy;
      for (i = info->xs; i <= info->xs + info->xm; i++) {
          if ((i == 0) || (i > info->mx-1))
              continue;
          x = i * hx;
          const PetscReal ff[4] = {user->f(x,y,user->p,user->eps),
                                   user->f(x-hx,y,user->p,user->eps),
                                   user->f(x-hx,y-hy,user->p,user->eps),
                                   user->f(x,y-hy,user->p,user->eps)};
          const PetscReal uu[4] = {au[j][i],au[j][i-1],
                                   au[j-1][i-1],au[j-1][i]};
          // loop over corners of element i,j
          for (l = 0; l < 4; l++) {
              PP = i + li[l];
              QQ = j + lj[l];
              // only update residual if we own node
              if (PP >= info->xs && PP < info->xs + info->xm
                  && QQ >= info->ys && QQ < info->ys + info->ym) {
                  // loop over quadrature points
                  for (r = 0; r < q.n; r++) {
                      for (s = 0; s < q.n; s++) {
                         FF[QQ][PP]
                             += 0.25 * hx * hy * q.w[r] * q.w[s]
                                * IntegrandRef(info,l,ff,uu,
                                               q.xi[r],q.xi[s],user);
                      }
                  }
              }
          }
      }
  }
  ierr = PetscLogFlops((5+q.n*q.n*149)*(info->xm+1)*(info->ym+1)); CHKERRQ(ierr);
  return 0;
}
//ENDFUNCTION