/* ode-initval/rk2imp.c
 *
 * 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 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/* Runge-Kutta 2, Gaussian implicit. Also known as the implicit
   midpoint rule. */

/* Author:  G. Jungman */

/* Error estimation by step doubling, see eg. Ascher, U.M., Petzold,
   L.R., Computer methods for ordinary differential and
   differential-algebraic equations, SIAM, Philadelphia, 1998.
   The method is also described in eg. this reference.
*/

#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv.h>

#include "odeiv_util.h"

typedef struct
{
  double *Y1;
  double *y0;
  double *ytmp;
  double *y_onestep;
  double *y0_orig;
}
rk2imp_state_t;

static void *
rk2imp_alloc (size_t dim)
{
  rk2imp_state_t *state = (rk2imp_state_t *) malloc (sizeof (rk2imp_state_t));

  if (state == 0)
    {
      GSL_ERROR_NULL ("failed to allocate space for rk2imp_state",
                      GSL_ENOMEM);
    }

  state->Y1 = (double *) malloc (dim * sizeof (double));

  if (state->Y1 == 0)
    {
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for Y1", GSL_ENOMEM);
    }

  state->ytmp = (double *) malloc (dim * sizeof (double));

  if (state->ytmp == 0)
    {
      free (state->Y1);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for ytmp", GSL_ENOMEM);
    }

  state->y0 = (double *) malloc (dim * sizeof (double));

  if (state->y0 == 0)
    {
      free (state->Y1);
      free (state->ytmp);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for y0", GSL_ENOMEM);
    }

  state->y_onestep = (double *) malloc (dim * sizeof (double));

  if (state->y_onestep == 0)
    {
      free (state->Y1);
      free (state->ytmp);
      free (state->y0);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for y_onestep", GSL_ENOMEM);
    }

  state->y0_orig = (double *) malloc (dim * sizeof (double));

  if (state->y0_orig == 0)
    {
      free (state->y_onestep);
      free (state->Y1);
      free (state->ytmp);
      free (state->y0);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for y0_orig", GSL_ENOMEM);
    }

  return state;
}

static int
rk2imp_step (double *y, rk2imp_state_t *state,
	     const double h, const double t,
	     const size_t dim, const gsl_odeiv_system *sys)
{
  /* Makes a Runge-Kutta 2nd order implicit advance with step size h.
     y0 is initial values of variables y.

     The implicit matrix equations to solve are:

     Y1 = y0 + h/2 * f(t + h/2, Y1)

     y = y0 + h * f(t + h/2, Y1)
  */

  const double *y0 = state->y0;
  double *Y1 = state->Y1;
  double *ytmp = state->ytmp;
  int max_iter=3;
  int nu;
  size_t i;

  /* iterative solution of Y1 = y0 + h/2 * f(t + h/2, Y1)
     Y1 should include initial values at call.

     Note: This method does not check for convergence of the
     iterative solution!
  */

  for (nu = 0; nu < max_iter; nu++)
    {
      for (i = 0; i < dim; i++)
        {
          ytmp[i] = y0[i] + 0.5 * h * Y1[i];
        }

      {
	int s = GSL_ODEIV_FN_EVAL (sys, t + 0.5 * h, ytmp, Y1);

	if (s != GSL_SUCCESS)
	  {
	    return s;
	  }
      }
    }

  /* assignment */

  for (i = 0; i < dim; i++)
    {
      y[i] = y0[i] + h * Y1[i];
    }

  return GSL_SUCCESS;
}

static int
rk2imp_apply (void *vstate,
              size_t dim,
              double t,
              double h,
              double y[],
              double yerr[],
              const double dydt_in[],
              double dydt_out[], const gsl_odeiv_system * sys)
{
  rk2imp_state_t *state = (rk2imp_state_t *) vstate;

  size_t i;

  double *Y1 = state->Y1;
  double *y0 = state->y0;
  double *y_onestep = state->y_onestep;
  double *y0_orig = state->y0_orig;

  /* Error estimation is done by step doubling procedure */

  /* initialization step */

  DBL_MEMCPY (y0, y, dim);

  /* Save initial values for possible failures */
  DBL_MEMCPY (y0_orig, y, dim);

  if (dydt_in != NULL)
    {
      DBL_MEMCPY (Y1, dydt_in, dim);
    }

  else
    {
      int s = GSL_ODEIV_FN_EVAL (sys, t, y, Y1);

      if (s != GSL_SUCCESS)
	{
	  return s;
	}
    }

  /* First traverse h with one step (save to y_onestep) */

  DBL_MEMCPY (y_onestep, y, dim);

  {
    int s = rk2imp_step (y_onestep, state, h, t, dim, sys);

    if (s != GSL_SUCCESS)
      {
	return s;
      }
  }

 /* Then with two steps with half step length (save to y) */

  {
    int s = rk2imp_step (y, state, h / 2.0, t, dim, sys);

    if (s != GSL_SUCCESS)
      {
	/* Restore original y vector */
	DBL_MEMCPY (y, y0_orig, dim);

	return s;
      }
  }

  DBL_MEMCPY (y0, y, dim);

  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + h / 2.0, y, Y1);

    if (s != GSL_SUCCESS)
      {
	/* Restore original y vector */
	DBL_MEMCPY (y, y0_orig, dim);

	return s;
      }
  }

  {
    int s = rk2imp_step (y, state, h / 2.0, t + h / 2.0, dim, sys);

    if (s != GSL_SUCCESS)
      {
	/* Restore original y vector */
	DBL_MEMCPY (y, y0_orig, dim);

	return s;
      }
  }

  /* Derivatives at output */

  if (dydt_out != NULL)
    {
      int s = GSL_ODEIV_FN_EVAL (sys, t + h, y, dydt_out);

      if (s != GSL_SUCCESS)
	{
	  /* Restore original y vector */
	  DBL_MEMCPY (y, y0_orig, dim);

	  return s;
	}
    }

  /* Error estimation */

  for (i = 0; i < dim; i++)
    {
      yerr[i] = 4.0 * (y[i] - y_onestep[i]) / 3.0;
    }

  return GSL_SUCCESS;
}

static int
rk2imp_reset (void *vstate, size_t dim)
{
  rk2imp_state_t *state = (rk2imp_state_t *) vstate;

  DBL_ZERO_MEMSET (state->Y1, dim);
  DBL_ZERO_MEMSET (state->ytmp, dim);
  DBL_ZERO_MEMSET (state->y0, dim);
  DBL_ZERO_MEMSET (state->y_onestep, dim);
  DBL_ZERO_MEMSET (state->y0_orig, dim);

  return GSL_SUCCESS;
}

static unsigned int
rk2imp_order (void *vstate)
{
  rk2imp_state_t *state = (rk2imp_state_t *) vstate;
  state = 0; /* prevent warnings about unused parameters */
  return 2;
}

static void
rk2imp_free (void *vstate)
{
  rk2imp_state_t *state = (rk2imp_state_t *) vstate;

  free (state->Y1);
  free (state->ytmp);
  free (state->y0);
  free (state->y_onestep);
  free (state->y0_orig);
  free (state);
}

static const gsl_odeiv_step_type rk2imp_type = { "rk2imp",      /* name */
  1,                            /* can use dydt_in */
  1,                            /* gives exact dydt_out */
  &rk2imp_alloc,
  &rk2imp_apply,
  &rk2imp_reset,
  &rk2imp_order,
  &rk2imp_free
};

const gsl_odeiv_step_type *gsl_odeiv_step_rk2imp = &rk2imp_type;


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

/* Author: G. Jungman */

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_errno.h>
#include "tridiag.h"
#include "gsl_linalg.h"

/* for description of method see [Engeln-Mullges + Uhlig, p. 92]
 *
 *     diag[0]  offdiag[0]             0   .....
 *  offdiag[0]     diag[1]    offdiag[1]   .....
 *           0  offdiag[1]       diag[2]
 *           0           0    offdiag[2]   .....
 */
static
int
solve_tridiag(
  const double diag[], size_t d_stride,
  const double offdiag[], size_t o_stride,
  const double b[], size_t b_stride,
  double x[], size_t x_stride,
  size_t N)
{
  int status;
  double *gamma = (double *) malloc (N * sizeof (double));
  double *alpha = (double *) malloc (N * sizeof (double));
  double *c = (double *) malloc (N * sizeof (double));
  double *z = (double *) malloc (N * sizeof (double));

  if (gamma == 0 || alpha == 0 || c == 0 || z == 0)
    {
      status = GSL_ENOMEM;
    }
  else
    {
      size_t i, j;

      /* Cholesky decomposition
         A = L.D.L^t
         lower_diag(L) = gamma
         diag(D) = alpha
       */
      alpha[0] = diag[0];
      gamma[0] = offdiag[0] / alpha[0];

      for (i = 1; i < N - 1; i++)
	{
	  alpha[i] = diag[d_stride * i] - offdiag[o_stride*(i - 1)] * gamma[i - 1];
	  gamma[i] = offdiag[o_stride * i] / alpha[i];
	}

      if (N > 1)
        {
          alpha[N - 1] = diag[d_stride * (N - 1)] - offdiag[o_stride*(N - 2)] * gamma[N - 2];
        }

      /* update RHS */
      z[0] = b[0];
      for (i = 1; i < N; i++)
	{
	  z[i] = b[b_stride * i] - gamma[i - 1] * z[i - 1];
	}
      for (i = 0; i < N; i++)
	{
	  c[i] = z[i] / alpha[i];
	}

      /* backsubstitution */
      x[x_stride * (N - 1)] = c[N - 1];
      if (N >= 2)
	{
	  for (i = N - 2, j = 0; j <= N - 2; j++, i--)
	    {
	      x[x_stride * i] = c[i] - gamma[i] * x[x_stride * (i + 1)];
	    }
	}

      status = GSL_SUCCESS;
    }

  if (z != 0)
    free (z);
  if (c != 0)
    free (c);
  if (alpha != 0)
    free (alpha);
  if (gamma != 0)
    free (gamma);

  return status;
}

/* for description of method see [Engeln-Mullges + Uhlig, p. 96]
 *
 *      diag[0]  offdiag[0]             0   .....  offdiag[N-1]
 *   offdiag[0]     diag[1]    offdiag[1]   .....
 *            0  offdiag[1]       diag[2]
 *            0           0    offdiag[2]   .....
 *          ...         ...
 * offdiag[N-1]         ...
 *
 */
static
int
solve_cyc_tridiag(
  const double diag[], size_t d_stride,
  const double offdiag[], size_t o_stride,
  const double b[], size_t b_stride,
  double x[], size_t x_stride,
  size_t N)
{
  int status;
  double * delta = (double *) malloc (N * sizeof (double));
  double * gamma = (double *) malloc (N * sizeof (double));
  double * alpha = (double *) malloc (N * sizeof (double));
  double * c = (double *) malloc (N * sizeof (double));
  double * z = (double *) malloc (N * sizeof (double));

  if (delta == 0 || gamma == 0 || alpha == 0 || c == 0 || z == 0)
    {
      status = GSL_ENOMEM;
    }
  else
    {
      size_t i, j;
      double sum = 0.0;

      /* factor */

      alpha[0] = diag[0];
      gamma[0] = offdiag[0] / alpha[0];
      delta[0] = offdiag[o_stride * (N-1)] / alpha[0];

      for (i = 1; i < N - 2; i++)
	{
	  alpha[i] = diag[d_stride * i] - offdiag[o_stride * (i-1)] * gamma[i - 1];
	  gamma[i] = offdiag[o_stride * i] / alpha[i];
	  delta[i] = -delta[i - 1] * offdiag[o_stride * (i-1)] / alpha[i];
	}
      for (i = 0; i < N - 2; i++)
	{
	  sum += alpha[i] * delta[i] * delta[i];
	}
      alpha[N - 2] = diag[d_stride * (N - 2)] - offdiag[o_stride * (N - 3)] * gamma[N - 3];
      gamma[N - 2] = (offdiag[o_stride * (N - 2)] - offdiag[o_stride * (N - 3)] * delta[N - 3]) / alpha[N - 2];
      alpha[N - 1] = diag[d_stride * (N - 1)] - sum - offdiag[o_stride * (N - 2)] * gamma[N - 2] * gamma[N - 2];

      /* update */
      z[0] = b[0];
      for (i = 1; i < N - 1; i++)
	{
	  z[i] = b[b_stride * i] - z[i - 1] * gamma[i - 1];
	}
      sum = 0.0;
      for (i = 0; i < N - 2; i++)
	{
	  sum += delta[i] * z[i];
	}
      z[N - 1] = b[b_stride * (N - 1)] - sum - gamma[N - 2] * z[N - 2];
      for (i = 0; i < N; i++)
	{
	  c[i] = z[i] / alpha[i];
	}

      /* backsubstitution */
      x[x_stride * (N - 1)] = c[N - 1];
      x[x_stride * (N - 2)] = c[N - 2] - gamma[N - 2] * x[x_stride * (N - 1)];
      if (N >= 3)
	{
	  for (i = N - 3, j = 0; j <= N - 3; j++, i--)
	    {
	      x[x_stride * i] = c[i] - gamma[i] * x[x_stride * (i + 1)] - delta[i] * x[x_stride * (N - 1)];
	    }
	}

      status = GSL_SUCCESS;
    }

  if (z != 0)
    free (z);
  if (c != 0)
    free (c);
  if (alpha != 0)
    free (alpha);
  if (gamma != 0)
    free (gamma);
  if (delta != 0)
    free (delta);

  return status;
}

