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


/*
 * Evaluation of the scheduling algorithms HET, RTA, RTA2 and RTA3.
 * v1.0 -- 12/01/2017 -- initial version.
 * v2.0 -- 16/05/2017 -- second version.
 */
#include <libxml/xmlreader.h>
#include <libxml/parser.h>
#include <libxml/xpath.h>
#include <math.h>
#include <unistd.h>
#include <stdlib.h>
#include <time.h>
#include <getopt.h>
#include <gsl/gsl_statistics.h>

/*
 * Ceil and floor operations without using the library math, when period
 * and wcet values are integers.
 */
#define U_CEIL( x, y )    ( ( x / y ) + ( x % y != 0 ) )
#define U_FLOOR( x, y )   ( x / y )

/*
 * Elementos en el archivo XML.
 */
#define ELEMENT             1                               // Tag end
#define END_ELEMENT         15                              // Tag start
#define SET_TAG             (const xmlChar*) "Set"          // <Set> tag -- set
#define S_TAG               (const xmlChar*) "S"            // <S> tag -- rts
#define I_TAG               (const xmlChar*) "i"            // <i> tag -- task
#define SET_SIZE_ATTR       (const xmlChar*) "size"         // "size" attribute in <Set> tag
#define SET_RTS_SIZE_ATTR   (const xmlChar*) "n"            // "n" attribute in <Set> tag
#define SET_UF_ATTR         (const xmlChar*) "u"            // "u" attribute in <Set> tag
#define RTS_ID_ATTR         (const xmlChar*) "count"        // "count" attribute in <S> tag
#define RTS_UF_ATTR         (const xmlChar*) "U"            // "U" attribute in <S> tag
#define ID_ATTR             (const xmlChar*) "nro"          // "nro" attribute in <i> tag
#define WCET_ATTR           (const xmlChar*) "C"            // "C" attribute in <i> tag
#define T_ATTR              (const xmlChar*) "T"            // "T" attribute in <i> tag
#define D_ATTR              (const xmlChar*) "D"            // "D" attribute in <i> tag

/*
 * Number of schedulability methods to test.
 */
#define NUM_SCHED_METHODS 4

/*
 * Name of the schedulability methods to evaluate.
 */
#define HET    "het"
#define RTA    "rta"
#define RTA2   "rta2"
#define RTA3   "rta3"

/*
 * Position of the method in the schedulable array.
 */
#define HET_ID    0
#define RTA_ID    1
#define RTA2_ID   2
#define RTA3_ID   3

/*
 * Return value for the schedulability methods.
 */
#define SCHED     1
#define NON_SCHED 0

/*
 * Global variables.
 */
int rts_founded = 0;        // Number of RTS in the XML file evaluated.
int verbose = 0;            // Print addtional info to stderr
FILE* out_file;             // Result file

// Tarea
struct task_t {
    int id;                         // task id
    int c;                          // wcet
    int t;                          // period
    int d;                          // deadline
    int tmc;                        // t - c
    int wcrt[NUM_SCHED_METHODS];    // wcrt
    int cc[NUM_SCHED_METHODS];      // cc
    int loops_w[NUM_SCHED_METHODS]; // number of while loops
    int loops_f[NUM_SCHED_METHODS]; // number of for loops
    int a_rta2;
    int b_rta2;
    int a_rta3;
    int b_rta3;
    int last_psi;           // het
    int last_workload;      // het
};

// rts
struct rts_t {
    int rts_id;
    int rts_uf;
    int rts_ntask;
    int *schedulable;
    struct task_t **tasks;
};

// set of rts
struct set_t {
    int set_size;
    int set_uf;
    int set_rts_ntask;
    struct rts_t **rts_list;
};

// prototipe for scheduling analysis methods
typedef int (*sched_test_method) (struct rts_t*);

// test method result
struct result_t {
    double *cc;
    double *loops;
    double cc_mean;
    double cc_std;
    double loops_mean;
    double loops_std;
};

struct method_t {
    char* method_name;
    int method_id;
    sched_test_method method;
    struct result_t *result;
};

/*
 * Prototipes
 */
