/*
ODE: a program to get optime Runge-Kutta and multi-steps methods.

Copyright 2011-2019, Javier Burguete Tolosa.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

	1. Redistributions of source code must retain the above copyright notice,
		this list of conditions and the following disclaimer.

	2. Redistributions in binary form must reproduce the above copyright notice,
		this list of conditions and the following disclaimer in the
		documentation and/or other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY Javier Burguete Tolosa ``AS IS'' AND ANY EXPRESS OR
IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
SHALL Javier Burguete Tolosa OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

/**
 * \file rk_4_3.c
 * \brief Source file to optimize Runge-Kutta 4 steps 3rd order methods.
 * \author Javier Burguete Tolosa.
 * \copyright Copyright 2011-2019.
 */
#define _GNU_SOURCE
#include <string.h>
#include <math.h>
#include <libxml/parser.h>
#include <glib.h>
#include <libintl.h>
#include <gsl/gsl_rng.h>
#include "config.h"
#include "utils.h"
#include "optimize.h"
#include "rk.h"
#include "rk_4_3.h"

#define DEBUG_RK_4_3 0          ///< macro to debug.

/**
 * Function to obtain the coefficients of a 4 steps 3rd order Runge-Kutta
 * method.
 */
int
rk_tb_4_3 (Optimize * optimize) ///< Optimize struct.
{
  long double *tb, *r;
#if DEBUG_RK_4_3
  fprintf (stderr, "rk_tb_4_3: start\n");
#endif
  tb = optimize->coefficient;
  r = optimize->random_data;
  t4 (tb) = 1.L;
  t1 (tb) = r[0];
  t2 (tb) = r[1];
  b21 (tb) = r[2];
  t3 (tb) = r[3];
  b32 (tb) = r[4];
  b43 (tb) = r[5];
  b42 (tb) = ((1.L / 3.L - b43 (tb) * sqr (t3 (tb)))
              - t1 (tb) * (0.5L - b43 (tb) * t3 (tb)))
    / (t2 (tb) * (t2 (tb) - t1 (tb)));
  if (isnan (b42 (tb)))
    return 0;
  b41 (tb) = (0.5L - b42 (tb) * t2 (tb) - b43 (tb) * t3 (tb)) / t1 (tb);
  if (isnan (b41 (tb)))
    return 0;
  b31 (tb) = ((1.L / 6.L - b42 (tb) * b21 (tb) * t1 (tb)) / b43 (tb)
              - b32 (tb) * t2 (tb)) / t1 (tb);
  if (isnan (b31 (tb)))
    return 0;
  rk_b_4 (tb);
#if DEBUG_RK_4_3
  fprintf (stderr, "rk_tb_4_3: end\n");
#endif
  return 1;
}

/**
 * Function to obtain the coefficients of a 4 steps 3rd order, 4th order in
 * equations depending only in time, Runge-Kutta method.
 */
int
rk_tb_4_3t (Optimize * optimize)        ///< Optimize struct.
{
  long double *tb, *r;
#if DEBUG_RK_4_3
  fprintf (stderr, "rk_tb_4_3t: start\n");
#endif
  tb = optimize->coefficient;
  r = optimize->random_data;
  t4 (tb) = 1.L;
  t1 (tb) = r[0];
  t2 (tb) = r[1];
  b21 (tb) = r[2];
  t3 (tb) = r[3];
  b32 (tb) = r[4];
  b43 (tb) = (0.25L - 1.L / 3.L * t1 (tb)
              - (1.L / 3.L - 0.5L * t1 (tb)) * t2 (tb))
    / (t3 (tb) * (t3 (tb) - t2 (tb)) * (t3 (tb) - t1 (tb)));
  b42 (tb) = ((1.L / 3.L - b43 (tb) * sqr (t3 (tb)))
              - t1 (tb) * (0.5L - b43 (tb) * t3 (tb)))
    / (t2 (tb) * (t2 (tb) - t1 (tb)));
  b41 (tb) = (0.5L - b42 (tb) * t2 (tb) - b43 (tb) * t3 (tb)) / t1 (tb);
  b31 (tb) = ((1.L / 6.L - b42 (tb) * b21 (tb) * t1 (tb)) / b43 (tb)
              - b32 (tb) * t2 (tb)) / t1 (tb);
  rk_b_4 (tb);
#if DEBUG_RK_4_3
  fprintf (stderr, "rk_tb_4_3t: end\n");
#endif
  if (isnan (b31 (tb)) || isnan (b41 (tb)) || isnan (b42 (tb))
      || isnan (b43 (tb)))
    return 0;
  return 1;
}

/**
 * Function to obtain the coefficients of a 4 steps 2nd-3rd order Runge-Kutta
 * pair.
 */
int
rk_tb_4_3p (Optimize * optimize)        ///< Optimize struct.
{
  long double *tb;
#if DEBUG_RK_4_3
  fprintf (stderr, "rk_tb_4_3p: start\n");
#endif
  if (!rk_tb_4_3 (optimize))
    return 0;
  tb = optimize->coefficient;
  e41 (tb) = 0.5L / t1 (tb);
  e42 (tb) = 0.L;
  rk_e_4 (tb);
#if DEBUG_RK_4_3
  rk_print_e (optimize, "rk_tb_4_3p", stderr);
  fprintf (stderr, "rk_tb_4_3p: end\n");
#endif
  return 1;
}

/**
 * Function to obtain the coefficients of a 4 steps 2nd-3rd order, 3rd-4th order
 * in equations depending only in time, Runge-Kutta pair.
 */
int
rk_tb_4_3tp (Optimize * optimize)       ///< Optimize struct.
{
  long double *tb;
#if DEBUG_RK_4_3
  fprintf (stderr, "rk_tb_4_3tp: start\n");
#endif
  if (!rk_tb_4_3t (optimize))
    return 0;
  tb = optimize->coefficient;
  e42 (tb) = (1.L / 3.L - 0.5L * t1 (tb)) / (t2 (tb) * (t2 (tb) - t1 (tb)));
  e41 (tb) = (0.5L - e42 (tb) * t2 (tb)) / t1 (tb);
  rk_e_4 (tb);
#if DEBUG_RK_4_3
  rk_print_e (optimize, "rk_tb_4_3tp", stderr);
  fprintf (stderr, "rk_tb_4_3tp: end\n");
#endif
  if (isnan (e42 (tb)) || isnan (e41 (tb)))
    return 0;
  return 1;
}

/**
 * Function to calculate the objective function of a 4 steps 3rd order
 * Runge-Kutta method.
 *
 * \return objective function value.
 */
long double
rk_objective_tb_4_3 (RK * rk)   ///< RK struct.
{
  long double *tb;
  long double o;
#if DEBUG_RK_4_3
  fprintf (stderr, "rk_objective_tb_4_3: start\n");
#endif
  tb = rk->tb->coefficient;
  o = fminl (0.L, b20 (tb));
  if (b30 (tb) < 0.L)
    o += b30 (tb);
  if (b31 (tb) < 0.L)
    o += b31 (tb);
  if (b40 (tb) < 0.L)
    o += b40 (tb);
  if (b41 (tb) < 0.L)
    o += b41 (tb);
  if (b42 (tb) < 0.L)
    o += b42 (tb);
  if (o < 0.L)
    {
      o = 40.L - o;
      goto end;
    }
  o = 30.L + fmaxl (1.L, fmaxl (t1 (tb), fmaxl (t2 (tb), t3 (tb))));
  if (rk->strong)
    {
      rk_bucle_ac (rk);
      o = fminl (o, *rk->ac0->optimal);
    }
end:
#if DEBUG_RK_4_3
  fprintf (stderr, "rk_objective_tb_4_3: optimal=%Lg\n", o);
  fprintf (stderr, "rk_objective_tb_4_3: end\n");
#endif
  return o;
}

/**
 * Function to calculate the objective function of a 4 steps 3rd order, 4th
 * oder in equations depending only in time, Runge-Kutta method.
 *
 * \return objective function value.
 */
long double
rk_objective_tb_4_3t (RK * rk)  ///< RK struct.
{
  long double *tb;
  long double o;
#if DEBUG_RK_4_3
  fprintf (stderr, "rk_objective_tb_4_3t: start\n");
#endif
  tb = rk->tb->coefficient;
  o = fminl (0.L, b20 (tb));
  if (b30 (tb) < 0.L)
    o += b30 (tb);
  if (b31 (tb) < 0.L)
    o += b31 (tb);
  if (b40 (tb) < 0.L)
    o += b40 (tb);
  if (b41 (tb) < 0.L)
    o += b41 (tb);
  if (b42 (tb) < 0.L)
    o += b42 (tb);
  if (b43 (tb) < 0.L)
    o += b43 (tb);
  if (o < 0.L)
    {
      o = 40.L - o;
      goto end;
    }
  o = 30.L + fmaxl (1.L, fmaxl (t1 (tb), fmaxl (t2 (tb), t3 (tb))));
  if (rk->strong)
    {
      rk_bucle_ac (rk);
      o = fminl (o, *rk->ac0->optimal);
    }
end:
#if DEBUG_RK_4_3
  fprintf (stderr, "rk_objective_tb_4_3t: optimal=%Lg\n", o);
  fprintf (stderr, "rk_objective_tb_4_3t: end\n");
#endif
  return o;
}

/**
 * Function to calculate the objective function of a 4 steps 2nd-3rd order
 * Runge-Kutta pair.
 *
 * \return objective function value.
 */
long double
rk_objective_tb_4_3p (RK * rk)  ///< RK struct.
{
  long double *tb;
  long double o;
#if DEBUG_RK_4_3
  fprintf (stderr, "rk_objective_tb_4_3p: start\n");
#endif
  tb = rk->tb->coefficient;
  o = fminl (0.L, b20 (tb));
  if (b30 (tb) < 0.L)
    o += b30 (tb);
  if (b31 (tb) < 0.L)
    o += b31 (tb);
  if (b40 (tb) < 0.L)
    o += b40 (tb);
  if (b41 (tb) < 0.L)
    o += b41 (tb);
  if (b42 (tb) < 0.L)
    o += b42 (tb);
  if (e40 (tb) < 0.L)
    o += e40 (tb);
  if (o < 0.L)
    {
      o = 40.L - o;
      goto end;
    }
  o = 30.L + fmaxl (1.L, fmaxl (t1 (tb), fmaxl (t2 (tb), t3 (tb))));
  if (rk->strong)
    {
      rk_bucle_ac (rk);
      o = fminl (o, *rk->ac0->optimal);
    }
end:
#if DEBUG_RK_4_3
  fprintf (stderr, "rk_objective_tb_4_3p: optimal=%Lg\n", o);
  fprintf (stderr, "rk_objective_tb_4_3p: end\n");
#endif
  return o;
}

/**
 * Function to calculate the objective function of a 4 steps 2nd-3rd order,
 * 3rd-4th oder in equations depending only in time, Runge-Kutta pair.
 *
 * \return objective function value.
 */
long double
rk_objective_tb_4_3tp (RK * rk) ///< RK struct.
{
  long double *tb;
  long double o;
#if DEBUG_RK_4_3
  fprintf (stderr, "rk_objective_tb_4_3tp: start\n");
#endif
  tb = rk->tb->coefficient;
  o = fminl (0.L, b20 (tb));
  if (b30 (tb) < 0.L)
    o += b30 (tb);
  if (b31 (tb) < 0.L)
    o += b31 (tb);
  if (b40 (tb) < 0.L)
    o += b40 (tb);
  if (b41 (tb) < 0.L)
    o += b41 (tb);
  if (b42 (tb) < 0.L)
    o += b42 (tb);
  if (b43 (tb) < 0.L)
    o += b43 (tb);
  if (e40 (tb) < 0.L)
    o += e40 (tb);
  if (e41 (tb) < 0.L)
    o += e41 (tb);
  if (e42 (tb) < 0.L)
    o += e42 (tb);
  if (o < 0.L)
    {
      o = 40.L - o;
      goto end;
    }
  o = 30.L + fmaxl (1.L, fmaxl (t1 (tb), fmaxl (t2 (tb), t3 (tb))));
  if (rk->strong)
    {
      rk_bucle_ac (rk);
      o = fminl (o, *rk->ac0->optimal);
    }
end:
#if DEBUG_RK_4_3
  fprintf (stderr, "rk_objective_tb_4_3tp: optimal=%Lg\n", o);
  fprintf (stderr, "rk_objective_tb_4_3tp: end\n");
#endif
  return o;
}


/**
 * \author Sylvain Marsat, University of Maryland - NASA GSFC
 *
 * \brief C code headers for the implementation of the Fourier domain response for LIGO-VIRGO detectors
 *
 *
 */

#ifndef _LLVFDRESPONSE_H
#define _LLVFDRESPONSE_H

#define _XOPEN_SOURCE 500

#ifdef __GNUC__
#define UNUSED __attribute__ ((unused))
#else
#define UNUSED
#endif

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <complex.h>
#include <time.h>
#include <unistd.h>
#include <getopt.h>
#include <stdbool.h>
#include <string.h>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_bspline.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_min.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_complex.h>

#include "constants.h"
#include "LLVgeometry.h"
#include "struct.h"
#include "waveform.h"
#include "timeconversion.h"

/**************************************************/
/**************** Prototypes **********************/

/* Function to convert string input network string to Networktag */
Networktag ParseNetworktag(char* string);