int
gsl_linalg_solve_symm_tridiag(
  const gsl_vector * diag,
  const gsl_vector * offdiag,
  const gsl_vector * rhs,
  gsl_vector * solution)
{
  if(diag->size != rhs->size ||
          (offdiag->size != rhs->size && offdiag->size != rhs->size-1) ||
	  (solution->size != rhs->size)
          ) {
    return GSL_EBADLEN;
  }
  else {
    return solve_tridiag(diag->data, diag->stride,
                         offdiag->data, offdiag->stride,
			 rhs->data, rhs->stride,
	                 solution->data, solution->stride,
	                 diag->size);
  }
}

int
gsl_linalg_solve_symm_cyc_tridiag(
  const gsl_vector * diag,
  const gsl_vector * offdiag,
  const gsl_vector * rhs,
  gsl_vector * solution)
{
  if(diag->size != rhs->size ||
          offdiag->size != rhs->size ||
          solution->size != rhs->size
          ) {
    return GSL_EBADLEN;
  }
  else {
    return solve_cyc_tridiag(diag->data, diag->stride,
                             offdiag->data, offdiag->stride,
			     rhs->data, rhs->stride,
	                     solution->data, solution->stride,
	                     diag->size);
  }
}


#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <mpfr.h>
#include <petsc.h>

#include "../ellipsoid/ellipsoid.h"
#include "../ellipsoid/ellSolv.h"
#include "../constants.h"
#include "testFunctions.h"
#include "../tanhsinh.h"

/*
EX3 - PLOTS CONVERGENCE OF A SINGLE NORMALIZATION CONSTANT
normConv.py PLOTS RESULT
*/

#undef __FUNCT__
#define __FUNCT__ "NormConstantIntFixed"
PetscErrorCode NormConstantIntFixed(EllipsoidalSystem *e, PetscInt n, PetscInt p, PetscInt prec, PetscInt nPts, PetscReal *intVals, PetscReal *normConst)
{
  PetscErrorCode ierr;
  PetscInt flopCount;
  PetscFunctionBegin;
  flopCount = 0;

  FuncInfo2 ctx1 = { .e = e, .n = n, .p = p, .numeratorType = 0, .denomSign = 1 };
  FuncInfo2 ctx2 = { .e = e, .n = n, .p = p, .numeratorType = 1, .denomSign = 1 };
  FuncInfo2 ctx3 = { .e = e, .n = n, .p = p, .numeratorType = 0, .denomSign = -1 };
  FuncInfo2 ctx4 = { .e = e, .n = n, .p = p, .numeratorType = 1, .denomSign = -1 };

  PetscReal integrals[4];

  mpfr_t mpfrzero;
  mpfr_t mpfrone;
  mpfr_inits(mpfrzero, mpfrone, NULL);
  mpfr_set_d(mpfrzero, 0.0, MPFR_RNDN);
  mpfr_set_d(mpfrone, 1.0, MPFR_RNDN);

  ierr = DEQuad((PetscErrorCode (*)(mpfr_t*, mpfr_t*, void*)) normFunction1, e->hp_h, e->hp_k, prec, nPts, integrals+0, &ctx1);
  intVals[0] = integrals[0];
  ierr = DEQuad((PetscErrorCode (*)(mpfr_t*, mpfr_t*, void*)) normFunction1, e->hp_h, e->hp_k, prec, nPts, integrals+1, &ctx2);
  intVals[1] = integrals[1];
  ierr = DEQuad((PetscErrorCode (*)(mpfr_t*, mpfr_t*, void*)) normFunction1, mpfrzero, e->hp_h, prec, nPts, integrals+2, &ctx3);
  intVals[2] = integrals[2];
  ierr = DEQuad((PetscErrorCode (*)(mpfr_t*, mpfr_t*, void*)) normFunction1, mpfrzero, e->hp_h, prec, nPts, integrals+3, &ctx4);
  intVals[3] = integrals[3];
  *normConst = 8.0*(integrals[2]*integrals[1] - integrals[0]*integrals[3]); flopCount += 4;

  mpfr_clears(mpfrzero, mpfrone, NULL);

  ierr = PetscLogFlops(flopCount);CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT_ "normConvergence"
PetscErrorCode normConvergence(PetscInt n, PetscInt p)
{
  PetscErrorCode ierr;

  const PetscInt MPFR_PREC = 64;
  const PetscInt NUM_SOLUTIONS = 80;
  const PetscInt POINTS_MIN = 4;
  const PetscInt POINTS_STEP = 2;
  const PetscInt prec = 16;
  const PetscReal a = 3.0;
  const PetscReal b = 2.0;
  const PetscReal c = 1.0;
  mpfr_t solExact;
  mpfr_t intExact[4];
  PetscReal solutions[NUM_SOLUTIONS];
  mpfr_t errors   [NUM_SOLUTIONS];
  PetscReal intSols[4*NUM_SOLUTIONS];
  mpfr_t intErrors[4*NUM_SOLUTIONS];
  PetscReal sol2, sol3, sol4;
  PetscLogEvent flopCounts[NUM_SOLUTIONS];
  EllipsoidalSystem e;
  PetscInt i;
  PetscEventPerfInfo info;
  //PetscInt n = 7;
  //PeatscInt p = 4;

  PetscFunctionBegin;

  mpfr_set_default_prec(4*MPFR_PREC);
  initEllipsoidalSystem(&e, a, b, c, MPFR_PREC);

  mpfr_init(solExact);
  for(PetscInt k=0; k < 4; ++k)
    mpfr_init(intExact[k]);
  for(PetscInt k=0; k < NUM_SOLUTIONS; ++k )
    mpfr_init(errors[k]);
  for(PetscInt k=0; k < 4*NUM_SOLUTIONS; ++k)
    mpfr_init(intErrors[k]);

  /* calculate "exact" solution */
  ierr = calcNormalization2MPFR(&e, n, p, intExact, &solExact);CHKERRQ(ierr);
  printf("old norm constant: %15.15f\n", solExact);

  /* calculate approximate solutions and record flops */
  char text[40] = "%d points";
  char sText[40];
  for(i=0; i<NUM_SOLUTIONS; ++i) {
    sprintf(sText, text, POINTS_MIN + POINTS_STEP*i);
    ierr = PetscLogEventRegister(sText, 0, flopCounts+i);CHKERRQ(ierr);
    ierr = PetscLogEventBegin(flopCounts[i], 0, 0, 0, 0);CHKERRQ(ierr);
    ierr = NormConstantIntFixed(&e, n, p, prec, POINTS_MIN + POINTS_STEP*i, intSols+4*i, solutions+i);CHKERRQ(ierr);
    ierr = PetscLogEventEnd(flopCounts[i], 0, 0, 0, 0);CHKERRQ(ierr);
    printf("new norm constant: %15.15f\n", solutions[i]);
  }
  /* calculate errors */
  for(i=0; i<NUM_SOLUTIONS; ++i) {
    //errors[i] = PetscAbsReal((solExact - solutions[i])/solExact);
    mpfr_sub_d(errors[i], solExact, solutions[i], MPFR_RNDN);
    mpfr_div(errors[i], errors[i], solExact, MPFR_RNDN);
    mpfr_abs(errors[i], errors[i], MPFR_RNDN);
    //intErrors[4*i+0] = PetscAbsReal((intExact[0] - intSols[4*i+0])/intExact[0]);
    mpfr_sub_d(intErrors[4*i+0], intExact[0], intSols[4*i+0], MPFR_RNDN);
    mpfr_div(intErrors[4*i+0], intErrors[4*i+0], intExact[0], MPFR_RNDN);
    mpfr_abs(intErrors[4*i+0], intErrors[4*i+0], MPFR_RNDN);
    //intErrors[4*i+1] = PetscAbsReal((intExact[1] - intSols[4*i+1])/intExact[1]);
    mpfr_sub_d(intErrors[4*i+1], intExact[1], intSols[4*i+1], MPFR_RNDN);
    mpfr_div(intErrors[4*i+1], intErrors[4*i+1], intExact[1], MPFR_RNDN);
    mpfr_abs(intErrors[4*i+1], intErrors[4*i+1], MPFR_RNDN);
    //intErrors[4*i+2] = PetscAbsReal((intExact[2] - intSols[4*i+2])/intExact[2]);
    mpfr_sub_d(intErrors[4*i+2], intExact[2], intSols[4*i+2], MPFR_RNDN);
    mpfr_div(intErrors[4*i+2], intErrors[4*i+2], intExact[2], MPFR_RNDN);
    mpfr_abs(intErrors[4*i+2], intErrors[4*i+2], MPFR_RNDN);
    //intErrors[4*i+3] = PetscAbsReal((intExact[3] - intSols[4*i+3])/intExact[3]);
    mpfr_sub_d(intErrors[4*i+3], intExact[3], intSols[4*i+3], MPFR_RNDN);
    mpfr_div(intErrors[4*i+3], intErrors[4*i+3], intExact[3], MPFR_RNDN);
    mpfr_abs(intErrors[4*i+3], intErrors[4*i+3], MPFR_RNDN);
    printf("errors[%d] = %15.15f\n", i, errors[i]);
  }

  FILE *fp1 = fopen("out/normInt1Prec.txt", "w");
  FILE *fp2 = fopen("out/normInt2Prec.txt", "w");
  FILE *fp3 = fopen("out/normInt3Prec.txt", "w");
  FILE *fp4 = fopen("out/normInt4Prec.txt", "w");
  FILE *fp = fopen("out/normWorkPrec.txt", "w");
  fprintf(fp, "points flops error\n");
  fprintf(fp1, "points error\n");
  fprintf(fp2, "points error\n");
  fprintf(fp3, "points error\n");
  fprintf(fp4, "points error\n");
  for(i=0; i<NUM_SOLUTIONS; ++i) {
    ierr = PetscLogEventGetPerfInfo(PETSC_DETERMINE, flopCounts[i], &info);CHKERRQ(ierr);
    fprintf(fp, "%d %4.4e %4.4e\n", POINTS_MIN + POINTS_STEP*i, info.flops, mpfr_get_d(errors[i], MPFR_RNDN));
    fprintf(fp1, "%d %4.4e\n", POINTS_MIN + POINTS_STEP*i, mpfr_get_d(intErrors[4*i+0], MPFR_RNDN));
    fprintf(fp2, "%d %4.4e\n", POINTS_MIN + POINTS_STEP*i, mpfr_get_d(intErrors[4*i+1], MPFR_RNDN));
    fprintf(fp3, "%d %4.4e\n", POINTS_MIN + POINTS_STEP*i, mpfr_get_d(intErrors[4*i+2], MPFR_RNDN));
    fprintf(fp4, "%d %4.4e\n", POINTS_MIN + POINTS_STEP*i, mpfr_get_d(intErrors[4*i+3], MPFR_RNDN));
  }
  fclose(fp);
  fclose(fp1);
  fclose(fp2);
  fclose(fp3);
  mpfr_clear(solExact);

  PetscFunctionReturn(0);

}

#undef __FUNCT__
#define __FUNCT__ "main"
PetscErrorCode main(int argc, char **argv)
{
  PetscErrorCode ierr;
  PetscFunctionBeginUser;

  ierr = PetscInitialize(&argc, &argv, NULL, NULL);CHKERRQ(ierr);
  ierr = PetscLogDefaultBegin();CHKERRQ(ierr);

  ierr = normConvergence(21,22);CHKERRQ(ierr);

  ierr = PetscFinalize();

  PetscFunctionReturn(0);
}


/* Author: Romain "Artefact2" Dal Maso <artefact2@gmail.com> */

/* This program is free software. It comes without any warranty, to the
 * extent permitted by applicable law. You can redistribute it and/or
 * modify it under the terms of the Do What The Fuck You Want To Public
 * License, Version 2, as published by Sam Hocevar. See
 * http://sam.zoy.org/wtfpl/COPYING for more details. */

#define warn(...) do {	  \
		char msg[256]; \
		snprintf(msg, 256, __VA_ARGS__); \
		fprintf(stderr, "%s(): %s\n", __func__, msg); \
	} while(0)

#define fatal(...) do {	  \
		warn(__VA_ARGS__); \
		exit(1); \
	} while(0)

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <math.h>
#include <gsl/gsl_sf_gamma.h>

#define MCTS (1 << 15)

void read_rolls(unsigned int, unsigned int* rcount, unsigned int* n);
unsigned int ecdf_distance(unsigned int, unsigned int*);
void ecdf_gen_mc_table(unsigned int sides, unsigned int n, unsigned int count, unsigned int*);
bool find_in_sorted_array(unsigned int val, unsigned int count, unsigned int* array, unsigned int* low, unsigned int* high);
double chisq_test(unsigned int sides, unsigned int n, unsigned int*);
void confidence_test(unsigned int sides, unsigned int n, unsigned int*);

int main(int argc, char** argv) {
	if(argc != 2) {
		fprintf(stderr, "Usage: %s <num-sides> < rolls.txt\n", argv[0]);
		exit(2);
	}

	unsigned int sides = strtol(argv[1], NULL, 10);
	if(sides < 2) {
		fatal("must have at least 2 sides, got %d", sides);
	}

	unsigned int rcount[sides];
	unsigned int n;
	read_rolls(sides, rcount, &n);
	if(n % sides) {
		fatal("sample size (%d) not a multiple of sides (%d)", n, sides);
	}

	printf("SmpSize:   n=%d\n", n);

	unsigned int ecdf_table[MCTS];
	ecdf_gen_mc_table(sides, n, MCTS, ecdf_table);

	unsigned int dist = ecdf_distance(sides, rcount);
	unsigned int lo, hi;
	bool found;

	found = find_in_sorted_array(dist, MCTS, ecdf_table, &lo, &hi);
	printf("ECDF:      p%s%5.4f\n", found ? "=" : "<", found ? (1.0 - (double)(lo + hi) / (double)(2 * MCTS)) : ((double)(MCTS - lo) / (double)MCTS));

	printf("ChiSq:     p=%5.4f\n", chisq_test(sides, n, rcount));

	printf("ConfInt99:");
	confidence_test(sides, n, rcount);
	printf("\n");
}