int rta_wcrt(struct rts_t*);
int rta2_wcrt(struct rts_t*);
int rta3_wcrt(struct rts_t*);
int het_workload(int i, int b, int n, struct task_t**);
int het_wcrt(struct rts_t*);

int het_workload(int i, int b, int n, struct task_t **tasks)
{
    tasks[n]->loops_w[HET_ID] += 1;

    double tmp = (double) b / (double) tasks[i]->t;
    int f = (int) floor(tmp);
    int c = (int) ceil(tmp);

    tasks[n]->cc[HET_ID] += 2;

    int branch0 = b - f * (tasks[i]->t - tasks[i]->c);
    int branch1 = c * tasks[i]->c;

    if (i > 0) {
        int l_w = tasks[i - 1]->last_workload;
        int tmp = f * tasks[i]->t;
        if (tmp > tasks[i - 1]->last_psi) {
            l_w = het_workload(i - 1, tmp, n, tasks);
        }

        branch0 += l_w;
        branch1 += het_workload(i - 1, b, n, tasks);
    }

    tasks[i]->last_psi = b;

    if (branch0 <= branch1) {
        tasks[i]->last_workload = branch0;
    } else {
        tasks[i]->last_workload = branch1;
    }

    return tasks[i]->last_workload;
}

/*
 * HET
 * "Schedulability Analysis of Periodic Fixed Priority Systems"
 * http://ieeexplore.ieee.org/document/1336766/
 * --
 * See also:
 * "Efficient Exact Schedulability Tests for Fixed Priority Real-Time Systems"
 * http://ieeexplore.ieee.org/document/4487061/
 */
int het_wcrt(struct rts_t *rts)
{
    struct task_t **tasks = rts->tasks;

    int i;
    for (i = 1; i < rts->rts_ntask; i++) {
        tasks[i]->loops_f[HET_ID] += 1;

        int w = het_workload(i - 1, tasks[i]->d, i, tasks);

        if ((w + tasks[i]->c) > tasks[i]->d) {
            rts->schedulable[HET_ID] = NON_SCHED;
            return NON_SCHED;
        }

        tasks[i]->wcrt[HET_ID] = w + tasks[i]->c;
    }

    rts->schedulable[HET_ID] = SCHED;
    return SCHED;
}

/*
 * RTA
 * "Improved Response-Time Analysis Calculations"
 * http://doi.ieeecomputersociety.org/10.1109/REAL.1998.739773
 */
int rta_wcrt(struct rts_t *rts)
{
    struct task_t **tasks = rts->tasks;

    int w = 0;
    int tr = 0;
    int t = tasks[0]->c;
    tasks[0]->wcrt[RTA_ID] = tasks[0]->c;

    int i, j;
    for (i = 1; i < rts->rts_ntask; i++) {
        tr = t + tasks[i]->c;
        tasks[i]->loops_f[RTA_ID] += 1;

        do {
            tasks[i]->loops_w[RTA_ID] += 1;
            t = tr;
            w = tasks[i]->c;

            for (j = 0; j < i; j++) {
                tasks[i]->loops_f[RTA_ID] += 1;

                int c_j = tasks[j]->c;
                int t_j = tasks[j]->t;
                int a = (int) ceil( ((double) tr) / ((double) t_j) );
                tasks[i]->cc[RTA_ID] += 1;

                w = w + (a * c_j);

                if (w > tasks[i]->d) {
                    rts->schedulable[RTA_ID] = NON_SCHED;
                    return NON_SCHED;
                }
            }

            tr = w;

        } while (t != tr);

        tasks[i]->wcrt[RTA_ID] = t;
    }

    rts->schedulable[RTA_ID] = SCHED;
    return SCHED;
}

/*
 * RTA2
 * "Reduced computational cost in the calculation of worst case response time for real time systems"
 * http://sedici.unlp.edu.ar/handle/10915/9654
 */
