// --------------------------------------------------------------
// File: ex10.c
// Description: Example with the sections directive
// Software stack: Exercises suite for V. Keller lecture
// Version: 1.0
// License: BSD
// Author: Vincent Keller (Vincent.Keller@epfl.ch), CADMOS, 2012
//
// COMPILATION AND LINKING
//
// WITHOUT MKL :  gcc -O3 -ftree-vectorize dgemm.c -lgsl -lgslcblas -lm -o dgemm
// WITH MKL : export MKLROOT=/software/intel/mkl; icc -DMKLINUSE -DMKL_ILP64 -openmp -I${MKLROOT}/include -O3 -xHost dgemm.c  -L${MKLROOT}/lib/intel64 -lmkl_intel_ilp64 -lmkl_core -lmkl_intel_thread -lpthread -lm -o dgemm
// --------------------------------------------------------------

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

#if defined(MKLINUSE)
#include "mkl.h"
#else
#include <gsl/gsl_blas.h>
#endif

double verification(double ** array, int N){
        double ret;
        int i,j;
        for (i=0;i<N;i++){
                for (j=0;j<N;j++){
                        ret+=array[i][j];
                }
        }
        return ret;
}

double second()
{
        struct timeval tp;
        int i;
        i = gettimeofday(&tp,NULL);
        return ( (double) tp.tv_sec + (double) tp.tv_usec * 1.e-6 );
}

int main( )
{
        int N, myrank,i,j,k,chunk, mysize;
        double ** A;
        double ** B;
        double ** C;
        double t1, t2;

        double *A_dgemm, *B_dgemm, *C_dgemm;
        double alpha, beta;

        mysize = 1;
        N=2000;

#if defined(MKLINUSE)
        A_dgemm = (double *)mkl_malloc( N*N*sizeof( double ), 64 );
        B_dgemm = (double *)mkl_malloc( N*N*sizeof( double ), 64 );
        C_dgemm = (double *)mkl_malloc( N*N*sizeof( double ), 64 );
#else
        A_dgemm = (double *)malloc( N*N*sizeof( double ));
        B_dgemm = (double *)malloc( N*N*sizeof( double ));
        C_dgemm = (double *)malloc( N*N*sizeof( double ));
#endif

        for (i = 0; i < (N*N); i++) {
                A_dgemm[i] = 1.0;
        }

        for (i = 0; i < (N*N); i++) {
                B_dgemm[i] = 2.0;
        }

        for (i = 0; i < (N*N); i++) {
                C_dgemm[i] = 0.0;
        }

        alpha= 1.0;
        beta = 0.0;

        A = (double**)malloc(N*sizeof(double*));
        B = (double**)malloc(N*sizeof(double*));
        C = (double**)malloc(N*sizeof(double*));

        for (i=0;i<N;i++){
                A[i]=(double*)malloc(N*sizeof(double));
                B[i]=(double*)malloc(N*sizeof(double));
                C[i]=(double*)malloc(N*sizeof(double));
        }

        for (i=0;i<N;i++){
                for (j=0;j<N;j++){
                        A[i][j] = 1.0;
                        B[i][j] = 2.0;
                        C[i][j] = 0.0;
                }
        }

// ============================================================================================================
        t1 = second();
        for (i=0;i<N;i++){
                for (j=0;j<N;j++){
                        for (k=0;k<N;k++){
                                C[i][j]=C[i][j] + A[i][k]*B[k][j];
                        }
                }
        }
        t2 = second();

        printf("[IJK]   Compute time [s]   : %6.3f \n", (t2-t1));
        printf("[IJK]   Performance  [GF/s]: %6.3f \n", (((double)N*(double)N*(double)N*2.0)/((t2-t1)*1.e9)));
        printf("[IJK]   Verification       : %6.3f \n\n", verification(C,N));

// ============================================================================================================
        for (i=0;i<N;i++){
                for (j=0;j<N;j++){
                        A[i][j] = 1.0;
                        B[i][j] = 2.0;
                        C[i][j] = 0.0;
                }
        }
        t1 = second();
        for (i=0;i<N;i++){
                for (k=0;k<N;k++){
                        for (j=0;j<N;j++){
                                C[i][j]=C[i][j] + A[i][k]*B[k][j];
                        }
                }
        }
        t2 = second();

        printf("[IKJ]   Compute time [s]   : %6.3f \n", (t2-t1));
        printf("[IKJ]   Performance  [GF/s]: %6.3f \n", (((double)N*(double)N*(double)N*2.0)/((t2-t1)*1.e9)));
        printf("[IKJ]   Verification       : %6.3f \n\n", verification(C,N));

// ============================================================================================================
        for (i=0;i<N;i++){
                for (j=0;j<N;j++){
                        A[i][j] = 1.0;
                        B[i][j] = 2.0;
                        C[i][j] = 0.0;
                }
        }
        t1 = second();
        for (j=0;j<N;j++){
                for (i=0;i<N;i++){
                        for (k=0;k<N;k++){
                                C[i][j]=C[i][j] + A[i][k]*B[k][j];
                        }
                }
        }
        t2 = second();

        printf("[JIK]   Compute time [s]   : %6.3f \n", (t2-t1));
        printf("[JIK]   Performance  [GF/s]: %6.3f \n", (((double)N*(double)N*(double)N*2.0)/((t2-t1)*1.e9)));
        printf("[JIK]   Verification       : %6.3f \n\n", verification(C,N));

// ============================================================================================================
        for (i=0;i<N;i++){
                for (j=0;j<N;j++){
                        A[i][j] = 1.0;
                        B[i][j] = 2.0;
                        C[i][j] = 0.0;
                }
        }
        t1 = second();
        for (j=0;j<N;j++){
                for (k=0;k<N;k++){
                        for (i=0;i<N;i++){
                                C[i][j]=C[i][j] + A[i][k]*B[k][j];
                        }
                }
        }
        t2 = second();

        printf("[JKI]   Compute time [s]   : %6.3f \n", (t2-t1));
        printf("[JKI]   Performance  [GF/s]: %6.3f \n", (((double)N*(double)N*(double)N*2.0)/((t2-t1)*1.e9)));
        printf("[JKI]   Verification       : %6.3f \n\n", verification(C,N));

// ============================================================================================================
        for (i=0;i<N;i++){
                for (j=0;j<N;j++){
                        A[i][j] = 1.0;
                        B[i][j] = 2.0;
                        C[i][j] = 0.0;
                }
        }
        t1 = second();
        for (k=0;k<N;k++){
                for (i=0;i<N;i++){
                        for (j=0;j<N;j++){
                                C[i][j]=C[i][j] + A[i][k]*B[k][j];
                        }
                }
        }
        t2 = second();

        printf("[KIJ]   Compute time [s]   : %6.3f \n", (t2-t1));
        printf("[KIJ]   Performance  [GF/s]: %6.3f \n", (((double)N*(double)N*(double)N*2.0)/((t2-t1)*1.e9)));
        printf("[KIJ]   Verification       : %6.3f \n\n", verification(C,N));

// ============================================================================================================
        for (i=0;i<N;i++){
                for (j=0;j<N;j++){
                        A[i][j] = 1.0;
                        B[i][j] = 2.0;
                        C[i][j] = 0.0;
                }
        }
        t1 = second();
        for (k=0;k<N;k++){
                for (j=0;j<N;j++){
                        for (i=0;i<N;i++){
                                C[i][j]=C[i][j] + A[i][k]*B[k][j];
                        }
                }
        }
        t2 = second();

        printf("[KJI]   Compute time [s]   : %6.3f \n", (t2-t1));
        printf("[KJI]   Performance  [GF/s]: %6.3f \n", (((double)N*(double)N*(double)N*2.0)/((t2-t1)*1.e9)));
        printf("[KJI]   Verification       : %6.3f \n\n", verification(C,N));

// ============================================================================================================
        t1 = second();
        cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, N,N,N, alpha, A_dgemm, N, B_dgemm, N, beta, C_dgemm, N);
        t2 = second();

        printf("[DGEMM OPT]   Compute time [s]   : %6.3f \n", (t2-t1));
        printf("[DGEMM OPT]   Performance  [GF/s]: %6.3f \n", (((double)N*(double)N*(double)N*2.0)/((t2-t1)*1.e9)));
        printf("[DGEMM OPT]   Verification       : %6.3f \n\n", verification(C,N));

        for (i=0;i<N;i++){
                free(A[i]);
                free(B[i]);
                free(C[i]);
        }

        free(A);
        free(B);
        free(C);

        return 0;
}


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

#include <config.h>

#include <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_multiroots.h>
#include <gsl/gsl_linalg.h>

#include "enorm.c"

/* Broyden's method. It is not an efficient or modern algorithm but
   gives an example of a rank-1 update.

   C.G. Broyden, "A Class of Methods for Solving Nonlinear
   Simultaneous Equations", Mathematics of Computation, vol 19 (1965),
   p 577-593

 */

typedef struct
  {
    gsl_matrix *H;
    gsl_matrix *lu;
    gsl_permutation *permutation;
    gsl_vector *v;
    gsl_vector *w;
    gsl_vector *y;
    gsl_vector *p;
    gsl_vector *fnew;
    gsl_vector *x_trial;
    double phi;
  }
broyden_state_t;

static int broyden_alloc (void *vstate, size_t n);
static int broyden_set (void *vstate, gsl_multiroot_function * function, gsl_vector * x, gsl_vector * f, gsl_vector * dx);
static int broyden_iterate (void *vstate, gsl_multiroot_function * function, gsl_vector * x, gsl_vector * f, gsl_vector * dx);
static void broyden_free (void *vstate);