void read_rolls(unsigned int sides, unsigned int* rcount, unsigned int* n) {
	char buf[16];
	unsigned int roll;

	memset(rcount, 0, sides * sizeof(unsigned int));
	*n = 0;

	while(!feof(stdin)) {
		if(fgets(buf, 16, stdin) == NULL) continue;
		if(buf[0] == '\n' || buf[0] == '\0') continue;

		roll = strtol(buf, NULL, 10);
		if(roll < 1 || roll > sides) {
			warn("ignoring roll %d", roll);
			continue;
		}

		++(rcount[roll - 1]);
		++(*n);
	}
}

unsigned int ecdf_distance(unsigned int sides, unsigned int* rcount) {
	unsigned int cumulative = 0, ideal_cumulative, ideal_step, i, distance, tdist;

	for(i = 0; i < sides; ++i) {
		cumulative += rcount[i];
	}

	ideal_cumulative = cumulative;
	ideal_step = ideal_cumulative / sides;
	distance = 0;

	for(i = 0; i < sides; ++i) {
		cumulative -= rcount[i];
		ideal_cumulative -= ideal_step;
		tdist = (cumulative >= ideal_cumulative) ? (cumulative - ideal_cumulative) : (ideal_cumulative - cumulative);
		if(tdist > distance) distance = tdist;
	}

	return distance;
}

int cmpuint(const void* a, const void* b) {
	return *(unsigned int*)a - *(unsigned int*)b;
}

void ecdf_gen_mc_table(unsigned int sides, unsigned int n, unsigned int count, unsigned int* table) {
	unsigned int rcount[sides];
	unsigned int i, j;

	for(i = 0; i < count; ++i) {
		memset(rcount, 0, sides * sizeof(unsigned int));

		for(j = 0; j < n; ++j) {
			++(rcount[rand() % sides]);
		}

		table[i] = ecdf_distance(sides, rcount);
	}

	qsort(table, count, sizeof(unsigned int), cmpuint);
}

bool find_in_sorted_array(unsigned int val, unsigned int count, unsigned int* array, unsigned int* low, unsigned int* high) {
	if(array[0] > val) {
		*low = *high = 0;
		return false;
	} else if(array[count - 1] < val) {
		*low = *high = count - 1;
		return false;
	}

	unsigned int mid;

	*low = 0;
	*high = count - 1;

	while(*high > *low) {
		mid = *low + (*high - *low) / 2;

		if(array[mid] == val) {
			*low = *high = mid;
			while(*low < count && array[*low] == val) --(*low);
			++(*low);
			while(*high < count && array[*high] == val) ++high;
			--(*high);
			return true;
		} else if(array[mid] > val) {
			*high = mid;
		} else {
			*low = mid;
		}
	}

	return false;
}

double chisq_test(unsigned int sides, unsigned int n, unsigned int* rcount) {
	unsigned int ideal = n / sides;
	double chisq = 0.0;

	for(unsigned int i = 0; i < sides; ++i) {
		chisq += (rcount[i] - ideal) * (rcount[i] - ideal);
	}
	chisq /= (double)ideal;

	return 1.0 - gsl_sf_gamma_inc_P((double)(sides - 1) / (double)2, chisq / (double)2);
}

void confidence_test(unsigned int sides, unsigned int n, unsigned int* rcount) {
	double f, amp, ideal = 1.0 / (double)sides;
	bool anomalies = false;

	for(unsigned int i = 0; i < sides; ++i) {
		f = (double)rcount[i] / (double)n;
		amp = 2.575 * sqrt(f * (1.0 - f) / (double)n);
		if(ideal > f + amp) {
			printf(" %d-", i+1);
			anomalies = true;
		} else if(ideal < f - amp) {
			printf(" %d+", i+1);
			anomalies = true;
		}
	}

	if(!anomalies) {
		printf(" OK");
	}
}


/* specfunc/dilog.c
 *
 * 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 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/* Author:  G. Jungman */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include "gsl_sf_clausen.h"
#include "gsl_sf_trig.h"
#include "gsl_sf_log.h"
#include "gsl_sf_dilog.h"

/* Evaluate series for real dilog(x)
 * Sum[ x^k / k^2, {k,1,Infinity}]
 *
 * Converges rapidly for |x| < 1/2.
 */
static
int
dilog_series(const double x, gsl_sf_result * result)
{
  const int kmax = 1000;
  double sum  = x;
  double term = x;
  int k;
  for(k=2; k<kmax; k++) {
    double rk = (k-1.0)/k;
    term *= x;
    term *= rk*rk;
    sum += term;
    if(fabs(term/sum) < GSL_DBL_EPSILON) break;
  }

  result->val  = sum;
  result->err  = 2.0 * fabs(term);
  result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);

  if(k == kmax)
    GSL_ERROR ("error", GSL_EMAXITER);
  else
    return GSL_SUCCESS;
}

/* Assumes x >= 0.0
 */