int rta2_wcrt(struct rts_t *rts)
{
    struct task_t **tasks = rts->tasks;

    int tr = 0;
    int t = tasks[0]->c;
    tasks[0]->wcrt[RTA2_ID] = tasks[0]->c;

    int i, j;
    for (i = 1; i < rts->rts_ntask; i++) {
        tr = t + tasks[i]->c;
        tasks[i]->loops_f[RTA2_ID] += 1;

        do {
            tasks[i]->loops_w[RTA2_ID] += 1;
            t = tr;

            for (j = 0; j < i; j++) {
                tasks[i]->loops_f[RTA2_ID] += 1;

                int a = (int) ceil( ((double) tr) / ((double) tasks[j]->t) );
                tasks[i]->cc[RTA2_ID] += 1;
                a = a * tasks[j]->c;

                if (a > tasks[j]->a_rta2) {
                    tr = tr + a - tasks[j]->a_rta2;
                    tasks[j]->a_rta2 = a;

                    if (tr > tasks[i]->d) {
                        rts->schedulable[RTA2_ID] = NON_SCHED;
                        return NON_SCHED;
                    }
                }
            }
        } while (t != tr);

        tasks[i]->wcrt[RTA2_ID] = t;
    }

    rts->schedulable[RTA2_ID] = SCHED;
    return SCHED;
}

/*
 * RTA3
 * "Computational Cost Reduction for Real-Time Schedulability Tests Algorithms"
 * http://ieeexplore.ieee.org/document/7404899/
 */
int rta3_wcrt(struct rts_t *rts)
{
    struct task_t **tasks = rts->tasks;

    int tr = 0;
    int t = tasks[0]->c;
    tasks[0]->wcrt[RTA3_ID] = tasks[0]->c;

    int i, j;
    for (i = 1; i < rts->rts_ntask; i++) {
        tr = t + tasks[i]->c;
        tasks[i]->loops_f[RTA3_ID] += 1;

        do {
            tasks[i]->loops_w[RTA3_ID] += 1;
            t = tr;

            for (j = i - 1; j >= 0; j--) {
                tasks[i]->loops_f[RTA3_ID] += 1;

                if (tr > tasks[j]->b_rta3) {
                    int a_t = (int) ceil( ((double)tr) / ((double)tasks[j]->t) );
                    tasks[i]->cc[RTA3_ID] += 1;

                    int a = a_t * tasks[j]->c;
                    tr = tr + a - tasks[j]->a_rta3;

                    tasks[j]->a_rta3 = a;
                    tasks[j]->b_rta3 = a_t * tasks[j]->t;

                    // verifica vencimiento
                    if (tr > tasks[i]->d) {
                        rts->schedulable[RTA3_ID] = NON_SCHED;
                        return NON_SCHED;
                    }
                }
            }
        } while (t != tr);

        tasks[i]->wcrt[RTA3_ID] = t;
    }

    rts->schedulable[RTA3_ID] = SCHED;
    return SCHED;
}

void reset_rts(struct rts_t *rts)
{
    int i, j;
    for (i = 0; i < rts->rts_ntask; i++) {
        struct task_t *task = rts->tasks[i];

        task->a_rta2 = task->c;
        task->b_rta2 = task->t;
        task->a_rta3 = task->c;
        task->b_rta3 = task->t;
        task->last_psi = 0;
        task->last_workload = 0;

        for (j = 0; j < NUM_SCHED_METHODS; j++) {
            task->cc[j] = 0;
            task->loops_w[j] = 0;
            task->loops_f[j] = 0;
            task->wcrt[j] = 0;
        }
    }
}

/*
 * Parse the XML file. If a new RTS is found, it is evalutad with the methods in method array.
 */