static int
broyden_alloc (void *vstate, size_t n)
{
  broyden_state_t *state = (broyden_state_t *) vstate;
  gsl_vector *v, *w, *y, *fnew, *x_trial, *p;
  gsl_permutation *perm;
  gsl_matrix *m, *H;

  m = gsl_matrix_calloc (n, n);

  if (m == 0)
    {
      GSL_ERROR ("failed to allocate space for lu", GSL_ENOMEM);
    }

  state->lu = m;

  perm = gsl_permutation_calloc (n);

  if (perm == 0)
    {
      gsl_matrix_free (m);

      GSL_ERROR ("failed to allocate space for permutation", GSL_ENOMEM);
    }

  state->permutation = perm;

  H = gsl_matrix_calloc (n, n);

  if (H == 0)
    {
      gsl_permutation_free (perm);
      gsl_matrix_free (m);

      GSL_ERROR ("failed to allocate space for d", GSL_ENOMEM);
    }

  state->H = H;

  v = gsl_vector_calloc (n);

  if (v == 0)
    {
      gsl_matrix_free (H);
      gsl_permutation_free (perm);
      gsl_matrix_free (m);

      GSL_ERROR ("failed to allocate space for v", GSL_ENOMEM);
    }

  state->v = v;

  w = gsl_vector_calloc (n);

  if (w == 0)
    {
      gsl_vector_free (v);
      gsl_matrix_free (H);
      gsl_permutation_free (perm);
      gsl_matrix_free (m);

      GSL_ERROR ("failed to allocate space for w", GSL_ENOMEM);
    }

  state->w = w;

  y = gsl_vector_calloc (n);

  if (y == 0)
    {
      gsl_vector_free (w);
      gsl_vector_free (v);
      gsl_matrix_free (H);
      gsl_permutation_free (perm);
      gsl_matrix_free (m);

      GSL_ERROR ("failed to allocate space for y", GSL_ENOMEM);
    }

  state->y = y;

  fnew = gsl_vector_calloc (n);

  if (fnew == 0)
    {
      gsl_vector_free (y);
      gsl_vector_free (w);
      gsl_vector_free (v);
      gsl_matrix_free (H);
      gsl_permutation_free (perm);
      gsl_matrix_free (m);

      GSL_ERROR ("failed to allocate space for fnew", GSL_ENOMEM);
    }

  state->fnew = fnew;

  x_trial = gsl_vector_calloc (n);

  if (x_trial == 0)
    {
      gsl_vector_free (fnew);
      gsl_vector_free (y);
      gsl_vector_free (w);
      gsl_vector_free (v);
      gsl_matrix_free (H);
      gsl_permutation_free (perm);
      gsl_matrix_free (m);

      GSL_ERROR ("failed to allocate space for x_trial", GSL_ENOMEM);
    }

  state->x_trial = x_trial;

  p = gsl_vector_calloc (n);

  if (p == 0)
    {
      gsl_vector_free (x_trial);
      gsl_vector_free (fnew);
      gsl_vector_free (y);
      gsl_vector_free (w);
      gsl_vector_free (v);
      gsl_matrix_free (H);
      gsl_permutation_free (perm);
      gsl_matrix_free (m);

      GSL_ERROR ("failed to allocate space for p", GSL_ENOMEM);
    }

  state->p = p;

  return GSL_SUCCESS;
}

static int
broyden_set (void *vstate, gsl_multiroot_function * function, gsl_vector * x, gsl_vector * f, gsl_vector * dx)
{
  broyden_state_t *state = (broyden_state_t *) vstate;
  size_t i, j, n = function->n;
  int signum = 0;

  GSL_MULTIROOT_FN_EVAL (function, x, f);

  gsl_multiroot_fdjacobian (function, x, f, GSL_SQRT_DBL_EPSILON, state->lu);
  gsl_linalg_LU_decomp (state->lu, state->permutation, &signum);
  gsl_linalg_LU_invert (state->lu, state->permutation, state->H);

  for (i = 0; i < n; i++)
    for (j = 0; j < n; j++)
      gsl_matrix_set(state->H,i,j,-gsl_matrix_get(state->H,i,j));

  for (i = 0; i < n; i++)
    {
      gsl_vector_set (dx, i, 0.0);
    }

  state->phi = enorm (f);

  return GSL_SUCCESS;
}

static int
broyden_iterate (void *vstate, gsl_multiroot_function * function, gsl_vector * x, gsl_vector * f, gsl_vector * dx)
{
  broyden_state_t *state = (broyden_state_t *) vstate;

  double phi0, phi1, t, lambda;

  gsl_matrix *H = state->H;
  gsl_vector *p = state->p;
  gsl_vector *y = state->y;
  gsl_vector *v = state->v;
  gsl_vector *w = state->w;
  gsl_vector *fnew = state->fnew;
  gsl_vector *x_trial = state->x_trial;
  gsl_matrix *lu = state->lu;
  gsl_permutation *perm = state->permutation;

  size_t i, j, iter;

  size_t n = function->n;

  /* p = H f */

  for (i = 0; i < n; i++)
    {
      double sum = 0;

      for (j = 0; j < n; j++)
        {
          sum += gsl_matrix_get (H, i, j) * gsl_vector_get (f, j);
        }
      gsl_vector_set (p, i, sum);
    }

  t = 1;
  iter = 0;

  phi0 = state->phi;

new_step:

  for (i = 0; i < n; i++)
    {
      double pi = gsl_vector_get (p, i);
      double xi = gsl_vector_get (x, i);
      gsl_vector_set (x_trial, i, xi + t * pi);
    }

  {
    int status = GSL_MULTIROOT_FN_EVAL (function, x_trial, fnew);

    if (status != GSL_SUCCESS)
      {
        return GSL_EBADFUNC;
      }
  }

  phi1 = enorm (fnew);

  iter++ ;

  if (phi1 > phi0 && iter < 10 && t > 0.1)
    {
      /* full step goes uphill, take a reduced step instead */

      double theta = phi1 / phi0;
      t *= (sqrt (1.0 + 6.0 * theta) - 1.0) / (3.0 * theta);
      goto new_step;
    }

  if (phi1 > phi0)
    {
      /* need to recompute Jacobian */
      int signum = 0;

      gsl_multiroot_fdjacobian (function, x, f, GSL_SQRT_DBL_EPSILON, lu);

      for (i = 0; i < n; i++)
        for (j = 0; j < n; j++)
          gsl_matrix_set(lu,i,j,-gsl_matrix_get(lu,i,j));

      gsl_linalg_LU_decomp (lu, perm, &signum);
      gsl_linalg_LU_invert (lu, perm, H);

      gsl_linalg_LU_solve (lu, perm, f, p);

      t = 1;

      for (i = 0; i < n; i++)
        {
          double pi = gsl_vector_get (p, i);
          double xi = gsl_vector_get (x, i);
          gsl_vector_set (x_trial, i, xi + t * pi);
        }

      {
        int status = GSL_MULTIROOT_FN_EVAL (function, x_trial, fnew);

        if (status != GSL_SUCCESS)
          {
            return GSL_EBADFUNC;
          }
      }

      phi1 = enorm (fnew);
    }

  /* y = f' - f */

  for (i = 0; i < n; i++)
    {
      double yi = gsl_vector_get (fnew, i) - gsl_vector_get (f, i);
      gsl_vector_set (y, i, yi);
    }

  /* v = H y */

  for (i = 0; i < n; i++)
    {
      double sum = 0;

      for (j = 0; j < n; j++)
        {
          sum += gsl_matrix_get (H, i, j) * gsl_vector_get (y, j);
        }

      gsl_vector_set (v, i, sum);
    }

  /* lambda = p . v */

  lambda = 0;

  for (i = 0; i < n; i++)
    {
      lambda += gsl_vector_get (p, i) * gsl_vector_get (v, i);
    }

  if (lambda == 0)
    {
      GSL_ERROR ("approximation to Jacobian has collapsed", GSL_EZERODIV) ;
    }

  /* v' = v + t * p */

  for (i = 0; i < n; i++)
    {
      double vi = gsl_vector_get (v, i) + t * gsl_vector_get (p, i);
      gsl_vector_set (v, i, vi);
    }

  /* w^T = p^T H */

  for (i = 0; i < n; i++)
    {
      double sum = 0;

      for (j = 0; j < n; j++)
        {
          sum += gsl_matrix_get (H, j, i) * gsl_vector_get (p, j);
        }

      gsl_vector_set (w, i, sum);
    }

  /* Hij -> Hij - (vi wj / lambda) */

  for (i = 0; i < n; i++)
    {
      double vi = gsl_vector_get (v, i);

      for (j = 0; j < n; j++)
        {
          double wj = gsl_vector_get (w, j);
          double Hij = gsl_matrix_get (H, i, j) - vi * wj / lambda;
          gsl_matrix_set (H, i, j, Hij);
        }
    }

  /* copy fnew into f */

  gsl_vector_memcpy (f, fnew);

  /* copy x_trial into x */

  gsl_vector_memcpy (x, x_trial);

  for (i = 0; i < n; i++)
    {
      double pi = gsl_vector_get (p, i);
      gsl_vector_set (dx, i, t * pi);
    }

  state->phi = phi1;

  return GSL_SUCCESS;
}

static void
broyden_free (void *vstate)
{
  broyden_state_t *state = (broyden_state_t *) vstate;

  gsl_matrix_free (state->H);

  gsl_matrix_free (state->lu);
  gsl_permutation_free (state->permutation);

  gsl_vector_free (state->v);
  gsl_vector_free (state->w);
  gsl_vector_free (state->y);
  gsl_vector_free (state->p);

  gsl_vector_free (state->fnew);
  gsl_vector_free (state->x_trial);

}

static const gsl_multiroot_fsolver_type broyden_type =
{"broyden",                     /* name */
 sizeof (broyden_state_t),
 &broyden_alloc,
 &broyden_set,
 &broyden_iterate,
 &broyden_free};

const gsl_multiroot_fsolver_type *gsl_multiroot_fsolver_broyden = &broyden_type;


static char help[] =
"Unstructured 2D FEM solution of nonlinear Poisson problem.  Option prefix un_.\n"
"Equation is\n"
"    - div( a(u,x,y) grad u ) = f(u,x,y)\n"
"on arbitrary 2D polygonal domain, with boundary data g_D(x,y), g_N(x,y).\n"
"Functions a(), f(), g_D(), g_N(), and u_exact() are given as formulas.\n"
"Input files in PETSc binary format contain node coordinates, elements, Neumann\n"
"boundary segments and boundary flags.  Allows non-homogeneous Dirichlet and/or\n"
"Neumann boundary conditions.  Five different solution cases are implemented.\n\n";

#include <petsc.h>
#include "../quadrature.h"
#include "um.h"
#include "cases.h"

//STARTCTX
typedef struct {
    UM     *mesh;
    int    solncase,
           quaddegree;
    double (*a_fcn)(double, double, double);
    double (*f_fcn)(double, double, double);
    double (*gD_fcn)(double, double);
    double (*gN_fcn)(double, double);
    double (*uexact_fcn)(double, double);
    PetscLogStage readstage, setupstage, solverstage, resstage, jacstage;  //STRIP
} unfemCtx;
//ENDCTX

//STARTFEM
double chi(int L, double xi, double eta) {
    const double z[3] = {1.0 - xi - eta, xi, eta};
    return z[L];
}

const double dchi[3][2] = {{-1.0,-1.0},{ 1.0, 0.0},{ 0.0, 1.0}};

// evaluate v(xi,eta) on reference element using local node numbering
double eval(const double v[3], double xi, double eta) {
    double sum = 0.0;
    int    L;
    for (L = 0; L < 3; L++)
        sum += v[L] * chi(L,xi,eta);
    return sum;
}

double InnerProd(const double V[2], const double W[2]) {
    return V[0] * W[0] + V[1] * W[1];
}
//ENDFEM

extern PetscErrorCode FillExact(Vec, unfemCtx*);
extern PetscErrorCode FormFunction(SNES, Vec, Vec, void*);
extern PetscErrorCode FormPicard(SNES, Vec, Mat, Mat, void*);
extern PetscErrorCode Preallocation(Mat, unfemCtx*);

