#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);
}


#ifndef FEODOROV_SQC_LAB1_GAUSS_H
#define FEODOROV_SQC_LAB1_GAUSS_H

#include <gsl/gsl_matrix.h>

#define EPSILON .0000001

#ifdef __cplusplus
extern "C" {
#endif

void gauss_init(size_t size, const double *d_matrix, const double *d_vector, gsl_matrix **matrix, gsl_vector **vector, gsl_vector **result);
int gauss(size_t size, gsl_matrix *matrix, gsl_vector *vector, gsl_vector *result, double *det);
void gauss_print(size_t size, gsl_vector *result);
int gauss_check(size_t size, const double *d_matrix, const double *d_vector, gsl_vector *result);

#ifdef __cplusplus
}
#endif

#endif //FEODOROV_SQC_LAB1_GAUSS_H


/* 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;
}


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

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_ieee_utils.h>
#include <gsl/gsl_permute_vector.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_permutation.h>

int test_LQ_solve_dim(const gsl_matrix * m, const double * actual, double eps);
int test_LQ_solve(void);
int test_LQ_LQsolve_dim(const gsl_matrix * m, const double * actual, double eps);
int test_LQ_LQsolve(void);
int test_LQ_lssolve_T_dim(const gsl_matrix * m, const double * actual, double eps);
int test_LQ_lssolve_T(void);
int test_LQ_lssolve_dim(const gsl_matrix * m, const double * actual, double eps);
int test_LQ_lssolve(void);
int test_LQ_decomp_dim(const gsl_matrix * m, double eps);
int test_LQ_decomp(void);
int test_PTLQ_solve_dim(const gsl_matrix * m, const double * actual, double eps);
int test_PTLQ_solve(void);
int test_PTLQ_LQsolve_dim(const gsl_matrix * m, const double * actual, double eps);
int test_PTLQ_LQsolve(void);
int test_PTLQ_decomp_dim(const gsl_matrix * m, double eps);
int test_PTLQ_decomp(void);
int test_LQ_update_dim(const gsl_matrix * m, double eps);
int test_LQ_update(void);

int
test_LQ_solve_dim(const gsl_matrix * m, const double * actual, double eps)
{
  int s = 0;
  unsigned long i, dim = m->size1;

  gsl_vector * rhs = gsl_vector_alloc(dim);
  gsl_matrix * lq  = gsl_matrix_alloc(dim,dim);
  gsl_vector * d = gsl_vector_alloc(dim);
  gsl_vector * x = gsl_vector_alloc(dim);

  gsl_matrix_transpose_memcpy(lq,m);
  for(i=0; i<dim; i++) gsl_vector_set(rhs, i, i+1.0);
  s += gsl_linalg_LQ_decomp(lq, d);
  s += gsl_linalg_LQ_solve_T(lq, d, rhs, x);
  for(i=0; i<dim; i++) {
    int foo = check(gsl_vector_get(x, i), actual[i], eps);
    if(foo) {
      printf("%3lu[%lu]: %22.18g   %22.18g\n", dim, i, gsl_vector_get(x, i), actual[i]);
    }
    s += foo;
  }

  gsl_vector_free(x);
  gsl_vector_free(d);
  gsl_matrix_free(lq);
  gsl_vector_free(rhs);

  return s;
}

int
test_LQ_solve(void)
{
  int f;
  int s = 0;

  f = test_LQ_solve_dim(hilb2, hilb2_solution, 2 * 8.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_solve hilbert(2)");
  s += f;

  f = test_LQ_solve_dim(hilb3, hilb3_solution, 2 * 64.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_solve hilbert(3)");
  s += f;

  f = test_LQ_solve_dim(hilb4, hilb4_solution, 4 * 1024.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_solve hilbert(4)");
  s += f;

  f = test_LQ_solve_dim(hilb12, hilb12_solution, 0.5);
  gsl_test(f, "  LQ_solve hilbert(12)");
  s += f;

  f = test_LQ_solve_dim(vander2, vander2_solution, 8.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_solve vander(2)");
  s += f;

  f = test_LQ_solve_dim(vander3, vander3_solution, 64.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_solve vander(3)");
  s += f;

  f = test_LQ_solve_dim(vander4, vander4_solution, 1024.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_solve vander(4)");
  s += f;

  f = test_LQ_solve_dim(vander12, vander12_solution, 0.05);
  gsl_test(f, "  LQ_solve vander(12)");
  s += f;

  return s;
}

int
test_LQ_LQsolve_dim(const gsl_matrix * m, const double * actual, double eps)
{
  int s = 0;
  unsigned long i, dim = m->size1;

  gsl_vector * rhs = gsl_vector_alloc(dim);
  gsl_matrix * lq  = gsl_matrix_alloc(dim,dim);
  gsl_matrix * q  = gsl_matrix_alloc(dim,dim);
  gsl_matrix * l  = gsl_matrix_alloc(dim,dim);
  gsl_vector * d = gsl_vector_alloc(dim);
  gsl_vector * x = gsl_vector_alloc(dim);

  gsl_matrix_transpose_memcpy(lq,m);
  for(i=0; i<dim; i++) gsl_vector_set(rhs, i, i+1.0);
  s += gsl_linalg_LQ_decomp(lq, d);
  s += gsl_linalg_LQ_unpack(lq, d, q, l);
  s += gsl_linalg_LQ_LQsolve(q, l, rhs, x);
  for(i=0; i<dim; i++) {
    int foo = check(gsl_vector_get(x, i), actual[i], eps);
    if(foo) {
      printf("%3lu[%lu]: %22.18g   %22.18g\n", dim, i, gsl_vector_get(x, i), actual[i]);
    }
    s += foo;
  }

  gsl_vector_free(x);
  gsl_vector_free(d);
  gsl_matrix_free(lq);
  gsl_matrix_free(q);
  gsl_matrix_free(l);
  gsl_vector_free(rhs);

  return s;
}

int
test_LQ_LQsolve(void)
{
  int f;
  int s = 0;

  f = test_LQ_LQsolve_dim(hilb2, hilb2_solution, 2 * 8.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_LQsolve hilbert(2)");
  s += f;

  f = test_LQ_LQsolve_dim(hilb3, hilb3_solution, 2 * 64.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_LQsolve hilbert(3)");
  s += f;

  f = test_LQ_LQsolve_dim(hilb4, hilb4_solution, 4 * 1024.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_LQsolve hilbert(4)");
  s += f;

  f = test_LQ_LQsolve_dim(hilb12, hilb12_solution, 0.5);
  gsl_test(f, "  LQ_LQsolve hilbert(12)");
  s += f;

  f = test_LQ_LQsolve_dim(vander2, vander2_solution, 8.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_LQsolve vander(2)");
  s += f;

  f = test_LQ_LQsolve_dim(vander3, vander3_solution, 64.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_LQsolve vander(3)");
  s += f;

  f = test_LQ_LQsolve_dim(vander4, vander4_solution, 1024.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_LQsolve vander(4)");
  s += f;

  f = test_LQ_LQsolve_dim(vander12, vander12_solution, 0.05);
  gsl_test(f, "  LQ_LQsolve vander(12)");
  s += f;

  return s;
}

int
test_LQ_lssolve_T_dim(const gsl_matrix * m, const double * actual, double eps)
{
  int s = 0;
  unsigned long i, M = m->size1, N = m->size2;

  gsl_vector * rhs = gsl_vector_alloc(M);
  gsl_matrix * lq  = gsl_matrix_alloc(N,M);
  gsl_vector * d = gsl_vector_alloc(N);
  gsl_vector * x = gsl_vector_alloc(N);
  gsl_vector * r = gsl_vector_alloc(M);
  gsl_vector * res = gsl_vector_alloc(M);

  gsl_matrix_transpose_memcpy(lq,m);
  for(i=0; i<M; i++) gsl_vector_set(rhs, i, i+1.0);
  s += gsl_linalg_LQ_decomp(lq, d);
  s += gsl_linalg_LQ_lssolve_T(lq, d, rhs, x, res);

  for(i=0; i<N; i++) {
    int foo = check(gsl_vector_get(x, i), actual[i], eps);
    if(foo) {
      printf("(%3lu,%3lu)[%lu]: %22.18g   %22.18g\n", M, N, i, gsl_vector_get(x, i), actual[i]);
    }
    s += foo;
  }

   /* compute residual r = b - m x */
  if (M == N) {
    gsl_vector_set_zero(r);
  } else {
    gsl_vector_memcpy(r, rhs);
    gsl_blas_dgemv(CblasNoTrans, -1.0, m, x, 1.0, r);
  };

  for(i=0; i<N; i++) {
    int foo = check(gsl_vector_get(res, i), gsl_vector_get(r,i), sqrt(eps));
    if(foo) {
      printf("(%3lu,%3lu)[%lu]: %22.18g   %22.18g\n", M, N, i, gsl_vector_get(res, i), gsl_vector_get(r,i));
    }
    s += foo;
  }

  gsl_vector_free(r);
  gsl_vector_free(res);
  gsl_vector_free(x);
  gsl_vector_free(d);
  gsl_matrix_free(lq);
  gsl_vector_free(rhs);

  return s;
}