void processXmlFile(xmlTextReaderPtr reader, struct set_t *rts_set, struct method_t *methods)
{
    xmlChar *name = xmlTextReaderLocalName(reader);

    // Tag <Set> -- initial tag
    if (xmlStrcasecmp(name, SET_TAG) == 0) {
        if (xmlTextReaderNodeType(reader) == ELEMENT) {
            xmlChar *c_set_rts_uf = xmlTextReaderGetAttribute(reader, SET_UF_ATTR);
            xmlChar *c_set_rts_size = xmlTextReaderGetAttribute(reader, SET_SIZE_ATTR);
            xmlChar *c_set_rts_ntask = xmlTextReaderGetAttribute(reader, SET_RTS_SIZE_ATTR);

            rts_set->set_uf = atoi((char*) c_set_rts_uf);
            rts_set->set_size = atoi((char*) c_set_rts_size);
            rts_set->set_rts_ntask = atoi((char*) c_set_rts_ntask);

            xmlFree(c_set_rts_uf);
            xmlFree(c_set_rts_size);
            xmlFree(c_set_rts_ntask);
        }
    }

    // Tag <S> -- RTS
    if (xmlStrcasecmp(name, S_TAG) == 0) {
        if (xmlTextReaderNodeType(reader) == ELEMENT) {
            xmlChar *c_rts_id = xmlTextReaderGetAttribute(reader, RTS_ID_ATTR);
            xmlChar *c_rts_uf = xmlTextReaderGetAttribute(reader, RTS_UF_ATTR);

            // reserve memory for the rts and add it to rts_list
            struct rts_t *new_rts = malloc(sizeof(struct rts_t));

            // reserve memory for the methods results
            new_rts->schedulable = malloc(sizeof(int) * NUM_SCHED_METHODS);
            // reserve memory for the rts tasks
            new_rts->tasks = malloc(sizeof(struct task_t*) * rts_set->set_rts_ntask);

            // complete data about this rts
            new_rts->rts_id = atoi((char*) c_rts_id);
            new_rts->rts_uf = atoi((char*) c_rts_uf);
            new_rts->rts_ntask = rts_set->set_rts_ntask;

            // add rts to set
            rts_set->rts_list[rts_founded] = new_rts;

            // free memory
            xmlFree(c_rts_id);
            xmlFree(c_rts_uf);
        }

        if (xmlTextReaderNodeType(reader) == END_ELEMENT) {
            struct rts_t *rts = rts_set->rts_list[rts_founded];
            reset_rts(rts);

            // evaluate the methods
            int i, j;
            for (i = 0; i < NUM_SCHED_METHODS; i++) {
                int method_id = methods[i].method_id;
                rts->schedulable[method_id] = (*methods[i].method)(rts);

                // store totals
                for (j = 0; j < rts->rts_ntask; j++) {
                    struct task_t *task = rts->tasks[j];
                    methods[i].result->cc[rts_founded] += task->cc[method_id];
                    methods[i].result->loops[rts_founded] += task->loops_w[method_id] + task->loops_f[method_id];
                }
            }

            rts_founded = rts_founded + 1;
        }
    }

    // Tag <i> -- a real-time task
    if (xmlStrcasecmp(name, I_TAG) == 0) {
        xmlChar *c_id = xmlTextReaderGetAttribute(reader, ID_ATTR);
        xmlChar *wcet = xmlTextReaderGetAttribute(reader, WCET_ATTR);
        xmlChar *t = xmlTextReaderGetAttribute(reader, T_ATTR);
        xmlChar *d = xmlTextReaderGetAttribute(reader, D_ATTR);

        int id = atoi((char*) c_id) - 1;

        struct rts_t *rts = rts_set->rts_list[rts_founded];

        // reserve memory for the task
        struct task_t *task = malloc(sizeof(struct task_t));

        // complete the basic task data
        task->id = id + 1;
        task->c = atoi((char*) wcet);
        task->t = atoi((char*) t);
        task->d = atoi((char*) d);
        task->tmc = task->t - task->c;

        // add the task to rts
        rts->tasks[id] = task;

        xmlFree(c_id);
        xmlFree(wcet);
        xmlFree(t);
        xmlFree(d);
    }

    xmlFree(name);
}

void testRtsInXml(char *file, struct set_t* rts_set, struct method_t *methods, int limit)
{
    // get read pointer
    xmlTextReaderPtr reader = xmlNewTextReaderFilename(file);
    if (reader == NULL) {
        fprintf(stderr, "Unable to open %s\n", file);
        exit(EXIT_FAILURE);
    }

    // parse xml file and evaluate schedulability methods
    int ret = xmlTextReaderRead(reader);
    while (ret == 1) {
        if (limit > 0 && rts_founded == limit) {
            break;
        }
        processXmlFile(reader, rts_set, methods);
        ret = xmlTextReaderRead(reader);
    }

    if (rts_set->set_size < limit) {
        fprintf(stderr, "Warning: %d str in file according to XML info, but %d to be tested.\n", rts_set->set_size, limit);
    }

    xmlFreeTextReader(reader);
}