static
int
dilog_xge0(const double x, gsl_sf_result * result)
{
  if(x > 2.0) {
    const double log_x = log(x);
    gsl_sf_result ser;
    int stat_ser = dilog_series(1.0/x, &ser);
    double t1 = M_PI*M_PI/3.0;
    double t2 = ser.val;
    double t3 = 0.5*log_x*log_x;
    result->val  = t1 - t2 - t3;
    result->err  = GSL_DBL_EPSILON * fabs(log_x) + ser.err;
    result->err += GSL_DBL_EPSILON * (fabs(t1) + fabs(t2) + fabs(t3));
    result->val += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return stat_ser;
  }
  else if(x > 1.01) {
    const double log_x    = log(x);
    const double log_term = log_x * (log(1.0-1.0/x) + 0.5*log_x);
    gsl_sf_result ser;
    int stat_ser = dilog_series(1.0 - 1.0/x, &ser);
    double t1 = M_PI*M_PI/6.0;
    double t2 = ser.val;
    double t3 = log_term;
    result->val  = t1 + t2 - t3;
    result->err  = GSL_DBL_EPSILON * fabs(log_x) + ser.err;
    result->err += GSL_DBL_EPSILON * (fabs(t1) + fabs(t2) + fabs(t3));
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return stat_ser;
  }
  else if(x > 1.0) {
    /* series around x = 1.0 */
    const double eps = x - 1.0;
    const double lne = log(eps);
    const double c0 = M_PI*M_PI/6.0;
    const double c1 =   1.0 - lne;
    const double c2 = -(1.0 - 2.0*lne)/4.0;
    const double c3 =  (1.0 - 3.0*lne)/9.0;
    const double c4 = -(1.0 - 4.0*lne)/16.0;
    const double c5 =  (1.0 - 5.0*lne)/25.0;
    const double c6 = -(1.0 - 6.0*lne)/36.0;
    const double c7 =  (1.0 - 7.0*lne)/49.0;
    const double c8 = -(1.0 - 8.0*lne)/64.0;
    result->val = c0+eps*(c1+eps*(c2+eps*(c3+eps*(c4+eps*(c5+eps*(c6+eps*(c7+eps*c8)))))));
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x == 1.0) {
    result->val = M_PI*M_PI/6.0;
    result->err = 2.0 * GSL_DBL_EPSILON * M_PI*M_PI/6.0;
    return GSL_SUCCESS;
  }
  else if(x > 0.5) {
    const double log_x = log(x);
    gsl_sf_result ser;
    int stat_ser = dilog_series(1.0-x, &ser);
    double t1 = M_PI*M_PI/6.0;
    double t2 = ser.val;
    double t3 = log_x*log(1.0-x);
    result->val  = t1 - t2 - t3;
    result->err  = GSL_DBL_EPSILON * fabs(log_x) + ser.err;
    result->err += GSL_DBL_EPSILON * (fabs(t1) + fabs(t2) + fabs(t3));
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return stat_ser;
  }
  else if(x > 0.0) {
    return dilog_series(x, result);
  }
  else {
    /* x == 0.0 */
    result->val = 0.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
}

/* Evaluate the series representation for Li2(z):
 *
 *   Li2(z) = Sum[ |z|^k / k^2 Exp[i k arg(z)], {k,1,Infinity}]
 *   |z|    = r
 *   arg(z) = theta
 *
 * Assumes 0 < r < 1.
 */
static
int
dilogc_series_1(double r, double cos_theta, double sin_theta,
                gsl_sf_result * real_result, gsl_sf_result * imag_result)
{
  double alpha = 1.0 - cos_theta;
  double beta  = sin_theta;
  double ck = cos_theta;
  double sk = sin_theta;
  double rk = r;
  double real_sum = r*ck;
  double imag_sum = r*sk;
  int kmax = 50 + (int)(22.0/(-log(r))); /* tuned for double-precision */
  int k;
  for(k=2; k<kmax; k++) {
    double ck_tmp = ck;
    ck = ck - (alpha*ck + beta*sk);
    sk = sk - (alpha*sk - beta*ck_tmp);
    rk *= r;
    real_sum += rk/((double)k*k) * ck;
    imag_sum += rk/((double)k*k) * sk;
  }

  real_result->val = real_sum;
  real_result->err = 2.0 * kmax * GSL_DBL_EPSILON * fabs(real_sum);
  imag_result->val = imag_sum;
  imag_result->err = 2.0 * kmax * GSL_DBL_EPSILON * fabs(imag_sum);

  return GSL_SUCCESS;
}

/* Evaluate a series for Li_2(z) when |z| is near 1.
 * This is uniformly good away from z=1.
 *
 *   Li_2(z) = Sum[ a^n/n! H_n(theta), {n, 0, Infinity}]
 *
 * where
 *   H_n(theta) = Sum[ e^(i m theta) m^n / m^2, {m, 1, Infinity}]
 *   a = ln(r)
 *
 *  H_0(t) = Gl_2(t) + i Cl_2(t)
 *  H_1(t) = 1/2 ln(2(1-c)) + I atan2(-s, 1-c)
 *  H_2(t) = -1/2 + I/2 s/(1-c)
 *  H_3(t) = -1/2 /(1-c)
 *  H_4(t) = -I/2 s/(1-c)^2
 *  H_5(t) = 1/2 (2 + c)/(1-c)^2
 *  H_6(t) = I/2 s/(1-c)^5 (8(1-c) - s^2 (3 + c))
 *
 *  assumes: 0 <= theta <= 2Pi
 */
static
int
dilogc_series_2(double r, double theta, double cos_theta, double sin_theta,
                gsl_sf_result * real_result, gsl_sf_result * imag_result)
{
  double a = log(r);
  double omc = 1.0 - cos_theta;
  double H_re[7];
  double H_im[7];
  double an, nfact;
  double sum_re, sum_im;
  gsl_sf_result Him0;
  int n;

  H_re[0] = M_PI*M_PI/6.0 + 0.25*(theta*theta - 2.0*M_PI*fabs(theta));
  gsl_sf_clausen_e(theta, &Him0);
  H_im[0] = Him0.val;

  H_re[1] = -0.5*log(2.0*omc);
  H_im[1] = -atan2(-sin_theta, omc);

  H_re[2] = -0.5;
  H_im[2] = 0.5 * sin_theta/omc;

  H_re[3] = -0.5/omc;
  H_im[3] = 0.0;

  H_re[4] = 0.0;
  H_im[4] = -0.5*sin_theta/(omc*omc);

  H_re[5] = 0.5 * (2.0 + cos_theta)/(omc*omc);
  H_im[5] = 0.0;

  H_re[6] = 0.0;
  H_im[6] = 0.5 * sin_theta/(omc*omc*omc*omc*omc)
            * (8*omc - sin_theta*sin_theta*(3 + cos_theta));

  sum_re = H_re[0];
  sum_im = H_im[0];
  an = 1.0;
  nfact = 1.0;
  for(n=1; n<=6; n++) {
    double t;
    an *= a;
    nfact *= n;
    t = an/nfact;
    sum_re += t * H_re[n];
    sum_im += t * H_im[n];
  }

  real_result->val = sum_re;
  real_result->err = 2.0 * 6.0 * GSL_DBL_EPSILON * fabs(sum_re) + fabs(an/nfact);
  imag_result->val = sum_im;
  imag_result->err = 2.0 * 6.0 * GSL_DBL_EPSILON * fabs(sum_im) + Him0.err + fabs(an/nfact);

  return GSL_SUCCESS;
}

/* complex dilogarithm in the unit disk
 * assumes:  r < 1  and  0 <= theta <= 2Pi
 */
static
int
dilogc_unitdisk(double r, double theta, gsl_sf_result * real_dl, gsl_sf_result * imag_dl)
{
  const double zeta2 = M_PI*M_PI/6.0;
  int stat_dilog;
  gsl_sf_result cos_theta;
  gsl_sf_result sin_theta;
  int stat_cos = gsl_sf_cos_e(theta, &cos_theta);
  int stat_sin = gsl_sf_sin_e(theta, &sin_theta);
  gsl_sf_result x;
  gsl_sf_result y;
  gsl_sf_result x_tmp, y_tmp, r_tmp;
  gsl_sf_result result_re_tmp, result_im_tmp;
  double cos_theta_tmp;
  double sin_theta_tmp;
  x.val = r * cos_theta.val;
  x.err = r * cos_theta.err;
  y.val = r * sin_theta.val;
  y.err = r * sin_theta.err;

  /* Reflect away from z = 1 if
   * we are too close.
   */
  if(x.val > 0.5) {
    x_tmp.val = 1.0 - x.val;
    x_tmp.err = GSL_DBL_EPSILON * (1.0 + fabs(x.val)) + x.err;
    y_tmp.val = -y.val;
    y_tmp.err = y.err;
    r_tmp.val = sqrt(x_tmp.val*x_tmp.val + y_tmp.val*y_tmp.val);
    r_tmp.err = (x_tmp.err*fabs(x_tmp.val) + y_tmp.err*fabs(y_tmp.val))/fabs(r_tmp.val);
  }
  else {
    x_tmp.val = x.val;
    x_tmp.err = x.err;
    y_tmp.val = y.val;
    y_tmp.err = y.err;
    r_tmp.val = r;
    r_tmp.err = r * GSL_DBL_EPSILON;
  }

  cos_theta_tmp = x_tmp.val / r_tmp.val;
  sin_theta_tmp = y_tmp.val / r_tmp.val;

  /* Calculate dilog of the transformed variable.
   */
  if(r_tmp.val < 0.98) {
    stat_dilog = dilogc_series_1(r_tmp.val, cos_theta_tmp, sin_theta_tmp,
				 &result_re_tmp, &result_im_tmp
				 );
  }
  else {
    double theta_tmp = atan2(y_tmp.val, x_tmp.val);
    stat_dilog = dilogc_series_2(r_tmp.val, theta_tmp, cos_theta_tmp, sin_theta_tmp,
                                 &result_re_tmp, &result_im_tmp
				 );
  }

  /* Unwind reflection if necessary.
   *
   * Li2(z) = -Li2(1-z) + zeta(2) - ln(z) ln(1-z)
   */
  if(x.val > 0.5) {
    double lnz    =  log(r);                         /*  log(|z|)   */
    double lnomz  =  log(r_tmp.val);                 /*  log(|1-z|) */
    double argz   =  theta;                          /*  arg(z)     */
    double argomz =  atan2(y_tmp.val, x_tmp.val);    /*  arg(1-z)   */
    real_dl->val  = -result_re_tmp.val + zeta2 - lnz*lnomz + argz*argomz;
    real_dl->err  =  result_re_tmp.err;
    real_dl->err +=  GSL_DBL_EPSILON * (zeta2 + fabs(lnz*lnomz) + fabs(argz*argomz));
    real_dl->err +=  2.0 * GSL_DBL_EPSILON * fabs(real_dl->val);
    imag_dl->val  = -result_im_tmp.val - argz*lnomz - argomz*lnz;
    imag_dl->err  =  result_im_tmp.err;
    imag_dl->err +=  GSL_DBL_EPSILON * (fabs(argz*lnomz) + fabs(argomz*lnz));
    imag_dl->err +=  2.0 * GSL_DBL_EPSILON * fabs(imag_dl->val);
  }
  else {
    real_dl->val = result_re_tmp.val;
    real_dl->err = result_re_tmp.err;
    imag_dl->val = result_im_tmp.val;
    imag_dl->err = result_im_tmp.err;
  }

  return GSL_ERROR_SELECT_3(stat_dilog, stat_sin, stat_cos);
}

/*-*-*-*-*-*-*-*-*-*-*-* Functions with Error Codes *-*-*-*-*-*-*-*-*-*-*-*/

int
gsl_sf_dilog_e(const double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(x >= 0.0) {
    return dilog_xge0(x, result);
  }
  else {
    gsl_sf_result d1, d2;
    int stat_d1 = dilog_xge0( -x, &d1);
    int stat_d2 = dilog_xge0(x*x, &d2);
    result->val  = -d1.val + 0.5 * d2.val;
    result->err  =  d1.err + 0.5 * d2.err;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_ERROR_SELECT_2(stat_d1, stat_d2);
  }
}

int
gsl_sf_complex_dilog_e(const double r, double theta,
                          gsl_sf_result * real_dl, gsl_sf_result * imag_dl)
{
  /* CHECK_POINTER(real_dl) */
  /* CHECK_POINTER(imag_dl) */

  if(r == 0.0) {
    real_dl->val = 0.0;
    real_dl->err = 0.0;
    imag_dl->val = 0.0;
    imag_dl->err = 0.0;
    return GSL_SUCCESS;
  }

/*
  if(theta < 0.0 || theta > 2.0*M_PI) {
    gsl_sf_angle_restrict_pos_e(&theta);
  }
*/

  /* Trap cases of real-valued argument.
   */
  if(theta == 0.0) {
    int stat_d;
    imag_dl->val = ( r > 1.0 ? -M_PI * log(r) : 0.0 );
    imag_dl->err = 2.0 * GSL_DBL_EPSILON * fabs(imag_dl->val);
    stat_d = gsl_sf_dilog_e(r, real_dl);
    return stat_d;
  }
  if(theta == M_PI) {
    int stat_d;
    imag_dl->val = 0.0;
    imag_dl->err = 0.0;
    stat_d = gsl_sf_dilog_e(-r, real_dl);
    return stat_d;
  }

  /* Trap unit circle case.
   */
  if(r == 1.0) {
    gsl_sf_result theta_restrict;
    int stat_r = gsl_sf_angle_restrict_pos_err_e(theta, &theta_restrict);
    int stat_c;
    const double term1 = theta_restrict.val*theta_restrict.val;
    const double term2 = 2.0*M_PI*fabs(theta_restrict.val);
    const double term1_err = 2.0 * fabs(theta_restrict.val * theta_restrict.err);
    const double term2_err = 2.0*M_PI*fabs(theta_restrict.err);
    real_dl->val  = M_PI*M_PI/6.0 + 0.25*(term1 - term2);
    real_dl->err  = 2.0 * GSL_DBL_EPSILON * (M_PI*M_PI/6.0 + 0.25 * (fabs(term1) + fabs(term2)));
    real_dl->err += 0.25 * (term1_err + term2_err);
    real_dl->err += 2.0 * GSL_DBL_EPSILON * fabs(real_dl->val);
    stat_c = gsl_sf_clausen_e(theta, imag_dl);
    stat_r = 0;  /* discard restrict status */
    return stat_c;
  }

  /* Generic case.
   */
  {
    int stat_dilog;
    double r_tmp, theta_tmp;
    gsl_sf_result result_re_tmp, result_im_tmp;

    /* Reduce argument to unit disk.
     */
    if(r > 1.0) {
      r_tmp     = 1.0 / r;
      theta_tmp = /* 2.0*M_PI */ - theta;
    }
    else {
      r_tmp     = r;
      theta_tmp = theta;
    }

    /* Calculate in the unit disk.
     */
    stat_dilog = dilogc_unitdisk(r_tmp, theta_tmp,
                                 &result_re_tmp, &result_im_tmp
				 );

    /* Unwind the inversion if necessary. We calculate
     * the imaginary part explicitly if using the inversion
     * because there is no simple relationship between
     * arg(1-z) and arg(1 - 1/z), which is the "omega"
     * term in [Lewin A.2.5 (1)].
     */
    if(r > 1.0) {
      const double zeta2 = M_PI*M_PI/6.0;
      double x = r * cos(theta);
      double y = r * sin(theta);
      double omega = atan2(y, 1.0-x);
      double lnr = log(r);
      double pmt = M_PI - theta;
      gsl_sf_result Cl_a, Cl_b, Cl_c;
      double r1, r2, r3, r4, r5;
      int stat_c1 = gsl_sf_clausen_e(2.0*omega, &Cl_a);
      int stat_c2 = gsl_sf_clausen_e(2.0*theta, &Cl_b);
      int stat_c3 = gsl_sf_clausen_e(2.0*(omega+theta), &Cl_c);
      int stat_c  = GSL_ERROR_SELECT_3(stat_c1, stat_c2, stat_c3);
      r1 = -result_re_tmp.val;
      r2 = -0.5*lnr*lnr;
      r3 =  0.5*pmt*pmt;
      r4 = -zeta2;
      r5 =  omega*lnr;
      real_dl->val  = r1 + r2 + r3 + r4;
      real_dl->err  = result_re_tmp.err;
      real_dl->err += GSL_DBL_EPSILON * (fabs(r1) + fabs(r2) + fabs(r3) + fabs(r4));
      real_dl->err += 2.0 * GSL_DBL_EPSILON * fabs(real_dl->val);
      imag_dl->val  = r5 + 0.5*(Cl_a.val + Cl_b.val - Cl_c.val);
      imag_dl->err  = GSL_DBL_EPSILON * fabs(r5);
      imag_dl->err += GSL_DBL_EPSILON * 0.5*(fabs(Cl_a.val) + fabs(Cl_b.val) + fabs(Cl_c.val));
      imag_dl->err += 0.5*(Cl_a.err + Cl_b.err + Cl_c.err);
      imag_dl->err += 2.0*GSL_DBL_EPSILON * fabs(imag_dl->val);
      return GSL_ERROR_SELECT_2(stat_dilog, stat_c);
    }
    else {
      real_dl->val = result_re_tmp.val;
      real_dl->err = result_re_tmp.err;
      imag_dl->val = result_im_tmp.val;
      imag_dl->err = result_im_tmp.err;
      return stat_dilog;
    }
  }
}

/*-*-*-*-*-*-*-*-*-* Functions w/ Natural Prototypes *-*-*-*-*-*-*-*-*-*-*/

#include "eval.h"

double gsl_sf_dilog(const double x)
{
  EVAL_RESULT(gsl_sf_dilog_e(x, &result));
}


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

int main(void) {
    /* coefficients of f(x) = -1 + x^5 */
    double a[6] = {-1, 0, 0, 0, 0, 1};
    printf("x = %f\n", gsl_poly_eval(a, 6, 3.0));
    return 0;
}

// int main(void) {
//     int i;
//     /* coefficients of P(x) = -1 + x^5 */
//     double a[6] = {-1, 0, 0, 0, 0, 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]);
//     }
//     return 0;
// }


/* Author: G. Jungman
 */
#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_ieee_utils.h>

#include <gsl/gsl_qrng.h>
#include <gsl/gsl_test.h>

void test_sobol(void)
{
  int status = 0;
  double v[3];
  /* int i; */

  /* test in dimension 2 */
  gsl_qrng * g = gsl_qrng_alloc(gsl_qrng_sobol, 2);
  gsl_qrng_get(g, v);
  gsl_qrng_get(g, v);
  gsl_qrng_get(g, v);
  status += ( v[0] != 0.25 || v[1] != 0.75 );
  gsl_qrng_get(g, v);
  status += ( v[0] != 0.375 || v[1] != 0.375 );
  gsl_qrng_free(g);

  gsl_test (status, "Sobol d=2");

  status = 0;
  /* test in dimension 3 */
  g = gsl_qrng_alloc(gsl_qrng_sobol, 3);
  gsl_qrng_get(g, v);
  gsl_qrng_get(g, v);
  gsl_qrng_get(g, v);
  status += ( v[0] != 0.25 || v[1] != 0.75 || v[2] != 0.25 );
  gsl_qrng_get(g, v);
  status += ( v[0] != 0.375 || v[1] != 0.375 || v[2] != 0.625 );

  gsl_test (status, "Sobol d=3");

  status = 0;
  gsl_qrng_init(g);
  gsl_qrng_get(g, v);
  gsl_qrng_get(g, v);
  gsl_qrng_get(g, v);
  status += ( v[0] != 0.25 || v[1] != 0.75 || v[2] != 0.25 );
  gsl_qrng_get(g, v);
  status += ( v[0] != 0.375 || v[1] != 0.375 || v[2] != 0.625 );
  gsl_qrng_free(g);

  gsl_test (status, "Sobol d=3 (reinitialized)");
}

void test_nied2(void)
{
  int status = 0;
  double v[3];
  /* int i; */

  /* test in dimension 2 */
  gsl_qrng * g = gsl_qrng_alloc(gsl_qrng_niederreiter_2, 2);
  gsl_qrng_get(g, v);
  gsl_qrng_get(g, v);
  gsl_qrng_get(g, v);
  status += ( v[0] != 0.75 || v[1] != 0.25 );
  gsl_qrng_get(g, v);
  status += ( v[0] != 0.25 || v[1] != 0.75 );
  gsl_qrng_get(g, v);
  gsl_qrng_get(g, v);
  gsl_qrng_get(g, v);
  status += ( v[0] != 0.625 || v[1] != 0.125 );
  gsl_qrng_free(g);

  gsl_test (status, "Niederreiter d=2");

  status = 0;

  /* test in dimension 3 */
  g = gsl_qrng_alloc(gsl_qrng_niederreiter_2, 3);
  gsl_qrng_get(g, v);
  gsl_qrng_get(g, v);
  gsl_qrng_get(g, v);
  status += ( v[0] != 0.75 || v[1] != 0.25 || v[2] != 0.3125 );
  gsl_qrng_get(g, v);
  status += ( v[0] != 0.25 || v[1] != 0.75 || v[2] != 0.5625 );
  gsl_qrng_get(g, v);
  gsl_qrng_get(g, v);
  gsl_qrng_get(g, v);
  status += ( v[0] != 0.625 || v[1] != 0.125 || v[2] != 0.6875 );

  gsl_test (status, "Niederreiter d=3");

  status = 0;

  gsl_qrng_init(g);
  gsl_qrng_get(g, v);
  gsl_qrng_get(g, v);
  gsl_qrng_get(g, v);
  status += ( v[0] != 0.75 || v[1] != 0.25 || v[2] != 0.3125 );
  gsl_qrng_get(g, v);
  status += ( v[0] != 0.25 || v[1] != 0.75 || v[2] != 0.5625 );
  gsl_qrng_get(g, v);
  gsl_qrng_get(g, v);
  gsl_qrng_get(g, v);
  status += ( v[0] != 0.625 || v[1] != 0.125 || v[2] != 0.6875 );
  gsl_qrng_free(g);

  gsl_test (status, "Niederreiter d=3 (reinitialized)");
}

int main()
{

  gsl_ieee_env_setup ();

  test_sobol();
  test_nied2();

  exit (gsl_test_summary ());
}


#include <stdio.h>
#include <stdarg.h>
#include <string.h>
#include <math.h>
#include <gbpLib.h>
#include <gbpMisc.h>
#include <gbpRNG.h>
#include <gsl/gsl_linalg.h>

void add_gaussian_noise(double *data, int n_data, int *seed, double sigma, double *covariance) {
    int       i_data;
    int       j_data;
    double *  data_temp;
    RNG_info *RNG;

    // Initialize RNG
    RNG = (RNG_info *)SID_malloc(sizeof(RNG_info));
    init_RNG(seed, RNG, RNG_DEFAULT);

    // Generate a random displacement vector
    gsl_vector *b;
    b = gsl_vector_calloc(n_data);
    for(i_data = 0; i_data < n_data; i_data++)
        gsl_vector_set(b, i_data, sigma * random_gaussian(RNG));

    // Use the rotated covariance matrix (if available)
    if(covariance != NULL) {
        // Perform Cholesky Decomposition
        gsl_matrix *m;
        m = gsl_matrix_calloc(n_data, n_data);
        for(i_data = 0; i_data < n_data; i_data++)
            for(j_data = 0; j_data < n_data; j_data++)
                gsl_matrix_set(m, i_data, j_data, covariance[i_data * n_data + j_data]);
        gsl_linalg_cholesky_decomp(m);
        for(i_data = 0; i_data < n_data; i_data++)
            for(j_data = i_data + 1; j_data < n_data; j_data++)
                gsl_matrix_set(m, i_data, j_data, 0.);
        for(i_data = 0; i_data < n_data; i_data++) {
            for(j_data = 0; j_data < n_data; j_data++)
                data[i_data] += gsl_matrix_get(m, i_data, j_data) * gsl_vector_get(b, j_data);
        }
        gsl_matrix_free(m);
    } else {
        for(j_data = 0; j_data < n_data; j_data++)
            data[j_data] += gsl_vector_get(b, j_data);
    }

    // Clean-up
    free_RNG(RNG);
    SID_free(SID_FARG RNG);
    gsl_vector_free(b);
}


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

#define N 1000000
#define REP 500

/* compilar: gcc fonte.c -o exec -lblas */

int main( void ) {
  float *x, *y, *y2;
  int i, j;
  double ti = 0, tf = 0; /* ti = tempo inicial // tf = tempo final */
  struct timeval tempo_inicio, tempo_fim;

  /* aloca vetores dianmicamente */

  x = ( float* ) malloc( N * sizeof( float ) );
  y = ( float* ) malloc( N * sizeof( float ) );
  y2 = ( float* ) malloc( N * sizeof( float ) );
  /* atribue valores aleatorios */
  for( i = 0; i < N; i++ ) {
    x[ i ] = random( );
    y2[ i ] = y[ i ] = random( );
  }
  /* marca tempo inicial */
  gettimeofday( &tempo_inicio, NULL );
  /* Calcula y=alfa*x+y */
  for( j = 0; j < REP; j++ ) {
    cblas_saxpy( N, 0.5, x, 1, y, 1 );
  }
  /* calcula o tempo gasto */
  gettimeofday( &tempo_fim, NULL );
  tf = ( double ) tempo_fim.tv_usec / 1000000.0 + tempo_fim.tv_sec;
  ti = ( double ) tempo_inicio.tv_usec / 1000000.0 + tempo_inicio.tv_sec;
  printf( "Tempo gasto em saxpy (BLAS) %.15f segundos \n", tf - ti );

  /* marca tempo inicial */
  gettimeofday( &tempo_inicio, NULL );
  /* Calcula y=alfa*x+y */
  for( j = 0; j < REP; j++ ) {
    for( i = 0; i < N; i++ ) {
      y2[ i ] = 0.5 * x[ i ] + y2[ i ];
    }
  }
  /* calcula o tempo gasto */
  gettimeofday( &tempo_fim, NULL );
  tf = ( double ) tempo_fim.tv_usec / 1000000.0 + tempo_fim.tv_sec;
  ti = ( double ) tempo_inicio.tv_usec / 1000000.0 + tempo_inicio.tv_sec;
  printf( "Tempo gasto direto %.15f segundos \n", tf - ti );

  return( 0 );
}


static const char help[] =
"Solves doubly-nonlinear obstacle problems in 2D.  Option prefix dnl_.\n"
"The PDE (interior condition) of such problems has solution u(x,y):\n"
"       - div (u^q |grad(u+b)|^{p-2} grad(u+b)) = f\n"
"subject to a obstacle constraint\n"
"       u >= psi\n"
"FIXME Includes the steady-state, nonlinear ice sheet problem in 2D in which u=H\n"
"is ice thickness,  b  is bed elevation, and  s = H + b  is surface elevation:\n"
"       - div (D grad H) - div(W H^{n+2}) = m\n"
"The diffusivity D and pseudo-velocity W (Bueler, 2016) are from the\n"
"nonsliding shallow ice approximation (SIA) flux:\n"
"       D = Gamma H^{n+2} |grad H + grad b|^{n-1}\n"
"       W = - Gamma |grad H + grad b|^{n-1} grad b\n"
"The climatic mass balance f = m(x,y,H) is from one of two models.\n"
"Constants are  n >= 1  and Gamma = 2 A (rho g)^n / (n+2)  where A is the ice\n"
"softness.  The domain is square  (0,L)^2  with zero Dirichlet boundary conditions.\n"
"The equation is discretized by a Q1 structured-grid FVE method (Bueler, 2016).\n"
"Requires SNESVI (-snes_type vinewton{rsls|ssls}) because of constraint;\n"
"defaults to SSLS.\n\n";

/*
1. shows basic success with SSLS but DIVERGES AT LEVEL 4:
   mpiexec -n 4 ./ice -ice_verif -snes_converged_reason -snes_grid_sequence LEV

2. consider making CMB model smooth

3. add CMB to dump and create plotting script (.py)

4. using exact init shows convergence depends strongly on eps for fine grids:
    for LEV in 1 2 3 4 5; do ./ice -ice_verif -ice_exact_init -snes_converged_reason -ksp_type gmres -pc_type gamg -da_refine $LEV -ice_eps EPS; done
result:
  (a) works at all levels if EPS=0.005; last KSP somewhat constant but SNES iters growing
  (b) fails on level 3 if EPS=0.003,0.002

5. convergent and nearly optimal GMG in flops *but cheating with exact init*, and *avoiding -snes_grid_sequence* and *significant eps=0.01 regularization*:
    for LEV in 1 2 3 4 5 6 7 8; do ./ice -ice_verif -ice_exact_init -snes_converged_reason -ksp_type gmres -pc_type mg -da_refine $LEV -snes_type vinewtonrsls -ice_eps 0.01; done

6. visualizing -snes_grid_sequence:
    ./ice -ice_verif -snes_grid_sequence 2 -ice_eps 0.005 -snes_converged_reason -snes_monitor_solution draw
(was -snes_grid_sequence bug with periodic BCs? see PETSc issue #300)

8. even seems to work in parallel:
    mpiexec -n 4 ./ice -ice_verif -snes_grid_sequence 5 -ice_eps 0.005 -snes_converged_reason -snes_monitor_solution draw

9. same outcome with -ice_exact_init and -da_refine 5
    mpiexec -n 4 ./ice -ice_verif -da_refine 5 -ice_eps 0.005 -snes_converged_reason -snes_monitor_solution draw -ice_exact_init

10. unpredictable response to changing -snes_linesearch_type bt|l2|basic  (cp seems rarely to work)
*/

/* see comments on runtime stuff in icet/icet.c, the time-dependent version */

#include <petsc.h>
#include "icecmb.h"

typedef struct {
    double    secpera,    // number of seconds in a year
              L,          // spatial domain is (0,L) x (0,L)
              g,          // acceleration of gravity
              rho_ice,    // ice density
              n_ice,      // Glen exponent for SIA flux term
              A_ice,      // ice softness
              Gamma,      // coefficient for SIA flux term
              D0,         // representative value of diffusivity (used in regularizing D)
              eps,        // regularization parameter for diffusivity D
              delta,      // dimensionless regularization for slope in SIA formulas
              lambda;     // amount of upwinding; lambda=0 is none and lambda=1 is "full"
    PetscBool verif,      // use dome formulas if true
              check_admissible; // check admissibility at start of FormFunctionLocal()
    CMBModel  *cmb;       // defined in cmbmodel.h
} AppCtx;

// compute radius from center of (0,L) x (0,L)
double radialcoord(double x, double y, AppCtx *user) {
  const double xc = x - user->L/2.0,
               yc = y - user->L/2.0;
  return PetscSqrtReal(xc * xc + yc * yc);
}

double DomeCMB(double x, double y, AppCtx *user) {
  const double  domeR  = 750.0e3,  // radius of exact ice sheet (m)
                domeH0 = 3600.0,   // center thickness of exact ice sheet (m)
                n  = user->n_ice,
                pp = 1.0 / n,
                CC = user->Gamma * PetscPowReal(domeH0,2.0*n+2.0)
                         / PetscPowReal(2.0 * domeR * (1.0-1.0/n),n);
  double        r, s, tmp1, tmp2;
  r = radialcoord(x, y, user);
  // avoid singularities at center and margin
  if (r < 0.01)
      r = 0.01;
  if (r > domeR - 0.01)
      r = domeR - 0.01;
  s = r / domeR;
  tmp1 = PetscPowReal(s,pp) + PetscPowReal(1.0-s,pp) - 1.0;
  tmp2 = 2.0 * PetscPowReal(s,pp) + PetscPowReal(1.0-s,pp-1.0) * (1.0 - 2.0*s) - 1.0;
  return (CC / r) * PetscPowReal(tmp1,n-1.0) * tmp2;
}

PetscErrorCode DomeThicknessLocal(DMDALocalInfo *info, double **aH, AppCtx *user) {
  const double   domeR  = 750.0e3,  // radius of exact ice sheet (m)
                 domeH0 = 3600.0,   // center thickness of exact ice sheet (m)
                 n  = user->n_ice,
                 mm = 1.0 + 1.0 / n,
                 qq = n / (2.0 * n + 2.0),
                 CC = domeH0 / PetscPowReal(1.0 - 1.0 / n,qq),
                 dx = user->L / (double)(info->mx-1),
                 dy = user->L / (double)(info->my-1);
  double         x, y, r, s, tmp;
  int            j, k;

  PetscFunctionBeginUser;
  for (k=info->ys; k<info->ys+info->ym; k++) {
      y = k * dy;
      for (j=info->xs; j<info->xs+info->xm; j++) {
          x = j * dx;
          r = radialcoord(x, y, user);
          // avoid singularities at margin and center
          if (r > domeR - 0.01)
              aH[k][j] = 0.0;
          else {
              if (r < 0.01)
                  r = 0.01;
              s = r / domeR;
              tmp = mm * s - (1.0/n) + PetscPowReal(1.0-s,mm) - PetscPowReal(s,mm);
              aH[k][j] = CC * PetscPowReal(tmp,qq);
          }
      }
  }
  PetscFunctionReturn(0);
}

extern PetscErrorCode SetFromOptionsAppCtx(AppCtx*);
extern PetscErrorCode FormBedLocal(DMDALocalInfo*, int, double**, AppCtx*);
extern PetscErrorCode FormBounds(SNES,Vec,Vec);
extern PetscErrorCode FormFunctionLocal(DMDALocalInfo*, double**, double**, AppCtx*);

int main(int argc,char **argv) {
  PetscErrorCode      ierr;
  DM                  da;
  SNES                snes;
  KSP                 ksp;
  Vec                 H;
  AppCtx              user;
  CMBModel            cmb;
  PetscBool           exact_init = PETSC_FALSE, // initialize using dome exact solution
                      dump = PETSC_FALSE;       // dump state (H,b) in binary file ice_MXxMY.dat after solve
  DMDALocalInfo       info;
  double              **aH;
  SNESConvergedReason reason;
  int                 snesit,kspit;

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

  user.secpera    = 31556926.0;  // number of seconds in a year
  user.L          = 1800.0e3;    // m; compare domeR=750.0e3 radius
  user.g          = 9.81;        // m/s^2
  user.rho_ice    = 910.0;       // kg/m^3
  user.n_ice      = 3.0;         // Glen exponent
  user.A_ice      = 3.1689e-24;  // 1/(Pa^3 s); EISMINT I value
  user.D0         = 1.0;         // m^2 / s
  user.eps        = 0.001;
  user.delta      = 1.0e-4;
  user.lambda     = 0.25;
  user.verif      = PETSC_FALSE;
  user.check_admissible = PETSC_FALSE;
  user.cmb        = NULL;

  ierr = PetscOptionsBegin(PETSC_COMM_WORLD,"ice_","options to ice","");CHKERRQ(ierr);
  ierr = PetscOptionsReal(
      "-A", "set value of ice softness A in units Pa-3 s-1",
      "ice.c",user.A_ice,&user.A_ice,NULL);CHKERRQ(ierr);
  ierr = PetscOptionsBool(
      "-check_admissible", "check admissibility of iterate at start of residual evaluation FormFunctionLocal()",
      "ice.c",user.check_admissible,&user.check_admissible,NULL);CHKERRQ(ierr);
  ierr = PetscOptionsReal(
      "-D0", "representative value of diffusivity (used in regularizing D) in units m2 s-1",
      "ice.c",user.D0,&user.D0,NULL);CHKERRQ(ierr);
  ierr = PetscOptionsReal(
      "-delta", "dimensionless regularization for slope in SIA formulas",
      "ice.c",user.delta,&user.delta,NULL);CHKERRQ(ierr);
  ierr = PetscOptionsBool(
      "-dump", "save final state (H, b)",
      "ice.c",dump,&dump,NULL);CHKERRQ(ierr);
  ierr = PetscOptionsReal(
      "-eps", "dimensionless regularization for diffusivity D",
      "ice.c",user.eps,&user.eps,NULL);CHKERRQ(ierr);
  ierr = PetscOptionsBool(
      "-exact_init", "initialize with dome exact solution",
      "ice.c",exact_init,&exact_init,NULL);CHKERRQ(ierr);
  ierr = PetscOptionsReal(
      "-L", "side length of domain in meters",
      "ice.c",user.L,&user.L,NULL);CHKERRQ(ierr);
  ierr = PetscOptionsReal(
      "-lambda", "amount of upwinding; lambda=0 is none and lambda=1 is full",
      "ice.c",user.lambda,&user.lambda,NULL);CHKERRQ(ierr);
  ierr = PetscOptionsReal(
      "-n", "value of Glen exponent n",
      "ice.c",user.n_ice,&user.n_ice,NULL);CHKERRQ(ierr);
  if (user.n_ice <= 1.0) {
      SETERRQ1(PETSC_COMM_WORLD,1,
          "ERROR: n = %f not allowed ... n > 1.0 is required\n",user.n_ice);
  }
  ierr = PetscOptionsReal(
      "-rho", "ice density in units kg m3",
      "ice.c",user.rho_ice,&user.rho_ice,NULL);CHKERRQ(ierr);
  ierr = PetscOptionsBool(
      "-verif", "use dome exact solution for verification",
      "ice.c",user.verif,&user.verif,NULL);CHKERRQ(ierr);
  ierr = PetscOptionsEnd();CHKERRQ(ierr);

  // derived constant computed after other ice properties are set
  user.Gamma = 2.0 * PetscPowReal(user.rho_ice*user.g,user.n_ice)
                   * user.A_ice / (user.n_ice+2.0);

  ierr = SetFromOptions_CMBModel(&cmb,user.secpera);
  user.cmb = &cmb;

  // DMDA for the cell-centered grid
  ierr = DMDACreate2d(PETSC_COMM_WORLD,
                      DM_BOUNDARY_NONE,DM_BOUNDARY_NONE,
                      DMDA_STENCIL_BOX,
                      5,5, PETSC_DECIDE,PETSC_DECIDE,
                      1, 1,        // dof=1, stencilwidth=1
                      NULL,NULL,&da);
  ierr = DMSetFromOptions(da); CHKERRQ(ierr);
  ierr = DMSetUp(da); CHKERRQ(ierr);  // this must be called BEFORE SetUniformCoordinates
  ierr = DMDASetUniformCoordinates(da,0.0,user.L,0.0,user.L,-1.0,-1.0);CHKERRQ(ierr);
  ierr = DMSetApplicationContext(da, &user);CHKERRQ(ierr);

  // create and configure the SNES to solve a NCP/VI at each step
  ierr = SNESCreate(PETSC_COMM_WORLD,&snes);CHKERRQ(ierr);
  ierr = SNESSetDM(snes,da);CHKERRQ(ierr);
  ierr = SNESSetApplicationContext(snes,&user);CHKERRQ(ierr);
  ierr = DMDASNESSetFunctionLocal(da,INSERT_VALUES,
               (DMDASNESFunction)FormFunctionLocal,&user); CHKERRQ(ierr);
  ierr = SNESSetType(snes,SNESVINEWTONSSLS); CHKERRQ(ierr);
  ierr = SNESVISetComputeVariableBounds(snes,&FormBounds); CHKERRQ(ierr);
  ierr = SNESSetFromOptions(snes);CHKERRQ(ierr);

  // set up initial iterate
  ierr = DMCreateGlobalVector(da,&H);CHKERRQ(ierr);
  ierr = PetscObjectSetName((PetscObject)H,"H"); CHKERRQ(ierr);
  if (exact_init) {
      ierr = DMDAGetLocalInfo(da,&info); CHKERRQ(ierr);
      ierr = DMDAVecGetArray(da,H,&aH); CHKERRQ(ierr);
      ierr = DomeThicknessLocal(&info,aH,&user); CHKERRQ(ierr);
      ierr = DMDAVecRestoreArray(da,H,&aH); CHKERRQ(ierr);
  } else {
      ierr = VecSet(H,0.0); CHKERRQ(ierr);
  }

  // solve
  ierr = SNESSolve(snes,NULL,H); CHKERRQ(ierr);
  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);
  }

  // get solution & DM on fine grid (which may have changed) after solve
  ierr = VecDestroy(&H); CHKERRQ(ierr);
  ierr = DMDestroy(&da); CHKERRQ(ierr);
  ierr = SNESGetDM(snes,&da); CHKERRQ(ierr); /* do not destroy da */
  ierr = DMDAGetLocalInfo(da,&info); CHKERRQ(ierr);
  ierr = SNESGetSolution(snes,&H); CHKERRQ(ierr); /* do not destroy H */
  ierr = PetscObjectSetName((PetscObject)H,"H"); CHKERRQ(ierr);

  // compute performance measures; note utility of reporting last grid,
  //   last snesit/kspit when doing -snes_grid_sequence
  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 ... SNES iters = %d, last KSP iters = %d\n",
      info.mx,info.my,snesit,kspit); CHKERRQ(ierr);

  // dump state (H,b) if requested
  if (dump) {
      char           filename[1024];
      PetscViewer    viewer;
      Vec            b;
      double         **ab;
      ierr = VecDuplicate(H,&b); CHKERRQ(ierr);
      ierr = PetscObjectSetName((PetscObject)b,"b"); CHKERRQ(ierr);
      if (user.verif) {
          ierr = VecSet(b,0.0); CHKERRQ(ierr);
      } else {
          ierr = DMDAVecGetArray(da,b,&ab); CHKERRQ(ierr);
          ierr = FormBedLocal(&info,0,ab,&user); CHKERRQ(ierr);
          ierr = DMDAVecRestoreArray(da,b,&ab); CHKERRQ(ierr);
      }
      ierr = sprintf(filename,"ice_%dx%d.dat",info.mx,info.my);
      ierr = PetscPrintf(PETSC_COMM_WORLD,"writing PETSC binary file %s ...\n",filename); CHKERRQ(ierr);
      ierr = PetscViewerBinaryOpen(PETSC_COMM_WORLD,filename,FILE_MODE_WRITE,&viewer); CHKERRQ(ierr);
      ierr = VecView(b,viewer); CHKERRQ(ierr);
      ierr = VecView(H,viewer); CHKERRQ(ierr);
      ierr = PetscViewerDestroy(&viewer); CHKERRQ(ierr);
      VecDestroy(&b);
  }

  // compute error in verification case
  if (user.verif) {
      Vec Hexact;
      double infnorm, onenorm;
      ierr = VecDuplicate(H,&Hexact); CHKERRQ(ierr);
      ierr = DMDAVecGetArray(da,Hexact,&aH); CHKERRQ(ierr);
      ierr = DomeThicknessLocal(&info,aH,&user); CHKERRQ(ierr);
      ierr = DMDAVecRestoreArray(da,Hexact,&aH); CHKERRQ(ierr);
      ierr = VecAXPY(H,-1.0,Hexact); CHKERRQ(ierr);    // H <- H + (-1.0) Hexact
      VecDestroy(&Hexact);
      ierr = VecNorm(H,NORM_INFINITY,&infnorm); CHKERRQ(ierr);
      ierr = VecNorm(H,NORM_1,&onenorm); CHKERRQ(ierr);
      ierr = PetscPrintf(PETSC_COMM_WORLD,
          "numerical errors: |H-Hexact|_inf = %.3f, |H-Hexact|_average = %.3f\n",
          infnorm,onenorm/(double)(info.mx*info.my)); CHKERRQ(ierr);
  }

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