/* Core function processing a signal (in the form of a list of modes) through the Fourier-domain LLV response (for a given detector), for given values of the inclination, position in the sky and polarization angle */
int LLVSimFDResponse(
  struct tagListmodesCAmpPhaseFrequencySeries **listhlm,  /* Input: list of modes in Frequency-domain amplitude and phase form as produced by the ROM */
  struct tagListmodesCAmpPhaseFrequencySeries **lists,    /* Output: list of contribution of each mode in the detector signal, in Frequency-domain amplitude and phase form, for the given detector and sky position */
  const double gpstime,                                   /* GPS time (s) when the signal at coalescence reaches geocenter */
  const double ra,                                        /* Position in the sky: J2000.0 right ascension (rad) */
  const double dec,                                       /* Position in the sky: J2000.0 declination (rad) */
  const double inclination,                               /* Inclination of the source (rad) */
  const double psi,                                       /* Polarization angle (rad) */
  const Detectortag tag);                                 /* Tag identifying the detector */

  /* Core function processing a signal (in the form of a list of modes) through the Fourier-domain LLV response, for given values of the inclination, position in the sky and polarization angle */
  int LLVSimFDResponse3Det(
    struct tagListmodesCAmpPhaseFrequencySeries **listDet1,  /* Output: list of contribution of each mode in Frequency-domain amplitude and phase form, in the detector 1 */
    struct tagListmodesCAmpPhaseFrequencySeries **listDet2,  /* Output: list of contribution of each mode in Frequency-domain amplitude and phase form, in the detector 2 */
    struct tagListmodesCAmpPhaseFrequencySeries **listDet3,  /* Output: list of contribution of each mode in Frequency-domain amplitude and phase form, in the detector 3 */
    struct tagListmodesCAmpPhaseFrequencySeries **list,      /* Input: list of modes in Frequency-domain amplitude and phase form as produced by the ROM */
    const double gpstime,                                    /* GPS time (s) when the signal at coalescence reaches geocenter */
    const double ra,                                            /* First angle for the position in the sky */
    const double dec,                                           /* Second angle for the position in the sky */
    const double inclination,                                   /* Inclination of the source */
    const double psi,                                           /* Polarization angle */
    const Networktag tag);                               /* Selector for the detector network */

/* Function setting the response matrix of a given detector, in cartesian coordinates */
void SetMatrixD(
  gsl_matrix* D,                       /* Output: matrix of the detector response Dij */
  const Detectortag tag);              /* Tag identifying the detector */
/* Function setting the position of a detector, in cartesian coordinates */
void SetVectorXd(
  gsl_vector* Xd,                      /* Output: position vector of the detector */
  const Detectortag tag);              /* Tag identifying the detector */

/* Function setting the cartesian coordinates of the wave frame vectors (X,Y,Z), given the position in the sky and polarization */
void SetVectorsXYZ(
  gsl_vector* X,                       /* Output: cartesian vector of the wave frame unit vector X */
  gsl_vector* Y,                       /* Output: cartesian vector of the wave frame unit vector Y */
  gsl_vector* Z,                       /* Output: cartesian vector of the wave frame unit vector Z */
  const double theta,                  /* First angle for the position in the sky (Earth-based spherical angle) */
  const double phi,                    /* Second angle for the position in the sky (Earth-based spherical angle) */
  const double psi);                   /* Polarization angle */

#if 0
{ /* so that editors will match succeeding brace */
#elif defined(__cplusplus)
}
#endif

#endif /* _LLVFDRESPONSE_H */


/* Dickman's rho function (to compute probability of success of ecm).

Copyright 2004, 2005, 2006, 2008, 2009, 2010, 2011, 2012, 2013
Alexander Kruppa, Paul Zimmermann.

This file is part of the ECM Library.

The ECM Library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 3 of the License, or (at your
option) any later version.

The ECM Library 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 Lesser General Public
License for more details.

You should have received a copy of the GNU Lesser General Public License
along with the ECM Library; see the file COPYING.LIB.  If not, see
http://www.gnu.org/licenses/ or write to the Free Software Foundation, Inc.,
51 Franklin St, Fifth Floor, Boston, MA 02110-1301, USA. */

/* define TESTDRIVE to compile rho as a stand-alone program, in which case
   you need to have libgsl installed */

#include "config.h"
#if defined(TESTDRIVE)
#define _ISOC99_SOURCE 1
#endif
#if defined(DEBUG_NUMINTEGRATE) || defined(TESTDRIVE)
# include <stdio.h>
#endif
#include <stdlib.h>
#include <math.h>
#if defined(TESTDRIVE)
#include <string.h>
#include "primegen.h"
#endif
#if defined(TESTDRIVE)
#include <gsl/gsl_math.h>
#include <gsl/gsl_sf_expint.h>
#include <gsl/gsl_integration.h>
#endif
#include "ecm-impl.h"

/* For Suyama's curves, we have a known torsion factor of 12 = 2^2*3^1, and
   an average extra exponent of 1/2 for 2, and 1/3 for 3 due to the probability
   that the group order divided by 12 is divisible by 2 or 3, thus on average
   we should have 2^2.5*3^1.333 ~ 24.5, however experimentally we have
   2^3.323*3^1.687 ~ 63.9 (see Alexander Kruppa's thesis, Table 5.1 page 96,
   row sigma=2, http://tel.archives-ouvertes.fr/tel-00477005/en/).
   The exp(ECM_EXTRA_SMOOTHNESS) value takes into account the extra
   smoothness with respect to a random number. */
#ifndef ECM_EXTRA_SMOOTHNESS
#define ECM_EXTRA_SMOOTHNESS 3.134
#endif

#define M_PI_SQR   9.869604401089358619 /* Pi^2 */
#define M_PI_SQR_6 1.644934066848226436 /* Pi^2/6 */
/* gsl_math.h defines M_EULER */
#ifndef M_EULER
#define M_EULER    0.577215664901532861
#endif
#define M_EULER_1   0.422784335098467139 /* 1 - Euler */

#ifndef MAX
#define MAX(x,y) ((x) > (y) ? (x) : (y))
#endif
#ifndef MIN
#define MIN(x,y) ((x) < (y) ? (x) : (y))
#endif

void rhoinit (int, int); /* used in stage2.c */

static double *rhotable = NULL;
static int invh = 0;
static double h = 0.;
static int tablemax = 0;
#if defined(TESTDRIVE)
#define PRIME_PI_MAX 10000
#define PRIME_PI_MAP(x) (((x)+1)/2)
/* The number of primes up to i. Use prime_pi[PRIME_PI_MAP(i)].
   Only correct for i >= 2. */
static unsigned int prime_pi[PRIME_PI_MAP(PRIME_PI_MAX)+1];
#endif

/* Fixme: need prime generating funcion without static state variables */
const unsigned char primemap[667] = {
  254, 223, 239, 126, 182, 219, 61, 249, 213, 79, 30, 243, 234, 166, 237, 158,
  230, 12, 211, 211, 59, 221, 89, 165, 106, 103, 146, 189, 120, 30, 166, 86,
  86, 227, 173, 45, 222, 42, 76, 85, 217, 163, 240, 159, 3, 84, 161, 248, 46,
  253, 68, 233, 102, 246, 19, 58, 184, 76, 43, 58, 69, 17, 191, 84, 140, 193,
  122, 179, 200, 188, 140, 79, 33, 88, 113, 113, 155, 193, 23, 239, 84, 150,
  26, 8, 229, 131, 140, 70, 114, 251, 174, 101, 146, 143, 88, 135, 210, 146,
  216, 129, 101, 38, 227, 160, 17, 56, 199, 38, 60, 129, 235, 153, 141, 81,
  136, 62, 36, 243, 51, 77, 90, 139, 28, 167, 42, 180, 88, 76, 78, 38, 246,
  25, 130, 220, 131, 195, 44, 241, 56, 2, 181, 205, 205, 2, 178, 74, 148, 12,
  87, 76, 122, 48, 67, 11, 241, 203, 68, 108, 36, 248, 25, 1, 149, 168, 92,
  115, 234, 141, 36, 150, 43, 80, 166, 34, 30, 196, 209, 72, 6, 212, 58, 47,
  116, 156, 7, 106, 5, 136, 191, 104, 21, 46, 96, 85, 227, 183, 81, 152, 8,
  20, 134, 90, 170, 69, 77, 73, 112, 39, 210, 147, 213, 202, 171, 2, 131, 97,
  5, 36, 206, 135, 34, 194, 169, 173, 24, 140, 77, 120, 209, 137, 22, 176, 87,
  199, 98, 162, 192, 52, 36, 82, 174, 90, 64, 50, 141, 33, 8, 67, 52, 182,
  210, 182, 217, 25, 225, 96, 103, 26, 57, 96, 208, 68, 122, 148, 154, 9, 136,
  131, 168, 116, 85, 16, 39, 161, 93, 104, 30, 35, 200, 50, 224, 25, 3, 68,
  115, 72, 177, 56, 195, 230, 42, 87, 97, 152, 181, 28, 10, 104, 197, 129,
  143, 172, 2, 41, 26, 71, 227, 148, 17, 78, 100, 46, 20, 203, 61, 220, 20,
  197, 6, 16, 233, 41, 177, 130, 233, 48, 71, 227, 52, 25, 195, 37, 10, 48,
  48, 180, 108, 193, 229, 70, 68, 216, 142, 76, 93, 34, 36, 112, 120, 146,
  137, 129, 130, 86, 38, 27, 134, 233, 8, 165, 0, 211, 195, 41, 176, 194, 74,
  16, 178, 89, 56, 161, 29, 66, 96, 199, 34, 39, 140, 200, 68, 26, 198, 139,
  130, 129, 26, 70, 16, 166, 49, 9, 240, 84, 47, 24, 210, 216, 169, 21, 6, 46,
  12, 246, 192, 14, 80, 145, 205, 38, 193, 24, 56, 101, 25, 195, 86, 147, 139,
  42, 45, 214, 132, 74, 97, 10, 165, 44, 9, 224, 118, 196, 106, 60, 216, 8,
  232, 20, 102, 27, 176, 164, 2, 99, 54, 16, 49, 7, 213, 146, 72, 66, 18, 195,
  138, 160, 159, 45, 116, 164, 130, 133, 120, 92, 13, 24, 176, 97, 20, 29, 2,
  232, 24, 18, 193, 1, 73, 28, 131, 48, 103, 51, 161, 136, 216, 15, 12, 244,
  152, 136, 88, 215, 102, 66, 71, 177, 22, 168, 150, 8, 24, 65, 89, 21, 181,
  68, 42, 82, 225, 179, 170, 161, 89, 69, 98, 85, 24, 17, 165, 12, 163, 60,
  103, 0, 190, 84, 214, 10, 32, 54, 107, 130, 12, 21, 8, 126, 86, 145, 1, 120,
  208, 97, 10, 132, 168, 44, 1, 87, 14, 86, 160, 80, 11, 152, 140, 71, 108,
  32, 99, 16, 196, 9, 228, 12, 87, 136, 11, 117, 11, 194, 82, 130, 194, 57,
  36, 2, 44, 86, 37, 122, 49, 41, 214, 163, 32, 225, 177, 24, 176, 12, 138,
  50, 193, 17, 50, 9, 197, 173, 48, 55, 8, 188, 145, 130, 207, 32, 37, 107,
  156, 48, 143, 68, 38, 70, 106, 7, 73, 142, 9, 88, 16, 2, 37, 197, 196, 66,
  90, 128, 160, 128, 60, 144, 40, 100, 20, 225, 3, 132, 81, 12, 46, 163, 138,
  164, 8, 192, 71, 126, 211, 43, 3, 205, 84, 42, 0, 4, 179, 146, 108, 66, 41,
  76, 131, 193, 146, 204, 28};

#ifdef TESTDRIVE
unsigned long
gcd (unsigned long a, unsigned long b)
{
  unsigned long t;

  while (b != 0)
    {
      t = a % b;
      a = b;
      b = t;
    }

  return a;
}

unsigned long
eulerphi (unsigned long n)
{
  unsigned long phi = 1, p;

  for (p = 2; p * p <= n; p += 2)
    {
      if (n % p == 0)
        {
          phi *= p - 1;
          n /= p;
          while (n % p == 0)
            {
              phi *= p;
              n /= p;
            }
        }

      if (p == 2)
        p--;
    }

  /* now n is prime */

  return (n == 1) ? phi : phi * (n - 1);
}

/* The number of positive integers up to x that have no prime factor up to y,
   for x >= y >= 2. Uses Buchstab's identity */
unsigned long
Buchstab_Phi(unsigned long x, unsigned long y)
{
  unsigned long p, s;
  primegen pg[1];

  if (x < 1)
    return 0;
  if (x <= y)
    return 1;
#if 0
  if (x < y^2)
    return(1 + primepi(x) - primepi (y)));
#endif

  s = 1;
  primegen_init (pg);
  primegen_skipto (pg, y + 1);
  for (p = primegen_next(pg); p <= x; p = primegen_next(pg))
    s += Buchstab_Phi(x / p, p - 1);
  return (s);
}

/* The number of positive integers up to x that have no prime factor
   greter than y, for x >= y >= 2. Uses Buchstab's identity */
unsigned long
Buchstab_Psi(const unsigned long x, const unsigned long y)
{
  unsigned long r, p;
  primegen pg[1];

  if (x <= y)
    return (x);

  if (y == 1UL)
    return (1);

  /* If y^2 > x, then
     Psi(x,y) = x - \sum_{y < p < x, p prime} floor(x/p)

     We separate the sum into ranges where floor(x/p) = k,
     which is x/(k+1) < p <= x/k.
     We also need to satisfy y < p, so we need k < x/y - 1,
     or k_max = ceil (x/y) - 2.
     The primes y < p <= x/(k_max + 1) are summed separately. */
  if (x <= PRIME_PI_MAX && x < y * y)
    {
      unsigned long kmax = x / y - 1;
      unsigned long s1, s2, k;

        s1 = (kmax + 1) * (prime_pi [PRIME_PI_MAP(x / (kmax + 1))] -
                           prime_pi [PRIME_PI_MAP(y)]);
        s2 = 0;
        for (k = 1; k <= kmax; k++)
          s2 += prime_pi[PRIME_PI_MAP(x / k)];
        s2 -= kmax * prime_pi [PRIME_PI_MAP(x / (kmax+1))];
        return (x - s1 - s2);
    }

  r = 1;
  primegen_init (pg);
  for (p = primegen_next(pg); p <= y; p = primegen_next(pg))
    r += Buchstab_Psi (x / p, p);
  return (r);
}

#endif /* TESTDRIVE */

#if defined(TESTDRIVE)
static double
Li (const double x)
{
  return (- gsl_sf_expint_E1 (- log(x)));
}
#endif

/*
  Evaluate dilogarithm via the sum
  \Li_{2}(z)=\sum_{k=1}^{\infty} \frac{z^k}{k^2},
  see http://mathworld.wolfram.com/Dilogarithm.html
  Assumes |z| <= 0.5, for which the sum converges quickly.
 */