int main(int argc,char **argv) {
    PetscErrorCode ierr;
    PetscBool   viewmesh = PETSC_FALSE,
                viewsoln = PETSC_FALSE,
                noprealloc = PETSC_FALSE,
                savepintbinary = PETSC_FALSE,
                savepintmatlab = PETSC_FALSE;
    char        root[256] = "", nodesname[256], issname[256], solnname[256],
                pintname[256] = "";
    int         savepintlevel = -1, levels;
    UM          mesh;
    unfemCtx    user;
    SNES        snes;
    KSP         ksp;
    PC          pc;
    PCType      pctype;
    Mat         A;
    Vec         r, u, uexact;
    double      err, h_max;

    PetscInitialize(&argc,&argv,NULL,help);
    ierr = PetscLogStageRegister("Read mesh      ", &user.readstage); CHKERRQ(ierr);  //STRIP
    ierr = PetscLogStageRegister("Set-up         ", &user.setupstage); CHKERRQ(ierr);  //STRIP
    ierr = PetscLogStageRegister("Solver         ", &user.solverstage); CHKERRQ(ierr);  //STRIP
    ierr = PetscLogStageRegister("Residual eval  ", &user.resstage); CHKERRQ(ierr);  //STRIP
    ierr = PetscLogStageRegister("Jacobian eval  ", &user.jacstage); CHKERRQ(ierr);  //STRIP

    user.quaddegree = 1;
    user.solncase = 0;
    ierr = PetscOptionsBegin(PETSC_COMM_WORLD, "un_", "options for unfem", ""); CHKERRQ(ierr);
    ierr = PetscOptionsInt("-case",
           "exact solution cases: 0=linear, 1=nonlinear, 2=nonhomoNeumann, 3=chapter3, 4=koch",
           "unfem.c",user.solncase,&(user.solncase),NULL); CHKERRQ(ierr);
    ierr = PetscOptionsString("-gamg_save_pint_binary",
           "filename under which to save interpolation operator (Mat) in PETSc binary format",
           "unfem.c",pintname,pintname,sizeof(pintname),&savepintbinary); CHKERRQ(ierr);
    ierr = PetscOptionsString("-gamg_save_pint_matlab",
           "filename under which to save interpolation operator (Mat) in ascii Matlab format",
           "unfem.c",pintname,pintname,sizeof(pintname),&savepintmatlab); CHKERRQ(ierr);
    ierr = PetscOptionsInt("-gamg_save_pint_level",
           "saved interpolation operator is between L-1 and L where this option sets L; defaults to finest levels",
           "unfem.c",savepintlevel,&savepintlevel,NULL); CHKERRQ(ierr);
    ierr = PetscOptionsString("-mesh",
           "file name root of mesh stored in PETSc binary with .vec,.is extensions",
           "unfem.c",root,root,sizeof(root),NULL); CHKERRQ(ierr);
    ierr = PetscOptionsBool("-noprealloc",
           "do not perform preallocation before matrix assembly",
           "unfem.c",noprealloc,&noprealloc,NULL); CHKERRQ(ierr);
    ierr = PetscOptionsInt("-quaddegree",
           "quadrature degree (1,2,3)",
           "unfem.c",user.quaddegree,&(user.quaddegree),NULL); CHKERRQ(ierr);
    ierr = PetscOptionsBool("-view_mesh",
           "view loaded mesh (nodes and elements) at stdout",
           "unfem.c",viewmesh,&viewmesh,NULL); CHKERRQ(ierr);
    ierr = PetscOptionsBool("-view_solution",
           "view solution u(x,y) to binary file; uses root name of mesh plus .soln\nsee petsc2tricontour.py to view graphically",
           "unfem.c",viewsoln,&viewsoln,NULL); CHKERRQ(ierr);
    ierr = PetscOptionsEnd(); CHKERRQ(ierr);

    // determine filenames
    if (strlen(root) == 0) {
        SETERRQ(PETSC_COMM_WORLD,4,"no mesh name root given; rerun with '-un_mesh foo'");
    }
    strcpy(nodesname, root);
    strncat(nodesname, ".vec", 4);
    strcpy(issname, root);
    strncat(issname, ".is", 3);

    // set source/boundary functions and exact solution
    user.a_fcn = &a_lin;
    user.f_fcn = &f_lin;
    user.uexact_fcn = &uexact_lin;
    user.gD_fcn = &gD_lin;
    user.gN_fcn = &gN_lin;
    switch (user.solncase) {
        case 0 :
            break;
        case 1 :
            user.a_fcn = &a_nonlin;
            user.f_fcn = &f_nonlin;
            break;
        case 2 :
            user.gN_fcn = &gN_linneu;
            break;
        case 3 :
            user.a_fcn = &a_square;
            user.f_fcn = &f_square;
            user.uexact_fcn = &uexact_square;
            user.gD_fcn = &gD_square;
            user.gN_fcn = NULL;  // seg fault if ever called
            break;
        case 4 :
            user.a_fcn = &a_koch;
            user.f_fcn = &f_koch;
            user.uexact_fcn = NULL;  // seg fault if ever called
            user.gD_fcn = &gD_koch;
            user.gN_fcn = NULL;  // seg fault if ever called
            break;
        default :
            SETERRQ(PETSC_COMM_WORLD,1,"other solution cases not implemented");
    }

    PetscLogStagePush(user.readstage);
//STARTREADMESH
    // read mesh object of type UM
    ierr = UMInitialize(&mesh); CHKERRQ(ierr);
    ierr = UMReadNodes(&mesh,nodesname); CHKERRQ(ierr);
    ierr = UMReadISs(&mesh,issname); CHKERRQ(ierr);
    ierr = UMStats(&mesh, &h_max, NULL, NULL, NULL); CHKERRQ(ierr);
    user.mesh = &mesh;
//ENDREADMESH
    PetscLogStagePop();

    if (viewmesh) {
        PetscViewer stdoutviewer;
        ierr = PetscViewerASCIIGetStdout(PETSC_COMM_WORLD,&stdoutviewer); CHKERRQ(ierr);
        ierr = UMViewASCII(&mesh,stdoutviewer); CHKERRQ(ierr);
    }

    PetscLogStagePush(user.setupstage);
//STARTMAININITIAL
    // configure Vecs and SNES
    ierr = VecCreate(PETSC_COMM_WORLD,&r); CHKERRQ(ierr);
    ierr = VecSetSizes(r,PETSC_DECIDE,mesh.N); CHKERRQ(ierr);
    ierr = VecSetFromOptions(r); CHKERRQ(ierr);
    ierr = VecDuplicate(r,&u); CHKERRQ(ierr);
    ierr = VecSet(u,0.0); CHKERRQ(ierr);
    ierr = SNESCreate(PETSC_COMM_WORLD,&snes); CHKERRQ(ierr);
    ierr = SNESSetFunction(snes,r,FormFunction,&user); CHKERRQ(ierr);

    // reset default KSP and PC
    ierr = SNESGetKSP(snes,&ksp); CHKERRQ(ierr);
    ierr = KSPSetType(ksp,KSPCG); CHKERRQ(ierr);
    ierr = KSPGetPC(ksp,&pc); CHKERRQ(ierr);
    ierr = PCSetType(pc,PCICC); CHKERRQ(ierr);

    // setup matrix for Picard iteration, including preallocation
    ierr = MatCreate(PETSC_COMM_WORLD,&A); CHKERRQ(ierr);
    ierr = MatSetSizes(A,PETSC_DECIDE,PETSC_DECIDE,mesh.N,mesh.N); CHKERRQ(ierr);
    ierr = MatSetFromOptions(A); CHKERRQ(ierr);
    ierr = MatSetOption(A,MAT_SYMMETRIC,PETSC_TRUE); CHKERRQ(ierr);
    if (noprealloc) {
        ierr = MatSetUp(A); CHKERRQ(ierr);
    } else {
        ierr = Preallocation(A,&user); CHKERRQ(ierr);
    }
    ierr = SNESSetJacobian(snes,A,A,FormPicard,&user); CHKERRQ(ierr);
    ierr = SNESSetFromOptions(snes); CHKERRQ(ierr);
    PetscLogStagePop();  //STRIP

    // solve
    PetscLogStagePush(user.solverstage);  //STRIP
    ierr = SNESSolve(snes,NULL,u);CHKERRQ(ierr);
//ENDMAININITIAL
    PetscLogStagePop();

    // report if PC is GAMG
    ierr = PCGetType(pc,&pctype); CHKERRQ(ierr);
    if (strcmp(pctype,"gamg") == 0) {
        ierr = PCMGGetLevels(pc,&levels); CHKERRQ(ierr);
        ierr = PetscPrintf(PETSC_COMM_WORLD,
               "  PC is GAMG with %d levels\n",levels); CHKERRQ(ierr);
    }

    // save Pint from GAMG if requested
    if (strlen(pintname) > 0) {
        Mat         pint;
        PetscViewer viewer;
        if (strcmp(pctype,"gamg") != 0) {
            SETERRQ(PETSC_COMM_WORLD,2,"option -un_gamg_save_pint set but PC is not of type PCGAMG");
        }
        if (savepintlevel >= levels) {
            SETERRQ(PETSC_COMM_WORLD,3,"invalid level given in -un_gamg_save_pint_level");
        }
        if (savepintbinary && savepintmatlab) {
            SETERRQ(PETSC_COMM_WORLD,4,"only one of -un_gamg_save_pint_binary OR -un_gamg_save_pint_matlab is allowed");
        }
        if (savepintlevel <= 0) {
            savepintlevel = levels - 1;
        }
        ierr = PCMGGetInterpolation(pc,savepintlevel,&pint); CHKERRQ(ierr);
        if (savepintbinary) {
            ierr = PetscPrintf(PETSC_COMM_WORLD,
               "  saving interpolation operator on levels %d->%d in binary format to %s ...\n",
               savepintlevel-1,savepintlevel,pintname); CHKERRQ(ierr);
            ierr = PetscViewerBinaryOpen(PETSC_COMM_WORLD,pintname,FILE_MODE_WRITE,&viewer); CHKERRQ(ierr);
            ierr = PetscViewerPushFormat(viewer,PETSC_VIEWER_ASCII_MATLAB); CHKERRQ(ierr);
        } else {
            ierr = PetscPrintf(PETSC_COMM_WORLD,
               "  saving interpolation operator on levels %d->%d in ascii format to %s ...\n",
               savepintlevel-1,savepintlevel,pintname); CHKERRQ(ierr);
            ierr = PetscViewerASCIIOpen(PETSC_COMM_WORLD,pintname,&viewer); CHKERRQ(ierr);
            ierr = PetscViewerPushFormat(viewer,PETSC_VIEWER_ASCII_MATLAB); CHKERRQ(ierr);
        }
        ierr = MatView(pint,viewer); CHKERRQ(ierr);
    }

    // if exact solution available, report numerical error
    if (user.uexact_fcn) {
        ierr = VecDuplicate(r,&uexact); CHKERRQ(ierr);
        ierr = FillExact(uexact,&user); CHKERRQ(ierr);
        ierr = VecAXPY(u,-1.0,uexact); CHKERRQ(ierr);    // u <- u + (-1.0) uexact
        ierr = VecNorm(u,NORM_INFINITY,&err); CHKERRQ(ierr);
        ierr = PetscPrintf(PETSC_COMM_WORLD,
                   "case %d result for N=%d nodes with h = %.3e :  |u-u_ex|_inf = %.2e\n",
                   user.solncase,mesh.N,h_max,err); CHKERRQ(ierr);
        VecDestroy(&uexact);
    } else {
        ierr = PetscPrintf(PETSC_COMM_WORLD,
                   "case %d result for N=%d nodes with h = %.3e ... done\n",
                   user.solncase,mesh.N,h_max); CHKERRQ(ierr);
    }

    // save solution in PETSc binary if requested
    if (viewsoln) {
        strcpy(solnname, root);
        strncat(solnname, ".soln", 5);
        ierr = PetscPrintf(PETSC_COMM_WORLD,
                   "writing solution in binary format to %s ...\n",solnname); CHKERRQ(ierr);
        ierr = UMViewSolutionBinary(&mesh,solnname,u); CHKERRQ(ierr);
    }

    // clean-up
    VecDestroy(&u);  VecDestroy(&r);
    MatDestroy(&A);  SNESDestroy(&snes);  UMDestroy(&mesh);
    return PetscFinalize();
}