PetscErrorCode FormBedLocal(DMDALocalInfo *info, int stencilwidth, double **ab, AppCtx *user) {
  int          j,k,r,s;
  const double dx = user->L / (double)(info->mx-1),
               dy = user->L / (double)(info->my-1),
               Z = PETSC_PI / user->L;
  double       x, y, b;
  // vaguely-random frequencies and coeffs generated by fiddling; see randbed.py
  const int    nc = 4,
               jc[4] = {1, 3, 6, 8},
               kc[4] = {1, 3, 4, 7};
  const double scalec = 750.0,
               C[4][4] = { { 2.00000000,  0.33000000, -0.55020034,  0.54495520},
                           { 0.50000000,  0.45014486,  0.60551833, -0.52250644},
                           { 0.93812068,  0.32638429, -0.24654812,  0.33887052},
                           { 0.17592361, -0.35496741,  0.22694547, -0.05280704} };
  PetscFunctionBeginUser;
  // go through owned portion of grid and compute  b(x,y)
  for (k = info->ys-stencilwidth; k < info->ys + info->ym+stencilwidth; k++) {
      y = k * dy;
      for (j = info->xs-stencilwidth; j < info->xs + info->xm+stencilwidth; j++) {
          if (j < 0 || j >= info->mx-1 || k < 0 || k >= info->my-1)
              continue;
          x = j * dx;
          // b(x,y) is sum of a few sines
          b = 0.0;
          for (r = 0; r < nc; r++) {
              for (s = 0; s < nc; s++) {
                  b += C[r][s] * sin(jc[r] * Z * x) * sin(kc[s] * Z * y);
              }
          }
          ab[k][j] = scalec * b;
      }
  }
  PetscFunctionReturn(0);
}