static double
dilog_series (const double z)
{
  double r = 0.0, zk; /* zk = z^k */
  int k, k2; /* k2 = k^2 */
  /* Doubles have 53 bits in significand, with |z| <= 0.5 the k+1-st term
     is <= 1/(2^k k^2) of the result, so 44 terms should do */
  for (k = 1, k2 = 1, zk = z; k <= 44; k2 += 2 * k + 1, k++, zk *= z)
    r += zk / (double) k2;

  return r;
}

static double
dilog (double x)
{
  ASSERT(x <= -1.0); /* dilog(1-x) is called from rhoexact for 2 < x <= 3 */

  if (x <= -2.0)
    return -dilog_series (1./x) - M_PI_SQR_6 - 0.5 * log(-1./x) * log(-1./x);
  else /* x <= -1.0 */
    {
      /* L2(z) = -L2(1 - z) + 1/6 * Pi^2 - ln(1 - z)*ln(z)
         L2(z) = -L2(1/z) - 1/6 * Pi^2 - 0.5*ln^2(-1/z)
         ->
         L2(z) = -(-L2(1/(1-z)) - 1/6 * Pi^2 - 0.5*ln^2(-1/(1-z))) + 1/6 * Pi^2 - ln(1 - z)*ln(z)
               = L2(1/(1-z)) - 1/6 * Pi^2 + 0.5*ln(1 - z)^2 - ln(1 - z)*ln(-z)
         z in [-1, -2) -> 1/(1-z) in [1/2, 1/3)
      */
      double log1x = log (1. - x);
      return dilog_series (1. / (1. - x))
             - M_PI_SQR_6 + log1x * (0.5 * log1x - log (-x));
    }
}

#if 0
static double
L2 (double x)
{
  return log (x) * (1 - log (x-1)) + M_PI_SQR_6 - dilog (1 - x);
}
#endif

static double
rhoexact (double x)
{
  ASSERT(x <= 3.);
  if (x <= 0.)
    return 0.;
  else if (x <= 1.)
    return 1.;
  else if (x <= 2.)
    return 1. - log (x);
  else /* 2 < x <= 3 thus -2 <= 1-x < -1 */
    return 1. - log (x) * (1. - log (x - 1.)) + dilog (1. - x) + 0.5 * M_PI_SQR_6;
}

#if defined(TESTDRIVE)

/* The Buchstab omega(x) function, exact for x <= 4 where it can be
   evaluated without numerical integration, and approximated by
   exp(gamma) for larger x. */

static double
Buchstab_omega (const double x)
{
  /* magic = dilog(-1) + 1  = Pi^2/12 + 1 */
  const double magic = 1.82246703342411321824;

  if (x < 1.) return (0.);
  if (x <= 2.) return (1. / x);
  if (x <= 3.) return ((log (x - 1.) + 1.) / x);
  if (x <= 4.)
    return ((dilog(2. - x) + (1. + log(x - 2.)) * log(x - 1.) + magic) / x);

  /* If argument is out of range, return the limiting value for
     $x->\infty$: e^-gamma.
     For x only a little larger than 4., this has relative error 2.2e-6,
     for larger x the error rapidly drops further */

  return 0.56145948356688516982;
}

#endif

void
rhoinit (int parm_invh, int parm_tablemax)
{
  int i;

  if (parm_invh == invh && parm_tablemax == tablemax)
    return;

  if (rhotable != NULL)
    {
      free (rhotable);
      rhotable = NULL;
      invh = 0;
      h = 0.;
      tablemax = 0;
    }

  /* The integration below expects 3 * invh > 4 */
  if (parm_tablemax == 0 || parm_invh < 2)
    return;

  invh = parm_invh;
  h = 1. / (double) invh;
  tablemax = parm_tablemax;

  rhotable = (double *) malloc (parm_invh * parm_tablemax * sizeof (double));
  ASSERT_ALWAYS(rhotable != NULL);

  for (i = 0; i < (3 < parm_tablemax ? 3 : parm_tablemax) * invh; i++)
    rhotable[i] = rhoexact (i * h);

  for (i = 3 * invh; i < parm_tablemax * invh; i++)
    {
      /* rho(i*h) = 1 - \int_{1}^{i*h} rho(x-1)/x dx
                  = rho((i-4)*h) - \int_{(i-4)*h}^{i*h} rho(x-1)/x dx */

      rhotable[i] = rhotable[i - 4] - 2. / 45. * (
          7. * rhotable[i - invh - 4] / (double)(i - 4)
        + 32. * rhotable[i - invh - 3] / (double)(i - 3)
        + 12. * rhotable[i - invh - 2] / (double)(i - 2)
        + 32. * rhotable[i - invh - 1] / (double)(i - 1)
        + 7. * rhotable[i - invh]  / (double)i );
      if (rhotable[i] < 0.)
        {
#ifndef DEBUG_NUMINTEGRATE
          rhotable[i] = 0.;
#else
          printf (stderr, "rhoinit: rhotable[%d] = %.16f\n", i,
                   rhotable[i]);
          exit (EXIT_FAILURE);
#endif
        }
    }
}

/* assumes alpha < tablemax */
static double
dickmanrho (double alpha)
{
  ASSERT(alpha < tablemax);

  if (alpha <= 3.)
     return rhoexact (alpha);
  {
    int a = floor (alpha * invh);
    double rho1 = rhotable[a];
    double rho2 = (a + 1) < tablemax * invh ? rhotable[a + 1] : 0;
    return rho1 + (rho2 - rho1) * (alpha * invh - (double) a);
  }
}

#if 0
static double
dickmanrhosigma (double alpha, double x)
{
  if (alpha <= 0.)
    return 0.;
  if (alpha <= 1.)
    return 1.;
  if (alpha < tablemax)
    return dickmanrho (alpha) + M_EULER_1 * dickmanrho (alpha - 1.) / log (x);

  return 0.;
}

static double
dickmanrhosigma_i (int ai, double x)
{
  if (ai <= 0)
    return 0.;
  if (ai <= invh)
    return 1.;
  if (ai < tablemax * invh)
    return rhotable[ai] - M_EULER * rhotable[ai - invh] / log(x);

  return 0.;
}
#endif

static double
dickmanlocal (double alpha, double x)
{
  if (alpha <= 1.)
    return rhoexact (alpha);
  if (alpha < tablemax)
    return dickmanrho (alpha) - M_EULER * dickmanrho (alpha - 1.) / log (x);
  return 0.;
}

static double
dickmanlocal_i (int ai, double x)
{
  if (ai <= 0)
    return 0.;
  if (ai <= invh)
    return 1.;
  if (ai <= 2 * invh && ai < tablemax * invh)
    return rhotable[ai] - M_EULER / log (x);
  if (ai < tablemax * invh)
    {
      double logx = log (x);
      return rhotable[ai] - (M_EULER * rhotable[ai - invh]
             + M_EULER_1 * rhotable[ai - 2 * invh] / logx) / logx;
    }

  return 0.;
}

static int
isprime(unsigned long n)
{
  unsigned int r;

  if (n % 2 == 0)
    return (n == 2);
  if (n % 3 == 0)
    return (n == 3);
  if (n % 5 == 0)
    return (n == 5);

  if (n / 30 >= sizeof (primemap))
    abort();

  r = n % 30; /* 8 possible values: 1,7,11,13,17,19,23,29 */
  r = (r * 16 + r) / 64; /* maps the 8 values onto 0, ..., 7 */

  return ((primemap[n / 30] & (1 << r)) != 0);
}

static double
dickmanmu_sum (const unsigned long B1, const unsigned long B2,
	       const double x)
{
  double s = 0.;
  const double logB1 = 1. / log(B1);
  const double logx = log(x);
  unsigned long p;

  for (p = B1 + 1; p <= B2; p++)
    if (isprime(p))
      s += dickmanlocal ((logx - log(p)) * logB1, x / p) / p;

  return (s);
}

static double
dickmanmu (double alpha, double beta, double x)
{
  double a, b, sum;
  int ai, bi, i;
  ai = ceil ((alpha - beta) * invh);
  if (ai > tablemax * invh)
    ai = tablemax * invh;
  a = (double) ai * h;
  bi = floor ((alpha - 1.) * invh);
  if (bi > tablemax * invh)
    bi = tablemax * invh;
  b = (double) bi * h;
  sum = 0.;
  for (i = ai + 1; i < bi; i++)
    sum += dickmanlocal_i (i, x) / (alpha - i * h);
  sum += 0.5 * dickmanlocal_i (ai, x) / (alpha - a);
  sum += 0.5 * dickmanlocal_i (bi, x) / (alpha - b);
  sum *= h;
  sum += (a - alpha + beta) * 0.5 * (dickmanlocal_i (ai, x) / (alpha - a) + dickmanlocal (alpha - beta, x) / beta);
  sum += (alpha - 1. - b) * 0.5 * (dickmanlocal (alpha - 1., x) + dickmanlocal_i (bi, x) / (alpha - b));

  return sum;
}

static double
brentsuyama (double B1, double B2, double N, double nr)
{
  double a, alpha, beta, sum;
  int ai, i;
  alpha = log (N) / log (B1);
  beta = log (B2) / log (B1);
  ai = floor ((alpha - beta) * invh);
  if (ai > tablemax * invh)
    ai = tablemax * invh;
  a = (double) ai * h;
   sum = 0.;
  for (i = 1; i < ai; i++)
    sum += dickmanlocal_i (i, N) / (alpha - i * h) * (1 - exp (-nr * pow (B1, (-alpha + i * h))));
  sum += 0.5 * (1 - exp(-nr / pow (B1, alpha)));
  sum += 0.5 * dickmanlocal_i (ai, N) / (alpha - a) * (1 - exp(-nr * pow (B1, (-alpha + a))));
  sum *= h;
  sum += 0.5 * (alpha - beta - a) * (dickmanlocal_i (ai, N) / (alpha - a) + dickmanlocal (alpha - beta, N) / beta);

  return sum;
}

static double
brsudickson (double B1, double B2, double N, double nr, int S)
{
  int i, f;
  double sum;
  sum = 0;
  f = eulerphi (S) / 2;
  for (i = 1; i <= S / 2; i++)
      if (gcd (i, S) == 1)
        sum += brentsuyama (B1, B2, N, nr * (gcd (i - 1, S) + gcd (i + 1, S) - 4) / 2);

  return sum / (double)f;
}

static double
brsupower (double B1, double B2, double N, double nr, int S)
{
  int i, f;
  double sum;
  sum = 0;
  f = eulerphi (S);
  for (i = 1; i < S; i++)
      if (gcd (i, S) == 1)
        sum += brentsuyama (B1, B2, N, nr * (gcd (i - 1, S) - 2));

  return sum / (double)f;
}

/* Assume N is as likely smooth as a number around N/exp(delta) */

static double
prob (double B1, double B2, double N, double nr, int S, double delta)
{
  const double sumthresh = 20000.;
  double alpha, beta, stage1, stage2, brsu;
  const double effN = N / exp (delta);

  ASSERT(rhotable != NULL);

  /* What to do if rhotable is not initialised and asserting is not enabled?
     For now, bail out with 0. result. Not really pretty, either */
  if (rhotable == NULL)
    return 0.;

  if (B1 < 2. || N <= 1.)
    return 0.;

  if (effN <= B1)
    return 1.;

#ifdef TESTDRIVE
  printf ("B1 = %f, B2 = %f, N = %.0f, nr = %f, S = %d\n", B1, B2, N, nr, S);
#endif

  alpha = log (effN) / log (B1);
  stage1 = dickmanlocal (alpha, effN);
  stage2 = 0.;
  if (B2 > B1)
    {
      if (B1 < sumthresh)
	{
	  stage2 += dickmanmu_sum (B1, MIN(B2, sumthresh), effN);
	  beta = log (B2) / log (MIN(B2, sumthresh));
	}
      else
	beta = log (B2) / log (B1);

      if (beta > 1.)
	stage2 += dickmanmu (alpha, beta, effN);
    }
  brsu = 0.;
  if (S < -1)
    brsu = brsudickson (B1, B2, effN, nr, -S * 2);
  if (S > 1)
    brsu = brsupower (B1, B2, effN, nr, S * 2);

#ifdef TESTDRIVE
  printf ("stage 1 : %f, stage 2 : %f, Brent-Suyama : %f\n", stage1, stage2, brsu);
#endif

  return (stage1 + stage2 + brsu) > 0. ? (stage1 + stage2 + brsu) : 0.;
}

double
ecmprob (double B1, double B2, double N, double nr, int S)
{
  return prob (B1, B2, N, nr, S, ECM_EXTRA_SMOOTHNESS);
}

double
pm1prob (double B1, double B2, double N, double nr, int S, const mpz_t go)
{
  mpz_t cof;
  /* A prime power q^k divides p-1, p prime, with probability 1/(q^k-q^(k-1))
     not with probability 1/q^k as for random numbers. This is taken into
     account by the "smoothness" value here; a prime p-1 is about as likely
     smooth as a random number around (p-1)/exp(smoothness).
     smoothness = \sum_{q in Primes} log(q)/(q-1)^2 */
  double smoothness = 1.2269688;
  unsigned long i;

  if (go != NULL && mpz_cmp_ui (go, 1UL) > 0)
    {
      mpz_init (cof);
      mpz_set (cof, go);
      for (i = 2; i < 100; i++)
        if (mpz_divisible_ui_p (cof, i))
          {
            /* If we know that q divides p-1 with probability 1, we need to
               adjust the smoothness parameter */
            smoothness -= log ((double) i) / (double) ((i-1)*(i-1));
            /* printf ("pm1prob: Dividing out %lu\n", i); */
            while (mpz_divisible_ui_p (cof, i))
              mpz_tdiv_q_ui (cof, cof, i);
          }
      /* printf ("pm1prob: smoothness after dividing out go primes < 100: %f\n",
               smoothness); */
      return prob (B1, B2, N, nr, S, smoothness + log(mpz_get_d (cof)));
      mpz_clear (cof);
    }

  return prob (B1, B2, N, nr, S, smoothness);
}

#if defined(TESTDRIVE)

/* Compute probability for primes p == r (mod m) */