PetscErrorCode FillExact(Vec uexact, unfemCtx *ctx) {
    PetscErrorCode ierr;
    const Node   *aloc;
    double       *auexact;
    int          i;
    ierr = UMGetNodeCoordArrayRead(ctx->mesh,&aloc); CHKERRQ(ierr);
    ierr = VecGetArray(uexact,&auexact); CHKERRQ(ierr);
    for (i = 0; i < ctx->mesh->N; i++) {
        auexact[i] = ctx->uexact_fcn(aloc[i].x,aloc[i].y);
    }
    ierr = VecRestoreArray(uexact,&auexact); CHKERRQ(ierr);
    ierr = UMRestoreNodeCoordArrayRead(ctx->mesh,&aloc); CHKERRQ(ierr);
    return 0;
}

//STARTRESIDUAL
PetscErrorCode FormFunction(SNES snes, Vec u, Vec F, void *ctx) {
    PetscErrorCode ierr;
    unfemCtx        *user = (unfemCtx*)ctx;
    const Quad2DTri q = symmgauss[user->quaddegree-1];
    const int       *ae, *ans, *abf, *en;
    const Node      *aloc;
    const double    *au;
    int             p, na, nb, k, l, r;
    double          *aF, unode[3], gradu[2], gradpsi[3][2], uquad[4],
                    aquad[4], fquad[4], dx, dy, dx1, dx2, dy1, dy2,
                    detJ, ls, xmid, ymid, sint, xx, yy, psi, ip, sum;

    PetscLogStagePush(user->resstage);  //STRIP
    ierr = VecSet(F,0.0); CHKERRQ(ierr);
    ierr = VecGetArray(F,&aF); CHKERRQ(ierr);
    ierr = UMGetNodeCoordArrayRead(user->mesh,&aloc); CHKERRQ(ierr);
    ierr = ISGetIndices(user->mesh->bf,&abf); CHKERRQ(ierr);

    // Neumann boundary segment contributions (if any)
    if (user->mesh->P > 0) {
        ierr = ISGetIndices(user->mesh->ns,&ans); CHKERRQ(ierr);
        for (p = 0; p < user->mesh->P; p++) {
            na = ans[2*p+0];  nb = ans[2*p+1];  // nodes at end of segment
            dx = aloc[na].x-aloc[nb].x;  dy = aloc[na].y-aloc[nb].y;
            ls = sqrt(dx * dx + dy * dy);  // length of segment
            // midpoint rule; psi_na=psi_nb=0.5 at midpoint of segment
            xmid = 0.5*(aloc[na].x+aloc[nb].x);
            ymid = 0.5*(aloc[na].y+aloc[nb].y);
            sint = 0.5 * ls * user->gN_fcn(xmid,ymid);
            // nodes could be Dirichlet
            if (abf[na] != 2)
                aF[na] -= sint;
            if (abf[nb] != 2)
                aF[nb] -= sint;
        }
        ierr = ISRestoreIndices(user->mesh->ns,&ans); CHKERRQ(ierr);
    }

    // element contributions and Dirichlet node residuals
    ierr = VecGetArrayRead(u,&au); CHKERRQ(ierr);
    ierr = ISGetIndices(user->mesh->e,&ae); CHKERRQ(ierr);
    for (k = 0; k < user->mesh->K; k++) {
        // element geometry and hat function gradients
        en = ae + 3*k;  // en[0], en[1], en[2] are nodes of element k
        dx1 = aloc[en[1]].x - aloc[en[0]].x;
        dx2 = aloc[en[2]].x - aloc[en[0]].x;
        dy1 = aloc[en[1]].y - aloc[en[0]].y;
        dy2 = aloc[en[2]].y - aloc[en[0]].y;
        detJ = dx1 * dy2 - dx2 * dy1;
        for (l = 0; l < 3; l++) {
            gradpsi[l][0] = ( dy2 * dchi[l][0] - dy1 * dchi[l][1]) / detJ;
            gradpsi[l][1] = (-dx2 * dchi[l][0] + dx1 * dchi[l][1]) / detJ;
        }
        // u and grad u on element
        gradu[0] = 0.0;
        gradu[1] = 0.0;
        for (l = 0; l < 3; l++) {
            if (abf[en[l]] == 2)  // enforces symmetry
                unode[l] = user->gD_fcn(aloc[en[l]].x,aloc[en[l]].y);
            else
                unode[l] = au[en[l]];
            gradu[0] += unode[l] * gradpsi[l][0];
            gradu[1] += unode[l] * gradpsi[l][1];
        }
        // function values at quadrature points on element
        for (r = 0; r < q.n; r++) {
            uquad[r] = eval(unode,q.xi[r],q.eta[r]);
            xx = aloc[en[0]].x + dx1 * q.xi[r] + dx2 * q.eta[r];
            yy = aloc[en[0]].y + dy1 * q.xi[r] + dy2 * q.eta[r];
            aquad[r] = user->a_fcn(uquad[r],xx,yy);
            fquad[r] = user->f_fcn(uquad[r],xx,yy);
        }
        // residual contribution for each node of element
        for (l = 0; l < 3; l++) {
            if (abf[en[l]] == 2) { // set Dirichlet residual
                xx = aloc[en[l]].x;   yy = aloc[en[l]].y;
                aF[en[l]] = au[en[l]] - user->gD_fcn(xx,yy);
            } else {
                sum = 0.0;
                for (r = 0; r < q.n; r++) {
                    psi = chi(l,q.xi[r],q.eta[r]);
                    ip  = InnerProd(gradu,gradpsi[l]);
                    sum += q.w[r] * ( aquad[r] * ip - fquad[r] * psi );
                }
                aF[en[l]] += fabs(detJ) * sum;
            }
        }
    }

    ierr = ISRestoreIndices(user->mesh->e,&ae); CHKERRQ(ierr);
    ierr = VecRestoreArrayRead(u,&au); CHKERRQ(ierr);
    ierr = ISRestoreIndices(user->mesh->bf,&abf); CHKERRQ(ierr);
    ierr = UMRestoreNodeCoordArrayRead(user->mesh,&aloc); CHKERRQ(ierr);
    ierr = VecRestoreArray(F,&aF); CHKERRQ(ierr);
    PetscLogStagePop();  //STRIP
    return 0;
}
//ENDRESIDUAL