int
test_LQ_lssolve_dim(const gsl_matrix * m, const double * actual, double eps)
{
  int s = 0;
  unsigned long i, M = m->size1, N = m->size2;

  gsl_vector * rhs = gsl_vector_alloc(M);
  gsl_matrix * lq  = gsl_matrix_alloc(M, N);
  gsl_vector * tau = gsl_vector_alloc(N);
  gsl_vector * x = gsl_vector_alloc(N);
  gsl_vector * r = gsl_vector_alloc(M);
  gsl_vector * res = gsl_vector_alloc(M);

  for (i = 0; i < M; i++)
    gsl_vector_set(rhs, i, i + 1.0);

  gsl_matrix_memcpy(lq, m);
  gsl_linalg_LQ_decomp(lq, tau);
  gsl_linalg_LQ_lssolve(lq, tau, rhs, x, res);

  for (i = 0; i < N; i++)
    {
      int foo = check(gsl_vector_get(x, i), actual[i], eps);
      if(foo) {
        printf("(%3lu,%3lu)[%lu]: %22.18g   %22.18g\n", M, N, i, gsl_vector_get(x, i), actual[i]);
      }
      s += foo;
    }

  /* compute residual r = b - m x */
  if (M == N) {
    gsl_vector_set_zero(r);
  } else {
    gsl_vector_memcpy(r, rhs);
    gsl_blas_dgemv(CblasNoTrans, -1.0, m, x, 1.0, r);
  };

  for (i = 0; i < N; i++)
    {
      int foo = check(gsl_vector_get(res, i), gsl_vector_get(r,i), sqrt(eps));
      if(foo) {
        printf("(%3lu,%3lu)[%lu]: %22.18g   %22.18g\n", M, N, i, gsl_vector_get(res, i), gsl_vector_get(r,i));
      }
      s += foo;
    }

  gsl_vector_free(r);
  gsl_vector_free(res);
  gsl_vector_free(x);
  gsl_vector_free(tau);
  gsl_matrix_free(lq);
  gsl_vector_free(rhs);

  return s;
}