static double
pm1prob_rm (double B1, double B2, double N, double nr, int S, unsigned long r,
            unsigned long m)
{
  unsigned long cof;
  double smoothness = 1.2269688;
  unsigned long p;

  cof = m;

  for (p = 2UL; p < 100UL; p++)
    if (cof % p == 0UL) /* For each prime in m */
      {
        unsigned long cof_r, k, i;
        /* Divisibility by i is determined by r and m. We need to
           adjust the smoothness parameter. In P-1, we had estimated the
           expected value for the exponent of p as p/(p-1)^2. Undo that. */
        smoothness -= (double)p / ((p-1)*(p-1)) * log ((double) p);
        /* The expected value for the exponent of this prime is k s.t.
           p^k || r, plus 1/(p-1) if p^k || m as well */
        cof_r = gcd (r - 1UL, m);
        for (k = 0UL; cof_r % p == 0UL; k++)
          cof_r /= p;
        smoothness += k * log ((double) p);

        cof_r = m;
        for (i = 0UL; cof_r % p == 0UL; i++)
          cof_r /= p;

        if (i == k)
          smoothness += (1./(p - 1.) * log ((double) p));

        while (cof % p == 0UL)
          cof /= p;
        printf ("pm1prob_rm: p = %lu, k = %lu, i = %lu, new smoothness = %f\n",
                p, i, k, smoothness);
      }

  return prob (B1, B2, N, nr, S, smoothness);
}

/* The \Phi(x,y) function gives the number of natural numbers <= x
   that have no prime factor <= y, see Tenenbaum,
   "Introduction the analytical and probabilistic number theory", III.6.
   This function estimates the \Phi(x,y) function via eq. (48) of the 1st
   edition resp. equation (6.49) of the 3rd edition of Tenenbaum's book. */

static double
integrand1 (double x, double *y)
{
  return pow (*y, x) / x * log(x-1.);
}

static double
integrand2 (double v, double *y)
{
  return Buchstab_omega (v) * pow (*y, v);
}

/* Return approximate number of integers n with x1 < n <= x2
   that have no prime factor <= y */

double
no_small_prime (double x1, double x2, double y)
{
  double u1, u2;
  ASSERT (x1 >= 2.);
  ASSERT (x2 >= x1);
  ASSERT (y >= 2.);
  if (x1 == x2 || x2 <= y)
    return 0.;
  if (x1 < y)
    x1 = y;

  u1 = log(x1)/log(y);
  u2 = log(x2)/log(y);

   /* If no prime factors <= sqrt(x2), numbers must be a primes > y */
  if (x2 <= y*y)
    return (Li(x2) - Li(x1));

  if (u2 <= 3)
    {
      double r, abserr;
      size_t neval;
      gsl_function f;

      f.function = (double (*) (double, void *)) &integrand1;
      f.params = &y;

      /* intnum(v=1,u,buchstab(v)*y^v) */

      /* First part: intnum(v=u1, u, y^v/v*log(v-1.)) */
      gsl_integration_qng (&f, MAX(u1, 2.) , u2, 0., 0.001, &r, &abserr, &neval);

      /* Second part: intnum(v=u1, u2, y^v/v) = Li(x2) - Li(x1) */
      r += Li (x2) - Li (x1);

      return r;
    }

  {
    double r, abserr;
    size_t neval;
    gsl_function f;

    f.function = (double (*) (double, void *)) &integrand2;
    f.params = &y;

    gsl_integration_qng (&f, u1, u2, 0., 0.001, &r, &abserr, &neval);
    return r;
  }
}

static double
integrand3 (double p, double *param)
{
  const double x1 = param[0];
  const double x2 = param[1];
  const double y = param[2];

  return no_small_prime (x1 / p, x2 / p, y) / log(p);
}

double
no_small_prime_factor (const double x1, const double x2, const double y,
                       const double z1, const double z2)
{
  double r, abserr, param[3];
  size_t neval;
  gsl_function f;

  param[0] = x1;
  param[1] = x2;
  param[2] = y;
  f.function = (double (*) (double, void *)) &integrand3;
  f.params = &param;

  gsl_integration_qng (&f, z1, z2, 0., 0.01, &r, &abserr, &neval);

  return r;
}

#endif

#ifdef TESTDRIVE
int
main (int argc, char **argv)
{
  double B1, B2, N, nr, r, m;
  int S;
  unsigned long p, i, pi;
  primegen pg[1];

  primegen_init (pg);
  i = pi = 0;
  for (p = primegen_next (pg); p <= PRIME_PI_MAX; p = primegen_next (pg))
    {
      for ( ; i < p; i++)
        prime_pi[PRIME_PI_MAP(i)] = pi;
      pi++;
    }
  for ( ; i < p; i++)
    prime_pi[PRIME_PI_MAP(i)] = pi;

  if (argc < 2)
    {
      printf ("Usage: rho <B1> <B2> <N> <nr> <S> [<r> <m>]\n");
      return 1;
    }

  if (strcmp (argv[1], "-Buchstab_Phi") == 0)
    {
      unsigned long x, y, r;
      if (argc < 4)
        {
          printf ("-Buchstab_Phi needs x and y paramters\n");
          exit (EXIT_FAILURE);
        }
      x = strtoul (argv[2], NULL, 10);
      y = strtoul (argv[3], NULL, 10);
      r = Buchstab_Phi (x, y);
      printf ("Buchstab_Phi (%lu, %lu) = %lu\n", x, y, r);
      exit (EXIT_SUCCESS);
    }
  else if (strcmp (argv[1], "-Buchstab_Psi") == 0)
    {
      unsigned long x, y, r;
      if (argc < 4)
        {
          printf ("-Buchstab_Psi needs x and y paramters\n");
          exit (EXIT_FAILURE);
        }
      x = strtoul (argv[2], NULL, 10);
      y = strtoul (argv[3], NULL, 10);
      r = Buchstab_Psi (x, y);
      printf ("Buchstab_Psi (%lu, %lu) = %lu\n", x, y, r);
      exit (EXIT_SUCCESS);
    }
  else if (strcmp (argv[1], "-nsp") == 0)
    {
      double x1, x2, y, r;

      if (argc < 5)
        {
          printf ("-nsp needs x1, x2, and y paramters\n");
          exit (EXIT_FAILURE);
        }
      x1 = atof (argv[2]);
      x2 = atof (argv[3]);
      y = atof (argv[4]);
      r = no_small_prime (x1, x2, y);
      printf ("no_small_prime(%f, %f, %f) = %f\n", x1, x2, y, r);
      exit (EXIT_SUCCESS);
    }
  else if (strcmp (argv[1], "-nspf") == 0)
    {
      double x1, x2, y, z1, z2, r;

      if (argc < 7)
        {
          printf ("-nspf needs x1, x2, y, z1, and z2 paramters\n");
          exit (EXIT_FAILURE);
        }
      x1 = atof (argv[2]);
      x2 = atof (argv[3]);
      y = atof (argv[4]);
      z1 = atof (argv[5]);
      z2 = atof (argv[6]);
      r = no_small_prime_factor (x1, x2, y, z1, z2);
      printf ("no_small_prime(%f, %f, %f, %f, %f) = %f\n", x1, x2, y, z1, z2, r);
      exit (EXIT_SUCCESS);
    }

  if (argc < 6)
    {
      printf ("Need 5 or 7 arguments: B1 B2 N nr S [r m]\n");
      exit (EXIT_FAILURE);
    }

  B1 = atof (argv[1]);
  B2 = atof (argv[2]);
  N = atof (argv[3]);
  nr = atof (argv[4]);
  S = atoi (argv[5]);
  r = 0; m = 1;
  if (argc > 7)
    {
      r = atoi (argv[6]);
      m = atoi (argv[7]);
    }

  rhoinit (256, 10);
  if (N < 50.)
    {
      double sum;
      sum = ecmprob(B1, B2, exp2 (N), nr, S);
      sum += 4. * ecmprob(B1, B2, 3./2. * exp2 (N), nr, S);
      sum += ecmprob(B1, B2, 2. * exp2 (N), nr, S);
      sum *= 1./6.;
      printf ("ECM: %.16f\n", sum);

      sum = pm1prob_rm (B1, B2, exp2 (N), nr, S, r, m);
      sum += 4. * pm1prob_rm (B1, B2, 3./2. * exp2 (N), nr, S, r, m);
      sum += pm1prob_rm (B1, B2, 2. * exp2 (N), nr, S, r, m);
      sum *= 1./6.;
      printf ("P-1: %.16f\n", sum);
    }
  else
    {
      printf ("ECM: %.16f\n", ecmprob(B1, B2, N, nr, S));
      printf ("P-1: %.16f\n", pm1prob_rm (B1, B2, N, nr, S, r, m));
    }
  rhoinit (0, 0);
  return 0;
}
#endif


#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <gsl/gsl_sf_legendre.h>
#include <gsl/gsl_sf_bessel.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_interp.h>
#include <gsl/gsl_spline.h>
/************************************************************************************************************************************************************************/
/************************************************************************************************************************************************************************/

                        //VARIABLE GLOBALE
int size = 100;
int limit = 100;
gsl_interp_accel *acc[25];
gsl_spline *spline[25];
double	fg,										// growth factor
			 	b,											//Bias factor
			 	smin, 									// Minimum of s range
			 	smax, 									// Maximum of s range
			 	sigv, 									// Value of Sigma_v (define in sigma_v function)
			 	sig8, 									// Value of sigma 8 (define in main function)
				kmin,   								// minimum k in the power spectrum file (define in main function)
			 	kmax, 									// minimum k in the power spectrum file (define in main function)
			 	lk, 									    // lk = kmax-kmin lenth for the integral calculation (define in main function)
			 	f1,										//Bias factor CLPT
			 	f2,										//Bias factor CLPT
				sig_shift;
	const int val=160;									// Nb of lines in the files Xi_r_CLPT, Sigma_CLPT and V12_CLPT
struct my_f_params { double a; double b; };

/*********************************************************************************************************************************************************************/
/*******************\\ Gauss-Legendre integral quadrature\\***********************************************************************************************************/

static const double x[] = {
    1.56289844215430828714e-02,    4.68716824215916316162e-02,
    7.80685828134366366918e-02,    1.09189203580061115002e-01,
    1.40203137236113973212e-01,    1.71080080538603274883e-01,
    2.01789864095735997236e-01,    2.32302481844973969643e-01,
    2.62588120371503479163e-01,    2.92617188038471964730e-01,
    3.22360343900529151720e-01,    3.51788526372421720979e-01,
    3.80872981624629956772e-01,    4.09585291678301542532e-01,
    4.37897402172031513100e-01,    4.65781649773358042251e-01,
    4.93210789208190933576e-01,    5.20158019881763056670e-01,
    5.46597012065094167460e-01,    5.72501932621381191292e-01,
    5.97847470247178721259e-01,    6.22608860203707771585e-01,
    6.46761908514129279840e-01,    6.70283015603141015784e-01,
    6.93149199355801965946e-01,    7.15338117573056446485e-01,
    7.36828089802020705530e-01,    7.57598118519707176062e-01,
    7.77627909649495475605e-01,    7.96897892390314476375e-01,
    8.15389238339176254384e-01,    8.33083879888400823522e-01,
    8.49964527879591284320e-01,    8.66014688497164623416e-01,
    8.81218679385018415547e-01,    8.95561644970726986709e-01,
    9.09029570982529690453e-01,    9.21609298145333952679e-01,
    9.33288535043079545942e-01,    9.44055870136255977955e-01,
    9.53900782925491742847e-01,    9.62813654255815527284e-01,
    9.70785775763706331929e-01,    9.77809358486918288561e-01,
    9.83877540706057015509e-01,    9.88984395242991747997e-01,
    9.93124937037443459632e-01,    9.96295134733125149166e-01,
    9.98491950639595818382e-01,    9.99713726773441233703e-01
};

static const double A[] = {
    3.12554234538633569472e-02,    3.12248842548493577326e-02,
    3.11638356962099067834e-02,    3.10723374275665165874e-02,
    3.09504788504909882337e-02,    3.07983790311525904274e-02,
    3.06161865839804484966e-02,    3.04040795264548200160e-02,
    3.01622651051691449196e-02,    2.98909795933328309169e-02,
    2.95904880599126425122e-02,    2.92610841106382766198e-02,
    2.89030896011252031353e-02,    2.85168543223950979908e-02,
    2.81027556591011733175e-02,    2.76611982207923882944e-02,
    2.71926134465768801373e-02,    2.66974591835709626611e-02,
    2.61762192395456763420e-02,    2.56294029102081160751e-02,
    2.50575444815795897034e-02,    2.44612027079570527207e-02,
    2.38409602659682059633e-02,    2.31974231852541216230e-02,
    2.25312202563362727021e-02,    2.18430024162473863146e-02,
    2.11334421125276415432e-02,    2.04032326462094327666e-02,
    1.96530874944353058650e-02,    1.88837396133749045537e-02,
    1.80959407221281166640e-02,    1.72904605683235824399e-02,
    1.64680861761452126430e-02,    1.56296210775460027242e-02,
    1.47758845274413017686e-02,    1.39077107037187726882e-02,
    1.30259478929715422855e-02,    1.21314576629794974079e-02,
    1.12251140231859771176e-02,    1.03078025748689695861e-02,
    9.38041965369445795116e-03,    8.44387146966897140266e-03,
    7.49907325546471157895e-03,    6.54694845084532276405e-03,
    5.58842800386551515727e-03,    4.62445006342211935096e-03,
    3.65596120132637518238e-03,    2.68392537155348241939e-03,
    1.70939265351810523958e-03,    7.34634490505671730396e-04
};

#define NUM_OF_POSITIVE_ZEROS  sizeof(x) / sizeof(double)
#define NUM_OF_ZEROS           NUM_OF_POSITIVE_ZEROS+NUM_OF_POSITIVE_ZEROS

double
  Gauss_Legendre_Integration2_100pts(double a, double b, double (*f)(double, void *), void *prms)
{
   double integral = 0.0;
   double c = 0.5 * (b - a);
   double d = 0.5 * (b + a);
   double dum;
   const double *px = &x[NUM_OF_POSITIVE_ZEROS - 1];
   const double *pA = &A[NUM_OF_POSITIVE_ZEROS - 1];

   for (; px >= x; pA--, px--) {
      dum = c * *px;
      integral += *pA * ( (*f)(d - dum,prms) + (*f)(d + dum,prms) );
   }

   return c * integral;
}