/*
 * Print method results to out_file.
 */
void save_result(char* method, struct result_t* result, int use_csv, char *csv_sep)
{
    if (use_csv == 0) {
        fprintf(out_file, "%10s%15f%15f%15f%15f\n", method, result->cc_mean, result->cc_std,
                                                    result->loops_mean, result->loops_std);
    } else {
        fprintf(out_file, "%2$s%1$s%3$f%1$s%4$f%1$s%5$f%1$s%6$f\n", csv_sep, method, result->cc_mean, result->cc_std,
                                                                                result->loops_mean, result->loops_std);
    }
}

/*
 * Print help and usage information.
 */
void printUsage(char* progName, int exitCode)
{
    fprintf(stderr, "Usage: %s [options] file\n", progName);
    fprintf(stderr,
            "\t-v  --verbose\tDisplay additional information about the clock used.\n"
            "\t-h  --help\tDisplay this information.\n"
            "\t-l  --limit\tTest first n RTS in file.\n"
            "\t-c  --csv\tCSV output with specified line separator.\n");
    exit(exitCode);
}

int main(int argc, char **argv)
{
    int i, j, k;

    if (argc <= 1) {
        printUsage(argv[0], EXIT_FAILURE);
    }

    // options -- short format
    const char *shortOpts = "hvl:c:";
    // options -- long format
    const struct option longOpts[] = {
        {"help",    no_argument,        NULL, 'h'},
        {"verbose", no_argument,        NULL, 'v'},
        {"limit",   required_argument,  NULL, 'l'},
        {"csv",     required_argument,  NULL, 'c'},
        {0, 0, 0, 0}
    };

    verbose = 0;
    rts_founded = 0;
    int limit = 0;

    int use_csv = 0;
    char* csv_sep;

    int nextOption;

    do {
        nextOption = getopt_long(argc, argv, shortOpts, longOpts, NULL);
        switch (nextOption) {
            case 'h': // -h or --help
                printUsage(argv[0], EXIT_SUCCESS);
            case 'v': // -v or --verbose
                verbose = 1;
                break;
            case 'l': // -l or --limit
                limit = atoi(optarg);
                break;
            case 'c': // -c or --csv
                use_csv = 1;
                csv_sep = optarg;
                break;
            case '?': // invalid option
                printUsage(argv[0], EXIT_FAILURE);
            case -1: // no more options
                break;
            default:
                abort();
        }
    } while (nextOption != -1);

    // print info to stderr if requested
    if (verbose == 1) {
        fprintf(stderr, "Testing %s...\n", argv[optind]);
    }

    // stdout as default output file
    out_file = stdout;

    // reserve memory for the set
    struct set_t *rts_set = malloc(sizeof(struct set_t));
    rts_set->set_uf = 0;
    rts_set->set_size = 0;
    rts_set->set_rts_ntask = 0;
    // reserve memory to store the rts to test
    rts_set->rts_list = malloc(sizeof(struct rts_t*) * limit);

    // reserve memory for test results
    struct result_t* het_results = malloc(sizeof(struct result_t));
    struct result_t* rta_results = malloc(sizeof(struct result_t));
    struct result_t* rta2_results = malloc(sizeof(struct result_t));
    struct result_t* rta3_results = malloc(sizeof(struct result_t));

    // methods to test
    struct method_t methods[] = {[RTA_ID]  {RTA,  RTA_ID,  rta_wcrt,  rta_results},
                                 [RTA2_ID] {RTA2, RTA2_ID, rta2_wcrt, rta2_results},
                                 [RTA3_ID] {RTA3, RTA3_ID, rta3_wcrt, rta3_results},
                                 [HET_ID]  {HET,  HET_ID,  het_wcrt,  het_results}
                                 };

    for (i = 0; i < NUM_SCHED_METHODS; i++) {
        methods[i].result->cc = calloc(limit, sizeof(double));
        methods[i].result->loops = calloc(limit, sizeof(double));
    }

    if (verbose == 1) {
        fprintf(stderr, "Testing %d rts.\n", limit);
    }

    // read rts from xml file into rts_set
    char *filename = argv[optind];
    testRtsInXml(filename, rts_set, methods, limit);

    // compute means and stdev
    for (i = 0; i < NUM_SCHED_METHODS; i++) {
        methods[i].result->cc_mean = gsl_stats_mean(methods[i].result->cc, 1, rts_founded);
        methods[i].result->cc_std = gsl_stats_sd_m(methods[i].result->cc, 1, rts_founded, methods[i].result->cc_mean);
        methods[i].result->loops_mean = gsl_stats_mean(methods[i].result->loops, 1, rts_founded);
        methods[i].result->loops_std = gsl_stats_sd_m(methods[i].result->loops, 1, rts_founded, methods[i].result->loops_mean);
    }

    int rts_sched_cnt = 0;
    int rts_nonsched_cnt = 0;

    // verify that all methods give the same result
    for (i = 0; i < rts_founded; i++) {
        struct rts_t *rts = rts_set->rts_list[i];

        int sum = 0;
        for (j = 0; j < NUM_SCHED_METHODS; j++) {
            sum += rts->schedulable[j];
        }

        if (sum > 0 && sum < NUM_SCHED_METHODS) {
            fprintf(stderr, "Error! Method results are not the same. RTS %d\n", i);
            for (j = 0; j < NUM_SCHED_METHODS; j++) {
                fprintf(stderr, "%s: %d\n", methods[j].method_name, rts->schedulable[j]);
            }
            break;
        }

        if (rts->schedulable[RTA_ID] == SCHED) {
            rts_sched_cnt += 1;
        } else {
            rts_nonsched_cnt += 1;
        }
    }

    // verify that all wcrt are the same (only RTA methods)
    for (j = 0; j < rts_founded; j++) {
        struct rts_t *rts = rts_set->rts_list[j];
        for (k = 0; k < rts->rts_ntask; k++) {
            struct task_t *task = rts->tasks[k];
            int ref_wcrt = task->wcrt[RTA_ID];
            if (ref_wcrt != task->wcrt[RTA2_ID] || ref_wcrt != task->wcrt[RTA3_ID])
            {
                fprintf(stderr, "Error! WCRT are not the same. RTS %d, task %d\n", j, k);

                int i = 0;
                fprintf(stderr, "%13s%10s%10s%10s%10s\n", "RTA", "RTA2", "RTA3", "C_i", "D_i");
                for (i = 0; i < rts->rts_ntask; i++) {
                    fprintf(stderr, "%3d%10d%10d%10d%10d%10d\n", i, rts->tasks[i]->wcrt[RTA_ID], rts->tasks[i]->wcrt[RTA2_ID],
                                                                    rts->tasks[i]->wcrt[RTA3_ID], rts->tasks[i]->c, rts->tasks[i]->d );
                }

                return(EXIT_FAILURE);
            }
        }
    }

    // get timestamp for the test
    char test_date[50];
    time_t current_time = time(NULL);
    strftime(test_date, 50, "%R %d/%m/%Y", localtime(&current_time));

    // print header
    fprintf(out_file, "%s\n", filename);
    fprintf(out_file, "%s\n", test_date);
    fprintf(out_file, "Total: %d\n", rts_founded);
    fprintf(out_file, "Sched: %d\n", rts_sched_cnt);
    fprintf(out_file, "Non sched: %d\n", rts_nonsched_cnt);

    // print column names
    if (use_csv == 0) {
        fprintf(out_file, "%10s%15s%15s%15s%15s\n", "method", "cc_mean", "cc_mean_std", "loops_mean", "loops_mean_std");
    } else {
        fprintf(out_file, "method%1$scc_mean%1$scc_mean_std%1$sloops_mean%1$sloops_mean_std\n", csv_sep);
    }

    // print the results
    for (i = 0; i < NUM_SCHED_METHODS; i++) {
        save_result(methods[i].method_name, methods[i].result, use_csv, csv_sep);
    }

    return(EXIT_SUCCESS);
}