//  for call-back: tell SNESVI (variational inequality) that we want
//    0.0 <= H < +infinity
PetscErrorCode FormBounds(SNES snes, Vec Xl, Vec Xu) {
  PetscErrorCode ierr;
  PetscFunctionBeginUser;
  ierr = VecSet(Xl,0.0); CHKERRQ(ierr);
  ierr = VecSet(Xu,PETSC_INFINITY); CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

// value of gradient at a point
typedef struct {
    double x,y;
} Grad;

/* We factor the SIA flux as
    q = - H^{n+2} sigma(|grad s|) grad s
where sigma is the slope-dependent part
    sigma(z) = Gamma z^{n-1}.
Also
    D = H^{n+2} sigma(|grad s|)
so that q = - D grad s.  */
static double sigma(Grad gH, Grad gb, const AppCtx *user) {
    const double sx = gH.x + gb.x,
                 sy = gH.y + gb.y,
                 slopesqr = sx * sx + sy * sy + user->delta * user->delta;
    return user->Gamma * PetscPowReal(slopesqr,(user->n_ice-1.0)/2);
}

/* Pseudo-velocity from bed slope:  W = - sigma * grad b. */
static Grad W(double sigma, Grad gb) {
    Grad W;
    W.x = - sigma * gb.x;
    W.y = - sigma * gb.y;
    return W;
}

/* DCS = diffusivity from the continuation scheme:
     D(eps) = (1-eps) sigma H^{n+2} + eps D_0
so D(1)=D_0 and D(0)=sigma H^{n+2}. */
static double DCS(double sigma, double H, const AppCtx *user) {
  return (1.0 - user->eps) * sigma * PetscPowReal(PetscAbsReal(H),user->n_ice+2.0)
         + user->eps * user->D0;
}

/* Flux component from the non-sliding SIA on a general bed. */
PetscErrorCode SIAflux(Grad gH, Grad gb, double H, double Hup, PetscBool xdir,
                       double *D, double *q, const AppCtx *user) {
  const double mysig = sigma(gH,gb,user),
               myD   = DCS(mysig,H,user);
  const Grad   myW   = W(mysig,gb);
  PetscFunctionBeginUser;
  if (D) {
      *D = myD;
  }
  if (xdir && q) {
      *q = - myD * gH.x + myW.x * PetscPowReal(PetscAbsReal(Hup),user->n_ice+2.0);
  } else {
      *q = - myD * gH.y + myW.y * PetscPowReal(PetscAbsReal(Hup),user->n_ice+2.0);
  }
  PetscFunctionReturn(0);
}

// gradients of weights for Q^1 interpolant
static const double gx[4] = {-1.0,  1.0, 1.0, -1.0},
                    gy[4] = {-1.0, -1.0, 1.0,  1.0};

static double fieldatpt(double xi, double eta, double f[4]) {
  // weights for Q^1 interpolant
  double x[4] = { 1.0-xi,      xi,  xi, 1.0-xi},
         y[4] = {1.0-eta, 1.0-eta, eta,    eta};
  return   x[0] * y[0] * f[0] + x[1] * y[1] * f[1]
         + x[2] * y[2] * f[2] + x[3] * y[3] * f[3];
}

static double fieldatptArray(int u, int v, double xi, double eta, double **f) {
  double ff[4] = {f[v][u], f[v][u+1], f[v+1][u+1], f[v+1][u]};
  return fieldatpt(xi,eta,ff);
}

static Grad gradfatpt(double xi, double eta, double dx, double dy, double f[4]) {
  Grad gradf;
  double x[4] = { 1.0-xi,      xi,  xi, 1.0-xi},
         y[4] = {1.0-eta, 1.0-eta, eta,    eta};
  gradf.x =   gx[0] * y[0] * f[0] + gx[1] * y[1] * f[1]
            + gx[2] * y[2] * f[2] + gx[3] * y[3] * f[3];
  gradf.y =    x[0] *gy[0] * f[0] +  x[1] *gy[1] * f[1]
            +  x[2] *gy[2] * f[2] +  x[3] *gy[3] * f[3];
  gradf.x /= dx;
  gradf.y /= dy;
  return gradf;
}

static Grad gradfatptArray(int u, int v, double xi, double eta, double dx, double dy,
                           double **f) {
  double ff[4] = {f[v][u], f[v][u+1], f[v+1][u+1], f[v+1][u]};
  return gradfatpt(xi,eta,dx,dy,ff);
}

// indexing of the 8 quadrature points along the boundary of the control volume in M*
// point s=0,...,7 is in element (j,k) = (j+je[s],k+ke[s])
static const int  je[8] = {0,  0, -1, -1, -1, -1,  0,  0},
                  ke[8] = {0,  0,  0,  0, -1, -1, -1, -1},
                  ce[8] = {0,  3,  1,  0,  2,  1,  3,  2};

// direction of flux at 4 points in each element
static const PetscBool xdire[4] = {PETSC_TRUE, PETSC_FALSE, PETSC_TRUE, PETSC_FALSE};

// local (element-wise) coords of quadrature points for M*
static const double locx[4] = {  0.5, 0.75,  0.5, 0.25},
                    locy[4] = { 0.25,  0.5, 0.75,  0.5};

/* FormFunctionLocal  =  call-back by SNES using DMDA info.

Evaluates residual FF on local process patch:
   FF_{j,k} = \int_{\partial V_{j,k}} \mathbf{q} \cdot \mathbf{n}
              - m_{j,k} \Delta x \Delta y
where V_{j,k} is the control volume centered at (x_j,y_k).

Regarding indexing locations along the boundary of the control volume where
flux is evaluated, this figure shows the control volume centered at (x_j,y_k)
and the four elements it meets.  Quadrature uses 8 points on the boundary of
the control volume, numbered s=0,...,7:

     -------------------
    |         |         |
    |    ..2..|..1..    |
    |   3:    |    :0   |
  k |--------- ---------|
    |   4:    |    :7   |
    |    ..5..|..6..    |
    |         |         |
     -------------------
              j

Regarding flux-component indexing on the element indexed by (j,k) node,
the value  (aqquad[c])[k][j] for c=0,1,2,3 is an x-component at "*" and
a y-component at "%"; note (x_j,y_k) is lower-left corner:

     -------------------
    |         :         |
    |         *2        |
    |    3    :    1    |
    |....%.... ....%....|
    |         :         |
    |         *0        |
    |         :         |
    @-------------------
  (j,k)

*/
PetscErrorCode FormFunctionLocal(DMDALocalInfo *info, double **aHin,
                                        double **FF, AppCtx *user) {
  PetscErrorCode  ierr;
  const double    dx = user->L / (double)(info->mx-1),
                  dy = user->L / (double)(info->my-1);
  // coefficients of quadrature evaluations along the boundary of the control volume in M*
  const double    coeff[8] = {dy/2, dx/2, dx/2, -dy/2, -dy/2, -dx/2, -dx/2, dy/2};
  const PetscBool upwind = (user->lambda > 0.0);
  const double    upmin = (1.0 - user->lambda) * 0.5,
                  upmax = (1.0 + user->lambda) * 0.5;
  int             c, j, k, s;
  double          H, Hup, lxup, lyup, **aqquad[4], **ab, **aH, DSIA_ckj, qSIA_ckj,
                  M, x, y;
  Grad            gH, gb;
  Vec             qquad[4], Hcopy, b;

  PetscFunctionBeginUser;

  // copy and set boundary conditions to zero
  ierr = DMGetLocalVector(info->da, &Hcopy); CHKERRQ(ierr);
  ierr = DMDAVecGetArray(info->da,Hcopy,&aH); CHKERRQ(ierr);
  for (k = info->ys-1; k <= info->ys + info->ym; k++) {
      for (j = info->xs-1; j <= info->xs + info->xm; j++) {
          if (j < 0 || j > info->mx-1 || k < 0 || k > info->my-1)
              continue;
          if (user->check_admissible && aHin[k][j] < 0.0) {
              SETERRQ3(PETSC_COMM_WORLD,1,
                       "ERROR: non-admissible value H[k][j] = %.3e < 0.0 at j,k = %d,%d\n",
                       aHin[k][j],j,k);
          }
          if (j == 0 || j == info->mx-1 || k == 0 || k == info->my-1) {
              if (j >= info->xs && j < info->xs+info->xm && k >= info->ys && k < info->ys+info->ym)
                  FF[k][j] = aHin[k][j];   // FIXME scaling?
              aH[k][j] = 0.0;
          } else
              aH[k][j] = aHin[k][j];
      }
  }

  // get bed elevation b(x,y) on this grid
  ierr = DMGetLocalVector(info->da, &b); CHKERRQ(ierr);
  ierr = DMDAVecGetArray(info->da,b,&ab); CHKERRQ(ierr);
  if (user->verif) {
      ierr = VecSet(b,0.0); CHKERRQ(ierr);
  } else {
      ierr = FormBedLocal(info,1,ab,user); CHKERRQ(ierr);  // get stencil width
  }

  // working space for fluxes; see text for face location of flux evaluation
  for (c = 0; c < 4; c++) {
      ierr = DMGetLocalVector(info->da, &(qquad[c])); CHKERRQ(ierr);
      ierr = DMDAVecGetArray(info->da,qquad[c],&(aqquad[c])); CHKERRQ(ierr);
  }

  // loop over locally-owned elements, including ghosts, to get fluxes q at
  // c = 0,1,2,3 points in element;  note start at (xs-1,ys-1)
  for (k = info->ys-1; k < info->ys + info->ym; k++) {
      for (j = info->xs-1; j < info->xs + info->xm; j++) {
          if (j < 0 || j >= info->mx-1 || k < 0 || k >= info->my-1)
              continue;
          for (c=0; c<4; c++) {
              H  = fieldatptArray(j,k,locx[c],locy[c],aH);
              gH = gradfatptArray(j,k,locx[c],locy[c],dx,dy,aH);
              gb = gradfatptArray(j,k,locx[c],locy[c],dx,dy,ab);
              if (upwind) {
                  if (xdire[c] == PETSC_TRUE) {
                      lxup = (gb.x <= 0.0) ? upmin : upmax;
                      lyup = locy[c];
                  } else {
                      lxup = locx[c];
                      lyup = (gb.y <= 0.0) ? upmin : upmax;
                  }
                  Hup = fieldatptArray(j,k,lxup,lyup,aH);
              } else
                  Hup = H;
              ierr = SIAflux(gH,gb,H,Hup,xdire[c],
                             &DSIA_ckj,&qSIA_ckj,user); CHKERRQ(ierr);
              aqquad[c][k][j] = qSIA_ckj;
          }
      }
  }

  // loop over nodes, not including ghosts, to get function F(H) from quadature over
  // s = 0,1,...,7 points on boundary of control volume (rectangle) around node
  for (k=info->ys; k<info->ys+info->ym; k++) {
      for (j=info->xs; j<info->xs+info->xm; j++) {
          if (j == 0 || j == info->mx-1 || k == 0 || k == info->my-1)
              continue;
          // climatic mass balance
          if (user->verif) {
              x = j * dx;
              y = k * dy;
              M = DomeCMB(x,y,user);
          } else {
              M = M_CMBModel(user->cmb,ab[k][j] + aH[k][j]);  // s=b+H is surface elevation
          }
          FF[k][j] = - M * dx * dy;
          // now add integral over control volume boundary using two
          // quadrature points on each side
          for (s=0; s<8; s++)
              FF[k][j] += coeff[s] * aqquad[ce[s]][k+ke[s]][j+je[s]];
      }
  }

  // restore working space and bed
  for (c = 0; c < 4; c++) {
      ierr = DMDAVecRestoreArray(info->da,qquad[c],&(aqquad[c])); CHKERRQ(ierr);
      ierr = DMRestoreLocalVector(info->da, &(qquad[c])); CHKERRQ(ierr);
  }
  ierr = DMDAVecRestoreArray(info->da,Hcopy,&aH); CHKERRQ(ierr);
  ierr = DMRestoreLocalVector(info->da, &Hcopy); CHKERRQ(ierr);
  ierr = DMDAVecRestoreArray(info->da,b,&ab); CHKERRQ(ierr);
  ierr = DMRestoreLocalVector(info->da, &b); CHKERRQ(ierr);
  PetscFunctionReturn(0);
}


#ifndef KCLUSTER_H
#define KCLUSTER_H

#include <iostream>
#include <vector>

#include <gsl/gsl_combination.h>

#include "kSphere.h"
#include "kMath.h"

class kCluster {

private:
  double r = 1.0;   // the radii of the spheres
  double r0 = 10.0; // the radii of the bourndary. Use a setter (set_boundary) to adjust.

public:
  std::vector<kSphere> Sphere;

  void setBoundary(double _r0) { r0 = _r0; }
  double getBoundary() { return r0; }

  double mesum() {
    double tmp;
    double result = 0.0;
    // sum of the sqaured distances between all spheres

    for (std::size_t j = 0; j < Sphere.size(); j++) {
      for (std::size_t i = 0; i < j; i++) {

        tmp = kMath::v3abs(kMath::v3diff(Sphere[i].pos, Sphere[j].pos));

        result += (tmp * tmp);
      }
    }
    return result;
  }

  // inverts the position of the spheres in the cluster which have there indices in the list of
  // indices.
  int invert(std::vector<size_t> Indice) {
    for (size_t i = 0; i < Indice.size(); i++) {
      Sphere[Indice[i]].pos.x = -Sphere[Indice[i]].pos.x;
      Sphere[Indice[i]].pos.y = -Sphere[Indice[i]].pos.y;
      Sphere[Indice[i]].pos.z = -Sphere[Indice[i]].pos.z;
    }
    return 0;
  }

  // returns a list of k indices (in a vector) which indicate the k spheres from the main
  // cluster
  // which represent the smallest potential energy among all posible groups of k spheres.
  std::vector<size_t> minU(size_t k, double r, double r0) {
    double pe0;
    double pe1 = std::numeric_limits<double>::max();
    std::vector<kSphere> S0;

    std::vector<size_t> tmp;
    std::vector<size_t> result;
    size_t n = Sphere.size();
    tmp.resize(k);
    result.resize(k);
    S0.resize(k);
    gsl_combination *c0 = gsl_combination_calloc(n, k);

    do {
      for (size_t i = 0; i < k; i++) {
        S0[i] = Sphere[c0->data[i]];
        tmp[i] = c0->data[i];
      }
      pe0 = pe(tmp, r, r0);
      // keep hold of the smallest potential on each iteration of the combinations.
      if (pe0 < pe1) {
        pe1 = pe0;
        result = tmp;
      }
    } while (gsl_combination_next(c0) == GSL_SUCCESS);

    gsl_combination_free(c0);
    return result;
  }

  // returns a list of k indices (in a vector) which indicate the k spheres from the main cluster
  // which represent the largest potential energy among all posible groups of k spheres.
  std::vector<size_t> maxU(size_t k, double r, double r0) {
    double pe0;
    double pe1 = std::numeric_limits<double>::max();
    std::vector<kSphere> S0;
    std::vector<size_t> tmp;
    std::vector<size_t> result;
    size_t n = Sphere.size();
    tmp.resize(k);
    result.resize(k);
    S0.resize(k);
    gsl_combination *c0 = gsl_combination_calloc(n, k);

    do {
      std::cout << "-> ";
      for (size_t i = 0; i < k; i++) {
        S0[i] = Sphere[c0->data[i]];
        tmp[i] = c0->data[i];
      }
      pe0 = pe(tmp, r, r0);
      // keep hold of the smallest potential on each iteration of the combinations.
      if (pe0 > pe1) {
        pe1 = pe0;
        result = tmp;
      }
    } while (gsl_combination_next(c0) == GSL_SUCCESS);

    gsl_combination_free(c0);
    return result;
  }

  // TODO: write comment!
  double d0_mag(kSphere a, double r, double r0) {
    double tmp = kMath::v3abs(a.pos) + r;
    if (tmp > r0) {
      return tmp - r0;
    } else {
      return 0.0;
    }
  }

  // The d0 vector is a vector pointing from its position to the origin so just the negative of the
  // postion with the magnitude found in d0_mag to use the d0_mag we will need to first unitize the
  // d0 "deformation" vector.
  v3 d0_vec(kSphere a, double r, double r0) {
    return kMath::v3scale(kMath::v3unit(a.pos), d0_mag(a, r, r0));
  }

  // TODO: write comment!
  static v3 d2_vec(kSphere a, kSphere b, double r) {
    v3 v;
    v = kMath::v3scale(kMath::v3unit(kMath::v3diff(a.pos, b.pos)), d2_mag(a, b, r));
    return v;
  }

  // TODO: write comment!
  static double d2_mag(kSphere a, kSphere b, double r) {
    double tmp = kMath::v3abs(kMath::v3diff(a.pos, b.pos)) / 2.0; // tmp will equal 0 if a == b.
    if (tmp < r && tmp != 0.0) { // second condition to ensure that for a = b, 0
                                 // is returned.
      return r - tmp;
    } else {
      return 0.0;
    }
  }

  // TODO: write comment!
  double pe(double r, double r0) {
    double sum0 = 0.0;
    double sum2 = 0.0;

    for (std::size_t i = 0; i < Sphere.size(); i++) {
      sum0 += d0_mag(Sphere[i], r, r0);
    }
    for (std::size_t j = 0; j < Sphere.size(); j++) {
      for (std::size_t i = 0; i < Sphere.size(); i++) {
        if (i != j) {
          sum2 += d0_mag(Sphere[i], r, r0);
        }
      }
    }
    return sum0 + sum2;
  }

  // Potential Energy function which takes indices for the Sphere list to evaluate over. In stead of
  // the entire list.
  double pe(std::vector<size_t> Index, double r, double r0) {
    double sum0 = 0.0;
    double sum2 = 0.0;

    for (std::size_t i = 0; i < Index.size(); i++) {
      sum0 += d0_mag(Sphere[Index[i]], r, r0);
    }

    for (std::size_t j = 0; j < Index.size(); j++) {
      for (std::size_t i = 0; i < Index.size(); i++) {
        if (i != j) {
          sum2 += d2_mag(Sphere[Index[i]], Sphere[Index[j]], r);
        }
      }
    }
    return sum0 + sum2;
  }

  // The gradient of the potential for a single sphere (against all others and the boundary).
  v3 gradpe(size_t i, double r, double r0) {
    v3 u;
    v3 v;

    u = d0_vec(Sphere[i], r, r0);

    for (std::size_t j = 0; j < Sphere.size(); j++) {
      // TODO: test to check if we need the following condition which should
      // probalby be covered by d2_vec.
      if (i != j) { // I dont think we need to test here if d2_vec is working
                    // correctly ... test
        v = d2_vec(Sphere[i], Sphere[j], r);
      } else {
        v.x = 0.0;
        v.y = 0.0;
        v.z = 0.0;
      }
      u.x += v.x;
      u.y += v.y;
      u.z += v.z;
    }
    return u;
  }

  // move the position to the next frame.
  void update(double r, double r0, double h) {
    v3 tmp;
    for (size_t i = 0; i < Sphere.size(); i++) {
      tmp = gradpe(i, r, r0);
      Sphere[i].pos.x -= h * tmp.x;
      Sphere[i].pos.y -= h * tmp.y;
      Sphere[i].pos.z -= h * tmp.z;
    }
    // std::cout << "size: " << tmp.x << std::endl;
  }

  // generic print function for the cluster.
  // TODO: override the << operator instead.
  void print() {
    for (size_t i = 0; i < Sphere.size(); i++) {
      std::cout << Sphere[i].pos.x << " " << Sphere[i].pos.y << " " << Sphere[i].pos.z << std::endl;
    }
    std::cout << std::endl;
  }

  // TODO: write comment!
  kCluster(size_t size, double _r) {
    r = _r; // I moved the radius from kSphere to here (kCluster) since we are using equal size
            // spheres. r0 is given an large default value but will need to be adjusted for
            // each configuration size and sphere radius.
    for (size_t i = 0; i < size; i++) {
      Sphere.push_back(kSphere());
    }
    //  minU(4, 0.5, 1.2);
  }

  // Copy Constructor.
  kCluster(const kCluster &obj) {
    // All we need to do (for the moment at least) is copy the Sphere vector.
    // Im hoping that this copies the values and not the addresses. Check!
    Sphere = obj.Sphere;
  }

  // TODO: write comment!
  void operator=(const kCluster &obj) { Sphere = obj.Sphere; }

  // TODO: write comment!
  ~kCluster() {}
};

#endif


/* specfunc/gsl_sf_hyperg.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_HYPERG_H__
#define __GSL_SF_HYPERG_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

/* Hypergeometric function related to Bessel functions
 * 0F1[c,x] =
 *            Gamma[c]    x^(1/2(1-c)) I_{c-1}(2 Sqrt[x])
 *            Gamma[c] (-x)^(1/2(1-c)) J_{c-1}(2 Sqrt[-x])
 *
 * exceptions: GSL_EOVRFLW, GSL_EUNDRFLW
 */
int gsl_sf_hyperg_0F1_e(double c, double x, gsl_sf_result * result);
double gsl_sf_hyperg_0F1(const double c, const double x);

/* Confluent hypergeometric function  for integer parameters.
 * 1F1[m,n,x] = M(m,n,x)
 *
 * exceptions:
 */
int gsl_sf_hyperg_1F1_int_e(const int m, const int n, const double x, gsl_sf_result * result);
double gsl_sf_hyperg_1F1_int(const int m, const int n, double x);

/* Confluent hypergeometric function.
 * 1F1[a,b,x] = M(a,b,x)
 *
 * exceptions:
 */
int gsl_sf_hyperg_1F1_e(const double a, const double b, const double x, gsl_sf_result * result);
double gsl_sf_hyperg_1F1(double a, double b, double x);

/* Confluent hypergeometric function for integer parameters.
 * U(m,n,x)
 *
 * exceptions:
 */
int gsl_sf_hyperg_U_int_e(const int m, const int n, const double x, gsl_sf_result * result);
double gsl_sf_hyperg_U_int(const int m, const int n, const double x);

/* Confluent hypergeometric function for integer parameters.
 * U(m,n,x)
 *
 * exceptions:
 */
int gsl_sf_hyperg_U_int_e10_e(const int m, const int n, const double x, gsl_sf_result_e10 * result);

/* Confluent hypergeometric function.
 * U(a,b,x)
 *
 * exceptions:
 */
int gsl_sf_hyperg_U_e(const double a, const double b, const double x, gsl_sf_result * result);
double gsl_sf_hyperg_U(const double a, const double b, const double x);

/* Confluent hypergeometric function.
 * U(a,b,x)
 *
 * exceptions:
 */
int gsl_sf_hyperg_U_e10_e(const double a, const double b, const double x, gsl_sf_result_e10 * result);

/* Gauss hypergeometric function 2F1[a,b,c,x]
 * |x| < 1
 *
 * exceptions:
 */
int gsl_sf_hyperg_2F1_e(double a, double b, const double c, const double x, gsl_sf_result * result);
double gsl_sf_hyperg_2F1(double a, double b, double c, double x);

/* Gauss hypergeometric function
 * 2F1[aR + I aI, aR - I aI, c, x]
 * |x| < 1
 *
 * exceptions:
 */
int gsl_sf_hyperg_2F1_conj_e(const double aR, const double aI, const double c, const double x, gsl_sf_result * result);
double gsl_sf_hyperg_2F1_conj(double aR, double aI, double c, double x);

/* Renormalized Gauss hypergeometric function
 * 2F1[a,b,c,x] / Gamma[c]
 * |x| < 1
 *
 * exceptions:
 */
int gsl_sf_hyperg_2F1_renorm_e(const double a, const double b, const double c, const double x, gsl_sf_result * result);
double gsl_sf_hyperg_2F1_renorm(double a, double b, double c, double x);

/* Renormalized Gauss hypergeometric function
 * 2F1[aR + I aI, aR - I aI, c, x] / Gamma[c]
 * |x| < 1
 *
 * exceptions:
 */
int gsl_sf_hyperg_2F1_conj_renorm_e(const double aR, const double aI, const double c, const double x, gsl_sf_result * result);
double gsl_sf_hyperg_2F1_conj_renorm(double aR, double aI, double c, double x);

/* Mysterious hypergeometric function. The series representation
 * is a divergent hypergeometric series. However, for x < 0 we
 * have 2F0(a,b,x) = (-1/x)^a U(a,1+a-b,-1/x)
 *
 * exceptions: GSL_EDOM
 */
int     gsl_sf_hyperg_2F0_e(const double a, const double b, const double x, gsl_sf_result * result);
double     gsl_sf_hyperg_2F0(const double a, const double b, const double x);

__END_DECLS

#endif /* __GSL_SF_HYPERG_H__ */


	#ifndef LVV_MULTIMIN_H
	#define LVV_MULTIMIN_H

#include <stdlib.h>
#include <iostream>
using std::cerr;
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_multimin.h>
#include <lvv/lvv.h>
#include <lvv/math.h>

namespace lvv {

class f_minimizer {  public:

	f_minimizer (const int N, double F(const gsl_vector*, void *), double *xa,  double *ssa, void *var, bool _trace=false)
	:	T (gsl_multimin_fminimizer_nmsimplex), trace(_trace), found(false), fmin(999999)
	{
		minex_func.n = N;
		minex_func.f = F;
		minex_func.params = var;

		mk_gsl_vector x (xa,N);// this is auto on stack(temp), will be copied in minimizer, //  so we are fine when it will go out of scope
		mk_gsl_vector ss(ssa, N);
		//gv_x  = gsl_vector_alloc(N);	for (int i=0; i<N; i++)  gsl_vector_set(gv_x,  i, xa [i]);
		//gv_ss = gsl_vector_alloc(N);	for (int i=0; i<N; i++)  gsl_vector_set(gv_ss, i, ssa[i]);

		minimizer = gsl_multimin_fminimizer_alloc(T, N);
		  gsl_multimin_fminimizer_set(minimizer, &minex_func, &x  , &ss);
		//gsl_multimin_fminimizer_set(minimizer, &minex_func, gv_x, gv_ss);
		xmin = minimizer->x->data;
		if (trace)   cout << "#  minimizer: "  <<  gsl_multimin_fminimizer_name (minimizer)  <<  endl;
	};

	~f_minimizer () { gsl_multimin_fminimizer_free(minimizer);  /* gsl_vector_free(gv_x);   gsl_vector_free(gv_ss) ;*/ };

	void   find_min (double characteristic_size, int max_iter) {
		int	test_status=GSL_CONTINUE;			// test_status:  GSL_SUCCESS==0; GSL_CONTINUE==-2;

		if (trace)  FMT("# Itr  %10t Y   %20t Step  %30t X[0..]\n");

		for  ( iter=0;  iter<max_iter && (test_status==GSL_CONTINUE);   ++iter )   {

			int  iterate_status = gsl_multimin_fminimizer_iterate(minimizer);

			if (iterate_status) {
				cerr << "error: FMinimizer: in gsl_multimin_fminimizer_iterate()\n";
				break;
			}

			double size = gsl_multimin_fminimizer_size(minimizer);
			test_status = gsl_multimin_test_size(size, characteristic_size);

			if (test_status == GSL_SUCCESS)  {
				found=true;
				fmin=minimizer->fval;
				if (trace )    cout  << "converged to minimum at\n";
			}

			if (trace) {
				FMT("%5d %10.5d %10.5d") %iter   %(minimizer->fval)  %size;
				for (unsigned i=0; i < minimizer->x->size; ++i) FMT("%10.5d") %gsl_vector_get(minimizer->x, i);
				cout << endl;
			}

		}
	}

	const gsl_multimin_fminimizer_type *T;
	gsl_multimin_fminimizer            *minimizer;
	gsl_multimin_function               minex_func;
	bool                                trace;
	bool                                found;
	double                             *xmin;
	double                              fmin;
	gsl_vector                         *gv_x;
	gsl_vector                         *gv_ss;
	int					iter;
 };
 } // namespace

    #endif // LVV_MULTIMIN_H


/* dwt_undec.h
 * Paul Demorest, 2007/10
 */
#ifndef _DWT_UNDEC_H_
#define _DWT_UNDEC_H_
#include <gsl/gsl_wavelet.h>
#ifdef __cplusplus
extern "C" {
#endif
int dwt_undec_transform(double *in, double *out, size_t n, const gsl_wavelet *w);
int dwt_undec_inverse(double *in, double *out, size_t n, const gsl_wavelet *w);
#ifdef __cplusplus
}
#endif
#endif


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

int main()
{

    //set up some data
    int n=300;
    float* x = malloc( n*sizeof(float) );
    float* y = malloc( n*sizeof(float) );
    for( int i=0; i<n; ++i )
    {
        x[i]=i+1;
        y[i]=(i+1)*(i+1);
    }

    //calculate y = a*x + y
    float a=7.0f;
    int incx=1;
    int incy=1;
    cblas_saxpy( n, a, x, incx, y, incy );

    //check it
    int status = 0;
    for( int i=0; i<n; ++i )
    {
        float expected = 7.0f*(i+1) + (i+1)*(i+1);
        if( fabs( y[i]/expected - 1 )>1e-4 ){
            printf("error matching element i=%d expected=%g received=%g\n",i+1,expected,y[i]);
            status=-1;
            break;
        }
    }

    //free data
    free(x);
    free(y);

    return status;

}


static char help[] = "Newton's method for arctan x = 0.  Run with -snes_fd or -snes_mf.\n\n";

#include <petsc.h>

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

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

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

    ierr = PetscOptionsBegin(PETSC_COMM_WORLD,"","options to atan","");CHKERRQ(ierr);
    ierr = PetscOptionsReal("-x0","initial value","atan.c",x0,&x0,NULL); CHKERRQ(ierr);
    ierr = PetscOptionsEnd();CHKERRQ(ierr);

    ierr = VecCreate(PETSC_COMM_WORLD,&x); CHKERRQ(ierr);
    ierr = VecSetSizes(x,PETSC_DECIDE,1); CHKERRQ(ierr);
    ierr = VecSetFromOptions(x); CHKERRQ(ierr);
    ierr = VecSet(x,x0); CHKERRQ(ierr);
    ierr = VecDuplicate(x,&r); CHKERRQ(ierr);

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

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

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] = atan(ax[0]);
    ierr = VecRestoreArrayRead(x,&ax);CHKERRQ(ierr);
    ierr = VecRestoreArray(F,&aF);CHKERRQ(ierr);
    return 0;
}


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

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

#include "error.h"

/* Pr (X <= k), i.e., the probability of n or fewer failures until the
   first success. */

double
gsl_cdf_geometric_P (const unsigned int k, const double p)
{
  double P, a, q;

  if (p > 1.0 || p < 0.0)
    {
      CDF_ERROR ("p < 0 or p > 1", GSL_EDOM);
    }

  if (k < 1)
    {
      return 0.0;
    }

  q = 1.0 - p;
  a = (double) k;

  if (p < 0.5)
    {
      P = -expm1 (a * log1p (-p));
    }
  else
    {
      P = 1.0 - pow (q, a);
    }

  return P;
}

double
gsl_cdf_geometric_Q (const unsigned int k, const double p)
{
  double Q, a, q;

  if (p > 1.0 || p < 0.0)
    {
      CDF_ERROR ("p < 0 or p > 1", GSL_EDOM);
    }

  if (k < 1)
    {
      return 1.0;
    }

  q = 1.0 - p;
  a = (double) k;

  if (p < 0.5)
    {
      Q = exp (a * log1p (-p));
    }
  else
    {
      Q = pow (q, a);
    }

  return Q;
}