/* blas/blas.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2001 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/* GSL implementation of BLAS operations for vectors and dense
 * matrices.  Note that GSL native storage is row-major.  */

#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/blas/gsl_blas_types.h>
#include <gsl/blas/gsl_blas.h>
#include <gsl/blas/cblas.h>

/* ========================================================================
 * Level 1
 * ========================================================================
 */

/* CBLAS defines vector sizes in terms of int. GSL defines sizes in
   terms of size_t, so we need to convert these into integers.  There
   is the possibility of overflow here. FIXME: Maybe this could be
   caught */

#define INT(X) ((int)(X))

int gsl_blas_ddot (const gsl_vector * X, const gsl_vector * Y, double *result){
  if (X->size == Y->size){
	  *result = ddot (&INT (X->size), X->data, &INT (X->stride), Y->data, &INT (Y->stride));
      return GSL_SUCCESS;
    }
  else {
      GSL_ERROR ("invalid length", GSL_EBADLEN);
    }
}

/**
*  DNRM2 returns the euclidean norm of a vector via the function
*  name, so that
*
*     DNRM2 := sqrt( x'*x )
*/

double gsl_blas_dnrm2 (const gsl_vector * X){
  return dnrm2 (&INT (X->size), X->data, &INT (X->stride));
}

/**
*     interchanges two vectors.
*/
int gsl_blas_dswap (gsl_vector * X, gsl_vector * Y){
  if (X->size == Y->size){
      dswap (&INT (X->size), X->data, &INT (X->stride), Y->data, &INT (Y->stride));
      return GSL_SUCCESS;
    }
  else{
      GSL_ERROR ("invalid length", GSL_EBADLEN);
    };
}

/**
*     copies a vector, x, to a vector, y.
*/
int gsl_blas_dcopy (const gsl_vector * X, gsl_vector * Y){
  if (X->size == Y->size){
      dcopy (&INT (X->size), X->data, &INT (X->stride), Y->data, &INT (Y->stride));
      return GSL_SUCCESS;
    }
  else {
      GSL_ERROR ("invalid length", GSL_EBADLEN);
    }
}

/* ===========================================================================
 * Level 2
 * ===========================================================================
 */

int
gsl_blas_dgemv (CBLAS_TRANSPOSE_t TransA, double alpha, const gsl_matrix * A,
                const gsl_vector * X, double beta, gsl_vector * Y)
{
	//here the matrix is by default transposed, so to get the right behaviour we will transpose it when the user didn't want it tranposed, and leave it the way it is when it should be transposed

  //note that M and N are changed to show that the matrix is transposed by default.
  const size_t M = A->size1;
  const size_t N = A->size2;

  char aTransp = (TransA == CblasNoTrans) ? 'T' : 'N';

  if ((TransA == CblasNoTrans && N == X->size && M == Y->size)
      || (TransA == CblasTrans && M == X->size && N == Y->size))
    {
      dgemv (&aTransp, &INT (N), &INT (M), &alpha, A->data,
                   &INT (A->tda), X->data, &INT (X->stride), &beta, Y->data,
                   &INT (Y->stride));
      return GSL_SUCCESS;
    }
  else
    {
      GSL_ERROR ("invalid length", GSL_EBADLEN);
    }
}

/*
 * ===========================================================================
 * Prototypes for level 3 BLAS
 * ===========================================================================
 */

int gsl_blas_dgemm (CBLAS_TRANSPOSE_t TransA, CBLAS_TRANSPOSE_t TransB,
                double alpha, const gsl_matrix * A, const gsl_matrix * B,
                double beta, gsl_matrix * C)
{
/**
	Since BLAS uses column major matrices, when asked to perform C = A*B we will
	actually compute C = B*A, with the number of rows and cols switched up.
	(C' = B' * A' if C = A * B, and we're working with transposes by default)

	That's why we need to switch the order of the matrices as well as the order of M and N.

*/

  const size_t M = C->size1;
  const size_t N = C->size2;
  const size_t MA = (TransA == CblasNoTrans) ? A->size1 : A->size2;
  const size_t NA = (TransA == CblasNoTrans) ? A->size2 : A->size1;
  const size_t MB = (TransB == CblasNoTrans) ? B->size1 : B->size2;
  const size_t NB = (TransB == CblasNoTrans) ? B->size2 : B->size1;

  char aTransp = (TransA == CblasNoTrans) ? 'N' : 'T';
  char bTransp = (TransB == CblasNoTrans) ? 'N' : 'T';

  if (M == MA && N == NB && NA == MB)   /* [MxN] = [MAxNA][MBxNB] */
    {
	  dgemm (&bTransp, &aTransp, &INT (N), &INT (M), &INT (NA),
                   &alpha, B->data, &INT (B->tda), A->data, &INT (A->tda), &beta,
                   C->data, &INT (C->tda));
      return GSL_SUCCESS;
    }
  else
    {
      GSL_ERROR ("invalid length", GSL_EBADLEN);
    }
}


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

/* Factorise a hermitian matrix A into
 *
 * A = U T U'
 *
 * where U is unitary and T is real symmetric tridiagonal.  Only the
 * diagonal and lower triangular part of A is referenced and modified.
 *
 * On exit, T is stored in the diagonal and first subdiagonal of
 * A. Since T is symmetric the upper diagonal is not stored.
 *
 * U is stored as a packed set of Householder transformations in the
 * lower triangular part of the input matrix below the first subdiagonal.
 *
 * The full matrix for U can be obtained as the product
 *
 *       U = U_N ... U_2 U_1
 *
 * where
 *
 *       U_i = (I - tau_i * v_i * v_i')
 *
 * and where v_i is a Householder vector
 *
 *       v_i = [0, ..., 0, 1, A(i+2,i), A(i+3,i), ... , A(N,i)]
 *
 * This storage scheme is the same as in LAPACK.  See LAPACK's
 * chetd2.f for details.
 *
 * See Golub & Van Loan, "Matrix Computations" (3rd ed), Section 8.3 */

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

#include <gsl/gsl_linalg.h>

int
gsl_linalg_hermtd_decomp (gsl_matrix_complex * A, gsl_vector_complex * tau)
{
  if (A->size1 != A->size2)
    {
      GSL_ERROR ("hermitian tridiagonal decomposition requires square matrix",
                 GSL_ENOTSQR);
    }
  else if (tau->size + 1 != A->size1)
    {
      GSL_ERROR ("size of tau must be (matrix size - 1)", GSL_EBADLEN);
    }
  else
    {
      const size_t N = A->size1;
      size_t i;

      const gsl_complex zero = gsl_complex_rect (0.0, 0.0);
      const gsl_complex one = gsl_complex_rect (1.0, 0.0);
      const gsl_complex neg_one = gsl_complex_rect (-1.0, 0.0);

      for (i = 0 ; i < N - 1; i++)
        {
          gsl_vector_complex_view c = gsl_matrix_complex_column (A, i);
          gsl_vector_complex_view v = gsl_vector_complex_subvector (&c.vector, i + 1, N - (i + 1));
          gsl_complex tau_i = gsl_linalg_complex_householder_transform (&v.vector);

          /* Apply the transformation H^T A H to the remaining columns */

          if ((i + 1) < (N - 1)
              && !(GSL_REAL(tau_i) == 0.0 && GSL_IMAG(tau_i) == 0.0))
            {
              gsl_matrix_complex_view m =
                gsl_matrix_complex_submatrix (A, i + 1, i + 1,
                                              N - (i+1), N - (i+1));
              gsl_complex ei = gsl_vector_complex_get(&v.vector, 0);
              gsl_vector_complex_view x = gsl_vector_complex_subvector (tau, i, N-(i+1));
              gsl_vector_complex_set (&v.vector, 0, one);

              /* x = tau * A * v */
              gsl_blas_zhemv (CblasLower, tau_i, &m.matrix, &v.vector, zero, &x.vector);

              /* w = x - (1/2) tau * (x' * v) * v  */
              {
                gsl_complex xv, txv, alpha;
                gsl_blas_zdotc(&x.vector, &v.vector, &xv);
                txv = gsl_complex_mul(tau_i, xv);
                alpha = gsl_complex_mul_real(txv, -0.5);
                gsl_blas_zaxpy(alpha, &v.vector, &x.vector);
              }

              /* apply the transformation A = A - v w' - w v' */
              gsl_blas_zher2(CblasLower, neg_one, &v.vector, &x.vector, &m.matrix);

              gsl_vector_complex_set (&v.vector, 0, ei);
            }

          gsl_vector_complex_set (tau, i, tau_i);
        }

      return GSL_SUCCESS;
    }
}

/*  Form the orthogonal matrix U from the packed QR matrix */