void Gauss_Legendre_Zeros_100pts( double zeros[] ) {

   const double *px = &x[NUM_OF_POSITIVE_ZEROS - 1];
   double *pz = &zeros[NUM_OF_ZEROS - 1];

   for (; px >= x; px--)  {
      *(zeros++) = - *px;
      *(pz--) = *px;
   }
}

void Gauss_Legendre_Coefs_100pts( double coefs[] ) {

   const double *pA = &A[NUM_OF_POSITIVE_ZEROS - 1];
   double *pc = &coefs[NUM_OF_ZEROS - 1];

   for (; pA >= A; pA--)  {
      *(coefs++) =  *pA;
      *(pc--) = *pA;
   }
}

/***********************************************************************************************************************************************************************/
/********************************\\ Interpolation Function \\***********************************************************************************************************/

double Pm(double k)
{
    if (k>=kmin && k<=kmax)  return gsl_spline_eval (spline[1], k, acc[1]);
    else return 0;
}

double Xim_interp(double s)
{
	if (s>=smin && s<=smax-1)	return gsl_spline_eval (spline[2], s, acc[2]);
    else	return 0;
}

double V12_interp(double s)
{
	if (s>=smin && s<=smax-1)	return gsl_spline_eval (spline[3], s, acc[3]);
    else return 0;
}

double Psiper_interp(double s)
{
	if (s>=smin && s<=smax-1) 	return gsl_spline_eval (spline[4], s, acc[4]);
    else return 0;

}

double Psipar_interp(double s)
{
	if (s>=smin && s<=smax-1)	return gsl_spline_eval (spline[5], s, acc[5]);
    else    return 0;
}

double Xi_R_CLPT(double s)
{
	if (s>=smin && s<=smax)	return	 gsl_spline_eval (spline[6], s, acc[6])
														+ f1*gsl_spline_eval (spline[7], s, acc[7])
														+ f2*gsl_spline_eval (spline[8], s, acc[8])
														+ f1*f1*gsl_spline_eval (spline[9], s, acc[9])
														+ f1*f2*gsl_spline_eval (spline[10], s, acc[10])
														+ f2*f2*gsl_spline_eval (spline[11], s, acc[11]);
    else	return 0;
}

double V12_CLPT(double s)
{
	if (s>=smin && s<=smax)	return 	gsl_spline_eval (spline[12], s, acc[12])
														+ f1*gsl_spline_eval (spline[13], s, acc[13])
														+ f2*gsl_spline_eval (spline[14], s, acc[14])
														+ f1*f1*gsl_spline_eval (spline[15], s, acc[15])
														+ f1*f2*gsl_spline_eval (spline[16], s, acc[16]);
    else return 0;
}

double Sig_par_CLPT(double s)
{
	if (s>=smin && s<= smax) 	return 	gsl_spline_eval (spline[17], s, acc[17])
														+ f1*gsl_spline_eval (spline[18], s, acc[18])
														+ f2*gsl_spline_eval (spline[19], s, acc[19])
														+ f1*f1*gsl_spline_eval (spline[20], s, acc[20]);
    else return 0;
}

double Sig_per_CLPT(double s)
{
	if (s>=smin && s<=smax)	return 	gsl_spline_eval (spline[21], s, acc[21])
														+ f1*gsl_spline_eval (spline[22], s, acc[22])
														+ f2*gsl_spline_eval (spline[23], s, acc[23])
														+ f1*f1*gsl_spline_eval (spline[24], s, acc[24]);
    else    return 0;
}

/**********************************************************************************************************************************************************************/
/*************\\ Gaussian Function\\***********************************************************************************************************************************/

double gaus (double x, double moy, double var2)
{
    return 1./(sqrt(2.*M_PI*var2))*exp(-pow((x-moy),2)/(2.*var2));
}

/**********************************************************************************************************************************************************************/
/*************\\Decomposition of s and r\\*****************************************************************************************************************************/

double spar (double s, double mu_s)
{
    return s*mu_s;
}

double rperp (double s, double spar) // rp = sp (perp)
{
    return sqrt(s*s-spar*spar);
}

double r_real(double rp, double rpar)
{
    return sqrt(rp*rp+rpar*rpar);
}

double mu_r(double rpar, double r)
{
    return rpar/r;
}

/***********************************************************************************************************************************************************************/
/***************\\ Sigma_8\\********************************************************************************************************************************************/

double fS8 (double k, void * params)
{
  double x = k*8;
  return Pm(k)*k*k*(sin(x)-x*cos(x))*(sin(x)-x*cos(x))/pow(x,6);
}

double S8 (void)
{
    double result, error;

    gsl_function F;
    F.function = &fS8;

    gsl_integration_cquad_workspace *w = gsl_integration_cquad_workspace_alloc(size);
    gsl_integration_cquad(&F, smin, smax, 0, 1e-12, w, &result, &error, NULL);
    gsl_integration_cquad_workspace_free(w);

    return 9./(2.*M_PI*M_PI)*result;
}

/*********************************************************************************************************************************************************************/
/****************\\Normalisation of P_m\\*****************************************************************************************************************************/

double Pm_norm (double s)
{
	return Pm(s)/sig8;
}

/***********************************************************************************************************************************************************************/
/*****************\\ V_12(r) Function\\*********************************************************************************************************************************/

 //Decomposition of bessel function j1
double vsin (double k, void * params)
{
  double alpha = *(double *) params;
  return Pm_norm(k)*alpha/k;
}

double vcos (double k, void * params)
{
  double alpha = *(double *) params;
  return Pm_norm(k)*alpha;
}

double V12(double r)
{
	if(r>=.1){
		double result1, error1, result2, error2, alpha, alpha1, err;
		if(r<1) err = 0.1;
			else err= 0.1;

		// Vsin calculation
		alpha = 1./(r*r);
		gsl_function F1;
		F1.function = &vsin;
		F1.params = α

		gsl_integration_workspace *w1 = gsl_integration_workspace_alloc(size);
		gsl_integration_workspace *w2 = gsl_integration_workspace_alloc(size);
		gsl_integration_qawo_table *t1 = gsl_integration_qawo_table_alloc(r, lk, GSL_INTEG_SINE, size);
		gsl_integration_qawf(&F1, kmin, err, limit, w1, w2, t1, &result1, &error1);

		// Vcos calculation
		alpha1 = 1./r;
		gsl_function F2;
		F2.function = &vcos;
		F2.params = &alpha1;

		gsl_integration_workspace *w3 = gsl_integration_workspace_alloc(size);
		gsl_integration_workspace *w4 = gsl_integration_workspace_alloc(size);
		gsl_integration_qawo_table *t2 = gsl_integration_qawo_table_alloc(r, lk, GSL_INTEG_COSINE, size);
		gsl_integration_qawf(&F2, kmin, err, limit, w3, w4, t2, &result2, &error2);

		gsl_integration_workspace_free(w1);
		gsl_integration_workspace_free(w2);
		gsl_integration_qawo_table_free(t1);
		gsl_integration_workspace_free(w3);
		gsl_integration_workspace_free(w4);
		gsl_integration_qawo_table_free(t2);
		return result2-result1;
	}
	else return 0;
}

/************************************************************************************************************************************************************************/
/*************\\Sigma_12(mu,r) functions\\*******************************************************************************************************************************/

// Sigma_v
double fsigmav(double k, void * params)
{
    double alpha = *(double *) params;
    return alpha*Pm_norm(k);
}

double Sigmav(void)
{
    double result,error;
    double alpha = 1./3.;

    gsl_function F;
    F.function = &fsigmav;
    F.params = α

    gsl_integration_workspace *w =  gsl_integration_workspace_alloc(size);
    gsl_integration_qag(&F, kmin, kmax, 0, 1e-5, limit, 6, w, &result, &error);
    gsl_integration_workspace_free(w);

    return result;
}

//	Psiper(r)

 //Decomposition of bessel function j1
double Psipersin(double k, void * params)
{
    double alpha = *(double *) params;
    return alpha*Pm_norm(k)/(pow(k,3));
}

double Psipercos(double k, void * params)
{
    double alpha = *(double *) params;
    return alpha*Pm_norm(k)/(k*k);
}

double Psiper(double r)			// We adapt the error at lower scale to compute the integral
{
	if (r>=0.8){
		double err;
//		for sin part
		double result1,error1;
		double alpha = 1./pow(r,3);
//		for cos part
		double result2,error2;
		double alpha1 = 1./(r*r);

		if(r<10) {
			if (r<2.1) err = 10;
			else err= 1;}
		else err=0.5;

// 	Psipersin calculation
		gsl_function F1;
		F1.function = &Psipersin;
		F1.params = α

		gsl_integration_workspace *w1 = gsl_integration_workspace_alloc(size);
		gsl_integration_workspace *w2 = gsl_integration_workspace_alloc(size);
		gsl_integration_qawo_table *t1 = gsl_integration_qawo_table_alloc(r, lk, GSL_INTEG_SINE, size);
		gsl_integration_qawf(&F1, kmin, err, limit, w1, w2, t1, &result1, &error1);

		// Psipercos calculation
		gsl_function F2;
		F2.function = &Psipercos;
		F2.params = &alpha1;

		gsl_integration_workspace *w3 = gsl_integration_workspace_alloc(size);
		gsl_integration_workspace *w4 = gsl_integration_workspace_alloc(size);
		gsl_integration_qawo_table *t2 = gsl_integration_qawo_table_alloc(r, lk, GSL_INTEG_COSINE, size);
		gsl_integration_qawf(&F2, kmin, err, limit, w3, w4, t2, &result2, &error2);

		gsl_integration_workspace_free(w1);
		gsl_integration_workspace_free(w2);
		gsl_integration_qawo_table_free(t1);
		gsl_integration_workspace_free(w3);
		gsl_integration_workspace_free(w4);
		gsl_integration_qawo_table_free(t2);

		return (result1-result2);}
    else return 0;
}

//	 Psipar(r)
double psiparf(double k, void * params)
{
    double alpha = *(double *) params;
    return alpha*Pm_norm(k)/k;
}

double Psipar(double r)
{
    double result,error;
    double alpha = 1./r;

    gsl_function F;
    F.function = &psiparf;
    F.params = α

    gsl_integration_workspace *w1 = gsl_integration_workspace_alloc(size);
    gsl_integration_workspace *w2 = gsl_integration_workspace_alloc(size);
    gsl_integration_qawo_table *t1 = gsl_integration_qawo_table_alloc(r, lk, GSL_INTEG_SINE, size);
    gsl_integration_qawf(&F, kmin, 1e-1, limit, w1, w2, t1, &result, &error);

    gsl_integration_workspace_free(w1);
    gsl_integration_workspace_free(w2);
    gsl_integration_qawo_table_free(t1);

    return (result-2.*Psiper(r));
}

/********Sigma_12(mu,r)***************************************************************************************************************************************************/

double Sigma12 (double mu, double r, double sigv) 				// from linear (Gaussian streaming) model
{
    return 2.*(sigv-mu*mu*Psipar_interp(r)-(1.-(mu*mu))*Psiper_interp(r));
}

double Sigma12_CLPT (double mu, double r)							// from CLPT prediction
{
    return (mu*mu*Sig_par_CLPT(r)+(1.-mu*mu)*Sig_per_CLPT(r))/(1.+Xi_R_CLPT(r))-27.;
}
/***********************************************************************************************************************************************************************/
/************\\Correlation function in real space\\*********************************************************************************************************************/

double fXim(double k, void * params)
{
    double alpha = *(double *) params;
    double f = alpha*Pm_norm(k)*k;
    return f;
}

double Xim (double r)
{
    double result, error;
    double alpha = 1./(2.*M_PI*M_PI*r);

    gsl_function F;
    F.function = &fXim;
    F.params = α

    gsl_integration_workspace *w1 = gsl_integration_workspace_alloc(200);
    gsl_integration_workspace *w2 = gsl_integration_workspace_alloc(200);
    gsl_integration_qawo_table *t = gsl_integration_qawo_table_alloc(r, lk, GSL_INTEG_SINE, size);
    gsl_integration_qawf(&F, kmin, 1e-3, 200, w1, w2, t, &result, &error);

    gsl_integration_workspace_free(w1);
    gsl_integration_workspace_free(w2);
    gsl_integration_qawo_table_free(t);
    return result;
}

/***********************************************************************************************************************************************************************/
/*************\\Correlation function in z-space\\***********************************************************************************************************************/

double fXis (double y, void *p)
{
    struct my_f_params * params = (struct my_f_params *)p;
    double spp = (params->a);																			// parallel component of s
    double rp = (params->b);																				// perpendicular componant of s
    double r = r_real(rp, y);                        															// norm of r
	double v = fg*b/(M_PI*M_PI)*V12_interp(r); 											// V_12(r)
	double mu_r = y/r;
	double x = spp-y;
	double moy = mu_r*v;																					// average of the gaussian distribution
	double var = pow(fg,2)/(2.*M_PI*M_PI)*Sigma12(mu_r,r,sigv); 			// Variance Sigma_12
	if (var>0)	return (1.+Xim_interp(r))*gaus(x, moy, var);
	else				return 0;
}

double Xis (double sp, double spi)   //Compute in sperp, spar
{
	struct my_f_params params = {spi, sp};
    double result = Gauss_Legendre_Integration2_100pts(-200, 200, &fXis, &params);
    return result -1.;
}

/*********************************************************************************************************************************************************************/
/****************\\Correlation function in z-space for CLPT prediction\\**********************************************************************************************/

double fXis_CLPT (double y, void *p)
{
    struct my_f_params * params = (struct my_f_params *)p;
    double spp = (params->a);
    double rp = (params->b);
    double r = r_real(rp, y);
	double v = fg*V12_CLPT(r)/(1.+Xi_R_CLPT(r));
	double mu_r = y/r;
	double x = spp-y;
	double moy = mu_r*v;
	double var = pow(fg,2)*Sigma12_CLPT(mu_r,r);
	if (var>0)	return (1.+Xi_R_CLPT(r))*gaus(x, moy, var);
	else				return 0;
}