int
test_LQ_lssolve_T(void)
{
  int f;
  int s = 0;

  f = test_LQ_lssolve_T_dim(m53, m53_lssolution, 2 * 64.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_lssolve_T m(5,3)");
  s += f;

  f = test_LQ_lssolve_T_dim(hilb2, hilb2_solution, 2 * 8.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_lssolve_T hilbert(2)");
  s += f;

  f = test_LQ_lssolve_T_dim(hilb3, hilb3_solution, 2 * 64.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_lssolve_T hilbert(3)");
  s += f;

  f = test_LQ_lssolve_T_dim(hilb4, hilb4_solution, 4 * 1024.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_lssolve_T hilbert(4)");
  s += f;

  f = test_LQ_lssolve_T_dim(hilb12, hilb12_solution, 0.5);
  gsl_test(f, "  LQ_lssolve_T hilbert(12)");
  s += f;

  f = test_LQ_lssolve_T_dim(vander2, vander2_solution, 8.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_lssolve_T vander(2)");
  s += f;

  f = test_LQ_lssolve_T_dim(vander3, vander3_solution, 64.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_lssolve_T vander(3)");
  s += f;

  f = test_LQ_lssolve_T_dim(vander4, vander4_solution, 1024.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_lssolve_T vander(4)");
  s += f;

  f = test_LQ_lssolve_T_dim(vander12, vander12_solution, 0.05);
  gsl_test(f, "  LQ_lssolve_T vander(12)");
  s += f;

  return s;
}

int
test_LQ_lssolve(void)
{
  int f;
  int s = 0;

  f = test_LQ_lssolve_dim(hilb2, hilb2_solution, 2 * 8.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_lssolve hilbert(2)");
  s += f;

  f = test_LQ_lssolve_dim(hilb3, hilb3_solution, 2 * 64.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_lssolve hilbert(3)");
  s += f;

  f = test_LQ_lssolve_dim(hilb4, hilb4_solution, 4 * 1024.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_lssolve hilbert(4)");
  s += f;

  f = test_LQ_lssolve_dim(hilb12, hilb12_solution, 0.5);
  gsl_test(f, "  LQ_lssolve hilbert(12)");
  s += f;

  f = test_LQ_lssolve_dim(vander2, vander2_solution, 8.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_lssolve vander(2)");
  s += f;

  f = test_LQ_lssolve_dim(vander3, vander3_solution, 64.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_lssolve vander(3)");
  s += f;

  f = test_LQ_lssolve_dim(vander4, vander4_solution, 1024.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_lssolve vander(4)");
  s += f;

  f = test_LQ_lssolve_dim(vander12, vander12_solution, 0.05);
  gsl_test(f, "  LQ_lssolve vander(12)");
  s += f;

  return s;
}

int
test_LQ_decomp_dim(const gsl_matrix * m, double eps)
{
  int s = 0;
  unsigned long i,j, M = m->size1, N = m->size2;

  gsl_matrix * lq = gsl_matrix_alloc(M,N);
  gsl_matrix * a  = gsl_matrix_alloc(M,N);
  gsl_matrix * q  = gsl_matrix_alloc(N,N);
  gsl_matrix * l  = gsl_matrix_alloc(M,N);
  gsl_vector * d = gsl_vector_alloc(GSL_MIN(M,N));

  gsl_matrix_memcpy(lq,m);

  s += gsl_linalg_LQ_decomp(lq, d);
  s += gsl_linalg_LQ_unpack(lq, d, q, l);

   /* compute a = q r */
  gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0, l, q, 0.0, a);

  for(i=0; i<M; i++) {
    for(j=0; j<N; j++) {
      double aij = gsl_matrix_get(a, i, j);
      double mij = gsl_matrix_get(m, i, j);
      int foo = check(aij, mij, eps);
      if(foo) {
        printf("(%3lu,%3lu)[%lu,%lu]: %22.18g   %22.18g\n", M, N, i,j, aij, mij);
      }
      s += foo;
    }
  }

  gsl_vector_free(d);
  gsl_matrix_free(lq);
  gsl_matrix_free(a);
  gsl_matrix_free(q);
  gsl_matrix_free(l);

  return s;
}

int
test_LQ_decomp(void)
{
  int f;
  int s = 0;

  f = test_LQ_decomp_dim(m35, 2 * 8.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_decomp m(3,5)");
  s += f;

  f = test_LQ_decomp_dim(m53, 2 * 64.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_decomp m(5,3)");
  s += f;

  f = test_LQ_decomp_dim(hilb2, 2 * 8.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_decomp hilbert(2)");
  s += f;

  f = test_LQ_decomp_dim(hilb3, 2 * 64.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_decomp hilbert(3)");
  s += f;

  f = test_LQ_decomp_dim(hilb4, 4 * 1024.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_decomp hilbert(4)");
  s += f;

  f = test_LQ_decomp_dim(hilb12, 2 * 1024.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_decomp hilbert(12)");
  s += f;

  f = test_LQ_decomp_dim(vander2, 8.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_decomp vander(2)");
  s += f;

  f = test_LQ_decomp_dim(vander3, 64.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_decomp vander(3)");
  s += f;

  f = test_LQ_decomp_dim(vander4, 1024.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_decomp vander(4)");
  s += f;

  f = test_LQ_decomp_dim(vander12, 0.0005);  /* FIXME: bad accuracy */
  gsl_test(f, "  LQ_decomp vander(12)");
  s += f;

  return s;
}

int
test_PTLQ_solve_dim(const gsl_matrix * m, const double * actual, double eps)
{
  int s = 0;
  int signum;
  unsigned long i, dim = m->size1;

  gsl_permutation * perm = gsl_permutation_alloc(dim);
  gsl_vector * rhs = gsl_vector_alloc(dim);
  gsl_matrix * lq  = gsl_matrix_alloc(dim,dim);
  gsl_vector * d = gsl_vector_alloc(dim);
  gsl_vector * x = gsl_vector_alloc(dim);
  gsl_vector * norm = gsl_vector_alloc(dim);

  gsl_matrix_transpose_memcpy(lq,m);
  for(i=0; i<dim; i++) gsl_vector_set(rhs, i, i+1.0);
  s += gsl_linalg_PTLQ_decomp(lq, d, perm, &signum, norm);
  s += gsl_linalg_PTLQ_solve_T(lq, d, perm, rhs, x);
  for(i=0; i<dim; i++) {
    int foo = check(gsl_vector_get(x, i), actual[i], eps);
    if(foo) {
      printf("%3lu[%lu]: %22.18g   %22.18g\n", dim, i, gsl_vector_get(x, i), actual[i]);
    }
    s += foo;
  }

  gsl_vector_free(norm);
  gsl_vector_free(x);
  gsl_vector_free(d);
  gsl_matrix_free(lq);
  gsl_vector_free(rhs);
  gsl_permutation_free(perm);

  return s;
}

int test_PTLQ_solve(void)
{
  int f;
  int s = 0;

  f = test_PTLQ_solve_dim(hilb2, hilb2_solution, 2 * 8.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  PTLQ_solve hilbert(2)");
  s += f;

  f = test_PTLQ_solve_dim(hilb3, hilb3_solution, 2 * 64.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  PTLQ_solve hilbert(3)");
  s += f;

  f = test_PTLQ_solve_dim(hilb4, hilb4_solution, 2 * 2048.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  PTLQ_solve hilbert(4)");
  s += f;

  f = test_PTLQ_solve_dim(hilb12, hilb12_solution, 0.5);
  gsl_test(f, "  PTLQ_solve hilbert(12)");
  s += f;

  f = test_PTLQ_solve_dim(vander2, vander2_solution, 8.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  PTLQ_solve vander(2)");
  s += f;

  f = test_PTLQ_solve_dim(vander3, vander3_solution, 64.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  PTLQ_solve vander(3)");
  s += f;

  f = test_PTLQ_solve_dim(vander4, vander4_solution, 1024.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  PTLQ_solve vander(4)");
  s += f;

  f = test_PTLQ_solve_dim(vander12, vander12_solution, 0.05);
  gsl_test(f, "  PTLQ_solve vander(12)");
  s += f;

  return s;
}

int
test_PTLQ_LQsolve_dim(const gsl_matrix * m, const double * actual, double eps)
{
  int s = 0;
  int signum;
  unsigned long i, dim = m->size1;

  gsl_permutation * perm = gsl_permutation_alloc(dim);
  gsl_vector * rhs = gsl_vector_alloc(dim);
  gsl_matrix * lq  = gsl_matrix_alloc(dim,dim);
  gsl_matrix * q  = gsl_matrix_alloc(dim,dim);
  gsl_matrix * l  = gsl_matrix_alloc(dim,dim);
  gsl_vector * d = gsl_vector_alloc(dim);
  gsl_vector * x = gsl_vector_alloc(dim);
  gsl_vector * norm = gsl_vector_alloc(dim);

  gsl_matrix_transpose_memcpy(lq,m);
  for(i=0; i<dim; i++) gsl_vector_set(rhs, i, i+1.0);
  s += gsl_linalg_PTLQ_decomp2(lq, q, l, d, perm, &signum, norm);
  s += gsl_linalg_PTLQ_LQsolve_T(q, l, perm, rhs, x);
  for(i=0; i<dim; i++) {
    int foo = check(gsl_vector_get(x, i), actual[i], eps);
    if(foo) {
      printf("%3lu[%lu]: %22.18g   %22.18g\n", dim, i, gsl_vector_get(x, i), actual[i]);
    }
    s += foo;
  }

  gsl_vector_free(norm);
  gsl_vector_free(x);
  gsl_vector_free(d);
  gsl_matrix_free(lq);
  gsl_vector_free(rhs);
  gsl_permutation_free(perm);

  return s;
}

int test_PTLQ_LQsolve(void)
{
  int f;
  int s = 0;

  f = test_PTLQ_LQsolve_dim(hilb2, hilb2_solution, 2 * 8.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  PTLQ_LQsolve hilbert(2)");
  s += f;

  f = test_PTLQ_LQsolve_dim(hilb3, hilb3_solution, 2 * 64.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  PTLQ_LQsolve hilbert(3)");
  s += f;

  f = test_PTLQ_LQsolve_dim(hilb4, hilb4_solution, 2 * 2048.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  PTLQ_LQsolve hilbert(4)");
  s += f;

  f = test_PTLQ_LQsolve_dim(hilb12, hilb12_solution, 0.5);
  gsl_test(f, "  PTLQ_LQsolve hilbert(12)");
  s += f;

  f = test_PTLQ_LQsolve_dim(vander2, vander2_solution, 8.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  PTLQ_LQsolve vander(2)");
  s += f;

  f = test_PTLQ_LQsolve_dim(vander3, vander3_solution, 64.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  PTLQ_LQsolve vander(3)");
  s += f;

  f = test_PTLQ_LQsolve_dim(vander4, vander4_solution, 1024.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  PTLQ_LQsolve vander(4)");
  s += f;

  f = test_PTLQ_LQsolve_dim(vander12, vander12_solution, 0.05);
  gsl_test(f, "  PTLQ_LQsolve vander(12)");
  s += f;

  return s;
}

int
test_PTLQ_decomp_dim(const gsl_matrix * m, double eps)
{
  int s = 0, signum;
  unsigned long i,j, M = m->size1, N = m->size2;

  gsl_matrix * lq = gsl_matrix_alloc(N,M);
  gsl_matrix * a  = gsl_matrix_alloc(N,M);
  gsl_matrix * q  = gsl_matrix_alloc(M,M);
  gsl_matrix * l  = gsl_matrix_alloc(N,M);
  gsl_vector * d = gsl_vector_alloc(GSL_MIN(M,N));
  gsl_vector * norm = gsl_vector_alloc(N);

  gsl_permutation * perm = gsl_permutation_alloc(N);

  gsl_matrix_transpose_memcpy(lq,m);

  s += gsl_linalg_PTLQ_decomp(lq, d, perm, &signum, norm);
  s += gsl_linalg_LQ_unpack(lq, d, q, l);

   /* compute a = l q */
  gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0, l, q, 0.0, a);

   /* Compute P LQ  by permuting the rows of LQ */

  for (i = 0; i < M; i++) {
    gsl_vector_view col = gsl_matrix_column (a, i);
    gsl_permute_vector_inverse (perm, &col.vector);
  }

  for(i=0; i<M; i++) {
    for(j=0; j<N; j++) {
      double aij = gsl_matrix_get(a, j, i);
      double mij = gsl_matrix_get(m, i, j);
      int foo = check(aij, mij, eps);
      if(foo) {
        printf("(%3lu,%3lu)[%lu,%lu]: %22.18g   %22.18g\n", M, N, i,j, aij, mij);
      }
      s += foo;
    }
  }

  gsl_permutation_free (perm);
  gsl_vector_free(norm);
  gsl_vector_free(d);
  gsl_matrix_free(lq);
  gsl_matrix_free(a);
  gsl_matrix_free(q);
  gsl_matrix_free(l);

  return s;
}

int test_PTLQ_decomp(void)
{
  int f;
  int s = 0;

  f = test_PTLQ_decomp_dim(m35, 2 * 8.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  PTLQ_decomp m(3,5)");
  s += f;

  f = test_PTLQ_decomp_dim(m53, 2 * 8.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  PTLQ_decomp m(5,3)");
  s += f;

  f = test_PTLQ_decomp_dim(s35, 2 * 8.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  PTLQ_decomp s(3,5)");
  s += f;

  f = test_PTLQ_decomp_dim(s53, 2 * 8.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  PTLQ_decomp s(5,3)");
  s += f;

  f = test_PTLQ_decomp_dim(hilb2, 2 * 8.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  PTLQ_decomp hilbert(2)");
  s += f;

  f = test_PTLQ_decomp_dim(hilb3, 2 * 64.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  PTLQ_decomp hilbert(3)");
  s += f;

  f = test_PTLQ_decomp_dim(hilb4, 2 * 1024.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  PTLQ_decomp hilbert(4)");
  s += f;

  f = test_PTLQ_decomp_dim(hilb12, 2 * 1024.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  PTLQ_decomp hilbert(12)");
  s += f;

  f = test_PTLQ_decomp_dim(vander2, 8.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  PTLQ_decomp vander(2)");
  s += f;

  f = test_PTLQ_decomp_dim(vander3, 64.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  PTLQ_decomp vander(3)");
  s += f;

  f = test_PTLQ_decomp_dim(vander4, 1024.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  PTLQ_decomp vander(4)");
  s += f;

  f = test_PTLQ_decomp_dim(vander12, 0.0005);  /* FIXME: bad accuracy */
  gsl_test(f, "  PTLQ_decomp vander(12)");
  s += f;

  return s;
}

int
test_LQ_update_dim(const gsl_matrix * m, double eps)
{
  int s = 0;
  unsigned long i,j, M = m->size1, N = m->size2;

  gsl_matrix * lq1  = gsl_matrix_alloc(N,M);
  gsl_matrix * lq2  = gsl_matrix_alloc(N,M);
  gsl_matrix * q1  = gsl_matrix_alloc(M,M);
  gsl_matrix * l1  = gsl_matrix_alloc(N,M);
  gsl_matrix * q2  = gsl_matrix_alloc(M,M);
  gsl_matrix * l2  = gsl_matrix_alloc(N,M);
  gsl_vector * d2 = gsl_vector_alloc(GSL_MIN(M,N));
  gsl_vector * u = gsl_vector_alloc(M);
  gsl_vector * v = gsl_vector_alloc(N);
  gsl_vector * w = gsl_vector_alloc(M);

  gsl_matrix_transpose_memcpy(lq1,m);
  gsl_matrix_transpose_memcpy(lq2,m);
  for(i=0; i<M; i++) gsl_vector_set(u, i, sin(i+1.0));
  for(i=0; i<N; i++) gsl_vector_set(v, i, cos(i+2.0) + sin(i*i+3.0));

  /* lq1 is updated */

  gsl_blas_dger(1.0, v, u, lq1);

  /* lq2 is first decomposed, updated later */

  s += gsl_linalg_LQ_decomp(lq2, d2);
  s += gsl_linalg_LQ_unpack(lq2, d2, q2, l2);

  /* compute w = Q^T u */

  gsl_blas_dgemv(CblasNoTrans, 1.0, q2, u, 0.0, w);

  /* now lq2 is updated */

  s += gsl_linalg_LQ_update(q2, l2, v, w);

  /* multiply q2*l2 */

  gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,l2,q2,0.0,lq2);

  /*  check lq1==lq2 */

  for(i=0; i<N; i++) {
    for(j=0; j<M; j++) {
      double s1 = gsl_matrix_get(lq1, i, j);
      double s2 = gsl_matrix_get(lq2, i, j);

      int foo = check(s1, s2, eps);
#if 0
      if(foo) {
	  printf("LQ:(%3lu,%3lu)[%lu,%lu]: %22.18g   %22.18g\n", M, N, i,j, s1, s2);
      }
#endif
      s += foo;
    }
  }

  gsl_vector_free(d2);
  gsl_vector_free(u);
  gsl_vector_free(v);
  gsl_vector_free(w);
  gsl_matrix_free(lq1);
  gsl_matrix_free(lq2);
  gsl_matrix_free(q1);
  gsl_matrix_free(l1);
  gsl_matrix_free(q2);
  gsl_matrix_free(l2);

  return s;
}

int
test_LQ_update(void)
{
  int f;
  int s = 0;

  f = test_LQ_update_dim(m35, 2 * 512.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_update m(3,5)");
  s += f;

  f = test_LQ_update_dim(m53, 2 * 2048.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_update m(5,3)");
  s += f;

  f = test_LQ_update_dim(hilb2,  2 * 512.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_update hilbert(2)");
  s += f;

  f = test_LQ_update_dim(hilb3,  2 * 512.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_update hilbert(3)");
  s += f;

  f = test_LQ_update_dim(hilb4, 2 * 1024.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_update hilbert(4)");
  s += f;

  f = test_LQ_update_dim(hilb12, 2 * 1024.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_update hilbert(12)");
  s += f;

  f = test_LQ_update_dim(vander2, 8.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_update vander(2)");
  s += f;

  f = test_LQ_update_dim(vander3, 64.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_update vander(3)");
  s += f;

  f = test_LQ_update_dim(vander4, 1024.0 * GSL_DBL_EPSILON);
  gsl_test(f, "  LQ_update vander(4)");
  s += f;

  f = test_LQ_update_dim(vander12, 0.0005);  /* FIXME: bad accuracy */
  gsl_test(f, "  LQ_update vander(12)");
  s += f;

  return s;
}


#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);
}


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

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

/* compute Gaussian filter by explicitely constructing window and computing weighted sum */
int
slow_gaussian(const gsl_filter_end_t etype, const double alpha, const size_t order, const gsl_vector * x,
              gsl_vector * y, const size_t K)
{
  const size_t n = x->size;
  const size_t H = K / 2;
  double *window = malloc(K * sizeof(double));
  double *kernel = malloc(K * sizeof(double));
  gsl_vector_view k = gsl_vector_view_array(kernel, K);
  size_t i;

  gsl_filter_gaussian_kernel(alpha, order, 1, &k.vector);

  for (i = 0; i < n; ++i)
    {
      size_t wsize = gsl_movstat_fill(etype, x, i, H, H, window);
      double sum = 0.0;
      size_t j;

      for (j = 0; j < wsize; ++j)
        sum += window[j] * kernel[wsize - j - 1];

      gsl_vector_set(y, i, sum);
    }

  free(window);
  free(kernel);

  return GSL_SUCCESS;
}

static void
fdiff(const gsl_vector * x, gsl_vector * dx)
{
  const size_t N = x->size;
  size_t i;

  for (i = 1; i < N - 1; ++i)
    {
      double xm1 = gsl_vector_get(x, i - 1);
      double xp1 = gsl_vector_get(x, i + 1);
      gsl_vector_set(dx, i, 0.5 * (xp1 - xm1));
    }

  gsl_vector_set(dx, 0, gsl_vector_get(x, 1) - gsl_vector_get(x, 0));
  gsl_vector_set(dx, N - 1, gsl_vector_get(x, N - 1) - gsl_vector_get(x, N - 2));
}

static void
test_gaussian_kernel(const double alpha, const size_t K)
{
  const size_t max_order = 3;
  gsl_vector * kernel = gsl_vector_alloc(K);
  gsl_vector * deriv = gsl_vector_alloc(K);
  gsl_vector * deriv_fd = gsl_vector_alloc(K);
  char buf[2048];
  size_t order;

  gsl_filter_gaussian_kernel(alpha, 0, 0, kernel);

  for (order = 1; order <= max_order; ++order)
    {
      gsl_filter_gaussian_kernel(alpha, order, 0, deriv);
      fdiff(kernel, deriv_fd);

      sprintf(buf, "gaussian kernel order=%zu alpha=%g K=%zu", order, alpha, K);
      compare_vectors(1.0e-2, deriv_fd, deriv, buf);

      gsl_vector_memcpy(kernel, deriv);
    }

  gsl_vector_free(kernel);
  gsl_vector_free(deriv);
  gsl_vector_free(deriv_fd);
}

static void
test_gaussian_proc(const double tol, const double alpha, const size_t order, const size_t n, const size_t K,
                   const gsl_filter_end_t etype, gsl_rng * rng_p)
{
  gsl_filter_gaussian_workspace * w = gsl_filter_gaussian_alloc(K);
  gsl_vector * x = gsl_vector_alloc(n);
  gsl_vector * y = gsl_vector_alloc(n);
  gsl_vector * z = gsl_vector_alloc(n);
  char buf[2048];

  random_vector(x, rng_p);

  /* y = filter(x) with slow brute force method */
  slow_gaussian(etype, alpha, order, x, y, K);

  /* y = filter(x) with fast method */
  gsl_filter_gaussian(etype, alpha, order, x, z, w);

  /* test y = z */
  sprintf(buf, "n=%zu K=%zu endtype=%u alpha=%g order=%zu gaussian random", n, K, etype, alpha, order);
  compare_vectors(tol, z, y, buf);

  /* z = filter(x) in-place */
  gsl_vector_memcpy(z, x);
  gsl_filter_gaussian(etype, alpha, order, z, z, w);

  sprintf(buf, "n=%zu K=%zu endtype=%u alpha=%g order=%zu gaussian random in-place", n, K, etype, alpha, order);
  compare_vectors(tol, z, y, buf);

  gsl_filter_gaussian_free(w);
  gsl_vector_free(x);
  gsl_vector_free(y);
  gsl_vector_free(z);
}

static void
test_gaussian_deriv(const double alpha, const size_t n, const size_t K)
{
#if 0
  const double f_low = 1.0;
  const double f_high = 50.0;
  const double gamma = 2.0 * M_PI / (n - 1.0);
  const double dt = 1.0 / (n - 1.0);
  gsl_vector *x = gsl_vector_alloc(n);
  gsl_vector *dx = gsl_vector_alloc(n);
  gsl_vector *y1 = gsl_vector_alloc(n);
  gsl_vector *y2 = gsl_vector_alloc(n);
  gsl_filter_gaussian_workspace *w = gsl_filter_gaussian_alloc(K);
  size_t i;

  /* make input signal composed of two sine waves at different frequencies */
  for (i = 0; i < n; ++i)
    {
      double xi = sin(gamma * f_low * i) + sin(gamma * f_high * i);
      double dxi = gamma * f_low * cos(gamma * f_low * i) +
                   gamma * f_high * cos(gamma * f_high * i);

      gsl_vector_set(x, i, xi);
      gsl_vector_set(dx, i, dxi);
    }

  /* compute y1 = G * dx(t)/dt */
  gsl_filter_gaussian(alpha, 0, dx, y1, w);

  /* compute y2 = dG/dt * x(t) */
  gsl_filter_gaussian(alpha, 1, x, y2, w);

  for (i = 0; i < n; ++i)
    {
      printf("%zu %.12e %.12e %.12e %.12e\n",
             i,
             gsl_vector_get(x, i),
             gsl_vector_get(dx, i),
             gsl_vector_get(y1, i),
             gsl_vector_get(y2, i));
    }

  gsl_vector_free(x);
  gsl_vector_free(dx);
  gsl_vector_free(y1);
  gsl_vector_free(y2);
  gsl_filter_gaussian_free(w);
#endif
}

static void
test_gaussian(gsl_rng * r)
{
  const double tol = 1.0e-10;
  size_t order;

  test_gaussian_kernel(3.0, 2001);

  for (order = 0; order <= 3; ++order)
    {
      test_gaussian_proc(tol, 2.5, order, 1000, 21, GSL_FILTER_END_PADZERO, r);
      test_gaussian_proc(tol, 3.0, order, 500, 11, GSL_FILTER_END_PADZERO, r);
      test_gaussian_proc(tol, 1.0, order, 50, 101, GSL_FILTER_END_PADZERO, r);
      test_gaussian_proc(tol, 2.0, order, 50, 11, GSL_FILTER_END_PADZERO, r);

      test_gaussian_proc(tol, 2.5, order, 1000, 21, GSL_FILTER_END_PADVALUE, r);
      test_gaussian_proc(tol, 3.0, order, 500, 11, GSL_FILTER_END_PADVALUE, r);
      test_gaussian_proc(tol, 1.0, order, 50, 101, GSL_FILTER_END_PADVALUE, r);
      test_gaussian_proc(tol, 2.0, order, 50, 11, GSL_FILTER_END_PADVALUE, r);

      test_gaussian_proc(tol, 2.5, order, 1000, 21, GSL_FILTER_END_TRUNCATE, r);
      test_gaussian_proc(tol, 3.0, order, 500, 11, GSL_FILTER_END_TRUNCATE, r);
      test_gaussian_proc(tol, 1.0, order, 50, 101, GSL_FILTER_END_TRUNCATE, r);
      test_gaussian_proc(tol, 2.0, order, 50, 11, GSL_FILTER_END_TRUNCATE, r);
    }
}


#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


/* multimin/test.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Fabrice Rossi
 *
 * 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.
 */

/* Modified by Tuomo Keskitalo to add Nelder Mead Simplex test suite */

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_multimin.h>
#include <gsl/gsl_ieee_utils.h>

#include "test_funcs.h"

unsigned int fcount, gcount;

int
test_fdf(const char * desc, gsl_multimin_function_fdf *f,
         initpt_function initpt, const gsl_multimin_fdfminimizer_type *T);

int
test_f(const char * desc, gsl_multimin_function *f, initpt_function initpt,
       const gsl_multimin_fminimizer_type *T);

int
main (void)
{
  gsl_ieee_env_setup ();

  {
    const gsl_multimin_fdfminimizer_type *fdfminimizers[6];
    const gsl_multimin_fdfminimizer_type ** T;

    fdfminimizers[0] = gsl_multimin_fdfminimizer_steepest_descent;
    fdfminimizers[1] = gsl_multimin_fdfminimizer_conjugate_pr;
    fdfminimizers[2] = gsl_multimin_fdfminimizer_conjugate_fr;
    fdfminimizers[3] = gsl_multimin_fdfminimizer_vector_bfgs;
    fdfminimizers[4] = gsl_multimin_fdfminimizer_vector_bfgs2;
    fdfminimizers[5] = 0;

    T = fdfminimizers;

    while (*T != 0)
      {
        test_fdf("Roth", &roth, roth_initpt,*T);
        test_fdf("Wood", &wood, wood_initpt,*T);
        test_fdf("Rosenbrock", &rosenbrock, rosenbrock_initpt,*T);
        T++;
      }

    T = fdfminimizers;

    while (*T != 0)
      {
        test_fdf("NRoth", &Nroth, roth_initpt,*T);
        test_fdf("NWood", &Nwood, wood_initpt,*T);
        test_fdf("NRosenbrock", &Nrosenbrock, rosenbrock_initpt,*T);
        T++;
      }
  }

  {
    const gsl_multimin_fminimizer_type *fminimizers[4];
    const gsl_multimin_fminimizer_type ** T;

    fminimizers[0] = gsl_multimin_fminimizer_nmsimplex;
    fminimizers[1] = gsl_multimin_fminimizer_nmsimplex2;
    fminimizers[2] = gsl_multimin_fminimizer_nmsimplex2rand;
    fminimizers[3] = 0;

    T = fminimizers;

    while (*T != 0)
      {
        test_f("Roth", &roth_fmin, roth_initpt,*T);
        test_f("Wood", &wood_fmin, wood_initpt,*T);
        test_f("Rosenbrock", &rosenbrock_fmin, rosenbrock_initpt,*T);
        test_f("Spring", &spring_fmin, spring_initpt,*T);
        T++;
      }
  }

  exit (gsl_test_summary());
}

int
test_fdf(const char * desc,
         gsl_multimin_function_fdf *f,
         initpt_function initpt,
         const gsl_multimin_fdfminimizer_type *T)
{
  int status;
  size_t iter = 0;
  double step_size;

  gsl_vector *x = gsl_vector_alloc (f->n);

  gsl_multimin_fdfminimizer *s;
  fcount = 0; gcount = 0;

  (*initpt) (x);

  step_size = 0.1 * gsl_blas_dnrm2 (x);

  s = gsl_multimin_fdfminimizer_alloc(T, f->n);

  gsl_multimin_fdfminimizer_set (s, f, x, step_size, 0.1);

#ifdef DEBUG
  printf("x "); gsl_vector_fprintf (stdout, s->x, "%g");
  printf("g "); gsl_vector_fprintf (stdout, s->gradient, "%g");
#endif

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

#ifdef DEBUG
      printf("%i: \n",iter);
      printf("x "); gsl_vector_fprintf (stdout, s->x, "%g");
      printf("g "); gsl_vector_fprintf (stdout, s->gradient, "%g");
      printf("f(x) %g\n",s->f);
      printf("dx %g\n",gsl_blas_dnrm2(s->dx));
      printf("\n");
#endif

      status = gsl_multimin_test_gradient(s->gradient,1e-3);
    }
  while (iter < 5000 && status == GSL_CONTINUE);

  status |= (fabs(s->f) > 1e-5);

  gsl_test(status, "%s, on %s: %i iters (fn+g=%d+%d), f(x)=%g",
           gsl_multimin_fdfminimizer_name(s),desc, iter, fcount, gcount, s->f);

  gsl_multimin_fdfminimizer_free(s);
  gsl_vector_free(x);

  return status;
}

int
test_f(const char * desc, gsl_multimin_function *f, initpt_function initpt,
       const gsl_multimin_fminimizer_type *T)
{
  int status;
  size_t i, iter = 0;

  gsl_vector *x = gsl_vector_alloc (f->n);

  gsl_vector *step_size = gsl_vector_alloc (f->n);

  gsl_multimin_fminimizer *s;

  fcount = 0; gcount = 0;
  (*initpt) (x);

  for (i = 0; i < f->n; i++)
    gsl_vector_set (step_size, i, 1);

  s = gsl_multimin_fminimizer_alloc(T, f->n);

  gsl_multimin_fminimizer_set (s, f, x, step_size);

#ifdef DEBUG
  printf("x "); gsl_vector_fprintf (stdout, s->x, "%g");
#endif

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

#ifdef DEBUG
      printf("%i: \n",iter);
      printf("x "); gsl_vector_fprintf (stdout, s->x, "%g");
      printf("f(x) %g\n", gsl_multimin_fminimizer_minimum (s));
      printf("size: %g\n", gsl_multimin_fminimizer_size (s));
      printf("\n");
#endif

      status = gsl_multimin_test_size (gsl_multimin_fminimizer_size (s),
                                       1e-3);
    }
  while (iter < 5000 && status == GSL_CONTINUE);

  status |= (fabs(s->fval) > 1e-5);

  gsl_test(status, "%s, on %s: %d iter (fn=%d), f(x)=%g",
           gsl_multimin_fminimizer_name(s),desc, iter, fcount, s->fval);

  gsl_multimin_fminimizer_free(s);
  gsl_vector_free(x);
  gsl_vector_free(step_size);

  return status;
}