int
gsl_linalg_hermtd_unpack (const gsl_matrix_complex * A,
                          const gsl_vector_complex * tau,
                          gsl_matrix_complex * U,
                          gsl_vector * diag,
                          gsl_vector * sdiag)
{
  if (A->size1 !=  A->size2)
    {
      GSL_ERROR ("matrix A must be sqaure", GSL_ENOTSQR);
    }
  else if (tau->size + 1 != A->size1)
    {
      GSL_ERROR ("size of tau must be (matrix size - 1)", GSL_EBADLEN);
    }
  else if (U->size1 != A->size1 || U->size2 != A->size1)
    {
      GSL_ERROR ("size of U must match size of A", GSL_EBADLEN);
    }
  else if (diag->size != A->size1)
    {
      GSL_ERROR ("size of diagonal must match size of A", GSL_EBADLEN);
    }
  else if (sdiag->size + 1 != A->size1)
    {
      GSL_ERROR ("size of subdiagonal must be (matrix size - 1)", GSL_EBADLEN);
    }
  else
    {
      const size_t N = A->size1;

      size_t i;

      /* Initialize U to the identity */

      gsl_matrix_complex_set_identity (U);

      for (i = N - 1; i-- > 0;)
        {
          gsl_complex ti = gsl_vector_complex_get (tau, i);

          gsl_vector_complex_const_view c = gsl_matrix_complex_const_column (A, i);

          gsl_vector_complex_const_view h =
            gsl_vector_complex_const_subvector (&c.vector, i + 1, N - (i+1));

          gsl_matrix_complex_view m =
            gsl_matrix_complex_submatrix (U, i + 1, i + 1, N-(i+1), N-(i+1));

          gsl_linalg_complex_householder_hm (ti, &h.vector, &m.matrix);
        }

      /* Copy diagonal into diag */

      for (i = 0; i < N; i++)
        {
          gsl_complex Aii = gsl_matrix_complex_get (A, i, i);
          gsl_vector_set (diag, i, GSL_REAL(Aii));
        }

      /* Copy subdiagonal into sdiag */

      for (i = 0; i < N - 1; i++)
        {
          gsl_complex Aji = gsl_matrix_complex_get (A, i+1, i);
          gsl_vector_set (sdiag, i, GSL_REAL(Aji));
        }

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_hermtd_unpack_T (const gsl_matrix_complex * A,
                            gsl_vector * diag,
                            gsl_vector * sdiag)
{
  if (A->size1 !=  A->size2)
    {
      GSL_ERROR ("matrix A must be sqaure", GSL_ENOTSQR);
    }
  else if (diag->size != A->size1)
    {
      GSL_ERROR ("size of diagonal must match size of A", GSL_EBADLEN);
    }
  else if (sdiag->size + 1 != A->size1)
    {
      GSL_ERROR ("size of subdiagonal must be (matrix size - 1)", GSL_EBADLEN);
    }
  else
    {
      const size_t N = A->size1;

      size_t i;

      /* Copy diagonal into diag */

      for (i = 0; i < N; i++)
        {
          gsl_complex Aii = gsl_matrix_complex_get (A, i, i);
          gsl_vector_set (diag, i, GSL_REAL(Aii));
        }

      /* Copy subdiagonal into sd */

      for (i = 0; i < N - 1; i++)
        {
          gsl_complex Aji = gsl_matrix_complex_get (A, i+1, i);
          gsl_vector_set (sdiag, i, GSL_REAL(Aji));
        }

      return GSL_SUCCESS;
    }
}


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

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

#include "../include/run_svd.h"

int main(void) {
  FILE *fp = fopen("./data/my_data.csv", "r");
  if (fp == NULL) {
    perror("Unable to open file!");

    exit(1);
  }
  int m = 4;
  int n = 5;

  char chunk[128];

  const char s[2] = ",";
  int i = 0;
  int j = 0;
  gsl_matrix *mat = gsl_matrix_alloc(m, n);
  while (fgets(chunk, sizeof(chunk), fp) != NULL) {
    char *token;

    token = strtok(chunk, s);
    while (token != NULL) {
       double x = atof(token);
       gsl_matrix_set(mat, i, j, x);

       j = (j + 1) % n;
       if (j == 0) {
         i = (i + 1) % m;
       }

       token = strtok(NULL, s);
    }
  }
  printf("a_matrix\n");
  pretty_print(mat);

  int result = run_svd(mat);

  fclose(fp);

  return result;
}


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

void
print_all_multisets ( size_t n, size_t k );

int
main (void)
{
  gsl_multiset * c;
  size_t i;

  printf("all multisets of {0,1,2,3} by size (lex. order)\n") ;
  for(i = 0; i <= 4; i++)
    {
      c = gsl_multiset_calloc (4, i);
      do
        {
          printf("{");
          gsl_multiset_fprintf (stdout, c, " %u");
          printf(" }\n");
        }
      while (gsl_multiset_next(c) == GSL_SUCCESS);
      gsl_multiset_free(c);
    }
  printf("all multisets of {1,2,3,4} by size (reverse lex. order)\n") ;
  for(i = 0; i <= 4; i++)
    {
      c = gsl_multiset_alloc (4, i) ;
      gsl_multiset_init_last(c) ;
      do
        {
          printf("{");
          gsl_multiset_fprintf (stdout, c, " %u");
          printf(" }\n");
        }
      while (gsl_multiset_prev(c) == GSL_SUCCESS);
      gsl_multiset_free(c);
    }
  printf("\n");

  print_all_multisets(5, 3);
  print_all_multisets(5, 0);
  print_all_multisets(5, 5);
  print_all_multisets(1, 1);
  print_all_multisets(3, 1);

  return 0;
}

void
print_all_multisets (size_t n, size_t k)
{
  gsl_multiset * c = gsl_multiset_calloc (n, k);

  printf("multisets %u choose %u (with replacement)\n", n, k);
  do
    {
      gsl_multiset_fprintf (stdout, c, " %u");
      printf("\n");
    }
  while (gsl_multiset_next(c) == GSL_SUCCESS);
  while (gsl_multiset_next(c) == GSL_SUCCESS);
  do
    {
      gsl_multiset_fprintf (stdout, c, " %u");
      printf("\n");
    }
  while (gsl_multiset_prev(c) == GSL_SUCCESS);
  printf("\n");
  gsl_multiset_free(c);
}


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

/* vector_bfgs2.c -- Fletcher's implementation of the BFGS method,
   from R.Fletcher, "Practical Method's of Optimization", Second
   Edition, ISBN 0471915475.  Algorithms 2.6.2 and 2.6.4. */

/* Thanks to Alan Irwin irwin@beluga.phys.uvic.ca. for suggesting this
   algorithm and providing sample fortran benchmarks */

#include <config.h>
#include <gsl/gsl_multimin.h>
#include <gsl/gsl_blas.h>

#include "linear_minimize.c"
#include "linear_wrapper.c"

typedef struct
{
  int iter;
  double step;
  double g0norm;
  double pnorm;
  double delta_f;
  double fp0;                   /* f'(0) for f(x-alpha*p) */
  gsl_vector *x0;
  gsl_vector *g0;
  gsl_vector *p;
  /* work space */
  gsl_vector *dx0;
  gsl_vector *dg0;
  gsl_vector *x_alpha;
  gsl_vector *g_alpha;
  /* wrapper function */
  wrapper_t wrap;
  /* minimization parameters */
  double rho;
  double sigma;
  double tau1;
  double tau2;
  double tau3;
  int order;
}
vector_bfgs2_state_t;

static int
vector_bfgs2_alloc (void *vstate, size_t n)
{
  vector_bfgs2_state_t *state = (vector_bfgs2_state_t *) vstate;

  state->p = gsl_vector_calloc (n);

  if (state->p == 0)
    {
      GSL_ERROR ("failed to allocate space for p", GSL_ENOMEM);
    }

  state->x0 = gsl_vector_calloc (n);

  if (state->x0 == 0)
    {
      gsl_vector_free (state->p);
      GSL_ERROR ("failed to allocate space for g0", GSL_ENOMEM);
    }

  state->g0 = gsl_vector_calloc (n);

  if (state->g0 == 0)
    {
      gsl_vector_free (state->x0);
      gsl_vector_free (state->p);
      GSL_ERROR ("failed to allocate space for g0", GSL_ENOMEM);
    }

  state->dx0 = gsl_vector_calloc (n);

  if (state->dx0 == 0)
    {
      gsl_vector_free (state->g0);
      gsl_vector_free (state->x0);
      gsl_vector_free (state->p);
      GSL_ERROR ("failed to allocate space for g0", GSL_ENOMEM);
    }

  state->dg0 = gsl_vector_calloc (n);

  if (state->dg0 == 0)
    {
      gsl_vector_free (state->dx0);
      gsl_vector_free (state->g0);
      gsl_vector_free (state->x0);
      gsl_vector_free (state->p);
      GSL_ERROR ("failed to allocate space for g0", GSL_ENOMEM);
    }

  state->x_alpha = gsl_vector_calloc (n);

  if (state->x_alpha == 0)
    {
      gsl_vector_free (state->dg0);
      gsl_vector_free (state->dx0);
      gsl_vector_free (state->g0);
      gsl_vector_free (state->x0);
      gsl_vector_free (state->p);
      GSL_ERROR ("failed to allocate space for g0", GSL_ENOMEM);
    }

  state->g_alpha = gsl_vector_calloc (n);

  if (state->g_alpha == 0)
    {
      gsl_vector_free (state->x_alpha);
      gsl_vector_free (state->dg0);
      gsl_vector_free (state->dx0);
      gsl_vector_free (state->g0);
      gsl_vector_free (state->x0);
      gsl_vector_free (state->p);
      GSL_ERROR ("failed to allocate space for g0", GSL_ENOMEM);
    }

  return GSL_SUCCESS;
}

static int
vector_bfgs2_set (void *vstate, gsl_multimin_function_fdf * fdf,
                  const gsl_vector * x, double *f, gsl_vector * gradient,
                  double step_size, double tol)
{
  vector_bfgs2_state_t *state = (vector_bfgs2_state_t *) vstate;

  state->iter = 0;
  state->step = step_size;
  state->delta_f = 0;

  GSL_MULTIMIN_FN_EVAL_F_DF (fdf, x, f, gradient);

  /* Use the gradient as the initial direction */

  gsl_vector_memcpy (state->x0, x);
  gsl_vector_memcpy (state->g0, gradient);
  state->g0norm = gsl_blas_dnrm2 (state->g0);

  gsl_vector_memcpy (state->p, gradient);
  gsl_blas_dscal (-1 / state->g0norm, state->p);
  state->pnorm = gsl_blas_dnrm2 (state->p);     /* should be 1 */
  state->fp0 = -state->g0norm;

  /* Prepare the wrapper */

  prepare_wrapper (&state->wrap, fdf,
                   state->x0, *f, state->g0,
                   state->p, state->x_alpha, state->g_alpha);

  /* Prepare 1d minimisation parameters */

  state->rho = 0.01;
  state->sigma = tol;
  state->tau1 = 9;
  state->tau2 = 0.05;
  state->tau3 = 0.5;
  state->order = 3;  /* use cubic interpolation where possible */

  return GSL_SUCCESS;
}

static void
vector_bfgs2_free (void *vstate)
{
  vector_bfgs2_state_t *state = (vector_bfgs2_state_t *) vstate;

  gsl_vector_free (state->x_alpha);
  gsl_vector_free (state->g_alpha);
  gsl_vector_free (state->dg0);
  gsl_vector_free (state->dx0);
  gsl_vector_free (state->g0);
  gsl_vector_free (state->x0);
  gsl_vector_free (state->p);
}

static int
vector_bfgs2_restart (void *vstate)
{
  vector_bfgs2_state_t *state = (vector_bfgs2_state_t *) vstate;

  state->iter = 0;
  return GSL_SUCCESS;
}

static int
vector_bfgs2_iterate (void *vstate, gsl_multimin_function_fdf * fdf,
                      gsl_vector * x, double *f,
                      gsl_vector * gradient, gsl_vector * dx)
{
  vector_bfgs2_state_t *state = (vector_bfgs2_state_t *) vstate;
  double alpha = 0.0, alpha1;
  gsl_vector *x0 = state->x0;
  gsl_vector *g0 = state->g0;
  gsl_vector *p = state->p;

  double g0norm = state->g0norm;
  double pnorm = state->pnorm;
  double delta_f = state->delta_f;
  double pg, dir;
  int status;

  double f0 = *f;

  if (pnorm == 0.0 || g0norm == 0.0 || state->fp0 == 0)
    {
      gsl_vector_set_zero (dx);
      return GSL_ENOPROG;
    }

  if (delta_f < 0)
    {
      double del = GSL_MAX_DBL (-delta_f, 10 * GSL_DBL_EPSILON * fabs(f0));
      alpha1 = GSL_MIN_DBL (1.0, 2.0 * del / (-state->fp0));
    }
  else
    {
      alpha1 = fabs(state->step);
    }

  /* line minimisation, with cubic interpolation (order = 3) */

  status = minimize (&state->wrap.fdf_linear, state->rho, state->sigma,
                     state->tau1, state->tau2, state->tau3, state->order,
                     alpha1,  &alpha);

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

  update_position (&(state->wrap), alpha, x, f, gradient);

  state->delta_f = *f - f0;

  /* Choose a new direction for the next step */

  {
    /* This is the BFGS update: */
    /* p' = g1 - A dx - B dg */
    /* A = - (1+ dg.dg/dx.dg) B + dg.g/dx.dg */
    /* B = dx.g/dx.dg */

    gsl_vector *dx0 = state->dx0;
    gsl_vector *dg0 = state->dg0;

    double dxg, dgg, dxdg, dgnorm, A, B;

    /* dx0 = x - x0 */
    gsl_vector_memcpy (dx0, x);
    gsl_blas_daxpy (-1.0, x0, dx0);

    gsl_vector_memcpy (dx, dx0);  /* keep a copy */

    /* dg0 = g - g0 */
    gsl_vector_memcpy (dg0, gradient);
    gsl_blas_daxpy (-1.0, g0, dg0);

    gsl_blas_ddot (dx0, gradient, &dxg);
    gsl_blas_ddot (dg0, gradient, &dgg);
    gsl_blas_ddot (dx0, dg0, &dxdg);

    dgnorm = gsl_blas_dnrm2 (dg0);

    if (dxdg != 0)
      {
        B = dxg / dxdg;
        A = -(1.0 + dgnorm * dgnorm / dxdg) * B + dgg / dxdg;
      }
    else
      {
        B = 0;
        A = 0;
      }

    gsl_vector_memcpy (p, gradient);
    gsl_blas_daxpy (-A, dx0, p);
    gsl_blas_daxpy (-B, dg0, p);
  }

  gsl_vector_memcpy (g0, gradient);
  gsl_vector_memcpy (x0, x);
  state->g0norm = gsl_blas_dnrm2 (g0);
  state->pnorm = gsl_blas_dnrm2 (p);

  /* update direction and fp0 */

  gsl_blas_ddot (p, gradient, &pg);
  dir = (pg >= 0.0) ? -1.0 : +1.0;
  gsl_blas_dscal (dir / state->pnorm, p);
  state->pnorm = gsl_blas_dnrm2 (p);
  gsl_blas_ddot (p, g0, &state->fp0);

  change_direction (&state->wrap);

  return GSL_SUCCESS;
}

static const gsl_multimin_fdfminimizer_type vector_bfgs2_type = {
  "vector_bfgs2",               /* name */
  sizeof (vector_bfgs2_state_t),
  &vector_bfgs2_alloc,
  &vector_bfgs2_set,
  &vector_bfgs2_iterate,
  &vector_bfgs2_restart,
  &vector_bfgs2_free
};

const gsl_multimin_fdfminimizer_type
  * gsl_multimin_fdfminimizer_vector_bfgs2 = &vector_bfgs2_type;


// @Copyright 2007-2017 Kristjan Haule
#include <gsl/gsl_vector.h>
#include <gsl/gsl_multiroots.h>
#include "smesh.h"

struct rparams{
  double x0, L;
  int Nw;
};

int tanmesh_f(const gsl_vector* x, void* params, gsl_vector* f){
  double x0 = ((struct rparams *) params)->x0;
  double  L = ((struct rparams *) params)->L;
  int Nw = ((struct rparams *) params)->Nw;
  const double d = gsl_vector_get (x, 0);
  const double w = gsl_vector_get (x, 1);
  gsl_vector_set (f, 0, L-w/tan(d) );
  gsl_vector_set (f, 1, x0-w*tan(M_PI/(2*Nw)-d/Nw) );
  return GSL_SUCCESS;
}

typedef int (*gsl_FNC)(const gsl_vector * x, void * params, gsl_vector * f);

class FindRoot{
  const gsl_multiroot_fsolver_type *T;
  gsl_multiroot_fsolver *s;
  size_t n;
  void* params;
  gsl_vector *x;
  gsl_multiroot_function f;
public:
  FindRoot(size_t n_, gsl_FNC fnc, double x_init[], void* params_) : n(n_), params(params_)
  {
    x = gsl_vector_alloc (n);
    f.f = fnc;
    f.n = n;
    f.params = params;
    for (int i=0; i<n; i++) gsl_vector_set (x, i, x_init[i]);
    T = gsl_multiroot_fsolver_hybrids;
    s = gsl_multiroot_fsolver_alloc (T, 2);
    gsl_multiroot_fsolver_set (s, &f, x);
  }
  void print_state (size_t iter, gsl_multiroot_fsolver * s){
    clog<<"iter = "<<iter<<" x=";
    for (int i=0; i<n; i++) clog<<gsl_vector_get(s->x,i)<<" ";
    clog<<" f(x)=";
    for (int i=0; i<n; i++) clog<<gsl_vector_get(s->f,i)<<" ";
    clog<<endl;
  };
  vector<double> call(){
    size_t i, iter = 0;
    //print_state (iter, s);
    int status;
    do{
      iter++;
      status = gsl_multiroot_fsolver_iterate (s);
      //print_state (iter, s);
      if (status)   /* check if solver is stuck */
	break;
      status = gsl_multiroot_test_residual (s->f, 1e-7);
    } while (status == GSL_CONTINUE && iter < 1000);
    //clog<<"status = "<<gsl_strerror (status)<<endl;

    vector<double> res(n);
    for (int i=0; i<n; i++) res[i] = gsl_vector_get (s->x, i);
    return res;
  }
  ~FindRoot(){
    gsl_multiroot_fsolver_free (s);
    gsl_vector_free (x);
  }
};

double sqr(double x){return x*x;}

void GiveTanMesh(mesh1D& om, double& x0, double L, int Nw)
{
  double tnw = tan(M_PI/(2*Nw));
  if (x0 > L*0.25*Nw*sqr(tnw) ){
    x0 = L*0.25*Nw*sqr(tnw)-1e-15;
  }
  double d0 = Nw/2.*( tnw - sqrt( sqr(tnw) - 4*x0/(L*Nw)));
  double w0 = L*d0;
  double x_init[2] = {d0, w0};
  struct rparams p = {x0,L,Nw};

  FindRoot fr(2, &tanmesh_f, x_init, &p);
  vector<double> dw = fr.call();
  double d = dw[0];
  double w = dw[1];

  om.resize(2*Nw+1);
  double dh = 1.0/static_cast<double>(2*Nw);
  for (int i=0; i<2*Nw+1; i++){
    double t0 = i/static_cast<double>(2*Nw);
    double t = t0*(M_PI-2*d) - M_PI/2 + d;
    om[i]  = w*tan(t);
    om.Dh(i) = dh*w*(M_PI-2*d)/sqr(cos(t));
  }

  om.Delta(0) =  1.0/(om[1]-om[0]);
  for (int i=1; i<om.size()-1; i++) om.Delta(i) = 1/(om[i+1]-om[i]);
  om.Delta(om.size()-1) = 0.0;
}
void GiveDoubleExpMesh(mesh1D& tau, double beta, int Ntau_)
{
  double al=2.1;
  tau.resize(Ntau_);

  double Ntau = Ntau_-2;
  double dh = beta/(Ntau-1.);
  for (int i=0; i<Ntau; i++){
    double t = 2*i/(Ntau-1.)-1.;
    double x0 = M_PI/2.*sinh(al*t);
    tau[i+1] = (tanh(x0)+1.)*(beta/2.);
    tau.Dh(i+1) = dh*al*M_PI/2.*cosh(al*t)/sqr(cosh(x0));
  }
  tau[0]          = 0.0;
  tau[Ntau_-1]    = beta;
  tau.Dh(0)       = tau[1]-tau[0];
  tau.Dh(Ntau_-1) = beta-tau[Ntau_-2];

  for (int i=0; i<tau.size()-1; i++) tau.Delta(i) = 1/(tau[i+1]-tau[i]);
  tau.Delta(tau.size()-1) = 0.0;
}


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

int
main (void)
{
  int i, n = 4;
  double x[4] = { 1970, 1980, 1990, 2000 };
  double y[4] = {   12,   11,   14,   13 };
  double w[4] = {  0.1,  0.2,  0.3,  0.4 };

  double c0, c1, cov00, cov01, cov11, chisq;

  gsl_fit_wlinear (x, 1, w, 1, y, 1, n,
                   &c0, &c1, &cov00, &cov01, &cov11,
                   &chisq);

  printf ("# best fit: Y = %g + %g X\n", c0, c1);
  printf ("# covariance matrix:\n");
  printf ("# [ %g, %g\n#   %g, %g]\n",
          cov00, cov01, cov01, cov11);
  printf ("# chisq = %g\n", chisq);

  for (i = 0; i < n; i++)
    printf ("data: %g %g %g\n",
                   x[i], y[i], 1/sqrt(w[i]));

  printf ("\n");

  for (i = -30; i < 130; i++)
    {
      double xf = x[0] + (i/100.0) * (x[n-1] - x[0]);
      double yf, yf_err;

      gsl_fit_linear_est (xf,
                          c0, c1,
                          cov00, cov01, cov11,
                          &yf, &yf_err);

      printf ("fit: %g %g\n", xf, yf);
      printf ("hi : %g %g\n", xf, yf + yf_err);
      printf ("lo : %g %g\n", xf, yf - yf_err);
    }
  return 0;
}


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

#include <gsl/gsl_math.h>
#include <gsl/gsl_filter.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_vector.h>

int
main(void)
{
  const size_t N = 1000;                               /* length of time series */
  const size_t K = 25;                                 /* window size */
  const double t = 4.0;                                /* number of scale factors for outlier detection */
  gsl_vector *x = gsl_vector_alloc(N);                 /* input vector */
  gsl_vector *y = gsl_vector_alloc(N);                 /* output (filtered) vector */
  gsl_vector *xmedian = gsl_vector_alloc(N);           /* window medians */
  gsl_vector *xsigma = gsl_vector_alloc(N);            /* window scale estimates */
  gsl_vector_int *ioutlier = gsl_vector_int_alloc(N);  /* outlier detected? */
  gsl_filter_impulse_workspace * w = gsl_filter_impulse_alloc(K);
  gsl_rng *r = gsl_rng_alloc(gsl_rng_default);
  size_t noutlier;
  size_t i;

  /* generate input signal */
  for (i = 0; i < N; ++i)
    {
      double xi = 10.0 * sin(2.0 * M_PI * i / (double) N);
      double ei = gsl_ran_gaussian(r, 2.0);
      double u = gsl_rng_uniform(r);
      double outlier = (u < 0.01) ? 15.0*GSL_SIGN(ei) : 0.0;

      gsl_vector_set(x, i, xi + ei + outlier);
    }

  /* apply impulse detection filter */
  gsl_filter_impulse(GSL_FILTER_END_TRUNCATE, GSL_FILTER_SCALE_QN, t, x, y,
                     xmedian, xsigma, &noutlier, ioutlier, w);

  /* print results */
  for (i = 0; i < N; ++i)
    {
      double xi = gsl_vector_get(x, i);
      double yi = gsl_vector_get(y, i);
      double xmedi = gsl_vector_get(xmedian, i);
      double xsigmai = gsl_vector_get(xsigma, i);
      int outlier = gsl_vector_int_get(ioutlier, i);

      printf("%zu %f %f %f %f %d\n",
             i,
             xi,
             yi,
             xmedi + t * xsigmai,
             xmedi - t * xsigmai,
             outlier);
    }

  gsl_vector_free(x);
  gsl_vector_free(y);
  gsl_vector_free(xmedian);
  gsl_vector_free(xsigma);
  gsl_vector_int_free(ioutlier);
  gsl_filter_impulse_free(w);
  gsl_rng_free(r);

  return 0;
}


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

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

#define N (4096*4*16)

double g (double *x, int dim);

int
main (void)
{
  double sum;
  double s = 12.;
  int dim = 4;
  double x[4];

  gsl_rng *r;

  r = gsl_rng_alloc (gsl_rng_taus2);
  gsl_rng_set (r, 1UL);

  long i, j, nn;

  printf ("# Iter        Npoints   Volume     AbsErr\n");
  nn = N;
  for (j = 0; j < 16; j++)
    {
      sum = 0.;
      for (i = 0; i < nn; i++)
	{
	  // 4-dimensional random point
	  for (int k = 0; k < dim; k++)
	    {
	      x[k] = s * gsl_rng_uniform (r) - s / 2;
	    }
	  sum += g (x, dim);
	}
      double integ = (sum * pow (s, (double) dim)) / nn;
      double exact = M_PI * M_PI;

      printf ("%3ld  %16ld   %f   %f\n", j, nn, integ, fabs (exact - integ));
      nn *= 2;
    }

  gsl_rng_free (r);

  return 0;
}

double
g (double *x, int dim)
{
  double r2 = 0., q;

  for (int i = 0; i < dim; i++)
    {
      q = x[i];
      r2 += q * q;
    }

  return exp (-r2);
}


#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <string.h>
#include <stdint.h>
#include <math.h>
#include <assert.h>
#include <gsl/gsl_math.h>

#include "com3.h"

// #define debugmode 1
int unit_tests(void);

const int64_t comr = 2; // Radius of com filter in pixels
// The filter will be (2*comr+1)^3 pixels

int pointInDomain(int64_t * restrict Point, size_t * Domain, int nDim)
  // Check if a point is in the domain of a 3D image
{
  for(int kk = 0; kk<nDim; kk++)
    if(Point[kk]<0 || Point[kk]>= (int64_t) Domain[kk])
      return 0;

  return 1;
}

int checkBounds(int64_t * restrict D,
    const size_t M, const size_t N, const size_t P)
{
  if(D[0]>=comr && D[0]+comr<(int64_t) M &&
      D[1]>=comr && D[1]+comr<(int64_t) N &&
      D[2]>=comr && D[2]+comr<(int64_t) P)
    return 1; // ok

  return 0;
}

void com3_localw(double * restrict V, double * restrict W, const size_t M, const size_t N, const size_t P,
    int64_t * restrict D, double * restrict C)
{

#ifdef debugmode
  printf("D: [%u %u %u]\n", D[0], D[1], D[2]);
  printf("M: %lu N: %lu P: %lu\n", M, N, P);
#endif

  double sum = 0;
  double dx = 0;
  double dy = 0;
  double dz = 0;

#ifdef debugmode
  printf("sum: %f\n", sum);
#endif

  if(checkBounds(D, M, N, P))
  {

    double val = V[D[0] + D[1]*M + D[2]*M*N];

    for(int kk = -comr; kk<=comr; kk++) {
      for(int ll = -comr; ll<=comr; ll++) {
        for(int mm = -comr; mm<=comr; mm++) {

          size_t pos = (D[0]+kk) + (D[1]+ll)*M + (D[2]+mm)*M*N;
          //  printf("pos: %lu V[pos]: %f\n", pos, V[pos]);
          dx += kk*V[pos]*val/W[pos];
          dy += ll*V[pos]*val/W[pos];
          dz += mm*V[pos]*val/W[pos];

          sum += V[pos];
        } } }

    //printf("sum: %f\n", sum);
  }

  if(sum>0)
  {
#ifdef debugmode
    printf("sum: %f (%f, %f, %f)\n", sum, dx, dy, dz);
#endif
    C[0] = D[0] + dx/sum;
    C[1] = D[1] + dy/sum;
    C[2] = D[2] + dz/sum;
  }
  else
  {
#ifdef debugmode
    printf("No com calculation\n");
#endif
    C[0] = D[0];
    C[1] = D[1];
    C[2] = D[2];
  }
}

void com3_local(double * restrict V, const size_t M, const size_t N, const size_t P,
    int64_t * restrict D, double * restrict C)
{

#ifdef debugmode
  printf("D: [%u %u %u]\n", D[0], D[1], D[2]);
  printf("M: %lu N: %lu P: %lu\n", M, N, P);
#endif

  double sum = 0;
  double dx = 0;
  double dy = 0;
  double dz = 0;

#ifdef debugmode
  printf("sum: %f\n", sum);
#endif

  if(checkBounds(D, M, N, P))
  {

    for(int kk = -comr; kk<=comr; kk++) {
      for(int ll = -comr; ll<=comr; ll++) {
        for(int mm = -comr; mm<=comr; mm++) {

          size_t pos = (D[0]+kk) + (D[1]+ll)*M + (D[2]+mm)*M*N;
          //  printf("pos: %lu V[pos]: %f\n", pos, V[pos]);
          dx += kk*V[pos];
          dy += ll*V[pos];
          dz += mm*V[pos];

          sum += V[pos];
        } } }

    //printf("sum: %f\n", sum);
  }

  if(sum>0)
  {
#ifdef debugmode
    printf("sum: %f (%f, %f, %f)\n", sum, dx, dy, dz);
#endif
    C[0] = D[0] + dx/sum;
    C[1] = D[1] + dy/sum;
    C[2] = D[2] + dz/sum;
  }
  else
  {
#ifdef debugmode
    printf("No com calculation\n");
#endif
    C[0] = D[0];
    C[1] = D[1];
    C[2] = D[2];
  }
}

void setLmax(double *V, double * W, size_t * Domain, int64_t * Dot)
  // Adds V(dot) to all W within comr of dot
  // uses global value comr
{
  uint32_t m = Dot[0];
  uint32_t n = Dot[1];
  uint32_t p = Dot[2];

  size_t M = Domain[0];
  size_t N = Domain[1];
  size_t P = Domain[2];

  double val = V[m + n*M + p*M*N];

  for(uint32_t     pp = GSL_MAX(p-comr,0) ; pp<= GSL_MIN(p+comr, P-1) ; pp++) {
    for(uint32_t   nn = GSL_MAX(n-comr,0) ; nn<= GSL_MIN(n+comr, N-1) ; nn++) {
      for(uint32_t mm = GSL_MAX(m-comr,0) ; mm<= GSL_MIN(m+comr, M-1) ; mm++) {
        W[mm + nn*M + pp*M*N] += val;
      }
    }
  }
}

void setWeights(double * V, double * W, size_t M, size_t N, size_t P, double * D, size_t nD)
{
  size_t Domain[] = {M, N, P};
  int64_t Dot[] = {0,0,0};
  for(size_t kk = 0; kk<nD; kk++) {
    Dot[0] = D[kk*3 + 0];
    Dot[1] = D[kk*3 + 1];
    Dot[2] = D[kk*3 + 2];

    setLmax(V, W, Domain, Dot);
  }
}

void com3(double * V, size_t M, size_t N, size_t P,
    double * D, double * C, size_t L, int weighted) {
  /* Centre of mass
   *
   * V: MxNxP image
   * P 3xL list of dots
   * C 3xL list of fitted dots
   */

  int64_t Dround[] = {0,0,0};
  size_t Domain[] = {M, N, P};

  double * W;
  if(weighted == 1)
    // Set up the weighting matrix
  {
    W = (double *) calloc(M*N*P, sizeof(double));
    for(size_t kk = 0; kk<L; kk++)
    {
      Dround[0] = nearbyint(D[kk*3]-1);
      Dround[1] = nearbyint(D[kk*3+1]-1);
      Dround[2] = nearbyint(D[kk*3+2]-1);
      if(pointInDomain(Dround, Domain, 3))

        setLmax(V, W, Domain, Dround);
    }
  }

  if(weighted == 1)
    for(size_t kk = 0; kk<L; kk++)
    {
      Dround[0] = nearbyint(D[kk*3]-1);
      Dround[1] = nearbyint(D[kk*3+1]-1);
      Dround[2] = nearbyint(D[kk*3+2]-1);

      //    printf("%d %d %d\n", Dround[0], Dround[1], Dround[2]);

      com3_localw(V, W, M, N, P, Dround, C+kk*3);
    }

  if(weighted ==0)
    for(size_t kk = 0; kk<L; kk++)
    {
      Dround[0] = nearbyint(D[kk*3]-1);
      Dround[1] = nearbyint(D[kk*3+1]-1);
      Dround[2] = nearbyint(D[kk*3+2]-1);

      //    printf("%d %d %d\n", Dround[0], Dround[1], Dround[2]);

      com3_local(V, M, N, P, Dround, C+kk*3);
    }

  if(weighted == 1){

    // This was used for debugging
    //for(size_t xx = 0; xx<M*N*P; xx++)
    //  V[xx] = W[xx];

    free(W);
  }
}

#ifdef standalone

int unit_tests() {

  // Size of image
  uint32_t M = 100;
  uint32_t N = 100;
  uint32_t P = 100;
  uint32_t L = 2; // number of dots

  double * V = malloc(M*N*P*sizeof(double));
  double * D = malloc(L*3*sizeof(double));
  double * C = malloc(L*3*sizeof(double));

  memset(V, 0, M*N*P*sizeof(double));
  memset(C, 0, 3*L*sizeof(double));

  for(size_t kk = 0; kk<M*N*P; kk++)
    V[kk] = 0;

  D[0] = 11; D[1] = 12; D[2] = 13;
  D[3] = 0; D[4] = 15; D[5] = 16;

  size_t pos = D[0] + M*D[1] + M*N*D[2];
  V[pos] = 3;
  //  V[pos+1] = 1;
  //  V[pos + M] = 1;
  V[pos + M*N] = 1;

  // Ordinary
  com3(V, M, N, P,
      D, C, L, 0);

  // Weighted
  com3(V, M, N, P,
      D, C, L, 1);

  for(uint32_t kk=0; kk<L; kk++) {
    printf("%d [%f %f %f] -> ", kk, D[3*kk], D[3*kk+1], D[3*kk+2]);
    printf(" [%f %f %f]\n",         C[3*kk], C[3*kk+1], C[3*kk+2]);
  }

  free(C);
  free(D);
  free(V);

  return 0;
}

int main(int argc, char ** argv)
{
  printf("%s\n", argv[0]);
  if(argc == 1)
    return unit_tests();
}
#endif


#include <math.h>
#include <time.h>
#include <stdio.h>
#include <stdlib.h>
#include <mpfr.h>
#include <petsc.h>
#include "ellipsoid.h"
#include "../sphere/sphere.h"
#include "ellSolv.h"

#undef __FUNCT__
#define __FUNCT__ "InitEllipsoidalAndConvertPoints"
PetscErrorCode InitEllipsoidalAndConvertPoints(EllipsoidalSystem *e, PetscReal a, PetscReal b, PetscReal c, Vec xyz, Vec ell)
{
  PetscErrorCode ierr;
  PetscLogEvent initEvent;
  PetscFunctionBegin;

  ierr = PetscLogEventRegister("Init ellipsoidal system", 0, &initEvent);CHKERRQ(ierr);
  ierr = PetscLogEventBegin(initEvent, 0, 0, 0, 0);CHKERRQ(ierr);
  ierr = initEllipsoidalSystem(e, a, b, c, 32);CHKERRQ(ierr);
  ierr = PetscLogEventEnd(initEvent, 0, 0, 0, 0);CHKERRQ(ierr);

  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "HowMany"
PetscErrorCode HowMany(PetscInt N, PetscInt *num)
{
  PetscErrorCode ierr;
  PetscInt n, p;
  PetscFunctionBegin;
  *num = 0;
  for(n = 0; n <= N; ++n) {
    for(p=0; p < 2*n + 1; ++p)
      *num += 1;
  }
  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "CalcEllipsoidFreeEnergy"
PetscErrorCode CalcEllipsoidFreeEnergy(EllipsoidalSystem *e, PetscReal eps1, PetscReal eps2, PetscInt nSrc, Vec srcXYZ, Vec srcMag, PetscReal tol, PetscInt Nmax, Vec tarSol, PetscReal *freeE)
{
  PetscErrorCode ierr;
  PetscInt flopCount;
  //EllipsoidalSystem e;
  PetscReal x, y, z;
  PetscInt n, p, npsize;
  Vec srcEll, tarEll;
  PetscScalar *tarEllArray;
  Vec coulCoefs, reactCoefs, extCoefs;
  const PetscScalar *coulCoefsArray, *reactCoefsArray, *extCoefsArray;
  const PetscScalar *srcMagArray;
  Vec EnpVals, FnpVals;
  const PetscScalar *EnpValsArray, *FnpValsArray;
  PetscScalar *tarSolArray;
  PetscFunctionBegin;
  flopCount = 0;

  /* init ellipsoidal system */
  //ierr = initEllipsoidalSystem(&e, a, b, c);CHKERRQ(ierr);

  /* initialize expansion vectors */
  ierr = HowMany(Nmax, &npsize);CHKERRQ(ierr);
  //ierr = VecCreateSeq(PETSC_COMM_SELF, npsize, &coulCoefs);CHKERRQ(ierr);
  ierr = VecCreateSeq(PETSC_COMM_SELF, npsize, &reactCoefs);CHKERRQ(ierr);
  ierr = VecCreateSeq(PETSC_COMM_SELF, npsize, &extCoefs);CHKERRQ(ierr);

  /* initialize point vectors */
  ierr = VecDuplicate(srcXYZ, &srcEll);CHKERRQ(ierr);
  ierr = VecDuplicate(srcEll, &tarEll);CHKERRQ(ierr);

  /* convert points from cartesian to ellipsoidal */
  ierr = CartesianToEllipsoidalVec(e, srcXYZ, srcEll);CHKERRQ(ierr);
  ierr = VecCopy(srcEll, tarEll);CHKERRQ(ierr); //source+target same

  /* Calculate Gnp */
  ierr = CalcCoulombEllCoefs(e, nSrc, srcEll, srcMag, Nmax, &coulCoefs);CHKERRQ(ierr);
  /* calculate Bnp and Cnp */
  ierr = CalcReactAndExtCoefsFromCoulomb(e, eps1, eps2, Nmax, coulCoefs, reactCoefs, extCoefs);

  // create EnpVals and FnpVals vectors
  ierr = VecCreateSeq(PETSC_COMM_SELF, nSrc, &EnpVals);CHKERRQ(ierr);
  ierr = VecCreateSeq(PETSC_COMM_SELF, nSrc, &FnpVals);CHKERRQ(ierr);

  // get read-only pointers for expansion coefficients
  ierr = VecGetArrayRead(coulCoefs, &coulCoefsArray);CHKERRQ(ierr);
  ierr = VecGetArrayRead(reactCoefs, &reactCoefsArray);CHKERRQ(ierr);
  ierr = VecGetArrayRead(extCoefs, &extCoefsArray);CHKERRQ(ierr);
  // get write pointer for solution vector
  ierr = VecZeroEntries(tarSol);CHKERRQ(ierr);
  ierr = VecGetArray(tarSol, &tarSolArray);CHKERRQ(ierr);
  PetscInt ind = 0;
  for(n=0; n <= Nmax; ++n) {
    for(p=0; p < 2*n+1; ++p) {
      PetscReal Gnp =  coulCoefsArray[ind];
      PetscReal Cnp =   extCoefsArray[ind];
      PetscReal Bnp = reactCoefsArray[ind];

      ierr = CalcSolidInteriorHarmonicVec(e, tarEll, n, p, EnpVals);CHKERRQ(ierr);
      //ierr = CalcSolidExteriorHarmonicVec(&e, tarEll, n, p, FnpVals);CHKERRQ(ierr);

      ierr = VecGetArrayRead(EnpVals, &EnpValsArray);CHKERRQ(ierr);
      //ierr = VecGetArrayRead(FnpVals, &FnpValsArray);CHKERRQ(ierr);
      for(PetscInt k=0; k < nSrc; ++k) {
	PetscReal Enp = EnpValsArray[k];
	//PetscReal Fnp = FnpValsArray[k];
	tarSolArray[k] += Bnp*Enp; flopCount += 2;
      }
      ierr = VecRestoreArrayRead(EnpVals, &EnpValsArray);CHKERRQ(ierr);
      //ierr = VecRestoreArrayRead(FnpVals, &FnpValsArray);CHKERRQ(ierr);

      ind++;
    }
  }
  // get read-only pointers for expansion coefficients
  ierr = VecRestoreArrayRead(coulCoefs, &coulCoefsArray);CHKERRQ(ierr);
  ierr = VecRestoreArrayRead(reactCoefs, &reactCoefsArray);CHKERRQ(ierr);
  ierr = VecRestoreArrayRead(extCoefs, &extCoefsArray);CHKERRQ(ierr);
  *freeE = 0;
  ierr = VecGetArrayRead(srcMag, &srcMagArray);CHKERRQ(ierr);
  /* calculate free energy */
  for(PetscInt k=0; k < nSrc; ++k) {
    printf("sum[d] = %15.15f\n", srcMagArray[k]*tarSolArray[k]);
    *freeE += srcMagArray[k]*tarSolArray[k];
  }
  ierr = VecRestoreArrayRead(srcMag, &srcMagArray);CHKERRQ(ierr);
  ierr = VecRestoreArray(tarSol, &tarSolArray);CHKERRQ(ierr);
  ierr = PetscLogFlops(flopCount);CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "CalcEllipsoidTester"
PetscErrorCode CalcEllipsoidTester(PetscReal a, PetscReal b, PetscReal c, PetscReal eps1, PetscReal eps2, PetscInt nSrc, Vec srcXYZ, Vec srcMag, PetscInt nTar, Vec tarXYZ, PetscInt Nmax, Vec tarSol)
{
  PetscErrorCode ierr;
  EllipsoidalSystem e;
  PetscReal x, y, z;
  PetscInt n, p, npsize;
  Vec srcEll, tarEll;
  PetscScalar *tarEllArray;
  Vec coulCoefs, reactCoefs, extCoefs;
  const PetscScalar *coulCoefsArray, *reactCoefsArray, *extCoefsArray;
  Vec EnpVals, FnpVals;
  const PetscScalar *EnpValsArray, *FnpValsArray;
  PetscScalar *tarSolArray;
  PetscFunctionBegin;

  /* init ellipsoidal system */
  ierr = initEllipsoidalSystem(&e, a, b, c, 32);CHKERRQ(ierr);

  /* initialize expansion vectors */
  ierr = HowMany(Nmax, &npsize);CHKERRQ(ierr);
  //ierr = VecCreateSeq(PETSC_COMM_SELF, npsize, &coulCoefs);CHKERRQ(ierr);
  ierr = VecCreateSeq(PETSC_COMM_SELF, npsize, &reactCoefs);CHKERRQ(ierr);
  ierr = VecCreateSeq(PETSC_COMM_SELF, npsize, &extCoefs);CHKERRQ(ierr);

  /* initialize ellipsoid vectors */
  ierr = VecDuplicate(srcXYZ, &srcEll);CHKERRQ(ierr);
  ierr = VecDuplicate(tarXYZ, &tarEll);CHKERRQ(ierr);

  /* convert points from cartesian to ellipsoidal */
  ierr = CartesianToEllipsoidalVec(&e, srcXYZ, srcEll);CHKERRQ(ierr);
  ierr = CartesianToEllipsoidalVec(&e, tarXYZ, tarEll);CHKERRQ(ierr);

  /* Calculate Gnp */
  ierr = CalcCoulombEllCoefs(&e, nSrc, srcEll, srcMag, Nmax, &coulCoefs);CHKERRQ(ierr);
  //printf("\n\n##################################\n########## COUL COEFS #############\n###########################################\n\n");
  //printf("nmax: %d\n", Nmax);
  //ierr = VecView(coulCoefs, PETSC_VIEWER_STDOUT_SELF);CHKERRQ(ierr);
  //printf("\n\n##################################\n########## OVER #############\n###########################################\n\n");
  /* calculate Bnp and Cnp */
  ierr = CalcReactAndExtCoefsFromCoulomb(&e, eps1, eps2, Nmax, coulCoefs, reactCoefs, extCoefs);

  // create EnpVals and FnpVals vectors
  ierr = VecCreateSeq(PETSC_COMM_SELF, nTar, &EnpVals);CHKERRQ(ierr);
  ierr = VecCreateSeq(PETSC_COMM_SELF, nTar, &FnpVals);CHKERRQ(ierr);

  // get read-only pointers for expansion coefficients
  ierr = VecGetArrayRead(coulCoefs, &coulCoefsArray);CHKERRQ(ierr);
  ierr = VecGetArrayRead(reactCoefs, &reactCoefsArray);CHKERRQ(ierr);
  ierr = VecGetArrayRead(extCoefs, &extCoefsArray);CHKERRQ(ierr);
  // get write pointer for solution vector
  ierr = VecZeroEntries(tarSol);CHKERRQ(ierr);
  ierr = VecGetArray(tarSol, &tarSolArray);CHKERRQ(ierr);
  PetscInt ind = 0;
  for(n=0; n <= Nmax; ++n) {
    printf("n: %d\n", n);
    for(p=0; p < 2*n+1; ++p) {
      PetscReal Gnp =  coulCoefsArray[ind];
      PetscReal Cnp =   extCoefsArray[ind];
      PetscReal Bnp = reactCoefsArray[ind];

      ierr = CalcSolidInteriorHarmonicVec(&e, tarEll, n, p, EnpVals);CHKERRQ(ierr);
      ierr = CalcSolidExteriorHarmonicVec(&e, tarEll, n, p, FnpVals);CHKERRQ(ierr);

      ierr = VecGetArrayRead(EnpVals, &EnpValsArray);CHKERRQ(ierr);
      ierr = VecGetArrayRead(FnpVals, &FnpValsArray);CHKERRQ(ierr);
      for(PetscInt k=0; k < nTar; ++k) {
	if(n==0 && p==0) {
	  tarSolArray[k] = 0;
	}
	PetscReal lambda;
	PetscInt index = 3*k+0;
	ierr = VecGetValues(tarEll, 1, &index, &lambda);CHKERRQ(ierr);

	PetscReal Enp = EnpValsArray[k];
	PetscReal Fnp = FnpValsArray[k];
	if(PetscAbsReal(lambda) <= a)
	  tarSolArray[k] += Bnp*Enp; //(Gnp/eps1)*Fnp;
	else {
	  tarSolArray[k] += Cnp*Fnp;//(Gnp/eps1)*Fnp;
	}

      }
      ierr = VecRestoreArrayRead(EnpVals, &EnpValsArray);CHKERRQ(ierr);
      ierr = VecRestoreArrayRead(FnpVals, &FnpValsArray);CHKERRQ(ierr);

      ind++;
    }
  }

  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "CalcEllipsoidCoulombPotential"
PetscErrorCode CalcEllipsoidCoulombPotential(PetscReal a, PetscReal b, PetscReal c, PetscReal eps1, PetscReal eps2, PetscInt nCharges, Vec chargeXYZ, Vec chargeMag, PetscInt nSol, Vec solXYZ, PetscInt Nmax, Vec targetSol)
{
  PetscErrorCode ierr;
  Vec chargeEll;
  Vec solEll;
  Vec coulCoefs;
  Vec FnpVals;
  EllipsoidalSystem e;
  PetscScalar *vecPtr;
  PetscScalar *coulCoefsArray;
  PetscScalar *FnpValsArray;
  PetscFunctionBegin;

  ierr = initEllipsoidalSystem(&e, a, b, c, 32);CHKERRQ(ierr);

  // create charge ellipsoidal vec and convert from xyz
  ierr = VecCreateSeq(PETSC_COMM_SELF, 3*nCharges, &chargeEll);CHKERRQ(ierr);
  printf("Converting charge points to ellipsoidal\n");
  ierr = CartesianToEllipsoidalVec(&e, chargeXYZ, chargeEll);CHKERRQ(ierr);
  // create solution ellipsoidal vec and convert from xyz
  ierr = VecCreateSeq(PETSC_COMM_SELF, 3*nSol, &solEll);CHKERRQ(ierr);
  printf("Converting solution points to ellipsoidal\n");
  ierr = CartesianToEllipsoidalVec(&e, solXYZ, solEll);CHKERRQ(ierr);

  //calculate coulomb coefficients
  ierr = CalcCoulombEllCoefs(&e, nCharges, chargeEll, chargeMag, Nmax, &coulCoefs);CHKERRQ(ierr);

  //initialize vector for interior harmonic calculations
  ierr = VecCreateSeq(PETSC_COMM_SELF, nSol, &FnpVals);CHKERRQ(ierr);

  ierr = VecGetArrayRead(coulCoefs, &coulCoefsArray);CHKERRQ(ierr);
  ierr = VecGetArray(targetSol, &vecPtr);CHKERRQ(ierr);
  PetscInt index = 0;
  for(PetscInt n=0; n<=Nmax; ++n) {
    for(PetscInt p=0; p < 2*n+1; ++p) {
      //calculate Enp Vals
      ierr = CalcSolidExteriorHarmonicVec(&e, solEll, n, p, FnpVals);CHKERRQ(ierr);
      ierr = VecGetArrayRead(FnpVals, &FnpValsArray);CHKERRQ(ierr);
      for(PetscInt k=0; k<nSol; ++k) {
	vecPtr[k] += coulCoefsArray[index]*FnpValsArray[k];
      }
      ierr = VecRestoreArrayRead(FnpVals, &FnpValsArray);CHKERRQ(ierr);
      index++;
    }
  }
  ierr = VecRestoreArray(targetSol, &vecPtr);CHKERRQ(ierr);
  ierr = VecRestoreArrayRead(coulCoefs, &coulCoefsArray);CHKERRQ(ierr);

  ierr = VecDestroy(&solEll);CHKERRQ(ierr);
  ierr = VecDestroy(&chargeEll);CHKERRQ(ierr);
  ierr = VecDestroy(&coulCoefs);CHKERRQ(ierr);
  ierr = VecDestroy(&FnpVals);CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "CalcEllipsoidSolvationPotential"
PetscErrorCode CalcEllipsoidSolvationPotential(PetscReal a, PetscReal b, PetscReal c, PetscReal eps1, PetscReal eps2, PetscInt nSource, Vec sourceXYZ, Vec sourceMag, PetscInt nTarget, Vec targetXYZ, PetscInt Nmax, Vec targetSol)
{
  PetscErrorCode ierr;
  EllipsoidalSystem e;
  PetscReal x, y, z;
  PetscInt n, p;
  PetscInt intPts, extPts;
  Vec tarIntXYZ, tarExtXYZ;
  PetscScalar *tarIntXYZArray, *tarExtXYZArray;
  Vec tarIntEll, tarExtEll;
  Vec srcEll, tarEll;
  Vec coulCoefs, reactCoefs, extCoefs;
  Vec EnpVals, FnpVals;
  PetscInt *isExt;
  const PetscScalar *targetXYZArray, *coulCoefsArray, *reactCoefsArray, *extCoefsArray;
  PetscScalar *targetSolArray;
  const PetscScalar *EnpValsArray, *FnpValsArray;

  //PetscLogStage stageIntSolids, stageExtSolids;
  PetscLogEvent *INTERIOR_FLOPS;
  PetscLogEvent *EXTERIOR_FLOPS;
  //PetscLogDouble user_log_flops;
  PetscFunctionBegin;

  ierr = initEllipsoidalSystem(&e, a, b, c, 32);CHKERRQ(ierr);

  // calculate the number of interior and exterior points
  ierr = VecGetArrayRead(targetXYZ, &targetXYZArray);CHKERRQ(ierr);
  intPts = 0; extPts = 0;
  for(int k=0; k < nTarget; ++k) {
    x = targetXYZArray[3*k+0];
    y = targetXYZArray[3*k+1];
    z = targetXYZArray[3*k+2];
    if((x*x)/(a*a) + (y*y)/(b*b) + (z*z)/(c*c) <= 1)
      intPts++;
    else
      extPts++;
  }
  // init interior and exterior target point vectors
  ierr = VecCreateSeq(PETSC_COMM_SELF, 3*intPts, &tarIntXYZ);CHKERRQ(ierr);
  ierr = VecCreateSeq(PETSC_COMM_SELF, 3*extPts, &tarExtXYZ);CHKERRQ(ierr);
  // sort points into int and ext vectors
  ierr = VecGetArray(tarIntXYZ, &tarIntXYZArray);CHKERRQ(ierr);
  ierr = VecGetArray(tarExtXYZ, &tarExtXYZArray);CHKERRQ(ierr);

  ierr = PetscMalloc1(sizeof(PetscInt)*nTarget, &isExt);CHKERRQ(ierr);

  intPts = 0; extPts = 0;
  for(int k=0; k < nTarget; ++k) {
    x = targetXYZArray[3*k+0];
    y = targetXYZArray[3*k+1];
    z = targetXYZArray[3*k+2];
    if((x*x)/(a*a) + (y*y)/(b*b) + (z*z)/(c*c) <= 1) {
      tarIntXYZArray[3*intPts+0] = x;
      tarIntXYZArray[3*intPts+1] = y;
      tarIntXYZArray[3*intPts+2] = z;
      isExt[k] = 0;
      intPts++;
    }
    else {
      tarExtXYZArray[3*extPts+0] = x;
      tarExtXYZArray[3*extPts+1] = y;
      tarExtXYZArray[3*extPts+2] = z;
      isExt[k] = 1;
      extPts++;
    }
  }
  ierr = VecRestoreArray(tarIntXYZ, &tarIntXYZArray);CHKERRQ(ierr);
  ierr = VecRestoreArray(tarExtXYZ, &tarExtXYZArray);CHKERRQ(ierr);
  ierr = VecRestoreArrayRead(targetXYZ, &targetXYZArray);CHKERRQ(ierr);

  // create source ellipsoidal vec and convert from xyz
  ierr = VecCreateSeq(PETSC_COMM_SELF, 3*nSource, &srcEll);CHKERRQ(ierr);
  ierr = CartesianToEllipsoidalVec(&e, sourceXYZ, srcEll);CHKERRQ(ierr);
  // create target ellipsoidal vec and convert from xyz
  ierr = VecCreateSeq(PETSC_COMM_SELF, 3*nTarget, &tarEll);CHKERRQ(ierr);
  ierr = CartesianToEllipsoidalVec(&e, targetXYZ, tarEll);CHKERRQ(ierr);
  // create target interior ellipsoidal vec and convert from xyz
  ierr = VecCreateSeq(PETSC_COMM_SELF, 3*intPts, &tarIntEll);CHKERRQ(ierr);
  ierr = CartesianToEllipsoidalVec(&e, tarIntXYZ, tarIntEll);CHKERRQ(ierr);
  // create target exterior ellipsoidal vec and convert from xyz
  ierr = VecCreateSeq(PETSC_COMM_SELF, 3*extPts, &tarExtEll);CHKERRQ(ierr);
  ierr = CartesianToEllipsoidalVec(&e, tarExtXYZ, tarExtEll);CHKERRQ(ierr);

  // calculate coulomb coefficients
  ierr = CalcCoulombEllCoefs(&e, nSource, srcEll, sourceMag, Nmax, &coulCoefs);CHKERRQ(ierr);
  // init reacCoefs, extCoefs from size of coulCoefs
  ierr = VecDuplicate(coulCoefs, &reactCoefs);CHKERRQ(ierr);
  ierr = VecDuplicate(coulCoefs, &extCoefs);CHKERRQ(ierr);
  // calc reaction and exterior expansion coefs from coulomb coefs
  ierr = CalcReactAndExtCoefsFromCoulomb(&e, eps1, eps2, Nmax, coulCoefs, reactCoefs, extCoefs);

  // create EnpVals and FnpVals vectors
  ierr = VecCreateSeq(PETSC_COMM_SELF, intPts, &EnpVals);CHKERRQ(ierr);
  ierr = VecCreateSeq(PETSC_COMM_SELF, extPts, &FnpVals);CHKERRQ(ierr);

  // get read-only pointers for expansion coefficients
  ierr = VecGetArrayRead(coulCoefs, &coulCoefsArray);CHKERRQ(ierr);
  ierr = VecGetArrayRead(reactCoefs, &reactCoefsArray);CHKERRQ(ierr);
  ierr = VecGetArrayRead(extCoefs, &extCoefsArray);CHKERRQ(ierr);
  // get write pointer for solution vector
  ierr = VecZeroEntries(targetSol);CHKERRQ(ierr);
  ierr = VecGetArray(targetSol, &targetSolArray);CHKERRQ(ierr);
  // loop
  PetscInt howmany = 0;
  for(n=0; n <= Nmax; ++n) {
    for(p=0; p < 2*n+1; ++p)
      howmany++;
  }

  ierr = PetscMalloc1(sizeof(PetscLogEvent)*howmany, &INTERIOR_FLOPS);CHKERRQ(ierr);
  ierr = PetscMalloc1(sizeof(PetscLogEvent)*howmany, &EXTERIOR_FLOPS);CHKERRQ(ierr);
  char intStr[40] = "Interior Harmonic %d";
  char extStr[40] = "Exterior Harmonic %d";
  char indvIntStr[40];
  char indvExtStr[40];
  for(n=0; n<howmany; ++n) {
    sprintf(indvIntStr, intStr, n);
    sprintf(indvExtStr, extStr, n);
    ierr = PetscLogEventRegister(indvIntStr, 0, INTERIOR_FLOPS+n);CHKERRQ(ierr);
    ierr = PetscLogEventRegister(indvExtStr, 0, EXTERIOR_FLOPS+n);CHKERRQ(ierr);
  }
  PetscInt ind = 0;
  PetscInt intInd;
  PetscInt extInd;
  for(n=0; n <= Nmax; ++n) {
    printf("%d/%d\n", n, Nmax);
    for(p=0; p < 2*n+1; ++p) {

      PetscReal Gnp =  coulCoefsArray[ind];
      PetscReal Cnp =   extCoefsArray[ind];
      PetscReal Bnp = reactCoefsArray[ind];
      // calc solid harmonics for interior,exterior points

      //ierr = PetscLogStagePush(stageIntSolids);CHKERRQ(ierr);
      ierr = PetscLogEventBegin(INTERIOR_FLOPS[ind],0,0,0,0);CHKERRQ(ierr);
      ierr = CalcSolidInteriorHarmonicVec(&e, tarIntEll, n, p, EnpVals);CHKERRQ(ierr);
      ierr = PetscLogEventEnd  (INTERIOR_FLOPS[ind],0,0,0,0);CHKERRQ(ierr);
      //ierr = PetscLogStagePop();

      //ierr = PetscLogStagePush(stageExtSolids);CHKERRQ(ierr);
      ierr = PetscLogEventBegin(EXTERIOR_FLOPS[ind],0,0,0,0);CHKERRQ(ierr);
      ierr = CalcSolidExteriorHarmonicVec(&e, tarExtEll, n, p, FnpVals);CHKERRQ(ierr);
      ierr = PetscLogEventEnd(EXTERIOR_FLOPS[ind],0,0,0,0);CHKERRQ(ierr);
      //ierr = PetscLogStagePop();CHKERRQ(ierr);

      ierr = VecGetArrayRead(EnpVals, &EnpValsArray);CHKERRQ(ierr);
      ierr = VecGetArrayRead(FnpVals, &FnpValsArray);CHKERRQ(ierr);
      intInd = 0;
      extInd = 0;
      for(PetscInt k=0; k<nTarget; ++k) {

	if(isExt[k] == 0) { // interior points
	  //printf("E%d%d: %15.15f\nBnp %d%d: %15.15f\n\n", n, p, n, p, EnpValsArray[intInd], reactCoefsArray[ind]);
	  targetSolArray[k] += reactCoefsArray[ind]*EnpValsArray[intInd];
	  //ierr = CalcSolidInterior
	  //targetSolArray[k] += Gnp*EnpValsArray[intInd];
	  //targetSolArray[k] += 1;
	  intInd++;
	}
	else { // exterior points
	  //printf("F%d%d: %15.15f\nCnp %d%d: %15.15f\n\n", n, p, n, p, FnpValsArray[intInd], extCoefsArray[ind]);
	  //targetSolArray[k] += extCoefsArray[ind]*FnpValsArray[extInd];
	  //targetSolArray[k] += (Gnp*FnpValsArray[extInd])/eps1;
	  targetSolArray[k] += 0;
	  extInd++;
	}
      }
      ierr = VecRestoreArrayRead(EnpVals, &EnpValsArray);CHKERRQ(ierr);
      ierr = VecRestoreArrayRead(FnpVals, &FnpValsArray);CHKERRQ(ierr);

      ind++;

    }
  }
  //ierr = PetscLogEventEnd(SOLID_FLOPS, 0, 0, 0, 0);

  ierr = VecRestoreArray(targetSol, &targetSolArray);CHKERRQ(ierr);
  // restore read-only pointers for expansion coefficients
  ierr = VecRestoreArrayRead(coulCoefs , &coulCoefsArray );CHKERRQ(ierr);
  ierr = VecRestoreArrayRead(reactCoefs, &reactCoefsArray);CHKERRQ(ierr);
  ierr = VecRestoreArrayRead(extCoefs  , &extCoefsArray  );CHKERRQ(ierr);

  //output flops to file
  FILE *fpext = fopen("flopsext.txt", "w");
  FILE *fpint = fopen("flopsint.txt", "w");
  PetscStageLog stageLog;
  ierr = PetscLogGetStageLog(&stageLog);CHKERRQ(ierr);

  /*
  for(int i=0; i<howmany; ++i) {
    fprintf(fpext, "Ext %d = %.4e\n", i, stageLog->stageInfo[stageExtSolids].eventLog->eventInfo[EXTERIOR_FLOPS[i]].flops);
    fprintf(fpint, "Int %d = %.4e\n", i, stageLog->stageInfo[stageIntSolids].eventLog->eventInfo[INTERIOR_FLOPS[i]].flops);
  }
  */

  ierr = VecDestroy(&tarIntXYZ);CHKERRQ(ierr);CHKERRQ(ierr);
  ierr = VecDestroy(&tarExtXYZ);CHKERRQ(ierr);CHKERRQ(ierr);
  ierr = VecDestroy(&tarIntEll);CHKERRQ(ierr);CHKERRQ(ierr);
  ierr = VecDestroy(&tarExtEll);CHKERRQ(ierr);CHKERRQ(ierr);
  ierr = VecDestroy(&srcEll);CHKERRQ(ierr);CHKERRQ(ierr);
  ierr = VecDestroy(&tarEll);CHKERRQ(ierr);CHKERRQ(ierr);
  ierr = VecDestroy(&coulCoefs);CHKERRQ(ierr);
  ierr = VecDestroy(&reactCoefs);CHKERRQ(ierr);
  ierr = VecDestroy(&extCoefs);CHKERRQ(ierr);
  ierr = VecDestroy(&EnpVals); CHKERRQ(ierr);
  ierr = VecDestroy(&FnpVals); CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "CalcSolidInteriorHarmonic"
PetscErrorCode CalcSolidInteriorHarmonic(EllipsoidalSystem* e, PetscReal lambda, PetscReal mu, PetscReal nu, PetscInt n, PetscInt p, PetscReal *val)
{
  PetscErrorCode ierr;
  PetscInt signm = 1;
  PetscInt signn = 1;
  PetscReal eL, eM, eN;
  PetscFunctionBegin;

  if(mu < 0)
    signm = -1;
  if(nu < 0)
    signn = -1;

  calcLame(e, n, p, lambda, signm, signn, &eL);
  calcLame(e, n, p, mu, signm, signn, &eM);
  calcLame(e, n, p, nu, signm, signn, &eN);

  *val = eL*eM*eN;

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

#undef __FUNCT__
#define __FUNCT__ "CalcSolidInteriorHarmonicVec"
/*
  ellPoints is ellPoints of size 3*nPoints containing ellipsoidal coordinates
  outputs solid harmonics to values of size nPoints
*/
PetscErrorCode CalcSolidInteriorHarmonicVec(EllipsoidalSystem* e, Vec ellPoints, PetscInt n, PetscInt p, Vec values)
{
  PetscErrorCode ierr;
  PetscInt signm = 1;
  PetscInt signn = 1;
  PetscInt nPoints;
  PetscReal lambda, mu, nu;
  PetscReal eL, eM, eN;
  PetscReal Enp;
  const PetscScalar* ellPointsArray;
  PetscFunctionBegin;
  ierr = VecGetSize(ellPoints, &nPoints);CHKERRQ(ierr);
  nPoints = nPoints/3;

  ierr = VecGetArrayRead(ellPoints, &ellPointsArray); CHKERRQ(ierr);
  for(PetscInt k=0; k<nPoints; ++k) {
    lambda = ellPointsArray[3*k+0];
    mu     = ellPointsArray[3*k+1];
    nu     = ellPointsArray[3*k+2];
    if(mu < 0)
      signm = -1;
    else signm = 1;
    if(nu < 0)
      signn = -1;
    else signn = 1;
    ierr = calcLame(e, n, p, lambda, signm, signn, &eL);
    ierr = calcLame(e, n, p, mu, signm, signn, &eM);
    ierr = calcLame(e, n, p, nu, signm, signn, &eN);
    Enp = eL*eM*eN;
    ierr = VecSetValue(values, k, Enp, INSERT_VALUES); CHKERRQ(ierr);
  }
  ierr = VecAssemblyBegin(values); CHKERRQ(ierr);
  ierr = VecAssemblyEnd(values); CHKERRQ(ierr);
  ierr = VecRestoreArrayRead(ellPoints, &ellPointsArray); CHKERRQ(ierr);

  ierr = PetscLogFlops(3*nPoints);CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "CalcSolidExteriorHarmonic"
PetscErrorCode CalcSolidExteriorHarmonic(EllipsoidalSystem *e, PetscReal lambda, PetscReal mu, PetscReal nu, PetscInt n, PetscInt p, PetscReal *val)
{
  PetscErrorCode ierr;
  PetscReal Enp, Inp;
  PetscInt signm;
  PetscInt signn;
  PetscFunctionBegin;

  if(mu < 0)
    signm = -1;
  else signm = 1;
  if(nu < 0)
    signn = -1;
  else signn = 1;

  ierr = CalcSolidInteriorHarmonic(e, lambda, mu, nu, n, p, &Enp);CHKERRQ(ierr);
  ierr = calcI(e, n, p, lambda, signm, signn, &Inp);CHKERRQ(ierr);

  *val = (2*n + 1) * Enp * Inp;

  PetscLogFlops(4);CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "CalcSolidExteriorHarmonicVec"
PetscErrorCode CalcSolidExteriorHarmonicVec(EllipsoidalSystem* e, Vec ellPoints, PetscInt n, PetscInt p, Vec values)
{
  PetscErrorCode ierr;
  PetscReal Enp, Inp, Fnp;
  PetscReal lambda, mu, nu;
  PetscInt signm;
  PetscInt signn;
  PetscInt nPoints;
  const PetscScalar* ellPointsArray;
  PetscFunctionBegin;
  ierr = VecGetSize(ellPoints, &nPoints);CHKERRQ(ierr);
  nPoints = nPoints/3;

  ierr = VecGetArrayRead(ellPoints, &ellPointsArray);CHKERRQ(ierr);
  for(PetscInt k=0; k<nPoints; ++k) {
    lambda = ellPointsArray[3*k+0];
    mu = ellPointsArray[3*k+1];
    nu = ellPointsArray[3*k+2];

    if(mu < 0)
      signm = -1;
    else signm = 1;
    if(nu < 0)
      signn = -1;
    else signn = 1;

    ierr = CalcSolidInteriorHarmonic(e, lambda, mu, nu, n, p, &Enp);CHKERRQ(ierr);
    ierr = calcI(e, n, p, lambda, signm, signn, &Inp);CHKERRQ(ierr);
    Fnp = (2*n + 1) * Enp * Inp;
    ierr = VecSetValues(values, 1, &k, &Fnp, INSERT_VALUES);CHKERRQ(ierr);
  }
  ierr = VecAssemblyBegin(values);CHKERRQ(ierr); ierr = VecAssemblyEnd(values);CHKERRQ(ierr);
  ierr = VecRestoreArrayRead(ellPoints, &ellPointsArray);CHKERRQ(ierr);

  ierr = PetscLogFlops(4*nPoints);CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "CalcCoulombEllCoefs"
/*
  GnpVals should be uninitialized. I
*/
PetscErrorCode CalcCoulombEllCoefs(EllipsoidalSystem* e, PetscInt nSource, Vec srcPoints, Vec srcCharges, PetscInt Nmax, Vec* GnpVals)
{
  PetscErrorCode ierr;
  PetscReal normConstant;
  PetscReal Gnp;
  PetscInt count;

  Vec EnpVals;
  const PetscScalar* EnpValsArray;
  const PetscScalar* srcPointsArray;
  const PetscScalar* srcChargesArray;
  PetscFunctionBegin;

  ierr = VecCreateSeq(PETSC_COMM_SELF, nSource, &EnpVals);CHKERRQ(ierr);

  count = 0;
  for(PetscInt n=0; n<=Nmax; ++n) {
    for(PetscInt p=0; p<2*n+1; ++p)
      count++;
  }
  //ierr = VecDestroy(GnpVals);CHKERRQ(ierr);
  ierr = VecCreateSeq(PETSC_COMM_SELF, count, GnpVals);CHKERRQ(ierr);

  ierr = VecGetArrayRead(srcCharges, &srcChargesArray);CHKERRQ(ierr);
  count = 0;
  for(PetscInt n=0; n<=Nmax; ++n) {
    for(PetscInt p=0; p<2*n+1; ++p) {
      ierr = calcNormalization(e, n, p, &normConstant);CHKERRQ(ierr);

      ierr = CalcSolidInteriorHarmonicVec(e, srcPoints, n, p, EnpVals);

      ierr = VecGetArrayRead(EnpVals, &EnpValsArray);CHKERRQ(ierr);
      Gnp = 0;
      for(PetscInt k=0; k<nSource; ++k) {
	Gnp += srcChargesArray[k]*EnpValsArray[k];
      }
      ierr = VecRestoreArrayRead(EnpVals, &EnpValsArray);CHKERRQ(ierr);
      Gnp *= (4*PETSC_PI)/((2.0*n+1.0)*normConstant);
      ierr = VecSetValues(*GnpVals, 1, &count, &Gnp, INSERT_VALUES);CHKERRQ(ierr);
      count++;
    }
  }
  ierr = VecAssemblyBegin(*GnpVals);CHKERRQ(ierr); ierr = VecAssemblyEnd(*GnpVals);CHKERRQ(ierr);
  ierr = VecRestoreArrayRead(srcCharges, &srcChargesArray);CHKERRQ(ierr);

  ierr = VecDestroy(&EnpVals); CHKERRQ(ierr);

  ierr = PetscLogFlops(count * (6 + 3*nSource));CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "CalcReactAndExtCoefsFromCoulomb"
PetscErrorCode CalcReactAndExtCoefsFromCoulomb(EllipsoidalSystem* e, PetscReal eps1, PetscReal eps2, PetscInt Nmax, Vec coulCoefs, Vec reacCoefs, Vec extCoefs)
{
  PetscErrorCode ierr;
  PetscReal Ea   , Ia   , Fa;
  PetscReal EaDer, IaDer, FaDer;
  PetscReal Gnp, Bnp, Cnp;
  PetscReal temp;
  const PetscScalar* coulCoefsArray;
  PetscInt count;
  PetscFunctionBegin;

  ierr = VecGetArrayRead(coulCoefs, &coulCoefsArray);CHKERRQ(ierr);
  count = 0;
  for(PetscInt n=0; n<=Nmax; ++n) {
    for(PetscInt p=0; p<2*n+1; ++p) {

      Gnp = coulCoefsArray[count];

      ierr = calcLame(e, n, p, e->a, 1, 1, &Ea);CHKERRQ(ierr);
      ierr = calcI   (e, n, p, e->a, 1, 1, &Ia);CHKERRQ(ierr);
      Fa    = (2*n + 1) * Ea    * Ia;
      calcLameDerivative(e, n, p, e->a, 1, 1, &EaDer);
      calcIDerivative   (e, n, p, e->a, 1, 1, &IaDer);
      FaDer = (2*n + 1) * (Ea*IaDer + EaDer*Ia);

      temp = (Fa/Ea)*(eps1 - eps2)/(eps1*eps2);
      temp /= (1 - (eps1/eps2)*((EaDer*Fa)/(FaDer*Ea)));
      Bnp  = temp*Gnp;

      Cnp = (eps1/eps2)*(EaDer/FaDer)*Bnp;
      Cnp += (Gnp/eps2);

      ierr = VecSetValues(reacCoefs, 1, &count, &Bnp, INSERT_VALUES);CHKERRQ(ierr);
      ierr = VecSetValues(extCoefs, 1, &count, &Cnp, INSERT_VALUES);CHKERRQ(ierr);

      count++;
    }
  }

  ierr = VecAssemblyBegin(reacCoefs);CHKERRQ(ierr);
  ierr = VecAssemblyEnd  (reacCoefs);CHKERRQ(ierr);
  ierr = VecAssemblyBegin(extCoefs);CHKERRQ(ierr);
  ierr = VecAssemblyEnd  (extCoefs);CHKERRQ(ierr);
  ierr = VecRestoreArrayRead(coulCoefs, &coulCoefsArray);CHKERRQ(ierr);

  ierr = PetscLogFlops(29*count);
  PetscFunctionReturn(0);
}


#include <stdio.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_monte.h>
#include <gsl/gsl_monte_vegas.h>
#include "integral.c"
#include "timer.c"
#include "timer.h"
#define N 20
double g (double *t,size_t dim, void *params);

int main (void)
{
	double res, err;
	// 6 dimensional integral
	size_t dim = 6;

	int dims=6;
	//evaluating all dimensions from 0 to 1
	double x1[] = { 0., 0., 0. ,0.,0.,0. };
	double xu[] = { 1., 1., 1. ,1.,1.,1. };
	FILE *file;
	file=fopen("data.txt","w");

	double rmin=1.001;
	double rmax=4;
	double rtot=rmin;
	double rstep= (rmax-rmin)/(N);

	gsl_rng *r = gsl_rng_alloc (gsl_rng_taus2);
	unsigned long seed = 1UL;
	gsl_rng_set (r, seed);

	// 10^6 function evaluations per iteration

	size_t calls = 1000000;
	gsl_monte_function G= { &g, dim, &rtot};
	gsl_monte_vegas_state *sv = gsl_monte_vegas_alloc (dim);
	gsl_monte_vegas_init (sv);

	//Vegas integration
	double vegas[N];
	double rval[N];
	double di[N];
	timer_start ();
	for (int i = 0; i<=N-1; i++)
	{
		gsl_monte_vegas_integrate (&G,x1,xu,dim,calls/5,r,sv,&res,&err);
		do
		{
			gsl_monte_vegas_integrate (&G, x1,xu,dim,calls,r,sv,&res,&err);
			fflush (stdout);
		}

		while (fabs (gsl_monte_vegas_chisq (sv)-1.0) >0.2);
		rtot+=rstep;
		vegas[i]=res;
		rval[i]=rtot;
		di[i] = -.2 / pow (rtot,3.);
	}
	timer_stop ();
	double tvegas=timer_stop();
	gsl_monte_vegas_free (sv);
	rtot=rmin;
	printf (" %10.6f\n",tvegas);
	//homemade
	rtot=rmin;
	double x[6];

   	r = gsl_rng_alloc (gsl_rng_taus2);
   	gsl_rng_set (r, 1UL);
	double thome;
	long i, j;
	double r2=0.;
	double home[20];
	for (j = 0; j < N; j++)
   	{
		r2=0.;
       		for (i = 0; i < 1.e6; i++)
        	{
            		for (int k = 0; k <= dims; k++)
            		{
                		x[k] = gsl_rng_uniform (r) ;
            		}
		r2+= g(x,dim, &rtot);

        }
       res = r2/1.e6;
	fflush (stdout);
	rtot+=rstep;
	home [j]=res;
    }
	timer_stop();
	rtot=rmin;
	thome=timer_stop();
	printf (" %10.6f\n",thome);
	for (int t=0;t<N;t++)
        {
                fprintf (file, "%10.6f     %10.6f    %10.6f  %10.6f \n " , rval[t],fabs(-vegas[t]),fabs(-di[t]),fabs(-home[t]));
                rtot+=rstep;
        }

	return 0;
}


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

#include <config.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_interp.h>
#include <gsl/gsl_interp2d.h>

#define IDX2D(i, j, xsize, ysize) ((j) * (xsize) + (i))

static int
bilinear_init(void * state, const double xa[], const double ya[],
              const double za[], size_t xsize, size_t ysize)
{
  return GSL_SUCCESS;
}

static int
bilinear_eval(const void * state, const double xarr[], const double yarr[],
              const double zarr[], size_t xsize, size_t ysize,
              double x, double y, gsl_interp_accel * xa,
              gsl_interp_accel * ya, double * z)
{
  double xmin, xmax, ymin, ymax, zminmin, zminmax, zmaxmin, zmaxmax;
  double dx, dy;
  double t, u;
  size_t xi, yi;

  if (xa != NULL)
    xi = gsl_interp_accel_find(xa, xarr, xsize, x);
  else
    xi = gsl_interp_bsearch(xarr, x, 0, xsize - 1);

  if (ya != NULL)
    yi = gsl_interp_accel_find(ya, yarr, ysize, y);
  else
    yi = gsl_interp_bsearch(yarr, y, 0, ysize - 1);

  xmin = xarr[xi];
  xmax = xarr[xi + 1];
  ymin = yarr[yi];
  ymax = yarr[yi + 1];
  zminmin = zarr[IDX2D(xi, yi, xsize, ysize)];
  zminmax = zarr[IDX2D(xi, yi + 1, xsize, ysize)];
  zmaxmin = zarr[IDX2D(xi + 1, yi, xsize, ysize)];
  zmaxmax = zarr[IDX2D(xi + 1, yi + 1, xsize, ysize)];
  dx = xmax - xmin;
  dy = ymax - ymin;
  t = (x - xmin)/dx;
  u = (y - ymin)/dy;
  *z = (1.-t)*(1.-u)*zminmin + t*(1.-u)*zmaxmin + (1.-t)*u*zminmax + t*u*zmaxmax;

  return GSL_SUCCESS;
}

static int
bilinear_deriv_x(const void * state, const double xarr[],
                 const double yarr[], const double zarr[],
                 size_t xsize, size_t ysize, double x, double y,
                 gsl_interp_accel * xa, gsl_interp_accel * ya, double * z_p)
{
  double xmin, xmax, ymin, ymax, zminmin, zminmax, zmaxmin, zmaxmax;
  double dx, dy;
  double dt, u;
  size_t xi, yi;

  if (xa != NULL)
    xi = gsl_interp_accel_find(xa, xarr, xsize, x);
  else
    xi = gsl_interp_bsearch(xarr, x, 0, xsize - 1);

  if (ya != NULL)
    yi = gsl_interp_accel_find(ya, yarr, ysize, y);
  else
    yi = gsl_interp_bsearch(yarr, y, 0, ysize - 1);

  xmin = xarr[xi];
  xmax = xarr[xi + 1];
  ymin = yarr[yi];
  ymax = yarr[yi + 1];
  zminmin = zarr[IDX2D(xi, yi, xsize, ysize)];
  zminmax = zarr[IDX2D(xi, yi + 1, xsize, ysize)];
  zmaxmin = zarr[IDX2D(xi + 1, yi, xsize, ysize)];
  zmaxmax = zarr[IDX2D(xi + 1, yi + 1, xsize, ysize)];
  dx = xmax - xmin;
  dy = ymax - ymin;
  dt = 1./dx; /* partial t / partial x */
  u = (y - ymin)/dy;
  *z_p = dt*(-(1.-u)*zminmin + (1.-u)*zmaxmin - u*zminmax + u*zmaxmax);

  return GSL_SUCCESS;
}

static int
bilinear_deriv_y(const void * state, const double xarr[],
                 const double yarr[], const double zarr[],
                 size_t xsize, size_t ysize, double x, double y,
                 gsl_interp_accel * xa, gsl_interp_accel * ya, double * z_p)
{
  double xmin, xmax, ymin, ymax, zminmin, zminmax, zmaxmin, zmaxmax;
  double dx, dy;
  double t, du;
  size_t xi, yi;

  if (xa != NULL)
    xi = gsl_interp_accel_find(xa, xarr, xsize, x);
  else
    xi = gsl_interp_bsearch(xarr, x, 0, xsize - 1);

  if (ya != NULL)
    yi = gsl_interp_accel_find(ya, yarr, ysize, y);
  else
    yi = gsl_interp_bsearch(yarr, y, 0, ysize - 1);

  xmin = xarr[xi];
  xmax = xarr[xi + 1];
  ymin = yarr[yi];
  ymax = yarr[yi + 1];
  zminmin = zarr[IDX2D(xi, yi, xsize, ysize)];
  zminmax = zarr[IDX2D(xi, yi + 1, xsize, ysize)];
  zmaxmin = zarr[IDX2D(xi + 1, yi, xsize, ysize)];
  zmaxmax = zarr[IDX2D(xi + 1, yi + 1, xsize, ysize)];
  dx = xmax - xmin;
  dy = ymax - ymin;
  t = (x - xmin)/dx;
  du = 1./dy; /* partial u / partial y */
  *z_p = du*(-(1.-t)*zminmin - t*zmaxmin + (1.-t)*zminmax + t*zmaxmax);

  return GSL_SUCCESS;
}

static int
bilinear_deriv2(const void * state, const double xarr[],
                const double yarr[], const double zarr[],
                size_t xsize, size_t ysize, double x, double y,
                gsl_interp_accel * xa, gsl_interp_accel * ya, double * z_pp)
{
  *z_pp = 0.0;
  return GSL_SUCCESS;
}

static int
bilinear_derivxy(const void * state, const double xarr[],
                 const double yarr[], const double zarr[],
                 size_t xsize, size_t ysize, double x, double y,
                 gsl_interp_accel * xa, gsl_interp_accel * ya, double * z_pp)
{
  double xmin, xmax, ymin, ymax, zminmin, zminmax, zmaxmin, zmaxmax;
  double dx, dy;
  double dt, du;
  size_t xi, yi;

  if (xa != NULL)
    xi = gsl_interp_accel_find(xa, xarr, xsize, x);
  else
    xi = gsl_interp_bsearch(xarr, x, 0, xsize - 1);

  if (ya != NULL)
    yi = gsl_interp_accel_find(ya, yarr, ysize, y);
  else
    yi = gsl_interp_bsearch(yarr, y, 0, ysize - 1);

  xmin = xarr[xi];
  xmax = xarr[xi + 1];
  ymin = yarr[yi];
  ymax = yarr[yi + 1];
  zminmin = zarr[IDX2D(xi, yi, xsize, ysize)];
  zminmax = zarr[IDX2D(xi, yi + 1, xsize, ysize)];
  zmaxmin = zarr[IDX2D(xi + 1, yi, xsize, ysize)];
  zmaxmax = zarr[IDX2D(xi + 1, yi + 1, xsize, ysize)];
  dx = xmax - xmin;
  dy = ymax - ymin;
  dt = 1./dx; /* partial t / partial x */
  du = 1./dy; /* partial u / partial y */
  *z_pp = dt*du*(zminmin-zmaxmin-zminmax+zmaxmax);

  return GSL_SUCCESS;
}

static const gsl_interp2d_type bilinear_type = {
  "bilinear",
  2,
  NULL,
  &bilinear_init,
  &bilinear_eval,
  &bilinear_deriv_x,
  &bilinear_deriv_y,
  &bilinear_deriv2,
  &bilinear_derivxy,
  &bilinear_deriv2,
  NULL
};

const gsl_interp2d_type * gsl_interp2d_bilinear = &bilinear_type;

#undef IDX2D


/*
 * TableFunction.h
 *
 * Author:
 *       Oleg Kalashev
 *
 * Copyright (c) 2020 Institute for Nuclear Research, RAS
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

#ifndef TABLEFUNCTION_H_
#define TABLEFUNCTION_H_

#include <iostream>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>

#include "MathUtils.h"
#include "TableReader.h"

namespace Utils {

template<typename X = double >
class IScale
{
public:
	virtual X ToScale(X aX) = 0;
	virtual X FromScale(X aXprime) = 0;
	virtual IScale<X>* Clone() const = 0;
	virtual ~IScale(){}
};

template<typename X = double >
class LogScaleX : public IScale<X>
{
public:
	X ToScale(X aX);
	X FromScale(X aXprime);
	IScale<X>* Clone() const;
};

typedef LogScaleX<double> LogScale;

template<typename X = double >
class TableFunctionX : public SmartReferencedObj, virtual public FunctionX<X>{
public:
	static int FindLeftX(const std::vector<X>&, X xValue);
	TableFunctionX(const std::vector<X>& _x, const std::vector<X>& _y, X _leftVal=0., X _rightVal=0.);

	//takes ownership of aXscale and aYscale
	TableFunctionX(const std::vector<X>& _x, const std::vector<X>& _y, IScale<X>* aXscale, IScale<X>* aYscale, X _leftVal=0., X _rightVal=0.);

	//stores smart ref to aReader
	TableFunctionX(TableReaderX<X>* aReader, X _leftVal=0., X _rightVal=0.);

	bool InTableRange(X aArg) const;

	virtual X f(X _x) const;
	virtual X f_scaled(X _x) const = 0;
	virtual X Xmin() const;
	virtual X Xmax() const;

	void SetAutoLimits();
	void Print(std::ostream& aOut, X aXcoef=1., X aYcoef=1.);
private:
	//common part of constructors
	void Init();
protected:
	int FindLeftX(X xValue) const;
	//used for cloning
	TableFunctionX(const TableFunctionX<X>& aTableFunction);

	std::vector<X>			fCloneX;
	std::vector<X>			fCloneY;
	const std::vector<X>&	m_x;
	const std::vector<X>&	m_y;
	X						m_leftVal;
	X						m_rightVal;
	SafePtr<IScale<X> >		fXscale;
	SafePtr<IScale<X> >		fYscale;
	SmartPtr<TableReaderX<X> >	fTable;
	X						fXmin;
	X						fXmax;
};

typedef TableFunctionX<double> TableFunction;

template<typename X = double >
class LinearFuncX  : public TableFunctionX<X>
{
public:
	LinearFuncX(const std::vector<X>& _x,const  std::vector<X>& _y, X _leftVal=0., X _rightVal=0.):
	  TableFunctionX<X>(_x, _y, _leftVal, _rightVal)  {};

	//stores smart ref to aReader
	LinearFuncX(TableReaderX<X>* aReader, X _leftVal=0., X _rightVal=0.):
		TableFunctionX<X>(aReader,_leftVal,_rightVal) {};

	//takes ownership of aXscale and aYscale
	LinearFuncX(const std::vector<X>& _x,const std::vector<X>& _y, IScale<X>* aXscale, IScale<X>* aYscale, X _leftVal=0., X _rightVal=0.):
		TableFunctionX<X>(_x, _y, aXscale, aYscale, _leftVal, _rightVal) {}

	X f_scaled(X _x) const;
	FunctionX<X>* Clone() const { return new LinearFuncX<X>(*this); }
protected:
	LinearFuncX(const TableFunctionX<X>& aTableFunction):
		TableFunctionX<X>(aTableFunction) {};
};

typedef LinearFuncX<double> LinearFunc;

class GSLTableFunc  : public TableFunction
{
public:
	GSLTableFunc(const Vector& _x, const Vector& _y, const gsl_interp_type * aInterpType=gsl_interp_linear,
			double _leftVal=0., double _rightVal=0.):
	  TableFunction(_x, _y, _leftVal, _rightVal),fAcc(0),fSpline(0) {Init(aInterpType);}

	//stores smart ref to aReader
	GSLTableFunc(TableReader *aReader, const gsl_interp_type * aInterpType=gsl_interp_linear, double _leftVal=0., double _rightVal=0.):
		TableFunction(aReader,_leftVal,_rightVal),fAcc(0),fSpline(0) {Init(aInterpType);}

	//takes ownership of aXscale and aYscale
	//_x and _y are values are modified with scale functions given by aXscale and aYscale parameters
	GSLTableFunc(const Vector& _x, const Vector& _y, IScale<double>* aXscale, IScale<double>* aYscale, const gsl_interp_type * aInterpType=gsl_interp_linear, double _leftVal=0., double _rightVal=0.):
		TableFunction(_x, _y, aXscale, aYscale, _leftVal, _rightVal) {Init(aInterpType);}

	virtual ~GSLTableFunc();

	double f_scaled(double _x) const;
	Function* Clone() const { return new GSLTableFunc(*this); }
protected:
	GSLTableFunc(const GSLTableFunc& aTableFunction):
		TableFunction(aTableFunction) {Init(aTableFunction.fSpline->interp->type);};
private:
	void Init(const gsl_interp_type * aInterpType);
    gsl_interp_accel *fAcc;
    gsl_spline *fSpline;
    const gsl_interp_type *fInterpType;
};

template<typename X = double >
class MatrixFunctionX : public Function2X<X>
{
public:
	MatrixFunctionX(const std::string& aFile)
	{
		std::ifstream dataFile(aFile.c_str());
		if(!dataFile)
			Exception::Throw("Failed to open " + aFile);
		std::string header;
		std::getline(dataFile, header);
		int logscX,logscY;
		const char* headerFormat = "# minX=%lg stepX=%lg logscaleX=%d minY=%lg stepY=%lg logscaleY=%d";
		if(sscanf(header.c_str(),headerFormat,
				  &xMin,&xStep,&logscX,&yMin,&yStep,&logscY)!=6)
			Exception::Throw("Invalid header format in " + aFile + "\nExpected format: " + headerFormat);
		logScaleX = (bool)logscX;
		logScaleY = (bool)logscY;
		if((logScaleX && (xMin<=0 || xStep<=1.))||xStep<=0.)
			Exception::Throw("Invalid X scale in " + aFile);
		if((logScaleY && (yMin<=0 || yStep<=1.))||yStep<=0.)
			Exception::Throw("Invalid Y scale in " + aFile);
		fData = new TableReaderX<X>(dataFile, TableReader::Auto);
		nCols = fData->numberOfColumns();
		if(logScaleX) {
			xMax = xMin*pow(xStep,nCols-1);
			xStep = log(xStep);//convert multiplier to log step
		}
		else{
			xMax = xMin + xStep*(nCols-1);
		}
		nRows = fData->getColumn(0).size();
		if(logScaleY){
			yMax = yMin*pow(yStep,nRows-1);
			yStep = log(yStep);//convert multiplier to log step
		}
		else{
			yMax = yMin + yStep*(nRows-1);
		}
	}

	X f(double x, double y) const
	{
		double binX, binY;

		if(logScaleX)
		{//log scale
			binX = log(x/xMin)/xStep;
		}
		else
		{//linear scale
			binX = (x-xMin)/xStep;
		}
		if(binX<0. || binX>nCols-1)
			return 0.;
		if(logScaleY)
		{//log scale
			binY = log(y/yMin)/yStep;
		}
		else
		{//linear scale
			binY = (y-yMin)/yStep;
		}
		if(binY<0. || binY>nRows-1)
			return 0.;
		int iX = floor(binX);
		int iY = floor(binY);
		double weightX = 1.-binX+iX;
		double weightY = 1.-binY+iY;

		//linear f interpolation //TODO implement logscale f interpolation
		double f_11 = fData->getColumn(iX)[iY]*weightX*weightY;
		double f_21 = (weightX<1.)? (fData->getColumn(iX+1)[iY]*(1.-weightX)*weightY) : 0.;
		double f_12 = (weightY<1.)? (fData->getColumn(iX)[iY+1]*weightX*(1.-weightY)) : 0.;
		double f_22 = (weightX<1.&&weightY<1.)?(fData->getColumn(iX+1)[binY+1]*(1.-weightX)*(1.-weightY)) : 0.;
		double result = f_11 + f_21 + f_12 + f_22;
		return result;
	}

	virtual X MinArg(int aArgNo) const {return aArgNo==1?xMin:yMin;};
	virtual X MaxArg(int aArgNo) const {return aArgNo==1?xMax:yMax;}

private:
	SafePtr<TableReaderX<X> >  fData;
	X xMin;
	X yMin;
	X xMax;
	X yMax;
	X xStep;
	X yStep;
	int nCols;
	int nRows;
	bool logScaleX;
	bool logScaleY;
};

typedef MatrixFunctionX<double> MatrixFunction;

} /* namespace Utils */

#endif /* TABLEFUNCTION_H_ */


/***************************************************************************
    File                 : Fit.h
    Project              : QtiPlot
    --------------------------------------------------------------------
    Copyright            : (C) 2006 by Ion Vasilief
    Email (use @ for *)  : ion_vasilief*yahoo.fr
    Description          : Fit base class

 ***************************************************************************/

/***************************************************************************
 *                                                                         *
 *  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                                           *
 *                                                                         *
 ***************************************************************************/
#ifndef FIT_H
#define FIT_H

#include <QObject>

#include "../ApplicationWindow.h"
#include "Filter.h"

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

class Table;
class Matrix;

//! Fit base class
class Fit : public Filter
{
	Q_OBJECT

	public:

		typedef double (*fit_function_simplex)(const gsl_vector *, void *);
		typedef int (*fit_function)(const gsl_vector *, void *, gsl_vector *);
		typedef int (*fit_function_df)(const gsl_vector *, void *, gsl_matrix *);
		typedef int (*fit_function_fdf)(const gsl_vector *, void *, gsl_vector *, gsl_matrix *);

		enum Algorithm{ScaledLevenbergMarquardt, UnscaledLevenbergMarquardt, NelderMeadSimplex};
		enum WeightingMethod{NoWeighting, Instrumental, Statistical, Dataset};
        enum FitType{BuiltIn = 0, Plugin = 1, User = 2};

		Fit(ApplicationWindow *parent, Graph *g = 0, const QString& name = QString());
		Fit(ApplicationWindow *parent, Table *t, const QString& name = QString());
		~Fit();

		//! Actually does the fit. Should be reimplemented in derived classes.
		virtual void fit();
        virtual bool run(){fit(); return true;};

		//! Sets the data set to be used for weighting
		bool setWeightingData(WeightingMethod w, const QString& colName = QString::null);

		void setDataCurve(int curve, double start, double end);
		bool setDataFromTable(Table *t, const QString& xColName, const QString& yColName, int from = 1, int to = -1);

		QString resultFormula(){return d_result_formula;};
		QString formula(){return d_formula;};
		virtual void setFormula(const QString&){};

		int numParameters(){return d_p;};
		QStringList parameterNames(){return d_param_names;};
		virtual void setParametersList(const QStringList&){};
        void setParameterExplanations(const QStringList& lst){d_param_explain = lst;};

        double initialGuess(int parIndex){return gsl_vector_get(d_param_init, parIndex);};
		void setInitialGuess(int parIndex, double val){gsl_vector_set(d_param_init, parIndex, val);};
		void setInitialGuesses(double *x_init);

		virtual void guessInitialValues(){};

		void setParameterRange(int parIndex, double left, double right);
		void setAlgorithm(Algorithm s){d_solver = s;};

		//! Specifies weather the result of the fit is a function curve
		void generateFunction(bool yes, int points = 100);

		//! Output string added to the plot as a new legend
		virtual QString legendInfo();

		//! Returns a vector with the fit results
		double* results(){return d_results;};

		//! Returns a vector with the fit residuals
		double* residuals();

		//! Plot residuals and display data values in a column
		QwtPlotCurve* showResiduals();

		void showPredictionLimits(double confidenceLevel);
		void showConfidenceLimits(double confidenceLevel);
		//! Lower Confidence Limit
		double lcl(int parIndex, double confidenceLevel);
		//! Upper Confidence Limit
		double ucl(int parIndex, double confidenceLevel);

		//! Returns a vector with the standard deviations of the results
		double* errors();

		//! Returns the sum of squares of the residuals from the best-fit line
		double chiSquare() {return chi_2;};

		//! Returns R^2
		double rSquare();

		//! Returns adjusted R^2
		double adjustedRSquare(){return d_adjusted_r_square;};

		//! Returns the Residual Sum of Squares
		double rss(){return d_rss;};

		//! Returns the Root Mean Squared Error
		double rmse(){return sqrt(d_rss/(d_n - d_p));};

		//! Specifies wheather the errors must be scaled with sqrt(chi_2/dof)
		void scaleErrors(bool yes = true){d_scale_errors = yes;};

		Table* parametersTable(const QString& tableName);
		void writeParametersToTable(Table *t, bool append = false);

		Matrix* covarianceMatrix(const QString& matrixName);

        bool save(const QString& fileName);
        bool load(const QString& fileName);

        FitType type(){return d_fit_type;};
        void setType(FitType t){d_fit_type = t;};

        QString fileName(){return d_file_name;};
		void setFileName(const QString& fn){d_file_name = fn;};

        //! Frees the memory allocated for the X and Y data sets
        void freeMemory();

        //! Calculates the data for the output fit curve
        virtual double eval(double *, double){return 0.0;};

	private:
		void init();

		//! Pointer to the GSL multifit minimizer (for simplex algorithm)
		gsl_multimin_fminimizer * fitSimplex(gsl_multimin_function f, int &iterations, int &status);

		//! Pointer to the GSL multifit solver
		gsl_multifit_fdfsolver * fitGSL(gsl_multifit_function_fdf f, int &iterations, int &status);

		//! Customs and stores the fit results according to the derived class specifications. Used by exponential fits.
		virtual void customizeFitResults(){};

	protected:
		//! Allocates the memory for the fit workspace
		void initWorkspace(int par);
		//! Frees the memory allocated for the fit workspace
		void freeWorkspace();
		//! Adds the result curve as a FunctionCurve to the plot, if d_gen_function = true
		virtual FunctionCurve * insertFitFunctionCurve(const QString& name, double *x, double *y, int penWidth = 1);

		//! Adds the result curve to the plot
		virtual void generateFitCurve();

        //! Calculates the data for the output fit curve and store itin the X an Y vectors
		virtual void calculateFitCurveData(double *X, double *Y) {Q_UNUSED(X) Q_UNUSED(Y)};

		//! Output string added to the result log
		virtual QString logFitInfo(int iterations, int status);

		fit_function d_f;
		fit_function_df d_df;
		fit_function_fdf d_fdf;
		fit_function_simplex d_fsimplex;

		//! Number of fit parameters
		int d_p;

		//! Initial guesses for the fit parameters
		gsl_vector *d_param_init;

		/*! \brief Tells whether the fitter uses non-linear/simplex fitting
		 * with an initial parameters set, that must be freed in the destructor.
		 */
		bool is_non_linear;

		//! weighting data set used for the fit
		double *d_w;

		//! Names of the fit parameters
		QStringList d_param_names;

		//! Stores a list of short explanations for the significance of the fit parameters
		QStringList d_param_explain;

		//! Specifies weather the result curve is a FunctionCurve or a normal curve with the same x values as the fit data
		bool d_gen_function;

		//! Algorithm type
		Algorithm d_solver;

		//! The fit formula given on input
		QString d_formula;

		//! The result fit formula, where the fit parameters are replaced with the calculated values.
		QString d_result_formula;

		//! Covariance matrix
		gsl_matrix *covar;

		//! The kind of weighting to be performed on the data
		WeightingMethod d_weighting;

		//! The name of the weighting dataset
		QString weighting_dataset;

		//! Stores the result parameters
		double *d_results;

		//! Stores standard deviations of the result parameters
		double *d_errors;

		//! Stores fit residuals
		double *d_residuals;

		//! The sum of squares of the residuals from the best-fit line
		double chi_2;

		//! Residual sum of squares
		double d_rss;

		//! Adjusted R^2
		double d_adjusted_r_square;

		//! Specifies wheather the errors must be scaled with sqrt(chi_2/dof)
		bool d_scale_errors;

		//! Table window used for the output of fit parameters
		Table *d_param_table;

		//! Matrix window used for the output of covariance matrix
		Matrix *d_cov_matrix;

		FitType d_fit_type;

		//! Path of the XML file where the user stores the fit model
        QString d_file_name;

		//! Stores the left limits of the research interval for the result parameters
		double *d_param_range_left;

		//! Stores the right limits of the research interval for the result parameters
		double *d_param_range_right;
};

#endif


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

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

int gsl_cheb_calc_integ(gsl_cheb_series * integ, const gsl_cheb_series * f)
{
  const size_t n = f->order + 1;
  const double con = 0.25 * (f->b - f->a);

  if(integ->order != f->order)
    {
      GSL_ERROR ("order of chebyshev series must be equal", GSL_ENOMEM);
    }

  /* set the other parameters in the chebyshev struct */

  integ->a = f->a;
  integ->b = f->b;

  /* FIXME:  should probably set integ->f[] as well */

  if(n == 1) {
    integ->c[0] = 0.;
  }
  else if(n == 2) {
    integ->c[1] = con * f->c[0];
    integ->c[0] = 2.0 * integ->c[1];
  }
  else {
    double sum = 0.0;
    double fac = 1.0;
    size_t i;
    for(i=1; i<=n-2; i++) {
      integ->c[i] = con * (f->c[i-1] - f->c[i+1])/((double)i);
      sum += fac * integ->c[i];
      fac = -fac;
    }
    integ->c[n-1] = con * f->c[n-2]/(n-1.0);
    sum += fac * integ->c[n-1];
    integ->c[0] = 2.0 * sum;
  }

  return GSL_SUCCESS;
}