double Xis_CLPT (double sp, double spi)
{
	struct my_f_params params = {spi, sp};
    double result = Gauss_Legendre_Integration2_100pts(-200, 200, &fXis_CLPT, &params);
    return result -1.;
}

/**********************************************************************************************************************************************************************/
/*********\\Legendre Multipole\\***************************************************************************************************************************************/

double fmultipole(double mu, void * p)
{
    struct my_f_params * params = (struct my_f_params *)p;
    double s = (params->a);
    double l = (params->b);
    double rp = s*sqrt(1-mu*mu);			// Decomposition of S in spar (=spi) and sp=rp
    double spi = s*mu;
    double Xi_s= Xis(rp,spi);
	return Xi_s*gsl_sf_legendre_Pl(l,mu);
}

double multipole(double s, double l)
{
    double result;
    struct my_f_params params = {s, l};
	result = Gauss_Legendre_Integration2_100pts(-1, 1, &fmultipole, &params);
	return (2.*l+1.)/2.*result;
}

/********************************************************************************************************************************************************************/
/****************\\Legendre Multipole with CLPT prediction\\*********************************************************************************************************/

double fmultipole_CLPT(double mu, void * p)
{
    struct my_f_params * params = (struct my_f_params *)p;
    double s = (params->a);
    double l = (params->b);
    double rp = s*sqrt(1.-mu*mu);
    double spi = s*mu;												// Decomposition of S in spar (=spi) and sp=rp
    double Xi_s= Xis_CLPT(rp,spi);
	return Xi_s*gsl_sf_legendre_Pl(l,mu);
}

double multipole_CLPT(double s, double l)
{
    double result;
    struct my_f_params params = {s, l};
	result = Gauss_Legendre_Integration2_100pts(-1, 1, &fmultipole_CLPT, &params);
	return (2.*l+1.)/2.*result;
}
/************************************************************************************************************************************************************************/
/***********\\ Interpolation \\*****************************************************************************************************************************/

void interpole(int n, char ficher[100],int vmax)
{
	double T_x[vmax];
	double T_y[vmax];
	FILE* f;
	int i = 0;
	f =fopen(ficher, "r");
	for(i=0; i < vmax; i++) fscanf(f, "%lf %lf\n", &T_x[i], &T_y[i]);
	acc[n] = gsl_interp_accel_alloc ();
    spline[n] = gsl_spline_alloc(gsl_interp_cspline, vmax);
    gsl_spline_init (spline[n], T_x, T_y, vmax);
    fclose(f);
}

void interpole_Xi()
{
	FILE *fxi;
	int i;
	double Xi_x[val], Xi_f0[val], Xi_f1[val], Xi_f2[val], Xi_f1_2[val], Xi_f1_f2[val], Xi_f2_2[val];

	fxi = fopen("data/Xi_r_CLPT.dat", "r");
	for(i=0; i < val; i++) fscanf(fxi, "%lf %lf %lf %lf %lf %lf %lf\n",&Xi_x[i], &Xi_f0[i], &Xi_f1[i], &Xi_f2[i], &Xi_f1_2[i], &Xi_f1_f2[i], &Xi_f2_2[i]);
    acc[6] = gsl_interp_accel_alloc ();
    spline[6] = gsl_spline_alloc(gsl_interp_cspline, val);
    gsl_spline_init (spline[6], Xi_x, Xi_f0, val);
   	acc[7] = gsl_interp_accel_alloc ();
    spline[7] = gsl_spline_alloc(gsl_interp_cspline, val);
    gsl_spline_init (spline[7], Xi_x, Xi_f1, val);
    acc[8] = gsl_interp_accel_alloc ();
    spline[8] = gsl_spline_alloc(gsl_interp_cspline, val);
    gsl_spline_init (spline[8], Xi_x, Xi_f2, val);
    acc[9] = gsl_interp_accel_alloc ();
    spline[9] = gsl_spline_alloc(gsl_interp_cspline, val);
    gsl_spline_init (spline[9], Xi_x, Xi_f1_2, val);
    acc[10] = gsl_interp_accel_alloc ();
    spline[10] = gsl_spline_alloc(gsl_interp_cspline, val);
    gsl_spline_init (spline[10], Xi_x, Xi_f1_f2, val);
    acc[11] = gsl_interp_accel_alloc ();
    spline[11] = gsl_spline_alloc(gsl_interp_cspline, val);
    gsl_spline_init (spline[11], Xi_x, Xi_f2_2, val);
    fclose(fxi);
}

void interpole_V12()
{
	FILE *fv12;
	int i;
	double V12_x[val], V12_f0[val], V12_f1[val], V12_f2[val], V12_f1_2[val], V12_f1_f2[val];

	fv12 = fopen("data/V_12_CLPT.dat", "r");
	for(i=0; i < val; i++) fscanf(fv12, "%lf %lf %lf %lf %lf %lf\n",&V12_x[i], &V12_f0[i], &V12_f1[i], &V12_f2[i], &V12_f1_2[i], &V12_f1_f2[i]);
    acc[12] = gsl_interp_accel_alloc ();
    spline[12] = gsl_spline_alloc(gsl_interp_cspline, val);
    gsl_spline_init (spline[12], V12_x, V12_f0, val);
   	acc[13] = gsl_interp_accel_alloc ();
    spline[13] = gsl_spline_alloc(gsl_interp_cspline, val);
    gsl_spline_init (spline[13], V12_x, V12_f1, val);
    acc[14] = gsl_interp_accel_alloc ();
    spline[14] = gsl_spline_alloc(gsl_interp_cspline, val);
    gsl_spline_init (spline[14], V12_x, V12_f2, val);
    acc[15] = gsl_interp_accel_alloc ();
    spline[15] = gsl_spline_alloc(gsl_interp_cspline, val);
    gsl_spline_init (spline[15], V12_x, V12_f1_2, val);
    acc[16] = gsl_interp_accel_alloc ();
    spline[16] = gsl_spline_alloc(gsl_interp_cspline, val);
    gsl_spline_init (spline[16], V12_x, V12_f1_f2, val);
    fclose(fv12);
}
/*
void interpole_sigma()
{
	int i, val =150;
	double S_x[val], S_par[val], S_per[val];
	FILE* fx;
   	fx = fopen("../../CLPT/Sigma.dat", "r");
	for(i=0; i < val; i++) fscanf(fx, "%lf %lf %lf\n", &S_x[i], &S_par[i], &S_per[i]);
	acc[8] = gsl_interp_accel_alloc ();
    spline[8] = gsl_spline_alloc(gsl_interp_cspline, val);
    gsl_spline_init (spline[8], S_x, S_par, val);
    acc[9] = gsl_interp_accel_alloc ();
    spline[9] = gsl_spline_alloc(gsl_interp_cspline, val);
    gsl_spline_init (spline[9], S_x, S_per, val);
    fclose(fx);
}*/

void interpole_sigma()
{
	int i;
	double S_x[val], S_par_f0[val], S_par_f1[val], S_par_f2[val], S_par_f1_2[val], S_per_f0[val], S_per_f1[val], S_per_f2[val], S_per_f1_2[val];
	FILE* fsig;

   	fsig = fopen("data/Sigma_12_CLPT.dat", "r");
	for(i=0; i<val; i++) fscanf(fsig, "%lf %lf %lf %lf %lf %lf %lf %lf %lf\n", &S_x[i], &S_par_f0[i], &S_par_f1[i], &S_par_f2[i], &S_par_f1_2[i], &S_per_f0[i], &S_per_f1[i], &S_per_f2[i], &S_per_f1_2[i]);
   acc[17] = gsl_interp_accel_alloc ();
    spline[17] = gsl_spline_alloc(gsl_interp_cspline, val);
    gsl_spline_init (spline[17], S_x, S_par_f0, val);
   	acc[18] = gsl_interp_accel_alloc ();
    spline[18] = gsl_spline_alloc(gsl_interp_cspline, val);
    gsl_spline_init (spline[18], S_x, S_par_f1, val);
    acc[19] = gsl_interp_accel_alloc ();
    spline[19] = gsl_spline_alloc(gsl_interp_cspline, val);
    gsl_spline_init (spline[19], S_x, S_par_f2, val);
    acc[20] = gsl_interp_accel_alloc ();
    spline[20] = gsl_spline_alloc(gsl_interp_cspline, val);
    gsl_spline_init (spline[20], S_x, S_par_f1_2, val);
       acc[21] = gsl_interp_accel_alloc ();
    spline[21] = gsl_spline_alloc(gsl_interp_cspline, val);
    gsl_spline_init (spline[21], S_x, S_per_f0, val);
   	acc[22] = gsl_interp_accel_alloc ();
    spline[22] = gsl_spline_alloc(gsl_interp_cspline, val);
    gsl_spline_init (spline[22], S_x, S_per_f1, val);
    acc[23] = gsl_interp_accel_alloc ();
    spline[23] = gsl_spline_alloc(gsl_interp_cspline, val);
    gsl_spline_init (spline[23], S_x, S_per_f2, val);
    acc[24] = gsl_interp_accel_alloc ();
    spline[24] = gsl_spline_alloc(gsl_interp_cspline, val);
    gsl_spline_init (spline[24], S_x, S_per_f1_2, val);
    fclose(fsig);
}

/***********************************************************************************************************************************************************************/
/************\\Count the number of ligne in a file\\********************************************************************************************************************/

int compte(FILE *fichier)
{
	int c;
	int nLignes = 0;
	int c2 = '\0';
	while((c=fgetc(fichier)) != EOF){	if(c=='\n')	nLignes++;	c2 = c;}
	if(c2 != '\n') nLignes++;
	return nLignes;
}

/************************************************************************************************************************************************************************/
/******\\Write in a file\\***********************************************************************************************************************************************/

void write(char f[], double func(double))
{
	double spi;
	FILE* fi;
    fi = fopen(f,"w+");
    for(spi=smin; spi<smax; spi+=0.5)	fprintf(fi,"%le %le\n", spi, func(spi));
    fclose(fi);
}

/******************************************************************************************************************************************************************/
/***************\\MAIN FUNCTION\\**********************************************************************************************************************************/
int main (int argc, char *argv[])
{
	double spi,sp,li;
    int i, nLignes;
	FILE *fi, *par, *ps, *f;
	char file[BUFSIZ];
/*
	sig_shift = 0;
	b = 1;
	fg = 0.820165;
	f1 = atof(argv[1]);
	f2 = atof(argv[2]);
	smin=2.5;
	smax=158;
	val=160;
//	printf("f1=%lf f2=%lf\n",f1,f2);
//	char* file = "power_spec_EBOSS.txt";*/
	par = fopen(argv[1],"r");

// Check parameter file
	if (argc ==2 && par!=0) EXIT_SUCCESS;
	else {
		printf("Wrong parameter file\n");
		return EXIT_FAILURE;
	}

// Read parameter file
	fscanf(par,"%*s %lf\n", &smin);
	fscanf(par,"%*s %lf\n", &smax);
	fscanf(par,"%*s %lf\n", &b);
	fscanf(par,"%*s %lf\n", &fg);
	fscanf(par,"%*s %lf\n", &f1);
	fscanf(par,"%*s %lf\n", &f2);
	fscanf(par,"%*s %lf\n", &sig_shift);
	fscanf(par,"%*s %s\n",file);

//Check smin and smax
	li = (smax-smin)/0.5;
	if (li<0){
		printf("wrong parameters smin/smax");
		return EXIT_FAILURE;
		}

// Count number of lines
	ps = fopen(file, "r");
	if (ps!=0){
		nLignes = compte(ps);
		}
	else {
		printf("Error openning power spectrum file.\n");
		return 1;
	}

// Read kmin and kmax
	rewind(ps);
	kmin=1e30;
	kmax=0;
	for(i=0; i < nLignes; i++) {
		double va;
		fscanf(ps, "%lf %*f\n", &va);
		if (va<kmin) kmin=va;
		if (va>kmax) kmax=va;
	}
	fclose(ps);
	lk = kmax-kmin;
// Interpolation of power spectrum file
	interpole(1,file, nLignes);

	sig8=1.;
	sigv = Sigmav();
//	printf("s8=%lf\n",sig8);

// Compute and save functions in text files
	write("data/Xim.dat",Xim);
    write("data/V12.dat",V12);
    write("data/Psiper.dat",Psiper);
    write("data/Psipar.dat",Psipar);

    interpole(2,"data/Xim.dat", li);
    interpole(3,"data/V12.dat", li);
    interpole(4,"data/Psiper.dat", li);
    interpole(5,"data/Psipar.dat", li);

	interpole_Xi();
	interpole_V12();
	interpole_sigma();

//	Multipoles calculation
	fi = fopen("data/multipole.dat","w+"); 	;
	printf("Calcul des multipoles\n");
	for(spi=smin; spi<smax; spi+=1){
		double xi0=sig8*multipole(spi,0);
		double xi2=sig8*multipole(spi,2);
		double xi4=sig8*multipole(spi,4);
		fprintf(fi,"%le %le %le %le\n", spi, xi0, xi2, xi4);
	}
	fclose(fi);

	fi = fopen("data/multipole_CLPT.dat","w+");
	printf("Calcul des  multipoles CLPT\n");
	for(spi=smin; spi<smax; spi+=1){
		double xi0=multipole_CLPT(spi,0);
		double xi2=multipole_CLPT(spi,2);
		double xi4=multipole_CLPT(spi,4);
		//printf("%lf %lf %lf ", spi, xi0, xi2);
		fprintf(fi,"%lf %lf %lf %lf\n", spi, xi0, xi2,xi4);
	}
	fclose(fi);

	//Correlation function Xi_s(spar,sper)
	f = fopen("data/Xi_s.dat","w+");
    printf("Calcul de Xis\n");
    for(spi=smin; spi<smax; spi+=0.1){
   		for(sp=smin; sp<smax; sp+=0.1){
		 	double V=Xis(sp,spi);
		 	fprintf(f,"%le %le %le\n",sp, spi, V);
     	}
     }
    fclose(f);

	f = fopen("data/Xi_s_CLPT.dat","w+");
    printf("Calcul de Xis_CLPT\n");
    for(spi=smin; spi<smax; spi+=0.1){
   		for(sp=smin; sp<smax; sp+=0.1){
		 	double V=Xis_CLPT(sp,spi);
		 	fprintf(f,"%le %le %le\n",sp, spi, V);
     	}
     }
    fclose(f);

    for (i=0; i<25; i++){
    	gsl_spline_free (spline[i]);
   		gsl_interp_accel_free (acc[i]);
   	}

    return 0;
}


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