PetscErrorCode FormPicard(SNES snes, Vec u, Mat A, Mat P, void *ctx) {
    PetscErrorCode ierr;
    unfemCtx        *user = (unfemCtx*)ctx;
    const Quad2DTri q = symmgauss[user->quaddegree-1];
    const int       *ae, *abf, *en;
    const Node      *aloc;
    const double    *au;
    double          unode[3], gradpsi[3][2], uquad[4], aquad[4], v[9],
                    dx1, dx2, dy1, dy2, detJ, xx, yy, sum;
    int             n, k, l, m, r, cr, cv, row[3];

    PetscLogStagePush(user->jacstage);  //STRIP
    ierr = MatZeroEntries(P); CHKERRQ(ierr);
    ierr = ISGetIndices(user->mesh->bf,&abf); CHKERRQ(ierr);
    for (n = 0; n < user->mesh->N; n++) {
        if (abf[n] == 2) {
            v[0] = 1.0;
            ierr = MatSetValues(P,1,&n,1,&n,v,ADD_VALUES); CHKERRQ(ierr);
        }
    }
    ierr = ISGetIndices(user->mesh->e,&ae); CHKERRQ(ierr);
    ierr = VecGetArrayRead(u,&au); CHKERRQ(ierr);
    ierr = UMGetNodeCoordArrayRead(user->mesh,&aloc); CHKERRQ(ierr);
    for (k = 0; k < user->mesh->K; k++) {
        en = ae + 3*k;  // en[0], en[1], en[2] are nodes of element k
        // geometry of element
        dx1 = aloc[en[1]].x - aloc[en[0]].x;
        dx2 = aloc[en[2]].x - aloc[en[0]].x;
        dy1 = aloc[en[1]].y - aloc[en[0]].y;
        dy2 = aloc[en[2]].y - aloc[en[0]].y;
        detJ = dx1 * dy2 - dx2 * dy1;
        // gradients of hat functions and u on element
        for (l = 0; l < 3; l++) {
            gradpsi[l][0] = ( dy2 * dchi[l][0] - dy1 * dchi[l][1]) / detJ;
            gradpsi[l][1] = (-dx2 * dchi[l][0] + dx1 * dchi[l][1]) / detJ;
            if (abf[en[l]] == 2)
                unode[l] = user->gD_fcn(aloc[en[l]].x,aloc[en[l]].y);
            else
                unode[l] = au[en[l]];
        }
        // function values at quadrature points on element
        for (r = 0; r < q.n; r++) {
            uquad[r] = eval(unode,q.xi[r],q.eta[r]);
            xx = aloc[en[0]].x + dx1 * q.xi[r] + dx2 * q.eta[r];
            yy = aloc[en[0]].y + dy1 * q.xi[r] + dy2 * q.eta[r];
            aquad[r] = user->a_fcn(uquad[r],xx,yy);
        }
        // generate 3x3 element stiffness matrix (3x3 is max size but may be smaller)
        cr = 0;  cv = 0;  // cr = count rows; cv = value counter for all entries
        for (l = 0; l < 3; l++) {
            if (abf[en[l]] != 2) {
                row[cr++] = en[l];
                for (m = 0; m < 3; m++) {
                    if (abf[en[m]] != 2) {
                        sum = 0.0;
                        for (r = 0; r < q.n; r++) {
                            sum += q.w[r] * aquad[r] * InnerProd(gradpsi[l],gradpsi[m]);
                        }
                        v[cv++] = fabs(detJ) * sum;
                    }
                }
            }
        }
        // insert element stiffness matrix
        ierr = MatSetValues(P,cr,row,cr,row,v,ADD_VALUES); CHKERRQ(ierr);
    }
    ierr = ISRestoreIndices(user->mesh->e,&ae); CHKERRQ(ierr);
    ierr = ISRestoreIndices(user->mesh->bf,&abf); CHKERRQ(ierr);
    ierr = VecRestoreArrayRead(u,&au); CHKERRQ(ierr);
    ierr = UMRestoreNodeCoordArrayRead(user->mesh,&aloc); CHKERRQ(ierr);

    ierr = MatAssemblyBegin(P,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    ierr = MatAssemblyEnd(P,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    if (A != P) {
        ierr = MatAssemblyBegin(A,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
        ierr = MatAssemblyEnd(A,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    }
    ierr = MatSetOption(P,MAT_NEW_NONZERO_LOCATION_ERR,PETSC_TRUE); CHKERRQ(ierr);
    PetscLogStagePop();  //STRIP
    return 0;
}

/* Note that nnz[n] is the number of nonzeros in row n.  It equals one for
Dirichlet rows, one more than the number of incident triangles for an
interior point, and two more than the number of incident triangles for
Neumann boundary nodes. */
//STARTPREALLOC
PetscErrorCode Preallocation(Mat J, unfemCtx *user) {
    PetscErrorCode ierr;
    const int    *ae, *abf, *en;
    int          *nnz, n, k, l;

    nnz = (int *)malloc(sizeof(int)*(user->mesh->N));
    ierr = ISGetIndices(user->mesh->bf,&abf); CHKERRQ(ierr);
    for (n = 0; n < user->mesh->N; n++)
        nnz[n] = (abf[n] == 1) ? 2 : 1;
    ierr = ISGetIndices(user->mesh->e,&ae); CHKERRQ(ierr);
    for (k = 0; k < user->mesh->K; k++) {
        en = ae + 3*k;  // en[0], en[1], en[2] are nodes of element k
        for (l = 0; l < 3; l++)
            if (abf[en[l]] != 2)
                nnz[en[l]] += 1;
    }
    ierr = ISRestoreIndices(user->mesh->e,&ae); CHKERRQ(ierr);
    ierr = ISRestoreIndices(user->mesh->bf,&abf); CHKERRQ(ierr);
    ierr = MatSeqAIJSetPreallocation(J,-1,nnz); CHKERRQ(ierr);
    free(nnz);
    return 0;
}
//ENDPREALLOC


#include <cerrno>
#include <cstdio>
#include <cstdlib>
#include <sys/stat.h>
#include <cmath>
#include <vector>
#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <time.h>

#include <gsl/gsl_odeiv2.h>
#include <gsl/gsl_math.h>
#include <math.h>       /* pow */
#include <gsl/gsl_sf.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_spline.h>

// Common functions to all  .cpps

/* LCDM Hubble in 1/s */
double hubble(double om, double orad, double a){
	double a3 = pow(a,3);
	double a4 = a*a3;
	double ol = 1.-om-orad;
	return h0 * sqrt(om/a3+ orad/a4 + ol);
}

// Reheating time as a function of temperature --- Trh should be given in Kelvin
double timeofrh(double Trh){
	return sqrt(5./pow(M_PI,3)/gstar) * pow(10,lgmp)/pow(Trh*keltgev,2) *kgtgev / gevtds ;
}

/* Critical density in kg/m^3 */
double rhoc(double a){
	return 3.*pow(hubble(om, orad, a),2.)/(8.*M_PI*gnewton);
}

/* Omega_x for LCDM */
double omegalcdm(double ox, double a){
	return  ox/pow(a,3.) * pow(h0/hubble(om,orad,a),2.);
}

/* Log10 of Mass spectrum in units 1/kg */
// hi mass - Eq.2.5 of draft
//  Psi = 10^psihilg   as a function of Log10[M] = lgm and omega_pbh
double psihilg(double lgm, double opbh){
	double fpbh = opbh/oc;
	return -12.2764 + log10(fpbh) - 3./2. * lgm;
}

// low mass - Eq.2.2 of draft
//  Psi = 10^psilowlg  as a function of Log10[M] = lgm and peak mass
double psilowlg(double lgm, double peakm){
	double aofmf = (peakm+2.723)/(-0.6576);
	double expt = 2.85*(lgm-peakm);
	return 2.85*(aofmf + lgm) - pow(10,expt)/2.3026;
}

// full spectrum - polychromatic
// Psi = 10^psibroadlg as a function of Log10[M] = lgm and peak mass
double psibroadlg(double lgm, double peakm){
	//  if mass is less than planck mass, set to 0
	if(lgmp>lgm){
		 return -100.;
		}
	// if above peak mass, set to hi spectrum
	else if(lgm>peakm){
		return psihilg(lgm,oc);
		}
	//  if below peak mass, set to low spectrum
	else{
		return psilowlg(lgm,peakm);
	}
}

// calculate volume fration given number density
double epsilon(double lgni, double lgmass){
	double radius = 2. * gnewton * pow(10,lgmass)/pow(sofl,2);
	printf("%e \n", radius);
	return pow(10.,lgni) * 4. * M_PI * pow(radius,3) / 3.;
}

/* Entropy density */
// Temperature given in Kelvin
double entropy(double Trh,  double  a, double arh){
	return 2.*pow(M_PI,2)/45 * gstar * pow(Trh*keltgev*arh/a,3.);
}

// absolute maximum theoretical number density
// Some notes: A bh of mass 10^-5.96 kg has a volume of 1.76 x 10^-98 m^3  and so we can have number densities mathematically up to 10^98 x 0.74 where 0.74 is the close packing ratio (max ratio of cube to volume of packed spheres)
double maxn0(double  lgmass){
	double rsch = 2.*gnewton*pow(10.,lgmass)/pow(sofl,2); // schwarzschild radius in m
	double volume =4./3. * M_PI * pow(rsch,3);
	return 1./volume * 0.74;
}


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <float.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
#define deg 180.0/M_PI;

/*!
 * @brief Returns the angle between two vectors, in degrees.
 *
 * @param[in] n    Length of vectors.
 * @param[in] va   Initial vector.  This is an array of dimension [n].
 * @param[in] vb   Rotate vector.  This is an array of dimension [n].
 *
 * @result Rotation angle in degrees between va and vb.
 *
 * @author Ben Baker
 *
 * @copyright MIT
 *
 */
double compearth_matlab_fangle(const int n,
                               const double *__restrict__ va,
                               const double *__restrict__ vb)
{
    double angle, magVa, magVb, xden, xnum;
    xnum = cblas_ddot(n, va, 1, vb, 1);
    magVa = cblas_dnrm2(n, va, 1);
    magVb = cblas_dnrm2(n, vb, 1);
    xden = magVa*magVb;
    angle = (double) NAN;
    if (xden > 0.0){angle = acos(xnum/xden)*deg;}
    return angle;
}


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

/* zsolve_cubic.c - finds the complex roots of x^3 + a x^2 + b x + c = 0 */

#include <config.h>
#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_poly.h>

#define SWAP(a,b) do { double tmp = b ; b = a ; a = tmp ; } while(0)

int
gsl_poly_complex_solve_cubic (double a, double b, double c,
                              gsl_complex *z0, gsl_complex *z1,
                              gsl_complex *z2)
{
  double q = (a * a - 3 * b);
  double r = (2 * a * a * a - 9 * a * b + 27 * c);

  double Q = q / 9;
  double R = r / 54;

  double Q3 = Q * Q * Q;
  double R2 = R * R;

  double CR2 = 729 * r * r;
  double CQ3 = 2916 * q * q * q;

  if (R == 0 && Q == 0)
    {
      GSL_REAL (*z0) = -a / 3;
      GSL_IMAG (*z0) = 0;
      GSL_REAL (*z1) = -a / 3;
      GSL_IMAG (*z1) = 0;
      GSL_REAL (*z2) = -a / 3;
      GSL_IMAG (*z2) = 0;
      return 3;
    }
  else if (CR2 == CQ3)
    {
      /* this test is actually R2 == Q3, written in a form suitable
         for exact computation with integers */

      /* Due to finite precision some double roots may be missed, and
         will be considered to be a pair of complex roots z = x +/-
         epsilon i close to the real axis. */

      double sqrtQ = sqrt (Q);

      if (R > 0)
	{
	  GSL_REAL (*z0) = -2 * sqrtQ - a / 3;
	  GSL_IMAG (*z0) = 0;
	  GSL_REAL (*z1) = sqrtQ - a / 3;
	  GSL_IMAG (*z1) = 0;
	  GSL_REAL (*z2) = sqrtQ - a / 3;
	  GSL_IMAG (*z2) = 0;
	}
      else
	{
	  GSL_REAL (*z0) = -sqrtQ - a / 3;
	  GSL_IMAG (*z0) = 0;
	  GSL_REAL (*z1) = -sqrtQ - a / 3;
	  GSL_IMAG (*z1) = 0;
	  GSL_REAL (*z2) = 2 * sqrtQ - a / 3;
	  GSL_IMAG (*z2) = 0;
	}
      return 3;
    }
  else if (CR2 < CQ3)  /* equivalent to R2 < Q3 */
    {
      double sqrtQ = sqrt (Q);
      double sqrtQ3 = sqrtQ * sqrtQ * sqrtQ;
      double theta = acos (R / sqrtQ3);
      double norm = -2 * sqrtQ;
      double r0 = norm * cos (theta / 3) - a / 3;
      double r1 = norm * cos ((theta + 2.0 * M_PI) / 3) - a / 3;
      double r2 = norm * cos ((theta - 2.0 * M_PI) / 3) - a / 3;

      /* Sort r0, r1, r2 into increasing order */

      if (r0 > r1)
	SWAP (r0, r1);

      if (r1 > r2)
	{
	  SWAP (r1, r2);

	  if (r0 > r1)
	    SWAP (r0, r1);
	}

      GSL_REAL (*z0) = r0;
      GSL_IMAG (*z0) = 0;

      GSL_REAL (*z1) = r1;
      GSL_IMAG (*z1) = 0;

      GSL_REAL (*z2) = r2;
      GSL_IMAG (*z2) = 0;

      return 3;
    }
  else
    {
      double sgnR = (R >= 0 ? 1 : -1);
      double A = -sgnR * pow (fabs (R) + sqrt (R2 - Q3), 1.0 / 3.0);
      double B = Q / A;

      if (A + B < 0)
	{
	  GSL_REAL (*z0) = A + B - a / 3;
	  GSL_IMAG (*z0) = 0;

	  GSL_REAL (*z1) = -0.5 * (A + B) - a / 3;
	  GSL_IMAG (*z1) = -(sqrt (3.0) / 2.0) * fabs(A - B);

	  GSL_REAL (*z2) = -0.5 * (A + B) - a / 3;
	  GSL_IMAG (*z2) = (sqrt (3.0) / 2.0) * fabs(A - B);
	}
      else
	{
	  GSL_REAL (*z0) = -0.5 * (A + B) - a / 3;
	  GSL_IMAG (*z0) = -(sqrt (3.0) / 2.0) * fabs(A - B);

	  GSL_REAL (*z1) = -0.5 * (A + B) - a / 3;
	  GSL_IMAG (*z1) = (sqrt (3.0) / 2.0) * fabs(A - B);

	  GSL_REAL (*z2) = A + B - a / 3;
	  GSL_IMAG (*z2) = 0;
	}

      return 3;
    }
}


/*
Pierwszy program stosuje metode Brenta
gsl_root_fsolver_brent
dla rownania
x^2 - 5 = 0

rozwiazaniem jest x = \sqrt 5 = 2.236068...
*/
#include <stdio.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_roots.h>
#include <string.h>

#include "demo_fn.h"

int
main (int argc, char *argv[])
{
  int status;
  int iter = 0, max_iter = 100;
  const gsl_root_fsolver_type *T;
  gsl_root_fsolver *s;
  double r = 0, r_expected = sqrt (5.0);
  double x_lo = 0.0, x_hi = 5.0;
  gsl_function F;
  struct quadratic_params params = {1.0, -2.0, 1.0};

  F.function = &quadratic;
  F.params = &params;

  if (argc < 2) {
      errno = EINVAL;
      perror("");
      return errno;
  }
  char *method = argv[1];
  if (0 == strcmp("bisection", method)) {
      T = gsl_root_fsolver_bisection;
  } else if (0 == strcmp("secant", method)) {
      T = gsl_root_fsolver_brent;
  } else if (0 == strcmp("steffenson", method)) {
      T = gsl_root_fsolver_falsepos;
  } else {
      errno = EINVAL;
      perror("");
      return errno;
  }

  T = gsl_root_fsolver_bisection;
  s = gsl_root_fsolver_alloc (T);
  gsl_root_fsolver_set (s, &F, x_lo, x_hi);

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

  printf ("%5s [%9s, %9s] %9s %10s %9s\n",
          "iter", "lower", "upper", "root",
          "err", "err(est)");

  do
    {
      iter++;
      status = gsl_root_fsolver_iterate (s);
      r = gsl_root_fsolver_root (s);
      x_lo = gsl_root_fsolver_x_lower (s);
      x_hi = gsl_root_fsolver_x_upper (s);
      status = gsl_root_test_interval (x_lo, x_hi,
                                       0, 0.001);

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

      printf ("%5d [%.7f, %.7f] %.7f %+.7f %.7f\n",
              iter, x_lo, x_hi,
              r, r - r_expected,
              x_hi - x_lo);
    }
  while (status == GSL_CONTINUE && iter < max_iter);
  return status;
}


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_cdf.h>

unsigned int k;
unsigned int n1;
unsigned int n2;
unsigned int t;

static void usage(int argc, char *argv[]) {
  printf (
  "Usage: %s k n1 n2 t\n"
  "\n"
  "If a population contains n1 elements of \"type 1\" and n2 elements\n"
  "of \"type 2\" then the hypergeometric distribution gives the probability\n"
  "of obtaining k elements of \"type 1\" in t samples from the population\n"
  "without replacement.\n"
  "This program computes the Pvalue of such a distribution\n", argv[0]);
  exit (-1);
}

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

  if (argc != 5)
    usage(argc, argv);

  k = atoi(argv[1]);
  n1 = atoi(argv[2]);
  n2 = atoi(argv[3]);
  t = atoi(argv[4]);

  double Q = gsl_cdf_hypergeometric_Q (k, n1, n2, t);
  double P = gsl_cdf_hypergeometric_P (k, n1, n2, t);

  printf("%g\n", Q);
  if (P < Q)
    fprintf(stderr, "Warning: P < Q, %g < %g\n", P, Q);

}


/* Copyright (c) 2014, Giuseppe Argentieri <giuseppe.argentieri@ts.infn.it>

 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 */

/*
 * =====================================================================================
 *
 *       Filename:  Expi.c
 *
 *    Description:  Calculation of the exponential integrals (w/o the GSL special
 *    			functions).
 *
 *        Version:  1.0
 *        Created:  15/04/2013 11:54:19
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  Giuseppe Argentieri (ga), giuseppe.argentieri@ts.infn.it
 *   Organization:
 *
 * =====================================================================================
 */

#include "funcs.h"
#include <gsl/gsl_integration.h>
/* #include <gsl/gsl_sf_expint.h> */

double fu ( double t, void* params )
{
	double f = -exp(-t)/t ;
	return f ;
}

double ex ( double t, void* params )
{
	double e = exp(-t) ;
	return e ;
}

int expi ( double x, double* result, double* abserr )
{
	double r, err ;

	gsl_integration_workspace* expi_ws =
	       	gsl_integration_workspace_alloc (WS_SZ) ;

	gsl_function F ;
	F.function = &fu ;

	int status ;
	status = gsl_integration_qagiu ( &F, x, 10e-9, .001 , WS_SZ, expi_ws, &r,
				&err) ;

	*result = r ; *abserr = err ;

/*	Using the GSL special functions, it is simply:
 *
 *      *result = - gsl_sf_expint_E1(x) ;
 */

	gsl_integration_workspace_free (expi_ws) ;

	return status;
}

int expi_plus ( double x, double* result, double* abserr )
{
	double r, err ;

	gsl_integration_workspace *expi_ws =
	       	gsl_integration_workspace_alloc (WS_SZ) ;

	gsl_function F ;
	F.function = &ex ;

	int status ;
	status = gsl_integration_qawc ( &F, -x, x, 0, 1e-9, .001, WS_SZ,
			expi_ws, &r, &err ) ;

	double R , EXPI , ERREXPI ;
	int s = expi ( x , &EXPI, &ERREXPI ) ;
	R = - (r - EXPI) ;                          /* The minus (-) sign because of the def.
							of ex 				*/
	double ERR ;
	ERR = err + ERREXPI ;

	*result = R ;
	*abserr = ERR ;

/* 	Using the GSL exponential integral, it is simply:
 *
 * 	R = gsl_sf_expint_Ei(x)  ;
 *
 */
	gsl_integration_workspace_free (expi_ws) ;

	return status + s ;
}


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

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

int
main ()
{
  int i;
  double a[6] = { -1, 0, 0, 0, 0, 1 };  /*  P(x) =  x^5 - 1  */
  double z[10];

  gsl_poly_complex_workspace * w = gsl_poly_complex_workspace_alloc (6) ;

  gsl_poly_complex_solve (a, 6, w, z) ;

  gsl_poly_complex_workspace_free (w) ;

  for (i = 0; i < 5 ; i++)
    {
      printf("z%d = %+.18f %+.18f\n", i, z[2*i], z[2*i+1]) ;
    }
}


#include <stdio.h>
#include <gsl/gsl_integration.h>
#include "function.h"

extern double f (double, void*);

int
main (void)
{
  /* Claim memory needed for adaptive integration */
  gsl_integration_workspace * w
    = gsl_integration_workspace_alloc (1000);

  /* Construct GSL function object to describe solution */
  gsl_function F;
  F.function = &f;
  double alpha = 1.0;
  F.params = NULL;

  /* Parameters to control quadrature routine */
  double a, b, tol_abs, tol_rel;
  int limit;
  a = 0.0;
  b = 3.0;
  tol_abs = 0;
  tol_rel = 1.0e-7;
  limit = 1000;

  // Test that the function is implemented properly
  //double test = GSL_FN_EVAL(&F,2);
  //printf("TEST RESULT: %.18f\n", test);

  /* Actual call to quadrature routine */
  double result, error;
  gsl_integration_qag (&F, a, b, tol_abs, tol_rel, limit,
                       GSL_INTEG_GAUSS15, w, &result, &error);
  double mathematica_result = 1.29646778572437307899;

  printf ("result                = % .18f\n", result);
  printf ("mathematica result    = % .18f\n", mathematica_result);
  printf ("estimated error       = % .18f\n", error);
  printf ("error vs. mathematica = % .18f\n",
           fabs(result - mathematica_result));
  printf ("# intervals     = %d\n", (int) w->size);

  /* Return memory allocated for solver */
  gsl_integration_workspace_free (w);

  return 0;
}


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

double ar1_ts (double * params, int n, gsl_rng * r)
{
    double beta = params[0];
    double alpha = params[1];
    double s = params[2];

    int i;
    double x = beta / (1 - alpha);  // Start at mean of stationary dist
    double sum = 0;
    for (i = 1; i <= n; i++) {
        sum += x;
        x = beta + alpha * x + gsl_ran_gaussian(r, s);
     }

    return sum / n;
}

int main(void)
{
    clock_t start, end;
    double cpu_time_used;

    int N = 1e7;
    double beta = 1.0;
    double alpha = 0.9;
    double s = 1;
    double params[3] = {beta, alpha, s};

    /* create a generator via GSL_RNG_TYPE */
    const gsl_rng_type * T;
    gsl_rng * r;
    gsl_rng_env_setup();
    T = gsl_rng_default;
    r = gsl_rng_alloc(T);
    gsl_rng_set(r, 1);

    start = clock();
    double sample_mean = ar1_ts(params, N, r);
    end = clock();
    cpu_time_used = ((double) (end - start)) / CLOCKS_PER_SEC;

    gsl_rng_free (r);

    printf("mean = %g\n", sample_mean);
    printf("time elapsed = %g seconds\n", cpu_time_used);
    return 0;
}


#include "lah.h"

#ifdef HAVE_LAPACK
#include <lapacke.h>
#include <math.h>

/* Input Matrix P has to be symmetric positive semi-definite,
 * e.g. real covarianz matrix */
lah_Return lah_matSqrt(lah_mat *P, lah_mat *Psqrt, lah_mat *Work)
{
    lah_Return res = lahReturnOk;
    lah_index i, j;
    lah_value temp;
    lah_value *S;
    lah_mat *Z;

    if (P == NULL || P->nR != P->nC
        || Psqrt == NULL || P->nR != Psqrt->nR || P->nC != Psqrt->nC)
    {
        return lahReturnParameterError;
    }
    if (LAH_ISTYPE(P, lahTypeDiagonal))
    {
        for (j = 0; j < P->nC; j++)
            LAH_ENTRY(P, j, j) = sqrt(LAH_ENTRY(P, j, j));
        return lahReturnOk;
    }

    /* allocate space for the output parameters and workspace arrays */
    S = malloc(P->nR * sizeof(lah_value));
    Z = lah_matAlloc(P->nR, P->nR, 1);
    res = lah_eigenValue(P, S, Z);
    if(res != lahReturnOk)
        return res;

    /* Calculate Workspace = Z*D */
    for (j = 0; j < P->nC; ++j)
    {
        temp = S[j];
        if (temp < 0)
            return lahReturnMathError; /*Not positive semi definite*/
        else
            temp = sqrt(temp);
        for (i = 0; i< P->nR; ++i)
        {
            LAH_ENTRY(Work, i, j) = LAH_ENTRY(Z, i, j) * temp;
        }
    }

    /* Calculate End result P_sqrt = W*Z^T */
    res = lah_matMul(lahNorm, lahTrans, 0, 1, Psqrt, Work, Z);

    lah_matFree(Z);
    free(S);
    return res;
}
#endif


#include "lah.h"

#ifdef HAVE_LAPACK /* Use a LAPACK package to do the heavy work */
#include <lapacke.h>
lah_Return lah_chol(lah_mat *P, lah_index setZero)
{
    lah_index i, j;
    lapack_int res;
    if (P == NULL || !LAH_ISTYPE(P, lahTypeSquare))
        return lahReturnParameterError;

    res = POTRF(LAH_LAPACK_LAYOUT, 'L' , P->nR, P->data, P->nR);

    /* Set rest of matrix to zero, if setZero == 1 */
    if (setZero)
    {
        for (i = 0; i < P->nR; i++)
        {
            for(j = i + 1; j < P->nC; ++j)
            {
                LAH_ENTRY(P, i, j) = 0.0;
            }
        }
    }
    return (res == 0) ? lahReturnOk : lahReturnExternError;
}

#else /* fallback to primitive linear algebra subroutines */
#include <math.h>

lah_Return lah_chol(lah_mat *P, lah_index setZero)
{
    lah_index i, j, k, N;
    lah_value *diag, *value;

    if (P == NULL)
    {
        return lahReturnParameterError;
    }

    N = P->nR;

    /* see https://de.wikipedia.org/wiki/Cholesky-Zerlegung#Pseudocode */
    for (i = 0; i < N; i++)
    {
        /* Pointer to diagonal element */
        diag = P->data + i * (P->incRow + P->incCol);
        /* check if positive */
        if (*diag < 0.00001 )
        {
            return lahReturnMathError;
        }
        /* square root of diagonal element in place */
        *diag = sqrt(*diag);

        /* divide elements below diagonal by diagonal element */
        for (k = 1; k < N - i; k++)
        {
            *(diag + k * P->incRow) /= *diag;
        }
        /* compute upper right submatrix */
        for (j = i + 1; j < N; j++)
        {
            value = P->data + j * P->incRow + j * P->incCol;
            for (k = j; k < N; k++)
            {
                *value -= LAH_ENTRY(P, k, i) * LAH_ENTRY(P, j, i);
                value += P->incRow;
            }
        }
    }
    /* Set rest of matrix to zero, if setZero == 1 */
    if (setZero)
    {
        for (i = 0; i < N; i++)
        {
            for(j = i + 1; j < N; ++j)
            {
                LAH_ENTRY(P, i, j) = 0.0;
            }
        }
    }

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


static char help[] = "Solve a 2-variable polynomial optimization problem.\n"
"Implements an objective function and its gradient (the residual).  The \n"
"Jacobian (Hessian) is not implemented.  Usage is either objective-only\n"
"    ./cartoon -snes_[fd|mf] -snes_fd_function\n"
"or with gradient\n"
"    ./cartoon -snes_[fd|mf]\n\n";

#include <petsc.h>

PetscErrorCode FormObjective(SNES snes, Vec x, PetscReal *Phi, void *ctx) {
    PetscErrorCode  ierr;
    const PetscReal *ax;

    ierr = VecGetArrayRead(x,&ax); CHKERRQ(ierr);
    *Phi = 0.25 * (pow(ax[0],4.0) + pow(ax[1],4.0)) - 2.0 * ax[0] + 2.0 * ax[1];
    ierr = VecRestoreArrayRead(x,&ax); CHKERRQ(ierr);
    return 0;
}

// residual F = grad Phi
PetscErrorCode FormFunction(SNES snes, Vec x, Vec F, void *ctx) {
    PetscErrorCode  ierr;
    const PetscReal *ax;
    PetscReal       *aF;

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

int main(int argc,char **argv) {
    PetscErrorCode ierr;
    SNES           snes;
    Vec            x, r, xexact;
    const PetscInt ix[2] = {0,1};
    PetscReal      iv[2], err;

    PetscInitialize(&argc,&argv,NULL,help);

    ierr = VecCreate(PETSC_COMM_WORLD,&x); CHKERRQ(ierr);
    ierr = VecSetSizes(x,PETSC_DECIDE,2); CHKERRQ(ierr);
    ierr = VecSetFromOptions(x); CHKERRQ(ierr);

    iv[0] = 0.5;   iv[1] = 0.5;    // initial iterate corresponds to nonsingular Hessian
    ierr = VecSetValues(x,2,ix,iv,INSERT_VALUES); CHKERRQ(ierr);
    ierr = VecAssemblyBegin(x); CHKERRQ(ierr);
    ierr = VecAssemblyEnd(x); CHKERRQ(ierr);

    ierr = SNESCreate(PETSC_COMM_WORLD,&snes); CHKERRQ(ierr);
    ierr = SNESSetObjective(snes,FormObjective,NULL); CHKERRQ(ierr);
    ierr = VecDuplicate(x,&r); CHKERRQ(ierr);
    ierr = SNESSetFunction(snes,r,FormFunction,NULL); CHKERRQ(ierr);
    ierr = SNESSetFromOptions(snes); CHKERRQ(ierr);
    ierr = SNESSolve(snes,NULL,x); CHKERRQ(ierr);

    iv[0] = pow(2.0,1.0/3.0);  iv[1] = - iv[0];  // exact soln
    ierr = VecDuplicate(x,&xexact); CHKERRQ(ierr);
    ierr = VecSetValues(xexact,2,ix,iv,ADD_VALUES); CHKERRQ(ierr); // note ADD_VALUES
    ierr = VecAssemblyBegin(xexact); CHKERRQ(ierr);
    ierr = VecAssemblyEnd(xexact); CHKERRQ(ierr);
    ierr = VecAXPY(x,-1.0,xexact); CHKERRQ(ierr);    // x <-- x + (-1.0) xexact
    ierr = VecNorm(x,NORM_INFINITY,&err); CHKERRQ(ierr);
    ierr = PetscPrintf(PETSC_COMM_WORLD,"numerical error |x-x_exact|_inf = %g\n",err); CHKERRQ(ierr);

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


static const char help[] =
"Solves obstacle problem in 2D using SNESVI.  Option prefix -obs_.\n"
"The obstacle problem is a free boundary problem for the Poisson equation\n"
"in which the solution u(x,y) is constrained to be above the obstacle psi(x,y):\n"
"    - Lap u = f,  u >= psi.\n"
"Equivalently it is a variational inequality (VI), complementarity problem\n"
"(CP), or an inequality-constrained minimization.  The example here is\n"
"on the square (-2,2)^2 and has known exact solution.  Because of the\n"
"constraint, the problem is nonlinear but the code reuses the residual and\n"
"Jacobian evaluation code for the Poisson equation in ch6/.\n\n";

#include <petsc.h>
#include "../ch6/poissonfunctions.h"

// z = psi(x,y) is the hemispherical obstacle, but made C^1 with "skirt" at r=r0
PetscReal psi(PetscReal x, PetscReal y) {
    const PetscReal  r = x * x + y * y,
                     r0 = 0.9,
                     psi0 = PetscSqrtReal(1.0 - r0*r0),
                     dpsi0 = - r0 / psi0;
    if (r <= r0) {
        return PetscSqrtReal(1.0 - r);
    } else {
        return psi0 + dpsi0 * (r - r0);
    }
}

/*  This exact solution solves a 1D radial free-boundary problem for the
Laplace equation, on the interval 0 < r < 2, with hemispherical obstacle
    psi(r) =  / sqrt(1 - r^2),  r < 1
              \ -1,             otherwise
The Laplace equation applies where u(r) > psi(r),
    u''(r) + r^-1 u'(r) = 0
with boundary conditions including free b.c.s at an unknown location r = a:
    u(a) = psi(a),  u'(a) = psi'(a),  u(2) = 0
The solution is  u(r) = - A log(r) + B   on  r > a.  The boundary conditions
can then be reduced to a root-finding problem for a:
    a^2 (log(2) - log(a)) = 1 - a^2
The solution is a = 0.697965148223374 (giving residual 1.5e-15).  Then
A = a^2*(1-a^2)^(-0.5) and B = A*log(2) are as given below in the code.  */
PetscReal u_exact(PetscReal x, PetscReal y) {
    const PetscReal afree = 0.697965148223374,
                    A     = 0.680259411891719,
                    B     = 0.471519893402112;
    PetscReal       r;
    r = PetscSqrtReal(x * x + y * y);
    return (r <= afree) ? psi(x,y)  // active set; on the obstacle
                        : - A * PetscLogReal(r) + B; // solves laplace eqn
}

// boundary conditions from exact solution
PetscReal g_fcn(PetscReal x, PetscReal y, PetscReal z, void *ctx) {
    return u_exact(x,y);
}

// we solve Laplace's equation with f = 0
PetscReal zero(PetscReal x, PetscReal y, PetscReal z, void *ctx) {
    return 0.0;
}

extern PetscErrorCode FormUExact(DMDALocalInfo*, Vec);
extern PetscErrorCode GetActiveSet(SNES, DMDALocalInfo*, Vec, Vec,
                                   PetscInt*, PetscReal*);
extern PetscErrorCode FormBounds(SNES, Vec, Vec);

int main(int argc,char **argv) {
  PetscErrorCode ierr;
  DM                  da, da_after;
  SNES                snes;
  KSP                 ksp;
  Vec                 u_initial, u, u_exact, Xl, Xu;
  PoissonCtx          user;
  const PetscReal     aexact = 0.697965148223374;
  SNESConvergedReason reason;
  PetscInt            snesit, kspit;
  PetscReal           error1,errorinf,actarea,exactarea,areaerr;
  DMDALocalInfo       info;
  char                dumpname[256] = "dump.dat";
  PetscBool           dumpbinary = PETSC_FALSE;

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

  ierr = PetscOptionsBegin(PETSC_COMM_WORLD,"obs_","options to obstacle","");CHKERRQ(ierr);
  ierr = PetscOptionsString("-dump_binary",
           "filename for saving solution AND OBSTACLE in PETSc binary format",
           "obstacle.c",dumpname,dumpname,sizeof(dumpname),&dumpbinary); CHKERRQ(ierr);
  ierr = PetscOptionsEnd();CHKERRQ(ierr);

  ierr = DMDACreate2d(PETSC_COMM_WORLD,
      DM_BOUNDARY_NONE, DM_BOUNDARY_NONE, DMDA_STENCIL_STAR,
      3,3,                       // override with -da_grid_x,_y
      PETSC_DECIDE,PETSC_DECIDE, // num of procs in each dim
      1,1,NULL,NULL,             // dof = 1 and stencil width = 1
      &da);CHKERRQ(ierr);
  ierr = DMSetFromOptions(da); CHKERRQ(ierr);
  ierr = DMSetUp(da); CHKERRQ(ierr);
  ierr = DMDASetUniformCoordinates(da,-2.0,2.0,-2.0,2.0,-1.0,-1.0);CHKERRQ(ierr);

  user.cx = 1.0;
  user.cy = 1.0;
  user.cz = 1.0;
  user.g_bdry = &g_fcn;
  user.f_rhs = &zero;
  user.addctx = NULL;
  ierr = DMSetApplicationContext(da,&user);CHKERRQ(ierr);

  ierr = SNESCreate(PETSC_COMM_WORLD,&snes);CHKERRQ(ierr);
  ierr = SNESSetDM(snes,da);CHKERRQ(ierr);
  ierr = SNESSetApplicationContext(snes,&user);CHKERRQ(ierr);

  // set the SNES type to a variational inequality (VI) solver of reduced-space
  // (RS) type
  ierr = SNESSetType(snes,SNESVINEWTONRSLS);CHKERRQ(ierr);
  ierr = SNESVISetComputeVariableBounds(snes,&FormBounds);CHKERRQ(ierr);

  // reuse residual and jacobian from ch6/
  ierr = DMDASNESSetFunctionLocal(da,INSERT_VALUES,
             (DMDASNESFunction)Poisson2DFunctionLocal,&user); CHKERRQ(ierr);
  ierr = DMDASNESSetJacobianLocal(da,
             (DMDASNESJacobian)Poisson2DJacobianLocal,&user); CHKERRQ(ierr);
  ierr = SNESGetKSP(snes,&ksp); CHKERRQ(ierr);
  ierr = KSPSetType(ksp,KSPCG); CHKERRQ(ierr);
  ierr = SNESSetFromOptions(snes);CHKERRQ(ierr);

  // initial iterate is zero for simplicity
  ierr = DMCreateGlobalVector(da,&u_initial);CHKERRQ(ierr);
  ierr = VecSet(u_initial,0.0); CHKERRQ(ierr);

  /* solve and get solution, DM after solution*/
  ierr = SNESSolve(snes,NULL,u_initial);CHKERRQ(ierr);
  ierr = VecDestroy(&u_initial); CHKERRQ(ierr);
  ierr = DMDestroy(&da); CHKERRQ(ierr);
  ierr = SNESGetDM(snes,&da_after); CHKERRQ(ierr);
  ierr = SNESGetSolution(snes,&u); CHKERRQ(ierr); /* do not destroy u */
  ierr = DMDAGetLocalInfo(da_after,&info); CHKERRQ(ierr);
  ierr = VecDuplicate(u,&Xl); CHKERRQ(ierr);
  ierr = VecDuplicate(u,&Xu); CHKERRQ(ierr);
  ierr = FormBounds(snes,Xl,Xu); CHKERRQ(ierr);

  /* save solution to binary file if requested */
  if (dumpbinary) {
      PetscViewer dumpviewer;
      ierr = PetscPrintf(PETSC_COMM_WORLD,
               "writing u,psi in binary format to %s ...\n",dumpname); CHKERRQ(ierr);
      ierr = PetscViewerBinaryOpen(PETSC_COMM_WORLD,dumpname,FILE_MODE_WRITE,&dumpviewer); CHKERRQ(ierr);
      ierr = VecView(u,dumpviewer); CHKERRQ(ierr);
      ierr = VecView(Xl,dumpviewer); CHKERRQ(ierr);
      ierr = PetscViewerDestroy(&dumpviewer); CHKERRQ(ierr);
  }

  /* compute final performance measures */
  ierr = SNESGetConvergedReason(snes,&reason); CHKERRQ(ierr);
  if (reason <= 0) {
      ierr = PetscPrintf(PETSC_COMM_WORLD,
          "WARNING: SNES not converged ... use -snes_converged_reason to check\n"); CHKERRQ(ierr);
  }
  ierr = SNESGetIterationNumber(snes,&snesit); CHKERRQ(ierr);
  ierr = SNESGetKSP(snes,&ksp); CHKERRQ(ierr);
  ierr = KSPGetIterationNumber(ksp,&kspit); CHKERRQ(ierr);
  ierr = PetscPrintf(PETSC_COMM_WORLD,
      "done on %d x %d grid ... %s, SNES iters = %d, last KSP iters = %d\n",
      info.mx,info.my,SNESConvergedReasons[reason],snesit,kspit); CHKERRQ(ierr);

  /* compare to exact */
  ierr = GetActiveSet(snes,&info,u,Xl,NULL,&actarea); CHKERRQ(ierr);
  exactarea = PETSC_PI * aexact * aexact;
  areaerr = PetscAbsReal(actarea - exactarea) / exactarea;
  ierr = VecDuplicate(u,&u_exact); CHKERRQ(ierr);
  ierr = FormUExact(&info,u_exact); CHKERRQ(ierr);
  ierr = VecAXPY(u,-1.0,u_exact); CHKERRQ(ierr); /* u <- u - u_exact */
  ierr = VecNorm(u,NORM_1,&error1); CHKERRQ(ierr);
  error1 /= (PetscReal)info.mx * (PetscReal)info.my;
  ierr = VecNorm(u,NORM_INFINITY,&errorinf); CHKERRQ(ierr);
  ierr = PetscPrintf(PETSC_COMM_WORLD,
      "errors: av |u-uexact| = %.3e, |u-uexact|_inf = %.3e, active area error = %.3f%%\n",
      error1,errorinf,100.0*areaerr); CHKERRQ(ierr);

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

PetscErrorCode GetActiveSet(SNES snes, DMDALocalInfo *info, Vec u, Vec Xl,
                            PetscInt *act, PetscReal *actarea) {
  PetscErrorCode ierr;
  Vec              F;
  const PetscReal  *au, *aXl, *aF, zerotol = 1.0e-8;  // see petsc/src/snes/impls/vi/vi.c for value
  PetscReal        dx, dy;
  PetscInt         i,n,lact,gact;

  dx = 4.0 / (PetscReal)(info->mx-1);
  dy = 4.0 / (PetscReal)(info->my-1);
  ierr = VecGetLocalSize(u,&n);CHKERRQ(ierr);
  ierr = VecGetArrayRead(u,&au); CHKERRQ(ierr);
  ierr = VecGetArrayRead(Xl,&aXl); CHKERRQ(ierr);
  ierr = SNESGetFunction(snes,&F,NULL,NULL); CHKERRQ(ierr); /* do not destroy F */
  ierr = VecGetArrayRead(F,&aF); CHKERRQ(ierr);
  lact = 0;
  for (i=0; i<n; i++) {
    if ((au[i] <= aXl[i] + zerotol) && (aF[i] > 0.0))
        lact++;
  }
  ierr = VecRestoreArrayRead(u,&au);CHKERRQ(ierr);
  ierr = VecRestoreArrayRead(Xl,&aXl);CHKERRQ(ierr);
  ierr = VecRestoreArrayRead(F,&aF); CHKERRQ(ierr);
  ierr = MPI_Allreduce(&lact,&gact,1,MPIU_INT,MPIU_SUM,PetscObjectComm((PetscObject)snes));CHKERRQ(ierr);
  if (act) {
      *act = gact;
  }
  if (actarea) {
      *actarea = dx * dy * gact;
  }
  return 0;
}

PetscErrorCode FormUExact(DMDALocalInfo *info, Vec u) {
  PetscErrorCode ierr;
  PetscInt   i,j;
  PetscReal  **au, dx, dy, x, y;
  dx = 4.0 / (PetscReal)(info->mx-1);
  dy = 4.0 / (PetscReal)(info->my-1);
  ierr = DMDAVecGetArray(info->da, u, &au);CHKERRQ(ierr);
  for (j=info->ys; j<info->ys+info->ym; j++) {
    y = -2.0 + j * dy;
    for (i=info->xs; i<info->xs+info->xm; i++) {
      x = -2.0 + i * dx;
      au[j][i] = u_exact(x,y);
    }
  }
  ierr = DMDAVecRestoreArray(info->da, u, &au);CHKERRQ(ierr);
  return 0;
}

//STARTBOUNDS
// for call-back: tell SNESVI we want  psi <= u < +infinity
PetscErrorCode FormBounds(SNES snes, Vec Xl, Vec Xu) {
  PetscErrorCode ierr;
  DM             da;
  DMDALocalInfo  info;
  PetscInt       i, j;
  PetscReal      **aXl, dx, dy, x, y;
  ierr = SNESGetDM(snes,&da);CHKERRQ(ierr);
  ierr = DMDAGetLocalInfo(da,&info); CHKERRQ(ierr);
  dx = 4.0 / (PetscReal)(info.mx-1);
  dy = 4.0 / (PetscReal)(info.my-1);
  ierr = DMDAVecGetArray(da, Xl, &aXl);CHKERRQ(ierr);
  for (j=info.ys; j<info.ys+info.ym; j++) {
    y = -2.0 + j * dy;
    for (i=info.xs; i<info.xs+info.xm; i++) {
      x = -2.0 + i * dx;
      aXl[j][i] = psi(x,y);
    }
  }
  ierr = DMDAVecRestoreArray(da, Xl, &aXl);CHKERRQ(ierr);
  ierr = VecSet(Xu,PETSC_INFINITY);CHKERRQ(ierr);
  return 0;
}
//ENDBOUNDS


/*
 least_square_solver.c
 written by Eunseok Lee

 function: obtain the least-square solution of a linear equation system

 v1: Feb 2, 2018
 */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
//#include <cem.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>

void least_square_solver(double *matA, int n_row, int n_col, double *y, double *sol_x) {

    int i, j, k, tmp;
    double sum;
    gsl_matrix * A = gsl_matrix_alloc (n_row,n_col);
    gsl_matrix * V = gsl_matrix_alloc (n_col,n_col);
    gsl_vector * S = gsl_vector_alloc (n_col);
    gsl_vector * work = gsl_vector_alloc (n_col);
//    gsl_matrix * U = gsl_matrix_alloc (n_row-1,n_col_ids);
    gsl_vector * b = gsl_vector_alloc (n_row);
    gsl_vector * x = gsl_vector_alloc (n_col);

    // need to copy matA to A, y to b!
    for (i=0;i<n_row;i++) {
        for (j=0; j<n_col; j++) {
            gsl_matrix_set (A, i, j, *(matA+i*n_col+j));
        }
        gsl_vector_set (b, i, y[i]);
    }
    gsl_linalg_SV_decomp(A, V, S, work);    // on output, A is replaced by U!
    gsl_linalg_SV_solve (A, V, S, b, x);    // So, A should be used instead of U, here.

    for (i=0;i<n_col;i++) {
        sol_x[i] = gsl_vector_get(x,i);
    }

    gsl_matrix_free(A);
    gsl_matrix_free(V);
    gsl_vector_free(S);
    gsl_vector_free(work);
    gsl_vector_free(b);
    gsl_vector_free(x);

}