/* Gear 2 */

/* Author:  G. Jungman
 */
#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include "odeiv_util.h"
#include "gsl_odeiv.h"

/* gear2 state object */
typedef struct {
  int primed;                /* flag indicating that yim1 is ready */
  double t_primed;           /* system was primed for this value of t */
  double last_h;             /* last step size */
  gsl_odeiv_step * primer;   /* stepper to use for priming */
  double * yim1;             /* y_{i-1}    */
  double * k;                /* work space */
  double * y0;               /* work space */
  int stutter;
}
gear2_state_t;

static void *
gear2_alloc (size_t dim)
{
  gear2_state_t *state = (gear2_state_t *) malloc (sizeof (gear2_state_t));

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

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

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

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

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

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

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

  state->primed = 0;
  state->primer = gsl_odeiv_step_alloc (gsl_odeiv_step_rk4imp, dim);
  state->last_h = 0.0;

  return state;
}

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

  state->stutter = 0;

  if(state->primed == 0 || t == state->t_primed || h != state->last_h) {

    /* Execute a single-step method to prime the process.  Note that
     * we do this if the step size changes, so frequent step size
     * changes will cause the method to stutter.
     *
     * Note that we reuse this method if the time has not changed,
     * which can occur when the adaptive driver is attempting to find
     * an appropriate step-size on its first iteration */

    int status;
    DBL_MEMCPY(state->yim1, y, dim);

    status = gsl_odeiv_step_apply(state->primer, t, h, y, yerr, dydt_in, dydt_out, sys);

    /* Make note of step size and indicate readiness for a Gear step. */

    state->primed = 1;
    state->t_primed = t;
    state->last_h = h;
    state->stutter = 1;

    return status;
  } else {
    /* We have a previous y value in the buffer, and the step
     * sizes match, so we go ahead with the Gear step.
     */

    double * const k  = state->k;
    double * const y0 = state->y0;
    double * const yim1 = state->yim1;

    const int iter_steps = 3;
    int status = 0;
    int nu;
    size_t i;

    DBL_MEMCPY(y0, y, dim);

    /* iterative solution */

    if(dydt_out != NULL) {
      DBL_MEMCPY(k, dydt_out, dim);
    }

    for(nu=0; nu<iter_steps; nu++) {
      int s = GSL_ODEIV_FN_EVAL(sys, t + h, y, k);
      GSL_STATUS_UPDATE(&status, s);
      for(i=0; i<dim; i++) {
        y[i] = ((4.0 * y0[i] - yim1[i]) + 2.0 * h * k[i]) / 3.0;
      }
    }

    /* Estimate error and update the state buffer. */
    for(i=0; i<dim; i++) {
      yerr[i] = h * h * (y[i] - y0[i]); /* error is third order */
      yim1[i] = y0[i];
    }

    /* Make note of step size. */
    state->last_h = h;

    return status;
  }
}

static int
gear2_reset(void * vstate, size_t dim)
{
  gear2_state_t *state = (gear2_state_t *) vstate;

  DBL_ZERO_MEMSET (state->yim1, dim);
  DBL_ZERO_MEMSET (state->k, dim);
  DBL_ZERO_MEMSET (state->y0, dim);

  state->primed = 0;
  state->last_h = 0.0;
  return GSL_SUCCESS;
}

static unsigned int
gear2_order (void *vstate)
{
  gear2_state_t *state = (gear2_state_t *) vstate;
  state = 0; /* prevent warnings about unused parameters */
  return 3;
}

static void
gear2_free(void *vstate)
{
  gear2_state_t *state = (gear2_state_t *) vstate;

  free(state->yim1);
  free(state->k);
  free(state->y0);
  gsl_odeiv_step_free(state->primer);

  free(state);
}

static const gsl_odeiv_step_type gear2_type = { "gear2",	/* name */
  1,				/* can use dydt_in */
  0,				/* gives exact dydt_out */
  &gear2_alloc,
  &gear2_apply,
  &gear2_reset,
  &gear2_order,
  &gear2_free
};

const gsl_odeiv_step_type *gsl_odeiv_step_gear2 = &gear2_type;


#include <gsl/gsl_complex.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_poly.h>

void mgsl_poly_complex_eval(const double c[], const int len, const gsl_complex *z, gsl_complex *ret)
{
  *ret = gsl_poly_complex_eval(c, len, *z);
}

void mgsl_complex_poly_complex_eval(gsl_complex **c, const int len, const gsl_complex *z, gsl_complex *ret)
{
  gsl_complex *zc = calloc(len, sizeof(gsl_complex));
  int i;
  for(i=0; i<len; i++) zc[i] = *c[i];
  *ret = gsl_complex_poly_complex_eval(zc, len, *z);
}


/* linalg/test_common.c
 *
 * Copyright (C) 2017, 2018, 2019, 2020 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_math.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_rng.h>

static int create_random_vector(gsl_vector * v, gsl_rng * r);
static int create_random_matrix(gsl_matrix * m, gsl_rng * r);
static int create_posdef_matrix(gsl_matrix * m, gsl_rng * r);
static int create_hilbert_matrix2(gsl_matrix * m);

static int
create_random_vector(gsl_vector * v, gsl_rng * r)
{
  const size_t N = v->size;
  size_t i;

  for (i = 0; i < N; ++i)
    {
      double vi = gsl_rng_uniform(r);
      gsl_vector_set(v, i, vi);
    }

  return GSL_SUCCESS;
}

static int
create_random_complex_vector(gsl_vector_complex * v, gsl_rng * r)
{
  const size_t N = v->size;
  size_t i;

  for (i = 0; i < N; ++i)
    {
      gsl_complex vi;
      GSL_REAL(vi) = gsl_rng_uniform(r);
      GSL_IMAG(vi) = gsl_rng_uniform(r);
      gsl_vector_complex_set(v, i, vi);
    }

  return GSL_SUCCESS;
}

static int
create_random_matrix(gsl_matrix * m, gsl_rng * r)
{
  const size_t M = m->size1;
  const size_t N = m->size2;
  size_t i, j;

  for (i = 0; i < M; ++i)
    {
      for (j = 0; j < N; ++j)
        {
          double mij = gsl_rng_uniform(r);
          gsl_matrix_set(m, i, j, mij);
        }
    }

  return GSL_SUCCESS;
}

static int
create_random_complex_matrix(gsl_matrix_complex * m, gsl_rng * r)
{
  const size_t M = m->size1;
  const size_t N = m->size2;
  size_t i, j;

  for (i = 0; i < M; ++i)
    {
      for (j = 0; j < N; ++j)
        {
          gsl_complex mij;

          GSL_REAL(mij) = gsl_rng_uniform(r);
          GSL_IMAG(mij) = gsl_rng_uniform(r);

          gsl_matrix_complex_set(m, i, j, mij);
        }
    }

  return GSL_SUCCESS;
}

static int
create_symm_matrix(gsl_matrix * m, gsl_rng * r)
{
  const size_t N = m->size1;
  size_t i, j;

  for (i = 0; i < N; ++i)
    {
      for (j = 0; j <= i; ++j)
        {
          double mij = gsl_rng_uniform(r);
          gsl_matrix_set(m, i, j, mij);
        }
    }

  /* copy lower triangle to upper */
  gsl_matrix_transpose_tricpy(CblasLower, CblasUnit, m, m);

  return GSL_SUCCESS;
}

static int
create_herm_matrix(gsl_matrix_complex * m, gsl_rng * r)
{
  const size_t N = m->size1;
  size_t i, j;

  for (i = 0; i < N; ++i)
    {
      for (j = 0; j <= i; ++j)
        {
          double re = gsl_rng_uniform(r);
          double im = (i != j) ? gsl_rng_uniform(r) : 0.0;
          gsl_complex z = gsl_complex_rect(re, im);

          gsl_matrix_complex_set(m, i, j, z);

          if (i != j)
            gsl_matrix_complex_set(m, j, i, gsl_complex_conjugate(z));
        }
    }

  return GSL_SUCCESS;
}

/* create symmetric banded matrix with p sub/super-diagonals */
static int
create_symm_band_matrix(const size_t p, gsl_matrix * m, gsl_rng * r)
{
  size_t i;

  gsl_matrix_set_zero(m);

  for (i = 0; i < p + 1; ++i)
    {
      gsl_vector_view subdiag = gsl_matrix_subdiagonal(m, i);
      create_random_vector(&subdiag.vector, r);

      if (i > 0)
        {
          gsl_vector_view superdiag = gsl_matrix_superdiagonal(m, i);
          gsl_vector_memcpy(&superdiag.vector, &subdiag.vector);
        }
    }

  return GSL_SUCCESS;
}

/* create (p,q) banded matrix */
static int
create_band_matrix(const size_t p, const size_t q, gsl_matrix * m, gsl_rng * r)
{
  size_t i;

  gsl_matrix_set_zero(m);

  for (i = 0; i <= p; ++i)
    {
      gsl_vector_view v = gsl_matrix_subdiagonal(m, i);
      create_random_vector(&v.vector, r);
    }

  for (i = 1; i <= q; ++i)
    {
      gsl_vector_view v = gsl_matrix_superdiagonal(m, i);
      create_random_vector(&v.vector, r);
    }

  return GSL_SUCCESS;
}

static int
create_posdef_matrix(gsl_matrix * m, gsl_rng * r)
{
  const size_t N = m->size1;
  const double alpha = 10.0 * N;
  size_t i;

  /* The idea is to make a symmetric diagonally dominant
   * matrix. Make a symmetric matrix and add alpha*I to
   * its diagonal */

  create_symm_matrix(m, r);

  for (i = 0; i < N; ++i)
    {
      double mii = gsl_matrix_get(m, i, i);
      gsl_matrix_set(m, i, i, mii + alpha);
    }

  return GSL_SUCCESS;
}

static int
create_posdef_complex_matrix(gsl_matrix_complex *m, gsl_rng *r)
{
  const size_t N = m->size1;
  const double alpha = 10.0 * N;
  size_t i;

  create_herm_matrix(m, r);

  for (i = 0; i < N; ++i)
    {
      gsl_complex * mii = gsl_matrix_complex_ptr(m, i, i);
      GSL_REAL(*mii) += alpha;
    }

  return GSL_SUCCESS;
}

static int
create_hilbert_matrix2(gsl_matrix * m)
{
  const size_t N = m->size1;
  size_t i, j;

  for (i = 0; i < N; i++)
    {
      for (j = 0; j < N; j++)
        {
          gsl_matrix_set(m, i, j, 1.0/(i+j+1.0));
        }
    }

  return GSL_SUCCESS;
}

static int
create_posdef_band_matrix(const size_t p, gsl_matrix * m, gsl_rng * r)
{
  const size_t N = m->size1;
  const double alpha = 10.0 * N;
  size_t i;

  /* The idea is to make a symmetric diagonally dominant
   * matrix. Make a symmetric matrix and add alpha*I to
   * its diagonal */

  create_symm_band_matrix(p, m, r);

  for (i = 0; i < N; ++i)
    {
      double *mii = gsl_matrix_ptr(m, i, i);
      *mii += alpha;
    }

  return GSL_SUCCESS;
}

/* transform dense symmetric banded matrix to compact form, with bandwidth p */
static int
symm2band_matrix(const size_t p, const gsl_matrix * m, gsl_matrix * bm)
{
  const size_t N = m->size1;

  if (bm->size1 != N)
    {
      GSL_ERROR("banded matrix requires N rows", GSL_EBADLEN);
    }
  else if (bm->size2 != p + 1)
    {
      GSL_ERROR("banded matrix requires p + 1 columns", GSL_EBADLEN);
    }
  else
    {
      size_t i;

      gsl_matrix_set_zero(bm);

      for (i = 0; i < p + 1; ++i)
        {
          gsl_vector_const_view diag = gsl_matrix_const_subdiagonal(m, i);
          gsl_vector_view v = gsl_matrix_subcolumn(bm, i, 0, N - i);

          gsl_vector_memcpy(&v.vector, &diag.vector);
        }

      return GSL_SUCCESS;
    }
}

/* transform general dense (p,q) banded matrix to compact banded form */
static int
gen2band_matrix(const size_t p, const size_t q, const gsl_matrix * A, gsl_matrix * AB)
{
  const size_t N = A->size2;

  if (AB->size1 != N)
    {
      GSL_ERROR("banded matrix requires N rows", GSL_EBADLEN);
    }
  else if (AB->size2 != 2*p + q + 1)
    {
      GSL_ERROR("banded matrix requires 2*p + q + 1 columns", GSL_EBADLEN);
    }
  else
    {
      size_t i;

      gsl_matrix_set_zero(AB);

      /* copy diagonal and subdiagonals */
      for (i = 0; i <= p; ++i)
        {
          gsl_vector_const_view v = gsl_matrix_const_subdiagonal(A, i);
          gsl_vector_view w = gsl_matrix_subcolumn(AB, p + q + i, 0, v.vector.size);
          gsl_vector_memcpy(&w.vector, &v.vector);
        }

      /* copy superdiagonals */
      for (i = 1; i <= q; ++i)
        {
          gsl_vector_const_view v = gsl_matrix_const_superdiagonal(A, i);
          gsl_vector_view w = gsl_matrix_subcolumn(AB, p + q - i, i, v.vector.size);
          gsl_vector_memcpy(&w.vector, &v.vector);
        }

      return GSL_SUCCESS;
    }
}


/*
  @copyright Russell Standish 2000-2013
  @author Russell Standish
  This file is part of EcoLab

  Open source licensed under the MIT license. See LICENSE for details.
*/

/**\file
\brief random generator based on GNUSL implementation
*/
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

namespace ecolab
{

  class urand: public random_gen
  {
    void operator=(urand&);
    CLASSDESC_ACCESS(urand);
  public:
    gsl_rng *gen;
    urand(const gsl_rng_type *descr=gsl_rng_mt19937) {gen=gsl_rng_alloc(descr);}
    ~urand() {gsl_rng_free(gen);}
    void Seed(int s) {gsl_rng_set(gen,s);}
    void seed(TCL_args args) {Seed(args);}
    double rand();
    /* select a different uniform random generator according the GSL's rng
       string interface */
    void set_gen(TCL_args args);
    void Set_gen(const string& descr)
    {
      static const gsl_rng_type ** rngTypes=gsl_rng_types_setup();
      const gsl_rng_type **g=rngTypes;
      for (; *g; ++g)
        if (descr==(*g)->name)
          {
            gsl_rng_free(gen);
            gen=gsl_rng_alloc(*g);
            break;
          }
      if (!*g) throw error("Cannot create generator %s",descr.c_str());
    }
  };

  class gaussrand: public random_gen
  {
    void operator=(gaussrand&);
    CLASSDESC_ACCESS(gaussrand);
  public:
    urand uni;
    double rand();
  };
}

 /* don't attempt to send these objects over the wire ! */
#ifdef CLASSDESC
#pragma omit pack  ecolab::urand
#pragma omit unpack  ecolab::urand
#endif
namespace classdesc_access
{
  template <>
  struct access_pack<ecolab::urand>:
    public classdesc::NullDescriptor<classdesc::pack_t> {};
  template <>
  struct access_unpack<ecolab::urand>:
    public classdesc::NullDescriptor<classdesc::unpack_t> {};
  template <>
  struct access_pack<ecolab::urand*>:
    public classdesc::NullDescriptor<classdesc::pack_t> {};
  template <>
  struct access_unpack<ecolab::urand*>:
    public classdesc::NullDescriptor<classdesc::unpack_t> {};
}


#include <math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>
#include <galpy_potentials.h>
// MovingObjectPotential
// 3 arguments: amp, t0, tf
void constrain_range(double * d) {
  // Constrains index to be within interpolation range
  if (*d < 0) *d = 0.0;
  if (*d > 1) *d = 1.0;
}
double MovingObjectPotentialRforce(double R,double z, double phi,
				   double t,
				   struct potentialArg * potentialArgs){
  double amp,t0,tf,d_ind,x,y,obj_x,obj_y,obj_z, Rdist,RF;
  double * args= potentialArgs->args;
  //Get args
  amp= *args;
  t0= *(args+1);
  tf= *(args+2);
  d_ind= (t-t0)/(tf-t0);
  x= R*cos(phi);
  y= R*sin(phi);
  constrain_range(&d_ind);
  // Interpolate x, y, z
  obj_x= gsl_spline_eval(*potentialArgs->spline1d,d_ind,*potentialArgs->acc1d);
  obj_y= gsl_spline_eval(*(potentialArgs->spline1d+1),d_ind,
			 *(potentialArgs->acc1d+1));
  obj_z= gsl_spline_eval(*(potentialArgs->spline1d+2),d_ind,
			 *(potentialArgs->acc1d+2));
  Rdist= pow(pow(x-obj_x, 2)+pow(y-obj_y, 2), 0.5);
  // Calculate R force
  RF= calcRforce(Rdist,(obj_z-z),phi,t,potentialArgs->nwrapped,
		 potentialArgs->wrappedPotentialArg);
  return -amp*RF*(cos(phi)*(obj_x-x)+sin(phi)*(obj_y-y))/Rdist;
}

double MovingObjectPotentialzforce(double R,double z,double phi,
				      double t,
				      struct potentialArg * potentialArgs){
  double amp,t0,tf,d_ind,x,y,obj_x,obj_y,obj_z, Rdist;
  double * args= potentialArgs->args;
  //Get args
  amp= *args;
  t0= *(args+1);
  tf= *(args+2);
  d_ind= (t-t0)/(tf-t0);
  x= R*cos(phi);
  y= R*sin(phi);
  constrain_range(&d_ind);
  // Interpolate x, y, z
  obj_x= gsl_spline_eval(*potentialArgs->spline1d,d_ind,*potentialArgs->acc1d);
  obj_y= gsl_spline_eval(*(potentialArgs->spline1d+1),d_ind,
			 *(potentialArgs->acc1d+1));
  obj_z= gsl_spline_eval(*(potentialArgs->spline1d+2),d_ind,
			 *(potentialArgs->acc1d+2));
  Rdist= pow(pow(x-obj_x, 2)+pow(y-obj_y, 2), 0.5);
  // Calculate z force
  return -amp * calczforce(Rdist,(obj_z-z),phi,t,potentialArgs->nwrapped,
			   potentialArgs->wrappedPotentialArg);
}

double MovingObjectPotentialphiforce(double R,double z,double phi,
					double t,
					struct potentialArg * potentialArgs){
  double amp,t0,tf,d_ind,x,y,obj_x,obj_y,obj_z, Rdist,RF;
  double * args= potentialArgs->args;
  //Get args
  amp= *args;
  t0= *(args+1);
  tf= *(args+2);
  d_ind= (t-t0)/(tf-t0);
  x= R*cos(phi);
  y= R*sin(phi);
  constrain_range(&d_ind);
  // Interpolate x, y, z
  obj_x= gsl_spline_eval(*potentialArgs->spline1d,d_ind,*potentialArgs->acc1d);
  obj_y= gsl_spline_eval(*(potentialArgs->spline1d+1),d_ind,
			 *(potentialArgs->acc1d+1));
  obj_z= gsl_spline_eval(*(potentialArgs->spline1d+2),d_ind,
			 *(potentialArgs->acc1d+2));
  Rdist= pow(pow(x-obj_x, 2)+pow(y-obj_y, 2), 0.5);
  // Calculate phiforce
  RF= calcRforce(Rdist,(obj_z-z),phi,t,potentialArgs->nwrapped,
		 potentialArgs->wrappedPotentialArg);
  return -amp*RF*R*(cos(phi)*(obj_y-y)-sin(phi)*(obj_x-x))/Rdist;
}

double MovingObjectPotentialPlanarRforce(double R, double phi,
				      double t,
				      struct potentialArg * potentialArgs){
  double amp,t0,tf,d_ind,x,y,obj_x,obj_y,Rdist,RF;
  double * args= potentialArgs->args;
  //Get args
  amp= *args;
  t0= *(args+1);
  tf= *(args+2);
  d_ind= (t-t0)/(tf-t0);
  x= R*cos(phi);
  y= R*sin(phi);
  constrain_range(&d_ind);
  // Interpolate x, y
  obj_x= gsl_spline_eval(*potentialArgs->spline1d,d_ind,*potentialArgs->acc1d);
  obj_y= gsl_spline_eval(*(potentialArgs->spline1d+1),d_ind,
			 *(potentialArgs->acc1d+1));
  Rdist= pow(pow(x-obj_x, 2)+pow(y-obj_y, 2), 0.5);
  // Calculate R force
  RF= calcPlanarRforce(Rdist, phi, t, potentialArgs->nwrapped,
		       potentialArgs->wrappedPotentialArg);
  return -amp*RF*(cos(phi)*(obj_x-x)+sin(phi)*(obj_y-y))/Rdist;
}

double MovingObjectPotentialPlanarphiforce(double R, double phi,
					double t,
					struct potentialArg * potentialArgs){
  double amp,t0,tf,d_ind,x,y,obj_x,obj_y,Rdist,RF;
  double * args= potentialArgs->args;
  // Get args
  amp= *args;
  t0= *(args+1);
  tf= *(args+2);
  d_ind= (t-t0)/(tf-t0);
  x= R*cos(phi);
  y= R*sin(phi);
  constrain_range(&d_ind);
  // Interpolate x, y
  obj_x= gsl_spline_eval(*potentialArgs->spline1d,d_ind,*potentialArgs->acc1d);
  obj_y= gsl_spline_eval(*(potentialArgs->spline1d+1),d_ind,
			 *(potentialArgs->acc1d+1));
  Rdist= pow(pow(x-obj_x, 2)+pow(y-obj_y, 2), 0.5);
  // Calculate phiforce
  RF= calcPlanarRforce(Rdist, phi, t, potentialArgs->nwrapped,
		       potentialArgs->wrappedPotentialArg);
  return -amp*RF*R*(cos(phi)*(obj_y-y)-sin(phi)*(obj_x-x))/Rdist;
}


/*
** Various utilities for LISA algorithm
**
** G.Lohmann, Jan 2017
*/
#include <viaio/VImage.h>
#include <viaio/Vlib.h>
#include <viaio/mu.h>
#include <viaio/option.h>

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

#include <gsl/gsl_math.h>
#include <gsl/gsl_histogram.h>

#define SQR(x) ((x)*(x))
#define ABS(x) ((x) > 0 ? (x) : -(x))

extern float kth_smallest(float *a, size_t n, size_t k);
#define Median(a,n) kth_smallest(a,n,(((n)&1)?((n)/2):(((n)/2)-1)))

/* update histogram */
void HistoUpdate(VImage src1,gsl_histogram *hist)
{
  float u,tiny = 1.0e-6;
  size_t i;
  float xmin = gsl_histogram_min (hist);
  float xmax = gsl_histogram_max (hist);

  float *pp1 = VImageData(src1);
  for (i=0; i<VImageNPixels(src1); i++) {
    u = *pp1++;
    if (ABS(u) < tiny) continue;
    if (u > xmax) u = xmax-tiny;
    if (u < xmin) u = xmin+tiny;
    gsl_histogram_increment (hist,u);
  }
}

/* In moderately skewed or asymmetrical distribution (Pearson) */
float VGetMode(VImage src)
{
  size_t i,n=0;
  float u,tiny=1.0e-8;
  VFloat *pp = VImageData(src);
  for (i=0; i<VImageNPixels(src); i++) {
    u = (*pp++);
    if (fabs(u) > tiny) n++;
  }
  float *data = (float *) VCalloc(n,sizeof(float));

  double sum=0,nx=0;
  n=0;
  pp = VImageData(src);
  for (i=0; i<VImageNPixels(src); i++) {
    u = (*pp++);
    if (fabs(u) > tiny) {
      data[n] = u;
      sum += u;
      n++;
    }
  }
  nx = (double)n;
  float median = Median(data,n);
  float mean = (float)(sum/nx);
  float mode = 3.0*median - 2.0*mean;
  /* fprintf(stderr," mean: %f,  median: %f,  mode: %f\n",mean,median,mode); */
  VFree(data);
  return mode;
}

/* scale z-values */
void VZScale(VImage src,float mode,float stddev)
{
  size_t i=0;
  float u=0,tiny=1.0e-8;
  VFloat *pp=VImageData(src);
  for (i=0; i<VImageNPixels(src); i++) {
    u = (*pp);
    if (fabs(u) > tiny) (*pp) = (u-mode)/stddev;
    pp++;
  }
}

/* get max z-value */
float VZMax(VImage src)
{
  size_t i=0;
  float u=0,umax=0;
  VFloat *pp=VImageData(src);
  for (i=0; i<VImageNPixels(src); i++) {
    u = (*pp++);
    if (u > umax) umax = u;
  }
  return umax;
}

/* get image variance */
double VImageVar(VImage src)
{
  size_t i=0;
  double u=0,s1=0,s2=0,nx=0,tiny=1.0e-8;
  VFloat *pp=VImageData(src);
  for (i=0; i<VImageNPixels(src); i++) {
    u = (double)(*pp++);
    if (ABS(u) < tiny) continue;
    s1 += u;
    s2 += u*u;
    nx++;
  }
  if (nx < 3.0) VError(" number of nonzero voxels: %g\n",nx);
  double mean = s1/nx;
  double var = (s2 - nx * mean * mean) / (nx - 1.0);
  return var;
}

/* count nonzero voxels */
void VImageCount(VImage src)
{
  float u=0;
  size_t i=0,npos=0;
  VFloat *pp=VImageData(src);
  for (i=0; i<VImageNPixels(src); i++) {
    u = (float)(*pp++);
    if (u > 0) npos++;
  }
}

/* check histogram range, image is normalized so [-10,10] should be okay */
void VGetHistRange(VImage src,double *hmin,double *hmax)
{
  size_t i=0;
  double u=0,x=0,y=0;
  double zmin = -10.0;
  double zmax = 10.0;

  VFloat *pp=VImageData(src);
  size_t npix=0,npos=0,nneg=0;
  for (i=0; i<VImageNPixels(src); i++) {
    u = (*pp++);
    if (u < zmin) nneg++;
    if (u > zmax) npos++;
    if (fabs(u) > 0) npix++;
  }
  if (npix < 1) VError(" image has no non-zero pixels");

  if (npos > 0 || nneg > 0) {
    x = (double)npos/(double)npix;
    y = (double)nneg/(double)npix;
    if (x > 0.01 || y > 0.01) {
      zmin = -20.0;
      zmax = 20.0;
    }
  }

  /* output */
  *hmin = zmin;
  *hmax = zmax;
}

/* remove isolated voxels */
void VIsolatedVoxels(VImage src,float threshold)
{
  int b,r,c,bb,rr,cc;
  float u=0;
  size_t i;

  VImage tmp = VCreateImage(VImageNBands(src),VImageNRows(src),VImageNColumns(src),VBitRepn);
  VFillImage(tmp,VAllBands,0);

  VFloat *pp=VImageData(src);
  VBit *pa=VImageData(tmp);
  for (i=0; i<VImageNPixels(src); i++) {
    u = (*pp++);
    if (u > threshold) *pa = 1;
    pa++;
  }

  for (b=1; b<VImageNBands(src)-1; b++) {
    for (r=1; r<VImageNRows(src)-1; r++) {
      for (c=1; c<VImageNColumns(src)-1; c++) {

	int n=0;
	for (bb=b-1; bb<=b+1; bb++) {
	  for (rr=r-1; rr<=r+1; rr++) {
	    for (cc=c-1; cc<=c+1; cc++) {
	      if (VPixel(tmp,bb,rr,cc,VBit) > 0) n++;
	      if (n >= 2) goto skip;
	    }
	  }
	}
	if (n < 2) VPixel(src,b,r,c,VFloat) = 0;
      skip: ;
      }
    }
  }
  VDestroyImage(tmp);
}