// Copyright (c) 2021 Stig Rune Sellevag
//
// This file is distributed under the MIT License. See the accompanying file
// LICENSE.txt or http://www.opensource.org/licenses/mit-license.php for terms
// and conditions.

#ifndef SCILIB_LINALG_BLAS3_MATRIX_PRODUCT_H
#define SCILIB_LINALG_BLAS3_MATRIX_PRODUCT_H

#ifdef USE_MKL
#include <mkl.h>
#else
#include <cblas.h>
#endif

#include <scilib/mdarray.h>
#include <scilib/linalg_impl/lapack_types.h>
#include <experimental/mdspan>
#include <complex>
#include <type_traits>

namespace Sci {
namespace Linalg {

namespace stdex = std::experimental;

template <class T_a,
          stdex::extents<>::size_type nrows_a,
          stdex::extents<>::size_type ncols_a,
          class Layout_a,
          class Accessor_a,
          class T_b,
          stdex::extents<>::size_type nrows_b,
          stdex::extents<>::size_type ncols_b,
          class Layout_b,
          class Accessor_b,
          class T_c,
          stdex::extents<>::size_type nrows_c,
          stdex::extents<>::size_type ncols_c,
          class Layout_c,
          class Accessor_c>
    requires(!std::is_const_v<T_c>)
inline void matrix_product(
    stdex::mdspan<T_a, stdex::extents<nrows_a, ncols_a>, Layout_a, Accessor_a>
        a,
    stdex::mdspan<T_b, stdex::extents<nrows_b, ncols_b>, Layout_b, Accessor_b>
        b,
    stdex::mdspan<T_c, stdex::extents<nrows_c, ncols_c>, Layout_c, Accessor_c>
        c)
{
    static_assert(a.static_extent(1) == b.static_extent(0));

    using size_type = stdex::extents<>::size_type;

    const size_type n = a.extent(0);
    const size_type m = a.extent(1);
    const size_type p = b.extent(1);

    for (size_type i = 0; i < n; ++i) {
        for (size_type j = 0; j < p; ++j) {
            c(i, j) = T_c{0};
            for (size_type k = 0; k < m; ++k) {
                c(i, j) += a(i, k) * b(k, j);
            }
        }
    }
}

template <class Layout>
inline void matrix_product(Sci::Matrix_view<double, Layout> a,
                           Sci::Matrix_view<double, Layout> b,
                           Sci::Matrix_view<double, Layout> c)
{
    constexpr double alpha = 1.0;
    constexpr double beta = 0.0;

    const BLAS_INT m = static_cast<BLAS_INT>(a.extent(0));
    const BLAS_INT n = static_cast<BLAS_INT>(b.extent(1));
    const BLAS_INT k = static_cast<BLAS_INT>(a.extent(1));

    auto matrix_layout = CblasRowMajor;
    BLAS_INT lda = k;
    BLAS_INT ldb = n;
    BLAS_INT ldc = n;

    if constexpr (std::is_same_v<Layout, stdex::layout_left>) {
        matrix_layout = CblasColMajor;
        lda = m;
        ldb = k;
        ldc = m;
    }
    cblas_dgemm(matrix_layout, CblasNoTrans, CblasNoTrans, m, n, k, alpha,
                a.data(), lda, b.data(), ldb, beta, c.data(), ldc);
}

template <class Layout>
inline void matrix_product(Sci::Matrix_view<const double, Layout> a,
                           Sci::Matrix_view<const double, Layout> b,
                           Sci::Matrix_view<double, Layout> c)
{
    constexpr double alpha = 1.0;
    constexpr double beta = 0.0;

    const BLAS_INT m = static_cast<BLAS_INT>(a.extent(0));
    const BLAS_INT n = static_cast<BLAS_INT>(b.extent(1));
    const BLAS_INT k = static_cast<BLAS_INT>(a.extent(1));

    auto matrix_layout = CblasRowMajor;
    BLAS_INT lda = k;
    BLAS_INT ldb = n;
    BLAS_INT ldc = n;

    if constexpr (std::is_same_v<Layout, stdex::layout_left>) {
        matrix_layout = CblasColMajor;
        lda = m;
        ldb = k;
        ldc = m;
    }
    cblas_dgemm(matrix_layout, CblasNoTrans, CblasNoTrans, m, n, k, alpha,
                a.data(), lda, b.data(), ldb, beta, c.data(), ldc);
}

#ifdef USE_MKL
template <class Layout>
inline void matrix_product(Sci::Matrix_view<std::complex<double>, Layout> a,
                           Sci::Matrix_view<std::complex<double>, Layout> b,
                           Sci::Matrix_view<std::complex<double>, Layout> c)
{
    constexpr std::complex<double> alpha = {1.0, 0.0};
    constexpr std::complex<double> beta = {0.0, 0.0};

    const BLAS_INT m = static_cast<BLAS_INT>(a.extent(0));
    const BLAS_INT n = static_cast<BLAS_INT>(b.extent(1));
    const BLAS_INT k = static_cast<BLAS_INT>(a.extent(1));

    auto matrix_layout = CblasRowMajor;
    BLAS_INT lda = k;
    BLAS_INT ldb = n;
    BLAS_INT ldc = n;

    if constexpr (std::is_same_v<Layout, stdex::layout_left>) {
        matrix_layout = CblasColMajor;
        lda = m;
        ldb = k;
        ldc = m;
    }
    cblas_zgemm(matrix_layout, CblasNoTrans, CblasNoTrans, m, n, k, &alpha,
                a.data(), lda, b.data(), ldb, &beta, c.data(), ldc);
}

template <class Layout>
inline void
matrix_product(Sci::Matrix_view<const std::complex<double>, Layout> a,
               Sci::Matrix_view<const std::complex<double>, Layout> b,
               Sci::Matrix_view<std::complex<double>, Layout> c)
{
    constexpr std::complex<double> alpha = {1.0, 0.0};
    constexpr std::complex<double> beta = {0.0, 0.0};

    const BLAS_INT m = static_cast<BLAS_INT>(a.extent(0));
    const BLAS_INT n = static_cast<BLAS_INT>(b.extent(1));
    const BLAS_INT k = static_cast<BLAS_INT>(a.extent(1));

    auto matrix_layout = CblasRowMajor;
    BLAS_INT lda = k;
    BLAS_INT ldb = n;
    BLAS_INT ldc = n;

    if constexpr (std::is_same_v<Layout, stdex::layout_left>) {
        matrix_layout = CblasColMajor;
        lda = m;
        ldb = k;
        ldc = m;
    }
    cblas_zgemm(matrix_layout, CblasNoTrans, CblasNoTrans, m, n, k, &alpha,
                a.data(), lda, b.data(), ldb, &beta, c.data(), ldc);
}
#endif

template <class T, class Layout, class Allocator>
inline Sci::Matrix<T, Layout, Allocator>
matrix_product(const Sci::Matrix<T, Layout, Allocator>& a,
               const Sci::Matrix<T, Layout, Allocator>& b)
{
    using size_type = stdex::extents<>::size_type;

    const size_type n = a.extent(0);
    const size_type p = b.extent(1);

    Sci::Matrix<T, Layout, Allocator> res(n, p);
    matrix_product(a.view(), b.view(), res.view());
    return res;
}

template <class T, class Layout, class Allocator>
inline void matrix_product(const Sci::Matrix<T, Layout, Allocator>& a,
                           const Sci::Matrix<T, Layout, Allocator>& b,
                           Sci::Matrix<T, Layout, Allocator>& c)
{
    matrix_product(a.view(), b.view(), c.view());
}

} // namespace Linalg
} // namespace Sci

#endif // SCILIB_LINALG_BLAS3_MATRIX_PRODUCT_H


static char help[] = "Compute ln 2 in serial with PETSc, using random\n"
"permutation of sum order.  Shows that floating-point arithmetic is not\n"
"associative.\n\n";

#include <petsc.h>
#include <time.h>

int main(int argc, char **args) {
  PetscErrorCode  ierr;
  PetscMPIInt     size;
  PetscInt        i, j, n=10;
  PetscReal       v, tmp, *a, sum;
  PetscRandom     r;

  PetscInitialize(&argc,&args,NULL,help);

  ierr = MPI_Comm_size(PETSC_COMM_WORLD,&size); CHKERRQ(ierr);
  if (size > 1) {
      SETERRQ(PETSC_COMM_SELF,1,"lntwo only works in serial\n");
  }

  ierr = PetscOptionsBegin(PETSC_COMM_WORLD,"","options for lntwo",""); CHKERRQ(ierr);
  ierr = PetscOptionsInt("-n","number of terms in sum",
                          "lntwo.c",n,&n,NULL); CHKERRQ(ierr);
  ierr = PetscOptionsEnd(); CHKERRQ(ierr);

  ierr = PetscRandomCreate(PETSC_COMM_WORLD,&r); CHKERRQ(ierr);
  ierr = PetscRandomSetType(r,PETSCRAND48); CHKERRQ(ierr);
  ierr = PetscRandomSetSeed(r,(int)time(NULL)); CHKERRQ(ierr);
  ierr = PetscRandomSeed(r); CHKERRQ(ierr);

  // fill array with the terms   (-1)^i / (i+1)  for i=0 .. n
  ierr = PetscMalloc1(n,&a); CHKERRQ(ierr);
  for (i=0; i<n; i++) {
      a[i] = pow(-1.0,(double)i) / (double)(i+1);
  }

  // shuffle the terms
  for (i=n-1; i>0; i--) {
      ierr = PetscRandomGetValueReal(r,&v); CHKERRQ(ierr);
      j = (int)floor(i*v);
      tmp = a[i];
      a[i] = a[j];
      a[j] = tmp;
  }

  // sum the terms
  sum = 0.0;
  for (i=0; i<n; i++) {
      sum += a[i];
  }

  // print the result
  ierr = PetscPrintf(PETSC_COMM_WORLD,"ln 2 is approximately %18.16f\n",sum); CHKERRQ(ierr);

  PetscRandomDestroy(&r);
  PetscFree(a);
  return PetscFinalize();
}


#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <assert.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_sort.h>

#include "cosmocalc.h"

static double linear_powspec_norm;
static gsl_spline *cosmocalc_sigmaR_spline = NULL;
static gsl_interp_accel *cosmocalc_sigmaR_acc = NULL;
static gsl_spline *cosmocalc_Rsigma_spline = NULL;
static gsl_interp_accel *cosmocalc_Rsigma_acc = NULL;

static void init_cosmocalc_peakheight_table(void);

static void init_cosmocalc_peakheight_table(void)
{
  static int initFlag = 1;
  static int currCosmoNum;

  double sigmar_table[COSMOCALC_PEAKHEIGHT_TABLE_LENGTH];
  double topHatRad_table[COSMOCALC_PEAKHEIGHT_TABLE_LENGTH];
  size_t sindex[COSMOCALC_PEAKHEIGHT_TABLE_LENGTH];
  double tmpDouble[COSMOCALC_PEAKHEIGHT_TABLE_LENGTH];
  long i;
  double rad;

  if(initFlag == 1 || currCosmoNum != cosmoData.cosmoNum)
    {
      initFlag = 0;
      currCosmoNum = cosmoData.cosmoNum;

      linear_powspec_norm = cosmoData.Sigma8*cosmoData.Sigma8/tophatradnorm_linear_powspec_exact_nonorm(8.0);

      for(i=0;i<COSMOCALC_PEAKHEIGHT_TABLE_LENGTH;++i)
        {
          rad = log(PH_R_MAX/PH_R_MIN)/(COSMOCALC_PEAKHEIGHT_TABLE_LENGTH-1.0)*((double) i) + log(PH_R_MIN);
          topHatRad_table[i] = rad;
          sigmar_table[i] = log(sqrt(linear_powspec_norm*tophatradnorm_linear_powspec_exact_nonorm(exp(rad))));
        }

      //init the spline and accelerators
      if(cosmocalc_sigmaR_spline != NULL)
        gsl_spline_free(cosmocalc_sigmaR_spline);
      cosmocalc_sigmaR_spline = gsl_spline_alloc(GSL_SPLINE_TYPE,(size_t) (COSMOCALC_PEAKHEIGHT_TABLE_LENGTH));
      gsl_spline_init(cosmocalc_sigmaR_spline,topHatRad_table,sigmar_table,(size_t) (COSMOCALC_PEAKHEIGHT_TABLE_LENGTH));
      if(cosmocalc_sigmaR_acc != NULL)
        gsl_interp_accel_reset(cosmocalc_sigmaR_acc);
      else
        cosmocalc_sigmaR_acc = gsl_interp_accel_alloc();

      //reverse the table
      gsl_sort_index(sindex,sigmar_table,(size_t) 1,(size_t) (COSMOCALC_PEAKHEIGHT_TABLE_LENGTH));
      for(i=0;i<COSMOCALC_PEAKHEIGHT_TABLE_LENGTH;++i)
        tmpDouble[i] = sigmar_table[sindex[i]];
      for(i=0;i<COSMOCALC_PEAKHEIGHT_TABLE_LENGTH;++i)
        sigmar_table[i] = tmpDouble[i];
      for(i=0;i<COSMOCALC_PEAKHEIGHT_TABLE_LENGTH;++i)
        tmpDouble[i] = topHatRad_table[sindex[i]];
      for(i=0;i<COSMOCALC_PEAKHEIGHT_TABLE_LENGTH;++i)
        topHatRad_table[i] = tmpDouble[i];

      //init the spline and accelerators
      if(cosmocalc_Rsigma_spline != NULL)
        gsl_spline_free(cosmocalc_Rsigma_spline);
      cosmocalc_Rsigma_spline = gsl_spline_alloc(GSL_SPLINE_TYPE,(size_t) (COSMOCALC_PEAKHEIGHT_TABLE_LENGTH));
      gsl_spline_init(cosmocalc_Rsigma_spline,sigmar_table,topHatRad_table,(size_t) (COSMOCALC_PEAKHEIGHT_TABLE_LENGTH));
      if(cosmocalc_Rsigma_acc != NULL)
        gsl_interp_accel_reset(cosmocalc_Rsigma_acc);
      else
        cosmocalc_Rsigma_acc = gsl_interp_accel_alloc();
    }
}

double sigmaRtophat_exact(double topHatRad, double a)
{
  static int initFlag = 1;
  static int currCosmoNum;
  static double linear_powspec_norm;
  double gf;

  if(initFlag == 1 || currCosmoNum != cosmoData.cosmoNum)
    {
      initFlag = 0;
      currCosmoNum = cosmoData.cosmoNum;

      linear_powspec_norm = cosmoData.Sigma8*cosmoData.Sigma8/tophatradnorm_linear_powspec_exact_nonorm(8.0);
    }

  gf = growth_function(a);

  return sqrt(linear_powspec_norm*tophatradnorm_linear_powspec_exact_nonorm(topHatRad))*gf;
}

double sigmaRtophat(double topHatRad, double a)
{
  static int initFlag = 1;
  static int currCosmoNum;

  if(initFlag == 1 || currCosmoNum != cosmoData.cosmoNum)
    {
      initFlag = 0;
      currCosmoNum = cosmoData.cosmoNum;
      init_cosmocalc_peakheight_table();
    }

  return exp(gsl_spline_eval(cosmocalc_sigmaR_spline,log(topHatRad),cosmocalc_sigmaR_acc))*growth_function(a);
}

double sigmaMtophat(double m, double a)
{
  return sigmaRtophat(pow(m/(4.0/3.0*M_PI*RHO_CRIT*cosmoData.OmegaM),1.0/3.0),a);
}

double sigmaMtophat_exact(double m, double a)
{
  return sigmaRtophat_exact(pow(m/(4.0/3.0*M_PI*RHO_CRIT*cosmoData.OmegaM),1.0/3.0),a);
}

double inverse_sigmaRtophat(double sigmaR, double a)
{
  static int initFlag = 1;
  static int currCosmoNum;

  if(initFlag == 1 || currCosmoNum != cosmoData.cosmoNum)
    {
      initFlag = 0;
      currCosmoNum = cosmoData.cosmoNum;
      init_cosmocalc_peakheight_table();
    }

  return exp(gsl_spline_eval(cosmocalc_Rsigma_spline,log(sigmaR/growth_function(a)),cosmocalc_Rsigma_acc));
}

double inverse_sigmaMtophat(double sigmaR, double a)
{
  return (4.0/3.0*M_PI*RHO_CRIT*cosmoData.OmegaM)*pow(inverse_sigmaRtophat(sigmaR,a),3.0);
}

double inverse_nuMtophat(double nu, double a)
{
  return (4.0/3.0*M_PI*RHO_CRIT*cosmoData.OmegaM)*pow(inverse_sigmaRtophat(DELTAC/nu,a),3.0);
}

double inverse_nuRtophat(double nu, double a)
{
  return inverse_sigmaRtophat(DELTAC/nu,a);
}


#include <math.h>
#include <stdlib.h>
#include <drfftw_mpi.h>
#include <mpi.h>
#include <gsl/gsl_rng.h>

#include "allvars.h"
#include "proto.h"

int main(int argc, char **argv)
{
  MPI_Init(&argc, &argv);
  MPI_Comm_rank(MPI_COMM_WORLD, &ThisTask);
  MPI_Comm_size(MPI_COMM_WORLD, &NTask);

  if(argc < 2)
    {
      if(ThisTask == 0)
	{
	  fprintf(stdout, "\nParameters are missing.\n");
	  fprintf(stdout, "Call with <ParameterFile>\n\n");
	}
      MPI_Finalize();
      exit(0);
    }

  read_parameterfile(argv[1]);

  set_units();

  initialize_powerspectrum();

  initialize_ffts();

  read_glass(GlassFile);

  displacement_fields();

  write_particle_data();

  if(NumPart)
    free(P);

  free_ffts();

  if(ThisTask == 0)
    {
      printf("\nIC's generated.\n\n");
      printf("Initial scale factor = %g\n", InitTime);
      printf("\n");
    }

  MPI_Barrier(MPI_COMM_WORLD);
  print_spec();

  MPI_Finalize();		/* clean up & finalize MPI */
  exit(0);
}

void displacement_fields(void)
{
  MPI_Request request;
  MPI_Status status;
  gsl_rng *random_generator;
  int i, j, k, ii, jj, kk, axes;
  int n;
  int sendTask, recvTask;
  double fac, vel_prefac;
  double kvec[3], kmag, kmag2, p_of_k;
  double delta, phase, ampl, hubble_a;
  double u, v, w;
  double f1, f2, f3, f4, f5, f6, f7, f8;
  double dis, maxdisp, max_disp_glob;
  unsigned int *seedtable;

#ifdef CORRECT_CIC
  double fx, fy, fz, ff, smth;
#endif

  if(ThisTask == 0)
    {
      printf("\nstart computing displacement fields...\n");
      fflush(stdout);
    }

  hubble_a =
    Hubble * sqrt(Omega / pow(InitTime, 3) + (1 - Omega - OmegaLambda) / pow(InitTime, 2) + OmegaLambda);

  vel_prefac = InitTime * hubble_a * F_Omega(InitTime);

  vel_prefac /= sqrt(InitTime);	/* converts to Gadget velocity */

  if(ThisTask == 0)
    printf("vel_prefac= %g  hubble_a=%g fom=%g \n", vel_prefac, hubble_a, F_Omega(InitTime));

  fac = pow(2 * PI / Box, 1.5);

  maxdisp = 0;

  random_generator = gsl_rng_alloc(gsl_rng_ranlxd1);

  gsl_rng_set(random_generator, Seed);

  if(!(seedtable = malloc(Nmesh * Nmesh * sizeof(unsigned int))))
    FatalError(4);

  for(i = 0; i < Nmesh / 2; i++)
    {
      for(j = 0; j < i; j++)
	seedtable[i * Nmesh + j] = 0x7fffffff * gsl_rng_uniform(random_generator);

      for(j = 0; j < i + 1; j++)
	seedtable[j * Nmesh + i] = 0x7fffffff * gsl_rng_uniform(random_generator);

      for(j = 0; j < i; j++)
	seedtable[(Nmesh - 1 - i) * Nmesh + j] = 0x7fffffff * gsl_rng_uniform(random_generator);

      for(j = 0; j < i + 1; j++)
	seedtable[(Nmesh - 1 - j) * Nmesh + i] = 0x7fffffff * gsl_rng_uniform(random_generator);

      for(j = 0; j < i; j++)
	seedtable[i * Nmesh + (Nmesh - 1 - j)] = 0x7fffffff * gsl_rng_uniform(random_generator);

      for(j = 0; j < i + 1; j++)
	seedtable[j * Nmesh + (Nmesh - 1 - i)] = 0x7fffffff * gsl_rng_uniform(random_generator);

      for(j = 0; j < i; j++)
	seedtable[(Nmesh - 1 - i) * Nmesh + (Nmesh - 1 - j)] = 0x7fffffff * gsl_rng_uniform(random_generator);

      for(j = 0; j < i + 1; j++)
	seedtable[(Nmesh - 1 - j) * Nmesh + (Nmesh - 1 - i)] = 0x7fffffff * gsl_rng_uniform(random_generator);
    }

  for(axes = 0; axes < 3; axes++)
    {
      if(ThisTask == 0)
	{
	  printf("\nstarting axes=%d...\n", axes);
	  fflush(stdout);
	}

      /* first, clean the array */
      for(i = 0; i < Local_nx; i++)
	for(j = 0; j < Nmesh; j++)
	  for(k = 0; k <= Nmesh / 2; k++)
	    {
	      Cdata[(i * Nmesh + j) * (Nmesh / 2 + 1) + k].re = 0;
	      Cdata[(i * Nmesh + j) * (Nmesh / 2 + 1) + k].im = 0;
	    }

      for(i = 0; i < Nmesh; i++)
	{
	  ii = Nmesh - i;
	  if(ii == Nmesh)
	    ii = 0;
	  if((i >= Local_x_start && i < (Local_x_start + Local_nx)) ||
	     (ii >= Local_x_start && ii < (Local_x_start + Local_nx)))
	    {
	      for(j = 0; j < Nmesh; j++)
		{
		  gsl_rng_set(random_generator, seedtable[i * Nmesh + j]);

		  for(k = 0; k < Nmesh / 2; k++)
		    {
		      phase = gsl_rng_uniform(random_generator) * 2 * PI;
		      do
			ampl = gsl_rng_uniform(random_generator);
		      while(ampl == 0);

		      if(i == Nmesh / 2 || j == Nmesh / 2 || k == Nmesh / 2)
			continue;
		      if(i == 0 && j == 0 && k == 0)
			continue;

		      if(i < Nmesh / 2)
			kvec[0] = i * 2 * PI / Box;
		      else
			kvec[0] = -(Nmesh - i) * 2 * PI / Box;

		      if(j < Nmesh / 2)
			kvec[1] = j * 2 * PI / Box;
		      else
			kvec[1] = -(Nmesh - j) * 2 * PI / Box;

		      if(k < Nmesh / 2)
			kvec[2] = k * 2 * PI / Box;
		      else
			kvec[2] = -(Nmesh - k) * 2 * PI / Box;

		      kmag2 = kvec[0] * kvec[0] + kvec[1] * kvec[1] + kvec[2] * kvec[2];
		      kmag = sqrt(kmag2);

		      if(SphereMode == 1)
			{
			  if(kmag * Box / (2 * PI) > Nsample / 2)	/* select a sphere in k-space */
			    continue;
			}
		      else
			{
			  if(fabs(kvec[0]) * Box / (2 * PI) > Nsample / 2)
			    continue;
			  if(fabs(kvec[1]) * Box / (2 * PI) > Nsample / 2)
			    continue;
			  if(fabs(kvec[2]) * Box / (2 * PI) > Nsample / 2)
			    continue;
			}

		      p_of_k = PowerSpec(kmag);

		      p_of_k *= -log(ampl);

		      delta = fac * sqrt(p_of_k) / Dplus;	/* scale back to starting redshift */

#ifdef CORRECT_CIC
		      /* do deconvolution of CIC interpolation */
		      fx = fy = fz = 1;
		      if(kvec[0] != 0)
			{
			  fx = (kvec[0] * Box / 2) / Nmesh;
			  fx = sin(fx) / fx;
			}
		      if(kvec[1] != 0)
			{
			  fy = (kvec[1] * Box / 2) / Nmesh;
			  fy = sin(fy) / fy;
			}
		      if(kvec[2] != 0)
			{
			  fz = (kvec[2] * Box / 2) / Nmesh;
			  fz = sin(fz) / fz;
			}
		      ff = 1 / (fx * fy * fz);
		      smth = ff * ff;

		      delta *= smth;
		      /* end deconvolution */
#endif
		      if(k > 0)
			{
			  if(i >= Local_x_start && i < (Local_x_start + Local_nx))
			    {
			      Cdata[((i - Local_x_start) * Nmesh + j) * (Nmesh / 2 + 1) + k].re =
				-kvec[axes] / kmag2 * delta * sin(phase);
			      Cdata[((i - Local_x_start) * Nmesh + j) * (Nmesh / 2 + 1) + k].im =
				kvec[axes] / kmag2 * delta * cos(phase);
			    }
			}
		      else	/* k=0 plane needs special treatment */
			{
			  if(i == 0)
			    {
			      if(j >= Nmesh / 2)
				continue;
			      else
				{
				  if(i >= Local_x_start && i < (Local_x_start + Local_nx))
				    {
				      jj = Nmesh - j;	/* note: j!=0 surely holds at this point */

				      Cdata[((i - Local_x_start) * Nmesh + j) * (Nmesh / 2 + 1) + k].re =
					-kvec[axes] / kmag2 * delta * sin(phase);
				      Cdata[((i - Local_x_start) * Nmesh + j) * (Nmesh / 2 + 1) + k].im =
					kvec[axes] / kmag2 * delta * cos(phase);

				      Cdata[((i - Local_x_start) * Nmesh + jj) * (Nmesh / 2 + 1) + k].re =
					-kvec[axes] / kmag2 * delta * sin(phase);
				      Cdata[((i - Local_x_start) * Nmesh + jj) * (Nmesh / 2 + 1) + k].im =
					-kvec[axes] / kmag2 * delta * cos(phase);
				    }
				}
			    }
			  else	/* here comes i!=0 : conjugate can be on other processor! */
			    {
			      if(i >= Nmesh / 2)
				continue;
			      else
				{
				  ii = Nmesh - i;
				  if(ii == Nmesh)
				    ii = 0;
				  jj = Nmesh - j;
				  if(jj == Nmesh)
				    jj = 0;

				  if(i >= Local_x_start && i < (Local_x_start + Local_nx))
				    {
				      Cdata[((i - Local_x_start) * Nmesh + j) * (Nmesh / 2 + 1) + k].re =
					-kvec[axes] / kmag2 * delta * sin(phase);
				      Cdata[((i - Local_x_start) * Nmesh + j) * (Nmesh / 2 + 1) + k].im =
					kvec[axes] / kmag2 * delta * cos(phase);
				    }

				  if(ii >= Local_x_start && ii < (Local_x_start + Local_nx))
				    {
				      Cdata[((ii - Local_x_start) * Nmesh + jj) * (Nmesh / 2 + 1) +
					    k].re = -kvec[axes] / kmag2 * delta * sin(phase);
				      Cdata[((ii - Local_x_start) * Nmesh + jj) * (Nmesh / 2 + 1) +
					    k].im = -kvec[axes] / kmag2 * delta * cos(phase);
				    }
				}
			    }
			}
		    }
		}
	    }
	}

      rfftwnd_mpi(Inverse_plan, 1, Disp, Workspace, FFTW_NORMAL_ORDER);		/** FFT **/

      /* now get the plane on the right side from neighbour on the right,
         and send the left plane */

      recvTask = ThisTask;
      do
	{
	  recvTask--;
	  if(recvTask < 0)
	    recvTask = NTask - 1;
	}
      while(Local_nx_table[recvTask] == 0);

      sendTask = ThisTask;
      do
	{
	  sendTask++;
	  if(sendTask >= NTask)
	    sendTask = 0;
	}
      while(Local_nx_table[sendTask] == 0);

      /* use non-blocking send */

      if(Local_nx > 0)
	{
	  MPI_Isend(&Disp[0],
		    sizeof(fftw_real) * Nmesh * (2 * (Nmesh / 2 + 1)),
		    MPI_BYTE, recvTask, 10, MPI_COMM_WORLD, &request);

	  MPI_Recv(&Disp[(Local_nx * Nmesh) * (2 * (Nmesh / 2 + 1))],
		   sizeof(fftw_real) * Nmesh * (2 * (Nmesh / 2 + 1)),
		   MPI_BYTE, sendTask, 10, MPI_COMM_WORLD, &status);

	  MPI_Wait(&request, &status);
	}

      /* read-out displacements */

      for(n = 0; n < NumPart; n++)
	{
	  {
	    u = P[n].Pos[0] / Box * Nmesh;
	    v = P[n].Pos[1] / Box * Nmesh;
	    w = P[n].Pos[2] / Box * Nmesh;

	    i = (int) u;
	    j = (int) v;
	    k = (int) w;

	    if(i == (Local_x_start + Local_nx))
	      i = (Local_x_start + Local_nx) - 1;
	    if(i < Local_x_start)
	      i = Local_x_start;
	    if(j == Nmesh)
	      j = Nmesh - 1;
	    if(k == Nmesh)
	      k = Nmesh - 1;

	    u -= i;
	    v -= j;
	    w -= k;

	    i -= Local_x_start;
	    ii = i + 1;
	    jj = j + 1;
	    kk = k + 1;

	    if(jj >= Nmesh)
	      jj -= Nmesh;
	    if(kk >= Nmesh)
	      kk -= Nmesh;

	    f1 = (1 - u) * (1 - v) * (1 - w);
	    f2 = (1 - u) * (1 - v) * (w);
	    f3 = (1 - u) * (v) * (1 - w);
	    f4 = (1 - u) * (v) * (w);
	    f5 = (u) * (1 - v) * (1 - w);
	    f6 = (u) * (1 - v) * (w);
	    f7 = (u) * (v) * (1 - w);
	    f8 = (u) * (v) * (w);

	    dis = Disp[(i * Nmesh + j) * (2 * (Nmesh / 2 + 1)) + k] * f1 +
	      Disp[(i * Nmesh + j) * (2 * (Nmesh / 2 + 1)) + kk] * f2 +
	      Disp[(i * Nmesh + jj) * (2 * (Nmesh / 2 + 1)) + k] * f3 +
	      Disp[(i * Nmesh + jj) * (2 * (Nmesh / 2 + 1)) + kk] * f4 +
	      Disp[(ii * Nmesh + j) * (2 * (Nmesh / 2 + 1)) + k] * f5 +
	      Disp[(ii * Nmesh + j) * (2 * (Nmesh / 2 + 1)) + kk] * f6 +
	      Disp[(ii * Nmesh + jj) * (2 * (Nmesh / 2 + 1)) + k] * f7 +
	      Disp[(ii * Nmesh + jj) * (2 * (Nmesh / 2 + 1)) + kk] * f8;

	    P[n].Vel[axes] = dis;

	    if(dis > maxdisp)
	      maxdisp = dis;
	  }
	}
    }

  /* now add displacement to Lagrangian coordinates, and multiply velocities by correct factor */
  for(n = 0; n < NumPart; n++)
    {
      for(axes = 0; axes < 3; axes++)
	{
	  P[n].Pos[axes] += P[n].Vel[axes];
	  P[n].Vel[axes] *= vel_prefac;
	  P[n].Pos[axes] = periodic_wrap(P[n].Pos[axes]);
	}
    }

  gsl_rng_free(random_generator);

  MPI_Reduce(&maxdisp, &max_disp_glob, 1, MPI_DOUBLE, MPI_MAX, 0, MPI_COMM_WORLD);

  if(ThisTask == 0)
    {
      printf("\nMaximum displacement: %g kpc/h, in units of the part-spacing= %g\n",
	     max_disp_glob, max_disp_glob / (Box / Nmesh));
    }
}

double periodic_wrap(double x)
{
  while(x >= Box)
    x -= Box;

  while(x < 0)
    x += Box;

  return x;
}

void set_units(void)		/* ... set some units */
{
  UnitTime_in_s = UnitLength_in_cm / UnitVelocity_in_cm_per_s;

  G = GRAVITY / pow(UnitLength_in_cm, 3) * UnitMass_in_g * pow(UnitTime_in_s, 2);
  Hubble = HUBBLE * UnitTime_in_s;
}

void initialize_ffts(void)
{
  int total_size, i, additional;
  int local_ny_after_transpose, local_y_start_after_transpose;
  int *slab_to_task_local;
  size_t bytes;

  Inverse_plan = rfftw3d_mpi_create_plan(MPI_COMM_WORLD,
					 Nmesh, Nmesh, Nmesh, FFTW_COMPLEX_TO_REAL, FFTW_ESTIMATE);

  rfftwnd_mpi_local_sizes(Inverse_plan, &Local_nx, &Local_x_start,
			  &local_ny_after_transpose, &local_y_start_after_transpose, &total_size);

  Local_nx_table = malloc(sizeof(int) * NTask);
  MPI_Allgather(&Local_nx, 1, MPI_INT, Local_nx_table, 1, MPI_INT, MPI_COMM_WORLD);

  if(ThisTask == 0)
    {
      for(i = 0; i < NTask; i++)
	printf("Task=%d Local_nx=%d\n", i, Local_nx_table[i]);
      fflush(stdout);
    }

  Slab_to_task = malloc(sizeof(int) * Nmesh);
  slab_to_task_local = malloc(sizeof(int) * Nmesh);

  for(i = 0; i < Nmesh; i++)
    slab_to_task_local[i] = 0;

  for(i = 0; i < Local_nx; i++)
    slab_to_task_local[Local_x_start + i] = ThisTask;

  MPI_Allreduce(slab_to_task_local, Slab_to_task, Nmesh, MPI_INT, MPI_SUM, MPI_COMM_WORLD);

  free(slab_to_task_local);

  additional = (Nmesh) * (2 * (Nmesh / 2 + 1));	/* additional plane on the right side */

  Disp = (fftw_real *) malloc(bytes = sizeof(fftw_real) * (total_size + additional));
  Workspace = (fftw_real *) malloc(bytes += sizeof(fftw_real) * total_size);

  if(Disp && Workspace)
    {
      if(ThisTask == 0)
	printf("\nallocated %g Mbyte on Task %d for FFT's\n", bytes / (1024.0 * 1024.0), ThisTask);
    }
  else
    {
      printf("failed to allocate %g Mbyte on Task %d\n", bytes / (1024.0 * 1024.0), ThisTask);
      printf("bailing out.\n");
      FatalError(1);
    }

  Cdata = (fftw_complex *) Disp;	/* transformed array */
}

void free_ffts(void)
{
  free(Workspace);
  free(Disp);
  free(Slab_to_task);
  rfftwnd_mpi_destroy_plan(Inverse_plan);
}

int FatalError(int errnum)
{
  printf("FatalError called with number=%d\n", errnum);
  fflush(stdout);
  MPI_Abort(MPI_COMM_WORLD, errnum);
  exit(0);
}

static double A, B, alpha, beta, V, gf;

double fnl(double x)		/* Peacock & Dodds formula */
{
  return x * pow((1 + B * beta * x + pow(A * x, alpha * beta)) /
		 (1 + pow(pow(A * x, alpha) * gf * gf * gf / (V * sqrt(x)), beta)), 1 / beta);
}

void print_spec(void)
{
  double k, knl, po, dl, dnl, neff, kf, kstart, kend, po2, po1, DDD;
  char buf[1000];
  FILE *fd;

  if(ThisTask == 0)
    {
      sprintf(buf, "%s/inputspec_%s.txt", OutputDir, FileBase);

      fd = fopen(buf, "w");

      gf = GrowthFactor(0.001, 1.0) / (1.0 / 0.001);

      DDD = GrowthFactor(1.0 / (Redshift + 1), 1.0);

      fprintf(fd, "%12g %12g\n", Redshift, DDD);	/* print actual starting redshift and
							   linear growth factor for this cosmology */

      kstart = 2 * PI / (1000.0 * (3.085678e24 / UnitLength_in_cm));	/* 1000 Mpc/h */
      kend = 2 * PI / (0.001 * (3.085678e24 / UnitLength_in_cm));	/* 0.001 Mpc/h */

      for(k = kstart; k < kend; k *= 1.025)
	{
	  po = PowerSpec(k);
	  dl = 4.0 * PI * k * k * k * po;

	  kf = 0.5;

	  po2 = PowerSpec(1.001 * k * kf);
	  po1 = PowerSpec(k * kf);

	  if(po != 0 && po1 != 0 && po2 != 0)
	    {
	      neff = (log(po2) - log(po1)) / (log(1.001 * k * kf) - log(k * kf));

	      if(1 + neff / 3 > 0)
		{
		  A = 0.482 * pow(1 + neff / 3, -0.947);
		  B = 0.226 * pow(1 + neff / 3, -1.778);
		  alpha = 3.310 * pow(1 + neff / 3, -0.244);
		  beta = 0.862 * pow(1 + neff / 3, -0.287);
		  V = 11.55 * pow(1 + neff / 3, -0.423) * 1.2;

		  dnl = fnl(dl);
		  knl = k * pow(1 + dnl, 1.0 / 3);
		}
	      else
		{
		  dnl = 0;
		  knl = 0;
		}
	    }
	  else
	    {
	      dnl = 0;
	      knl = 0;
	    }

	  fprintf(fd, "%12g %12g    %12g %12g\n", k, dl, knl, dnl);
	}
      fclose(fd);
    }
}


/*
  NAME:
     proj_gauss_mixtures_IDL
  PURPOSE:
     run the projected gaussian mixtures algorithm from IDL (or python)
  CALLING SEQUENCE:
     see IDL wrapper
  INPUT:
     from IDL wrapper
  OUTPUT:
     updated model gaussians and average loglikelihood, see IDL WRAPPER
  REVISION HISTORY:
     2008-09-21 - Written Bovy
     2010-03-01 Added noproj option - Bovy
     2010-04-01 Added noweight option and logweights - Bovy
*/
#include <stdio.h>
#include <stdbool.h>
#include <time.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <proj_gauss_mixtures.h>

int proj_gauss_mixtures_IDL(double * ydata, double * ycovar,
			    double * projection, double * logweights,
			    int N, int dy,
			    double * amp, double * xmean,
			    double * xcovar, int d, int K,
			    char * fixamp, char * fixmean,
			    char * fixcovar,
			    double * avgloglikedata, double tol,
			    int maxiter, char likeonly, double w,
			    char * logfilename, int slen, int splitnmerge,
			    char * convlogfilename, int convloglen,
			    char noprojection,char diagerrors,
			    char noweights){
  //Set up logfiles
  bool keeplog = true;
  char logname[slen+1];
  char convlogname[convloglen+1];
  int ss;
  if (*logfilename == 0 || likeonly != 0 || slen == 0)
    keeplog = false;
  else {
    for (ss = 0; ss != slen; ++ss)
      logname[ss] = (char) *(logfilename++);
    for (ss = 0; ss != convloglen; ++ss)
      convlogname[ss] = (char) *(convlogfilename++);
    logfilename -= slen;
    convlogfilename -= convloglen;
    logname[slen] = '\0';
    convlogname[convloglen] = '\0';
  }

  if (keeplog) {
    logfile = fopen(logname,"a");
    if (logfile == NULL) return -1;
    convlogfile = fopen(convlogname,"w");
    if (convlogfile == NULL) return -1;
  }

  if (keeplog){
    time_t now;
    time(&now);
    fprintf(logfile,"#----------------------------------\n");
    fprintf(logfile,"#\n#%s\n",asctime(localtime(&now)));
    fprintf(logfile,"#----------------------------------\n");
    fflush(logfile);
  }

  //Copy everything into the right formats
  struct datapoint * data = (struct datapoint *) malloc( N * sizeof (struct datapoint) );
  struct gaussian * gaussians = (struct gaussian *) malloc (K * sizeof (struct gaussian) );

  bool noproj= (bool) noprojection;
  bool noweight= (bool) noweights;
  bool diagerrs= (bool) diagerrors;
  int ii, jj,dd1,dd2;
  for (ii = 0; ii != N; ++ii){
    data->ww = gsl_vector_alloc(dy);
    if ( ! noweight ) data->logweight = *(logweights++);
    if ( diagerrs ) data->SS = gsl_matrix_alloc(dy,1);
    else data->SS = gsl_matrix_alloc(dy,dy);
    if ( ! noproj ) data->RR = gsl_matrix_alloc(dy,d);
    for (dd1 = 0; dd1 != dy;++dd1)
      gsl_vector_set(data->ww,dd1,*(ydata++));
    if ( diagerrs)
      for (dd1 = 0; dd1 != dy; ++dd1)
	  gsl_matrix_set(data->SS,dd1,0,*(ycovar++));
    else
      for (dd1 = 0; dd1 != dy; ++dd1)
	for (dd2 = 0; dd2 != dy; ++dd2)
	  gsl_matrix_set(data->SS,dd1,dd2,*(ycovar++));
    if ( ! noproj )
      for (dd1 = 0; dd1 != dy; ++dd1)
	for (dd2 = 0; dd2 != d; ++dd2)
	  gsl_matrix_set(data->RR,dd1,dd2,*(projection++));
    else data->RR= NULL;
    ++data;
  }
  data -= N;
  ydata -= N*dy;
  if ( diagerrs ) ycovar -= N*dy;
  else ycovar -= N*dy*dy;
  if ( ! noproj ) projection -= N*dy*d;

  for (jj = 0; jj != K; ++jj){
    gaussians->mm = gsl_vector_alloc(d);
    gaussians->VV = gsl_matrix_alloc(d,d);
    gaussians->alpha = *(amp++);
    for (dd1 = 0; dd1 != d; ++dd1)
      gsl_vector_set(gaussians->mm,dd1,*(xmean++));
    for (dd1 = 0; dd1 != d; ++dd1)
      for (dd2 = 0; dd2 != d; ++dd2)
	gsl_matrix_set(gaussians->VV,dd1,dd2,*(xcovar++));
    ++gaussians;
  }
  gaussians -= K;
  amp -= K;
  xmean -= K*d;
  xcovar -= K*d*d;

  //Print the initial model parameters to the logfile
  int kk;
  if (keeplog){
    fprintf(logfile,"#\n#Using %i Gaussians and w = %f\n\n",K,w);
    fprintf(logfile,"#\n#Initial model parameters used:\n\n");
    for (kk=0; kk != K; ++kk){
      fprintf(logfile,"#Gaussian ");
      fprintf(logfile,"%i",kk);
      fprintf(logfile,"\n");
      fprintf(logfile,"#amp\t=\t");
      fprintf(logfile,"%f",(*gaussians).alpha);
      fprintf(logfile,"\n");
      fprintf(logfile,"#mean\t=\t");
      for (dd1=0; dd1 != d; ++dd1){
	fprintf(logfile,"%f",gsl_vector_get(gaussians->mm,dd1));
	if (dd1 < d-1) fprintf(logfile,"\t");
      }
      fprintf(logfile,"\n");
      fprintf(logfile,"#covar\t=\t");
      for (dd1=0; dd1 != d; ++dd1)
	fprintf(logfile,"%f\t",gsl_matrix_get(gaussians->VV,dd1,dd1));
      for (dd1=0; dd1 != d-1; ++dd1)
	for (dd2=dd1+1; dd2 != d; ++dd2){
	  fprintf(logfile,"%f\t",gsl_matrix_get(gaussians->VV,dd1,dd2));
	}
      ++gaussians;
      fprintf(logfile,"\n#\n");
    }
    gaussians -= K;
    fflush(logfile);
  }

  //Then run projected_gauss_mixtures
  proj_gauss_mixtures(data,N,gaussians,K,(bool *) fixamp,
		      (bool *) fixmean, (bool *) fixcovar,avgloglikedata,
		      tol,(long long int) maxiter, (bool) likeonly, w,
		      splitnmerge,keeplog,logfile,convlogfile,noproj,diagerrs,
		      noweight);

  //Print the final model parameters to the logfile
  if (keeplog){
    fprintf(logfile,"\n#Final model parameters obtained:\n\n");
    for (kk=0; kk != K; ++kk){
      fprintf(logfile,"#Gaussian ");
      fprintf(logfile,"%i",kk);
      fprintf(logfile,"\n");
      fprintf(logfile,"#amp\t=\t");
      fprintf(logfile,"%f",(*gaussians).alpha);
      fprintf(logfile,"\n");
      fprintf(logfile,"#mean\t=\t");
      for (dd1=0; dd1 != d; ++dd1){
	fprintf(logfile,"%f",gsl_vector_get(gaussians->mm,dd1));
	if (dd1 < d-1) fprintf(logfile,"\t");
      }
      fprintf(logfile,"\n");
      fprintf(logfile,"#covar\t=\t");
      for (dd1=0; dd1 != d; ++dd1)
	fprintf(logfile,"%f\t",gsl_matrix_get(gaussians->VV,dd1,dd1));
      for (dd1=0; dd1 != d-1; ++dd1)
	for (dd2=dd1+1; dd2 != d; ++dd2){
	  fprintf(logfile,"%f\t",gsl_matrix_get(gaussians->VV,dd1,dd2));
	}
      ++gaussians;
      fprintf(logfile,"\n#\n");
    }
    gaussians -= K;
    fflush(logfile);
  }

  //Then update the arrays given to us by IDL
  for (jj = 0; jj != K; ++jj){
    *(amp++) = gaussians->alpha;
    for (dd1 = 0; dd1 != d; ++dd1)
      *(xmean++) = gsl_vector_get(gaussians->mm,dd1);
    for (dd1 = 0; dd1 != d; ++dd1)
      for (dd2 = 0; dd2 != d; ++dd2)
	*(xcovar++) = gsl_matrix_get(gaussians->VV,dd1,dd2);
    ++gaussians;
  }
  gaussians -= K;
  amp -= K;
  xmean -= K*d;
  xcovar -= K*d*d;

  //And free any memory we allocated
  for (ii = 0; ii != N; ++ii){
    gsl_vector_free(data->ww);
    gsl_matrix_free(data->SS);
    if ( ! noproj )  gsl_matrix_free(data->RR);
    ++data;
  }
  data -= N;
  free(data);

  for (jj = 0; jj != K; ++jj){
    gsl_vector_free(gaussians->mm);
    gsl_matrix_free(gaussians->VV);
    ++gaussians;
  }
  gaussians -= K;
  free(gaussians);

  if (keeplog){
    fclose(logfile);
    fclose(convlogfile);
  }

  return 0;
}


#include <math.h>
#include <stdlib.h>
#if !defined(__APPLE__)
#include <malloc.h>
#endif
#include <stdio.h>
#include <assert.h>
#include <time.h>
#include <string.h>
#include <fftw3.h>

#include <gsl/gsl_interp2d.h>
#include <gsl/gsl_spline2d.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_erf.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_sf_legendre.h>
#include <gsl/gsl_sf_bessel.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_sf_expint.h>
#include <gsl/gsl_deriv.h>

#include "../../../cosmolike_core/theory/basics.c"
#include "tinker_emulator.h"
#include "tinker_emulator.c"

int main(){
  //Test kernel
  double x0[7] = {0.0043*pow(0.7,2), 0.286*pow(0.7,2), -1, 1, 3.0, 70, 3.0};
  double x1[7] = {0.0043*pow(0.7,2), 0.286*pow(0.7,2), -1, 1, 3.0, 70, 5.0};
  printf("kernel : %e \n", kernel(x0, x1, emu_tinker_bias_log_lambda[0], tinkerEmuParam.tinker_bias_ncosmo));
  //Test bias emulator
  double emu_tinker_bias_param[6];
  predict_tinker_bias_parameters(0.0043*pow(0.7,2), 0.286*pow(0.7,2), -1, 1, 3.0, 70, 3.0, 0.3, emu_tinker_bias_param);
  double bias_ref[6] = {4.432419358797392, 0.32058653615384625, 1.3586846172228393, 1.347376928846154, 0.728081326923077, -1.0947315110189246};
  printf("bias result:\n");
  printf("item c ref\n");
  for(int i=0; i<tinkerEmuParam.tinker_bias_nparam_redshift; ++i) printf("%d, %e, %e\n", i,emu_tinker_bias_param[i], bias_ref[i]);
  printf("####################\n");

  //Test hmf emulator
  double emu_tinker_hmf_param[4];
  double hmf_ref[4] = { 2.5198116919192444, 0.8454997909106351, 0.5441108055593771, 1.3001900391246013};
  predict_tinker_hmf_parameters(0.0043*pow(0.7,2), 0.286*pow(0.7,2), -1, 1, 3.0, 70, 3.0, 0.3, emu_tinker_hmf_param);
  printf("hmf result:\n");
  printf("item c ref\n");
  for(int i=0; i<tinkerEmuParam.tinker_hmf_nparam_redshift; ++i) printf("%d, %e, %e\n", i,emu_tinker_hmf_param[i], hmf_ref[i]);
  printf("####################\n");

}


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

#include <fastpm/libfastpm.h>
#include <fastpm/logging.h>
#include <fastpm/constrainedgaussian.h>

#include "pmpfft.h"
#include <gsl/gsl_linalg.h>

double fastpm_2pcf_eval(FastPM2PCF* self, double r)
{
    double rMax = self->size * self->step_size;
    if(r > rMax)
        return 0.0;
    if(r == rMax)
        return self->xi[self->size];
    int i = (int)floor(r / self->step_size);
    double prev = self->xi[i], next = self->xi[i + 1];
    double deltaR = r - i * self->step_size;
    return prev + (next - prev) * deltaR / self->step_size;
}

/*
void
fastpm_generate_covariance_matrix(PM *pm, fastpm_fkfunc pkfunc, void * data, FastPMFloat *cov_x)
{
}
*/
void
fastpm_2pcf_from_powerspectrum(FastPM2PCF *self, fastpm_fkfunc pkfunc, void * data, double r_max, int steps)
{
    self->size = steps;
    self->step_size = r_max / steps;
    self->xi = malloc((steps + 1) * sizeof(double));

    double logKMin = -10, logKMax = 5;
    double logKSteps = 10000;
    double logKStepSize = (logKMax - logKMin) / logKSteps;
    double pi = 3.141593;
    int i, j;
    for(i = 0; i <= steps; ++i)
    {
        double r = i * self->step_size;
        double res = 0;
        double prev = 0;
        for(j = 1; j <= logKSteps; ++j)
        {
            double logK = logKMin + j * logKStepSize;
            double k = exp(logK);
            double kr = k * r;
            double func = 1;
            if(kr > 0)
                func = sin(kr) / kr;
            func *= pkfunc(k, data) * k * k * k;
            res += (prev + func) / 2;
            prev = func;
        }
        res *= logKStepSize / (2 * pi * pi);
        self->xi[i] = res;
    }
}

static void
_solve(int size, double * Cij, double * dfi, double * x)
{
    gsl_matrix_view m = gsl_matrix_view_array(Cij, size, size);
    gsl_vector_view b = gsl_vector_view_array(dfi, size);
    gsl_vector_view xx = gsl_vector_view_array(x, size);
    gsl_permutation *p = gsl_permutation_alloc(size);
    int s;
    gsl_linalg_LU_decomp(&m.matrix, p, &s);
    gsl_linalg_LU_solve(&m.matrix, p, &b.vector, &xx.vector);
}

static void
_readout(FastPMConstraint * constraints, int size, PM * pm, FastPMFloat * delta_x, double * dfi)
{
    int i;

    for(i = 0; i < size; ++i)
    {
        int ii[3];
        int d;
        int inBox = 1;
        int index = 0;
        for(d = 0; d < 3; ++d)
        {
            ii[d] = constraints[i].x[d] * pm->InvCellSize[d] - pm->IRegion.start[d];
            if(ii[d] < 0 || ii[d] > pm->IRegion.size[d])
                inBox = 0;
            index += ii[d] * pm->IRegion.strides[d];
        }

        if(inBox) {
            dfi[i] = delta_x[index];
        } else {
            dfi[i] = 0;
        }
    }
    MPI_Allreduce(MPI_IN_PLACE, dfi, size, MPI_DOUBLE, MPI_SUM, pm_comm(pm));
}
static double
_sigma(PM * pm, FastPMFloat * delta_x)
{
    double d2 = 0.0;
    PMXIter xiter;
    for(pm_xiter_init(pm, &xiter);
       !pm_xiter_stop(&xiter);
        pm_xiter_next(&xiter))
    {
        double od = delta_x[xiter.ind] - 1;
        d2 += od * od;
    }
    MPI_Allreduce(MPI_IN_PLACE, &d2, 1, MPI_DOUBLE, MPI_SUM, pm_comm(pm));
    /* unbiased estimator of the variance. the mean is 1. */
    d2 /= (pm_norm(pm) - 1);
    return sqrt(d2);
}

void
fastpm_cg_apply_constraints(FastPMConstrainedGaussian *cg, PM * pm, FastPM2PCF *xi, FastPMFloat * delta_k)
{
    FastPMConstraint * constraints = cg->constraints;

    int size;
    for(size = 0; constraints[size].x[0] >= 0; size ++)
        continue;

    int i;
    fastpm_info("Constrained Gaussian with %d constraints\n", size);
    double dfi[size];
    double e[size];
    double Cij[size * size];
    double sigma = 0;
    FastPMFloat * delta_x = pm_alloc(pm);

    pm_assign(pm, delta_k, delta_x);
    pm_c2r(pm, delta_x);

    sigma = _sigma(pm, delta_x);
    fastpm_info("Measured sigma on the grid = %g\n", sigma);

    _readout(constraints, size, pm, delta_x, dfi);

    for(i = 0; i < size; ++i)
    {
        dfi[i] = (1 + constraints[i].c * sigma) - dfi[i];

        int j;
        for(j = i; j < size; ++j)
        {
            int d;
            double r = 0;
            for(d = 0; d < 3; ++d) {
                double dx = constraints[i].x[d] - constraints[j].x[d];

                if(dx > 0.5*pm->BoxSize[d]){
                    dx -= pm->BoxSize[d];
                }
                else if(dx < -0.5*pm->BoxSize[d]){
                    dx += pm->BoxSize[d];
                }

                r += dx * dx;
            }
            r = sqrt(r);
            double v = fastpm_2pcf_eval(xi, r);
            Cij[i * size + j] = v;
            Cij[j * size + i] = v;
        }
    }

    _solve(size, Cij, dfi, e);

    PMXIter xiter;
    for(pm_xiter_init(pm, &xiter);
       !pm_xiter_stop(&xiter);
        pm_xiter_next(&xiter))
    {
        double v = 0;
        for(i = 0; i < size; ++i)
        {
            int d;

            double r = 0;
            for(d = 0; d < 3; d ++) {
                double dx = xiter.iabs[d] * pm->CellSize[d] - constraints[i].x[d];

                if(dx > 0.5*pm->BoxSize[d]){
                    dx -= pm->BoxSize[d];
                }
                else if(dx < -0.5*pm->BoxSize[d]){
                    dx += pm->BoxSize[d];
                }

                r += dx * dx;
            }
            r = sqrt(r);

            v += e[i] * fastpm_2pcf_eval(xi, r);
        }
        delta_x[xiter.ind] += v;
    }

    _readout(constraints, size, pm, delta_x, dfi);
    for(i = 0; i < size; i ++) {
        fastpm_info("After constraints, Realization x[] = %g %g %g overdensity = %g, peak-sigma= %g\n",
                constraints[i].x[0],
                constraints[i].x[1],
                constraints[i].x[2],
                (dfi[i] - 1.0), (dfi[i] - 1.0) / sigma);
    }
    pm_r2c(pm, delta_x, delta_k);
    pm_free(pm, delta_x);
}


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

/*
   - Originally written by Tuomo Keskitalo <tuomo.keskitalo@iki.fi>
   - Corrections to nmsimplex_iterate and other functions
     by Ivo Alxneit <ivo.alxneit@psi.ch>
   - Additional help by Brian Gough <bjg@network-theory.co.uk>
*/

/* The Simplex method of Nelder and Mead,
   also known as the polytope search alogorithm. Ref:
   Nelder, J.A., Mead, R., Computer Journal 7 (1965) pp. 308-313.

   This implementation uses n+1 corner points in the simplex.
*/

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

typedef struct
{
  gsl_matrix *x1;               /* simplex corner points */
  gsl_vector *y1;               /* function value at corner points */
  gsl_vector *ws1;              /* workspace 1 for algorithm */
  gsl_vector *ws2;              /* workspace 2 for algorithm */
}
nmsimplex_state_t;

static double
nmsimplex_move_corner (const double coeff, const nmsimplex_state_t * state,
                       size_t corner, gsl_vector * xc,
                       const gsl_multimin_function * f)
{
  /* moves a simplex corner scaled by coeff (negative value represents
     mirroring by the middle point of the "other" corner points)
     and gives new corner in xc and function value at xc as a
     return value
   */

  gsl_matrix *x1 = state->x1;

  size_t i, j;
  double newval, mp;

  for (j = 0; j < x1->size2; j++)
    {
      mp = 0.0;
      for (i = 0; i < x1->size1; i++)
        {
          if (i != corner)
            {
              mp += (gsl_matrix_get (x1, i, j));
            }
        }
      mp /= (double) (x1->size1 - 1);
      newval = mp - coeff * (mp - gsl_matrix_get (x1, corner, j));
      gsl_vector_set (xc, j, newval);
    }

  newval = GSL_MULTIMIN_FN_EVAL (f, xc);

  return newval;
}

static int
nmsimplex_contract_by_best (nmsimplex_state_t * state, size_t best,
                            gsl_vector * xc, gsl_multimin_function * f)
{

  /* Function contracts the simplex in respect to
     best valued corner. That is, all corners besides the
     best corner are moved. */

  /* the xc vector is simply work space here */

  gsl_matrix *x1 = state->x1;
  gsl_vector *y1 = state->y1;

  size_t i, j;
  double newval;

  int status = GSL_SUCCESS;

  for (i = 0; i < x1->size1; i++)
    {
      if (i != best)
        {
          for (j = 0; j < x1->size2; j++)
            {
              newval = 0.5 * (gsl_matrix_get (x1, i, j)
                              + gsl_matrix_get (x1, best, j));
              gsl_matrix_set (x1, i, j, newval);
            }

          /* evaluate function in the new point */

          gsl_matrix_get_row (xc, x1, i);
          newval = GSL_MULTIMIN_FN_EVAL (f, xc);
          gsl_vector_set (y1, i, newval);

	  /* notify caller that we found at least one bad function value.
	     we finish the contraction (and do not abort) to allow the user
	     to handle the situation */

          if(!gsl_finite(newval))
	    {
	      status = GSL_EBADFUNC;
	    }
        }
    }

  return status;
}

static int
nmsimplex_calc_center (const nmsimplex_state_t * state, gsl_vector * mp)
{
  /* calculates the center of the simplex to mp */

  gsl_matrix *x1 = state->x1;

  size_t i, j;
  double val;

  for (j = 0; j < x1->size2; j++)
    {
      val = 0.0;
      for (i = 0; i < x1->size1; i++)
        {
          val += gsl_matrix_get (x1, i, j);
        }
      val /= x1->size1;
      gsl_vector_set (mp, j, val);
    }

  return GSL_SUCCESS;
}

static double
nmsimplex_size (nmsimplex_state_t * state)
{
  /* calculates simplex size as average sum of length of vectors
     from simplex center to corner points:

     ( sum ( || y - y_middlepoint || ) ) / n
   */

  gsl_vector *s = state->ws1;
  gsl_vector *mp = state->ws2;

  gsl_matrix *x1 = state->x1;
  size_t i;

  double ss = 0.0;

  /* Calculate middle point */
  nmsimplex_calc_center (state, mp);

  for (i = 0; i < x1->size1; i++)
    {
      gsl_matrix_get_row (s, x1, i);
      gsl_blas_daxpy (-1.0, mp, s);
      ss += gsl_blas_dnrm2 (s);
    }

  return ss / (double) (x1->size1);
}

static int
nmsimplex_alloc (void *vstate, size_t n)
{
  nmsimplex_state_t *state = (nmsimplex_state_t *) vstate;

  if (n == 0)
    {
      GSL_ERROR("invalid number of parameters specified", GSL_EINVAL);
    }

  state->x1 = gsl_matrix_alloc (n + 1, n);

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

  state->y1 = gsl_vector_alloc (n + 1);

  if (state->y1 == NULL)
    {
      gsl_matrix_free(state->x1);
      GSL_ERROR ("failed to allocate space for y", GSL_ENOMEM);
    }

  state->ws1 = gsl_vector_alloc (n);

  if (state->ws1 == NULL)
    {
      gsl_matrix_free(state->x1);
      gsl_vector_free(state->y1);
      GSL_ERROR ("failed to allocate space for ws1", GSL_ENOMEM);
    }

  state->ws2 = gsl_vector_alloc (n);

  if (state->ws2 == NULL)
    {
      gsl_matrix_free(state->x1);
      gsl_vector_free(state->y1);
      gsl_vector_free(state->ws1);
      GSL_ERROR ("failed to allocate space for ws2", GSL_ENOMEM);
    }

  return GSL_SUCCESS;
}

static int
nmsimplex_set (void *vstate, gsl_multimin_function * f,
               const gsl_vector * x,
               double *size, const gsl_vector * step_size)
{
  int status;
  size_t i;
  double val;

  nmsimplex_state_t *state = (nmsimplex_state_t *) vstate;

  gsl_vector *xtemp = state->ws1;

  if (xtemp->size != x->size)
    {
      GSL_ERROR("incompatible size of x", GSL_EINVAL);
    }

  if (xtemp->size != step_size->size)
    {
      GSL_ERROR("incompatible size of step_size", GSL_EINVAL);
    }

  /* first point is the original x0 */

  val = GSL_MULTIMIN_FN_EVAL (f, x);

  if (!gsl_finite(val))
    {
      GSL_ERROR("non-finite function value encountered", GSL_EBADFUNC);
    }

  gsl_matrix_set_row (state->x1, 0, x);
  gsl_vector_set (state->y1, 0, val);

  /* following points are initialized to x0 + step_size */

  for (i = 0; i < x->size; i++)
    {
      status = gsl_vector_memcpy (xtemp, x);

      if (status != 0)
        {
          GSL_ERROR ("vector memcopy failed", GSL_EFAILED);
        }

      val = gsl_vector_get (xtemp, i) + gsl_vector_get (step_size, i);
      gsl_vector_set (xtemp, i, val);
      val = GSL_MULTIMIN_FN_EVAL (f, xtemp);

      if (!gsl_finite(val))
        {
          GSL_ERROR("non-finite function value encountered", GSL_EBADFUNC);
        }

      gsl_matrix_set_row (state->x1, i + 1, xtemp);
      gsl_vector_set (state->y1, i + 1, val);
    }

  /* Initialize simplex size */

  *size = nmsimplex_size (state);

  return GSL_SUCCESS;
}

static void
nmsimplex_free (void *vstate)
{
  nmsimplex_state_t *state = (nmsimplex_state_t *) vstate;

  gsl_matrix_free (state->x1);
  gsl_vector_free (state->y1);
  gsl_vector_free (state->ws1);
  gsl_vector_free (state->ws2);
}

static int
nmsimplex_iterate (void *vstate, gsl_multimin_function * f,
                   gsl_vector * x, double *size, double *fval)
{

  /* Simplex iteration tries to minimize function f value */
  /* Includes corrections from Ivo Alxneit <ivo.alxneit@psi.ch> */

  nmsimplex_state_t *state = (nmsimplex_state_t *) vstate;

  /* xc and xc2 vectors store tried corner point coordinates */

  gsl_vector *xc = state->ws1;
  gsl_vector *xc2 = state->ws2;
  gsl_vector *y1 = state->y1;
  gsl_matrix *x1 = state->x1;

  size_t n = y1->size;
  size_t i;
  size_t hi, s_hi, lo;
  double dhi, ds_hi, dlo;
  int status;
  double val, val2;

  if (xc->size != x->size)
    {
      GSL_ERROR("incompatible size of x", GSL_EINVAL);
    }

  /* get index of highest, second highest and lowest point */

  dhi = dlo = gsl_vector_get (y1, 0);
  hi = 0; lo = 0;

  ds_hi = gsl_vector_get(y1, 1);
  s_hi = 1;

  for (i = 1; i < n; i++)
    {
      val = (gsl_vector_get (y1, i));
      if (val < dlo)
        {
          dlo = val;
          lo = i;
        }
      else if (val > dhi)
        {
          ds_hi = dhi;
          s_hi = hi;
          dhi = val;
          hi = i;
        }
      else if (val > ds_hi)
        {
          ds_hi = val;
          s_hi = i;
        }
    }

  /* reflect the highest value */

  val = nmsimplex_move_corner (-1.0, state, hi, xc, f);

  if (gsl_finite(val) && val < gsl_vector_get (y1, lo))
    {

      /* reflected point becomes lowest point, try expansion */

      val2 = nmsimplex_move_corner (-2.0, state, hi, xc2, f);

      if (gsl_finite(val2) && val2 < gsl_vector_get (y1, lo))
        {
          gsl_matrix_set_row (x1, hi, xc2);
          gsl_vector_set (y1, hi, val2);
        }
      else
        {
          gsl_matrix_set_row (x1, hi, xc);
          gsl_vector_set (y1, hi, val);
        }
    }

  /* reflection does not improve things enough
     or
     we got a non-finite (illegal) function value */

  else if (!gsl_finite(val) || val > gsl_vector_get (y1, s_hi))
    {
      if (gsl_finite(val) && val <= gsl_vector_get (y1, hi))
        {

          /* if trial point is better than highest point, replace
             highest point */

          gsl_matrix_set_row (x1, hi, xc);
          gsl_vector_set (y1, hi, val);
        }

      /* try one dimensional contraction */

      val2 = nmsimplex_move_corner (0.5, state, hi, xc2, f);

      if (gsl_finite(val2) && val2 <= gsl_vector_get (y1, hi))
        {
          gsl_matrix_set_row (state->x1, hi, xc2);
          gsl_vector_set (y1, hi, val2);
        }

      else
        {

          /* contract the whole simplex in respect to the best point */

          status = nmsimplex_contract_by_best (state, lo, xc, f);
          if (status != GSL_SUCCESS)
            {
              GSL_ERROR ("nmsimplex_contract_by_best failed", GSL_EFAILED);
            }
        }
    }
  else
    {

      /* trial point is better than second highest point.
         Replace highest point by it */

      gsl_matrix_set_row (x1, hi, xc);
      gsl_vector_set (y1, hi, val);
    }

  /* return lowest point of simplex as x */

  lo = gsl_vector_min_index (y1);
  gsl_matrix_get_row (x, x1, lo);
  *fval = gsl_vector_get (y1, lo);

  /* Update simplex size */

  *size = nmsimplex_size (state);

  return GSL_SUCCESS;
}

static const gsl_multimin_fminimizer_type nmsimplex_type =
{ "nmsimplex",  /* name */
  sizeof (nmsimplex_state_t),
  &nmsimplex_alloc,
  &nmsimplex_set,
  &nmsimplex_iterate,
  &nmsimplex_free
};

const gsl_multimin_fminimizer_type
  * gsl_multimin_fminimizer_nmsimplex = &nmsimplex_type;


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

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

static int
linear_init (void * vstate,
             const double x_array[],
             const double y_array[],
             size_t size)
{
  return GSL_SUCCESS;
}

static
int
linear_eval (const void * vstate,
             const double x_array[], const double y_array[], size_t size,
             double x,
             gsl_interp_accel * a,
             double *y)
{
  double x_lo, x_hi;
  double y_lo, y_hi;
  double dx;
  size_t index;

  if (a != 0)
    {
      index = gsl_interp_accel_find (a, x_array, size, x);
    }
  else
    {
      index = gsl_interp_bsearch (x_array, x, 0, size - 1);
    }

  /* evaluate */
  x_lo = x_array[index];
  x_hi = x_array[index + 1];
  y_lo = y_array[index];
  y_hi = y_array[index + 1];
  dx = x_hi - x_lo;
  if (dx > 0.0)
    {
      *y = y_lo + (x - x_lo) / dx * (y_hi - y_lo);
      return GSL_SUCCESS;
    }
  else
    {
      *y = 0.0;
      return GSL_EINVAL;
    }
}

static
int
linear_eval_deriv (const void * vstate,
                   const double x_array[], const double y_array[], size_t size,
                   double x,
                   gsl_interp_accel * a,
                   double *dydx)
{
  double x_lo, x_hi;
  double y_lo, y_hi;
  double dx;
  double dy;
  size_t index;

  if (a != 0)
    {
      index = gsl_interp_accel_find (a, x_array, size, x);
    }
  else
    {
      index = gsl_interp_bsearch (x_array, x, 0, size - 1);
    }

  /* evaluate */
  x_lo = x_array[index];
  x_hi = x_array[index + 1];
  y_lo = y_array[index];
  y_hi = y_array[index + 1];
  dx = x_hi - x_lo;
  dy = y_hi - y_lo;
  if (dx > 0.0)
    {
      *dydx = dy / dx;;
      return GSL_SUCCESS;
    }
  else
    {
      *dydx = 0.0;
      return GSL_EINVAL;
    }
}

static
int
linear_eval_deriv2 (const void * vstate,
                    const double x_array[], const double y_array[], size_t size,
                    double x,
                    gsl_interp_accel * a,
                    double *y_pp)
{
  *y_pp = 0.0;

  return GSL_SUCCESS;
}

static
int
linear_eval_integ (const void * vstate,
                   const double x_array[], const double y_array[], size_t size,
                   gsl_interp_accel * acc,
                   double a, double b,
                   double * result)
{
  size_t i, index_a, index_b;

  if (acc != 0)
    {
      index_a = gsl_interp_accel_find (acc, x_array, size, a);
      index_b = gsl_interp_accel_find (acc, x_array, size, b);
    }
  else
    {
      index_a = gsl_interp_bsearch (x_array, a, 0, size - 1);
      index_b = gsl_interp_bsearch (x_array, b, 0, size - 1);
    }

    /* endpoints span more than one interval */

  *result = 0.0;

  /* interior intervals */
  for(i=index_a; i<=index_b; i++) {
    const double x_hi = x_array[i + 1];
    const double x_lo = x_array[i];
    const double y_lo = y_array[i];
    const double y_hi = y_array[i + 1];
    const double dx = x_hi - x_lo;

    if(dx != 0.0) {
      if (i == index_a || i == index_b)
        {
          double x1 = (i == index_a) ? a : x_lo;
          double x2 = (i == index_b) ? b : x_hi;
          const double D = (y_hi-y_lo)/dx;
          *result += (x2-x1) * (y_lo + 0.5*D*((x2-x_lo)+(x1-x_lo)));
        }
      else
        {
          *result += 0.5 * dx * (y_lo + y_hi);
        }
    }
  }

  return GSL_SUCCESS;
}

static const gsl_interp_type linear_type =
{
  "linear",
  2,
  NULL, /* alloc, not applicable */
  &linear_init,
  &linear_eval,
  &linear_eval_deriv,
  &linear_eval_deriv2,
  &linear_eval_integ,
  NULL, /* free, not applicable */
};

const gsl_interp_type * gsl_interp_linear = &linear_type;


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

/* Author:  G. Jungman */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_exp.h>
#include <gsl/gsl_sf_pow_int.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_sf_psi.h>
#include <gsl/gsl_sf_hyperg.h>

#include "error.h"

#define locEPS (1000.0*GSL_DBL_EPSILON)

/* Assumes c != negative integer.
 */
static int
hyperg_2F1_series(const double a, const double b, const double c,
                  const double x,
                  gsl_sf_result * result
                  )
{
  double sum_pos = 1.0;
  double sum_neg = 0.0;
  double del_pos = 1.0;
  double del_neg = 0.0;
  double del = 1.0;
  double k = 0.0;
  int i = 0;

  if(fabs(c) < GSL_DBL_EPSILON) {
    result->val = 0.0; /* FIXME: ?? */
    result->err = 1.0;
    GSL_ERROR ("error", GSL_EDOM);
  }

  do {
    if(++i > 30000) {
      result->val  = sum_pos - sum_neg;
      result->err  = del_pos + del_neg;
      result->err += 2.0 * GSL_DBL_EPSILON * (sum_pos + sum_neg);
      result->err += 2.0 * GSL_DBL_EPSILON * (2.0*sqrt(k)+1.0) * fabs(result->val);
      GSL_ERROR ("error", GSL_EMAXITER);
    }
    del *= (a+k)*(b+k) * x / ((c+k) * (k+1.0));  /* Gauss series */

    if(del > 0.0) {
      del_pos  =  del;
      sum_pos +=  del;
    }
    else if(del == 0.0) {
      /* Exact termination (a or b was a negative integer).
       */
      del_pos = 0.0;
      del_neg = 0.0;
      break;
    }
    else {
      del_neg  = -del;
      sum_neg -=  del;
    }

    k += 1.0;
  } while(fabs((del_pos + del_neg)/(sum_pos-sum_neg)) > GSL_DBL_EPSILON);

  result->val  = sum_pos - sum_neg;
  result->err  = del_pos + del_neg;
  result->err += 2.0 * GSL_DBL_EPSILON * (sum_pos + sum_neg);
  result->err += 2.0 * GSL_DBL_EPSILON * (2.0*sqrt(k) + 1.0) * fabs(result->val);

  return GSL_SUCCESS;
}

/* a = aR + i aI, b = aR - i aI */
static
int
hyperg_2F1_conj_series(const double aR, const double aI, const double c,
                       double x,
                       gsl_sf_result * result)
{
  if(c == 0.0) {
    result->val = 0.0; /* FIXME: should be Inf */
    result->err = 0.0;
    GSL_ERROR ("error", GSL_EDOM);
  }
  else {
    double sum_pos = 1.0;
    double sum_neg = 0.0;
    double del_pos = 1.0;
    double del_neg = 0.0;
    double del = 1.0;
    double k = 0.0;
    do {
      del *= ((aR+k)*(aR+k) + aI*aI)/((k+1.0)*(c+k)) * x;

      if(del >= 0.0) {
        del_pos  =  del;
        sum_pos +=  del;
      }
      else {
        del_neg  = -del;
        sum_neg -=  del;
      }

      if(k > 30000) {
        result->val  = sum_pos - sum_neg;
        result->err  = del_pos + del_neg;
        result->err += 2.0 * GSL_DBL_EPSILON * (sum_pos + sum_neg);
        result->err += 2.0 * GSL_DBL_EPSILON * (2.0*sqrt(k)+1.0) * fabs(result->val);
        GSL_ERROR ("error", GSL_EMAXITER);
      }

      k += 1.0;
    } while(fabs((del_pos + del_neg)/(sum_pos - sum_neg)) > GSL_DBL_EPSILON);

    result->val  = sum_pos - sum_neg;
    result->err  = del_pos + del_neg;
    result->err += 2.0 * GSL_DBL_EPSILON * (sum_pos + sum_neg);
    result->err += 2.0 * GSL_DBL_EPSILON * (2.0*sqrt(k) + 1.0) * fabs(result->val);

    return GSL_SUCCESS;
  }
}

/* Luke's rational approximation. The most accesible
 * discussion is in [Kolbig, CPC 23, 51 (1981)].
 * The convergence is supposedly guaranteed for x < 0.
 * You have to read Luke's books to see this and other
 * results. Unfortunately, the stability is not so
 * clear to me, although it seems very efficient when
 * it works.
 */
static
int
hyperg_2F1_luke(const double a, const double b, const double c,
                const double xin,
                gsl_sf_result * result)
{
  int stat_iter;
  const double RECUR_BIG = 1.0e+50;
  const int nmax = 20000;
  int n = 3;
  const double x  = -xin;
  const double x3 = x*x*x;
  const double t0 = a*b/c;
  const double t1 = (a+1.0)*(b+1.0)/(2.0*c);
  const double t2 = (a+2.0)*(b+2.0)/(2.0*(c+1.0));
  double F = 1.0;
  double prec;

  double Bnm3 = 1.0;                                  /* B0 */
  double Bnm2 = 1.0 + t1 * x;                         /* B1 */
  double Bnm1 = 1.0 + t2 * x * (1.0 + t1/3.0 * x);    /* B2 */

  double Anm3 = 1.0;                                                      /* A0 */
  double Anm2 = Bnm2 - t0 * x;                                            /* A1 */
  double Anm1 = Bnm1 - t0*(1.0 + t2*x)*x + t0 * t1 * (c/(c+1.0)) * x*x;   /* A2 */

  while(1) {
    double npam1 = n + a - 1;
    double npbm1 = n + b - 1;
    double npcm1 = n + c - 1;
    double npam2 = n + a - 2;
    double npbm2 = n + b - 2;
    double npcm2 = n + c - 2;
    double tnm1  = 2*n - 1;
    double tnm3  = 2*n - 3;
    double tnm5  = 2*n - 5;
    double n2 = n*n;
    double F1 =  (3.0*n2 + (a+b-6)*n + 2 - a*b - 2*(a+b)) / (2*tnm3*npcm1);
    double F2 = -(3.0*n2 - (a+b+6)*n + 2 - a*b)*npam1*npbm1/(4*tnm1*tnm3*npcm2*npcm1);
    double F3 = (npam2*npam1*npbm2*npbm1*(n-a-2)*(n-b-2)) / (8*tnm3*tnm3*tnm5*(n+c-3)*npcm2*npcm1);
    double E  = -npam1*npbm1*(n-c-1) / (2*tnm3*npcm2*npcm1);

    double An = (1.0+F1*x)*Anm1 + (E + F2*x)*x*Anm2 + F3*x3*Anm3;
    double Bn = (1.0+F1*x)*Bnm1 + (E + F2*x)*x*Bnm2 + F3*x3*Bnm3;
    double r = An/Bn;

    prec = fabs((F - r)/F);
    F = r;

    if(prec < GSL_DBL_EPSILON || n > nmax) break;

    if(fabs(An) > RECUR_BIG || fabs(Bn) > RECUR_BIG) {
      An   /= RECUR_BIG;
      Bn   /= RECUR_BIG;
      Anm1 /= RECUR_BIG;
      Bnm1 /= RECUR_BIG;
      Anm2 /= RECUR_BIG;
      Bnm2 /= RECUR_BIG;
      Anm3 /= RECUR_BIG;
      Bnm3 /= RECUR_BIG;
    }
    else if(fabs(An) < 1.0/RECUR_BIG || fabs(Bn) < 1.0/RECUR_BIG) {
      An   *= RECUR_BIG;
      Bn   *= RECUR_BIG;
      Anm1 *= RECUR_BIG;
      Bnm1 *= RECUR_BIG;
      Anm2 *= RECUR_BIG;
      Bnm2 *= RECUR_BIG;
      Anm3 *= RECUR_BIG;
      Bnm3 *= RECUR_BIG;
    }

    n++;
    Bnm3 = Bnm2;
    Bnm2 = Bnm1;
    Bnm1 = Bn;
    Anm3 = Anm2;
    Anm2 = Anm1;
    Anm1 = An;
  }

  result->val  = F;
  result->err  = 2.0 * fabs(prec * F);
  result->err += 2.0 * GSL_DBL_EPSILON * (n+1.0) * fabs(F);

  /* FIXME: just a hack: there's a lot of shit going on here */
  result->err *= 8.0 * (fabs(a) + fabs(b) + 1.0);

  stat_iter = (n >= nmax ? GSL_EMAXITER : GSL_SUCCESS );

  return stat_iter;
}

/* Luke's rational approximation for the
 * case a = aR + i aI, b = aR - i aI.
 */
static
int
hyperg_2F1_conj_luke(const double aR, const double aI, const double c,
                     const double xin,
                     gsl_sf_result * result)
{
  int stat_iter;
  const double RECUR_BIG = 1.0e+50;
  const int nmax = 10000;
  int n = 3;
  const double x = -xin;
  const double x3 = x*x*x;
  const double atimesb = aR*aR + aI*aI;
  const double apb     = 2.0*aR;
  const double t0 = atimesb/c;
  const double t1 = (atimesb +     apb + 1.0)/(2.0*c);
  const double t2 = (atimesb + 2.0*apb + 4.0)/(2.0*(c+1.0));
  double F = 1.0;
  double prec;

  double Bnm3 = 1.0;                                  /* B0 */
  double Bnm2 = 1.0 + t1 * x;                         /* B1 */
  double Bnm1 = 1.0 + t2 * x * (1.0 + t1/3.0 * x);    /* B2 */

  double Anm3 = 1.0;                                                      /* A0 */
  double Anm2 = Bnm2 - t0 * x;                                            /* A1 */
  double Anm1 = Bnm1 - t0*(1.0 + t2*x)*x + t0 * t1 * (c/(c+1.0)) * x*x;   /* A2 */

  while(1) {
    double nm1 = n - 1;
    double nm2 = n - 2;
    double npam1_npbm1 = atimesb + nm1*apb + nm1*nm1;
    double npam2_npbm2 = atimesb + nm2*apb + nm2*nm2;
    double npcm1 = nm1 + c;
    double npcm2 = nm2 + c;
    double tnm1  = 2*n - 1;
    double tnm3  = 2*n - 3;
    double tnm5  = 2*n - 5;
    double n2 = n*n;
    double F1 =  (3.0*n2 + (apb-6)*n + 2 - atimesb - 2*apb) / (2*tnm3*npcm1);
    double F2 = -(3.0*n2 - (apb+6)*n + 2 - atimesb)*npam1_npbm1/(4*tnm1*tnm3*npcm2*npcm1);
    double F3 = (npam2_npbm2*npam1_npbm1*(nm2*nm2 - nm2*apb + atimesb)) / (8*tnm3*tnm3*tnm5*(n+c-3)*npcm2*npcm1);
    double E  = -npam1_npbm1*(n-c-1) / (2*tnm3*npcm2*npcm1);

    double An = (1.0+F1*x)*Anm1 + (E + F2*x)*x*Anm2 + F3*x3*Anm3;
    double Bn = (1.0+F1*x)*Bnm1 + (E + F2*x)*x*Bnm2 + F3*x3*Bnm3;
    double r = An/Bn;

    prec = fabs(F - r)/fabs(F);
    F = r;

    if(prec < GSL_DBL_EPSILON || n > nmax) break;

    if(fabs(An) > RECUR_BIG || fabs(Bn) > RECUR_BIG) {
      An   /= RECUR_BIG;
      Bn   /= RECUR_BIG;
      Anm1 /= RECUR_BIG;
      Bnm1 /= RECUR_BIG;
      Anm2 /= RECUR_BIG;
      Bnm2 /= RECUR_BIG;
      Anm3 /= RECUR_BIG;
      Bnm3 /= RECUR_BIG;
    }
    else if(fabs(An) < 1.0/RECUR_BIG || fabs(Bn) < 1.0/RECUR_BIG) {
      An   *= RECUR_BIG;
      Bn   *= RECUR_BIG;
      Anm1 *= RECUR_BIG;
      Bnm1 *= RECUR_BIG;
      Anm2 *= RECUR_BIG;
      Bnm2 *= RECUR_BIG;
      Anm3 *= RECUR_BIG;
      Bnm3 *= RECUR_BIG;
    }

    n++;
    Bnm3 = Bnm2;
    Bnm2 = Bnm1;
    Bnm1 = Bn;
    Anm3 = Anm2;
    Anm2 = Anm1;
    Anm1 = An;
  }

  result->val  = F;
  result->err  = 2.0 * fabs(prec * F);
  result->err += 2.0 * GSL_DBL_EPSILON * (n+1.0) * fabs(F);

  /* FIXME: see above */
  result->err *= 8.0 * (fabs(aR) + fabs(aI) + 1.0);

  stat_iter = (n >= nmax ? GSL_EMAXITER : GSL_SUCCESS );

  return stat_iter;
}

/* Do the reflection described in [Moshier, p. 334].
 * Assumes a,b,c != neg integer.
 */
static
int
hyperg_2F1_reflect(const double a, const double b, const double c,
                   const double x, gsl_sf_result * result)
{
  const double d = c - a - b;
  const int intd  = floor(d+0.5);
  const int d_integer = ( fabs(d - intd) < locEPS );

  if(d_integer) {
    const double ln_omx = log(1.0 - x);
    const double ad = fabs(d);
    int stat_F2 = GSL_SUCCESS;
    double sgn_2;
    gsl_sf_result F1;
    gsl_sf_result F2;
    double d1, d2;
    gsl_sf_result lng_c;
    gsl_sf_result lng_ad2;
    gsl_sf_result lng_bd2;
    int stat_c;
    int stat_ad2;
    int stat_bd2;

    if(d >= 0.0) {
      d1 = d;
      d2 = 0.0;
    }
    else {
      d1 = 0.0;
      d2 = d;
    }

    stat_ad2 = gsl_sf_lngamma_e(a+d2, &lng_ad2);
    stat_bd2 = gsl_sf_lngamma_e(b+d2, &lng_bd2);
    stat_c   = gsl_sf_lngamma_e(c,    &lng_c);

    /* Evaluate F1.
     */
    if(ad < GSL_DBL_EPSILON) {
      /* d = 0 */
      F1.val = 0.0;
      F1.err = 0.0;
    }
    else {
      gsl_sf_result lng_ad;
      gsl_sf_result lng_ad1;
      gsl_sf_result lng_bd1;
      int stat_ad  = gsl_sf_lngamma_e(ad,   &lng_ad);
      int stat_ad1 = gsl_sf_lngamma_e(a+d1, &lng_ad1);
      int stat_bd1 = gsl_sf_lngamma_e(b+d1, &lng_bd1);

      if(stat_ad1 == GSL_SUCCESS && stat_bd1 == GSL_SUCCESS && stat_ad == GSL_SUCCESS) {
        /* Gamma functions in the denominator are ok.
         * Proceed with evaluation.
         */
        int i;
        double sum1 = 1.0;
        double term = 1.0;
        double ln_pre1_val = lng_ad.val + lng_c.val + d2*ln_omx - lng_ad1.val - lng_bd1.val;
        double ln_pre1_err = lng_ad.err + lng_c.err + lng_ad1.err + lng_bd1.err + GSL_DBL_EPSILON * fabs(ln_pre1_val);
        int stat_e;

        /* Do F1 sum.
         */
        for(i=1; i<ad; i++) {
          int j = i-1;
          term *= (a + d2 + j) * (b + d2 + j) / (1.0 + d2 + j) / i * (1.0-x);
          sum1 += term;
        }

        stat_e = gsl_sf_exp_mult_err_e(ln_pre1_val, ln_pre1_err,
                                       sum1, GSL_DBL_EPSILON*fabs(sum1),
                                       &F1);
        if(stat_e == GSL_EOVRFLW) {
          OVERFLOW_ERROR(result);
        }
      }
      else {
        /* Gamma functions in the denominator were not ok.
         * So the F1 term is zero.
         */
        F1.val = 0.0;
        F1.err = 0.0;
      }
    } /* end F1 evaluation */

    /* Evaluate F2.
     */
    if(stat_ad2 == GSL_SUCCESS && stat_bd2 == GSL_SUCCESS) {
      /* Gamma functions in the denominator are ok.
       * Proceed with evaluation.
       */
      const int maxiter = 2000;
      double psi_1 = -M_EULER;
      gsl_sf_result psi_1pd;
      gsl_sf_result psi_apd1;
      gsl_sf_result psi_bpd1;
      int stat_1pd  = gsl_sf_psi_e(1.0 + ad, &psi_1pd);
      int stat_apd1 = gsl_sf_psi_e(a + d1,   &psi_apd1);
      int stat_bpd1 = gsl_sf_psi_e(b + d1,   &psi_bpd1);
      int stat_dall = GSL_ERROR_SELECT_3(stat_1pd, stat_apd1, stat_bpd1);

      double psi_val = psi_1 + psi_1pd.val - psi_apd1.val - psi_bpd1.val - ln_omx;
      double psi_err = psi_1pd.err + psi_apd1.err + psi_bpd1.err + GSL_DBL_EPSILON*fabs(psi_val);
      double fact = 1.0;
      double sum2_val = psi_val;
      double sum2_err = psi_err;
      double ln_pre2_val = lng_c.val + d1*ln_omx - lng_ad2.val - lng_bd2.val;
      double ln_pre2_err = lng_c.err + lng_ad2.err + lng_bd2.err + GSL_DBL_EPSILON * fabs(ln_pre2_val);
      int stat_e;

      int j;

      /* Do F2 sum.
       */
      for(j=1; j<maxiter; j++) {
        /* values for psi functions use recurrence; Abramowitz+Stegun 6.3.5 */
        double term1 = 1.0/(double)j  + 1.0/(ad+j);
        double term2 = 1.0/(a+d1+j-1.0) + 1.0/(b+d1+j-1.0);
        double delta = 0.0;
        psi_val += term1 - term2;
        psi_err += GSL_DBL_EPSILON * (fabs(term1) + fabs(term2));
        fact *= (a+d1+j-1.0)*(b+d1+j-1.0)/((ad+j)*j) * (1.0-x);
        delta = fact * psi_val;
        sum2_val += delta;
        sum2_err += fabs(fact * psi_err) + GSL_DBL_EPSILON*fabs(delta);
        if(fabs(delta) < GSL_DBL_EPSILON * fabs(sum2_val)) break;
      }

      if(j == maxiter) stat_F2 = GSL_EMAXITER;

      if(sum2_val == 0.0) {
        F2.val = 0.0;
        F2.err = 0.0;
      }
      else {
        stat_e = gsl_sf_exp_mult_err_e(ln_pre2_val, ln_pre2_err,
                                       sum2_val, sum2_err,
                                       &F2);
        if(stat_e == GSL_EOVRFLW) {
          result->val = 0.0;
          result->err = 0.0;
          GSL_ERROR ("error", GSL_EOVRFLW);
        }
      }
      stat_F2 = GSL_ERROR_SELECT_2(stat_F2, stat_dall);
    }
    else {
      /* Gamma functions in the denominator not ok.
       * So the F2 term is zero.
       */
      F2.val = 0.0;
      F2.err = 0.0;
    } /* end F2 evaluation */

    sgn_2 = ( GSL_IS_ODD(intd) ? -1.0 : 1.0 );
    result->val  = F1.val + sgn_2 * F2.val;
    result->err  = F1.err + F2. err;
    result->err += 2.0 * GSL_DBL_EPSILON * (fabs(F1.val) + fabs(F2.val));
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return stat_F2;
  }
  else {
    /* d not an integer */

    gsl_sf_result pre1, pre2;
    double sgn1, sgn2;
    gsl_sf_result F1, F2;
    int status_F1, status_F2;

    /* These gamma functions appear in the denominator, so we
     * catch their harmless domain errors and set the terms to zero.
     */
    gsl_sf_result ln_g1ca,  ln_g1cb,  ln_g2a,  ln_g2b;
    double sgn_g1ca, sgn_g1cb, sgn_g2a, sgn_g2b;
    int stat_1ca = gsl_sf_lngamma_sgn_e(c-a, &ln_g1ca, &sgn_g1ca);
    int stat_1cb = gsl_sf_lngamma_sgn_e(c-b, &ln_g1cb, &sgn_g1cb);
    int stat_2a  = gsl_sf_lngamma_sgn_e(a, &ln_g2a, &sgn_g2a);
    int stat_2b  = gsl_sf_lngamma_sgn_e(b, &ln_g2b, &sgn_g2b);
    int ok1 = (stat_1ca == GSL_SUCCESS && stat_1cb == GSL_SUCCESS);
    int ok2 = (stat_2a  == GSL_SUCCESS && stat_2b  == GSL_SUCCESS);

    gsl_sf_result ln_gc,  ln_gd,  ln_gmd;
    double sgn_gc, sgn_gd, sgn_gmd;
    gsl_sf_lngamma_sgn_e( c, &ln_gc,  &sgn_gc);
    gsl_sf_lngamma_sgn_e( d, &ln_gd,  &sgn_gd);
    gsl_sf_lngamma_sgn_e(-d, &ln_gmd, &sgn_gmd);

    sgn1 = sgn_gc * sgn_gd  * sgn_g1ca * sgn_g1cb;
    sgn2 = sgn_gc * sgn_gmd * sgn_g2a  * sgn_g2b;

    if(ok1 && ok2) {
      double ln_pre1_val = ln_gc.val + ln_gd.val  - ln_g1ca.val - ln_g1cb.val;
      double ln_pre2_val = ln_gc.val + ln_gmd.val - ln_g2a.val  - ln_g2b.val + d*log(1.0-x);
      double ln_pre1_err = ln_gc.err + ln_gd.err + ln_g1ca.err + ln_g1cb.err;
      double ln_pre2_err = ln_gc.err + ln_gmd.err + ln_g2a.err  + ln_g2b.err;
      if(ln_pre1_val < GSL_LOG_DBL_MAX && ln_pre2_val < GSL_LOG_DBL_MAX) {
        gsl_sf_exp_err_e(ln_pre1_val, ln_pre1_err, &pre1);
        gsl_sf_exp_err_e(ln_pre2_val, ln_pre2_err, &pre2);
        pre1.val *= sgn1;
        pre2.val *= sgn2;
      }
      else {
        OVERFLOW_ERROR(result);
      }
    }
    else if(ok1 && !ok2) {
      double ln_pre1_val = ln_gc.val + ln_gd.val - ln_g1ca.val - ln_g1cb.val;
      double ln_pre1_err = ln_gc.err + ln_gd.err + ln_g1ca.err + ln_g1cb.err;
      if(ln_pre1_val < GSL_LOG_DBL_MAX) {
        gsl_sf_exp_err_e(ln_pre1_val, ln_pre1_err, &pre1);
        pre1.val *= sgn1;
        pre2.val = 0.0;
        pre2.err = 0.0;
      }
      else {
        OVERFLOW_ERROR(result);
      }
    }
    else if(!ok1 && ok2) {
      double ln_pre2_val = ln_gc.val + ln_gmd.val - ln_g2a.val - ln_g2b.val + d*log(1.0-x);
      double ln_pre2_err = ln_gc.err + ln_gmd.err + ln_g2a.err + ln_g2b.err;
      if(ln_pre2_val < GSL_LOG_DBL_MAX) {
        pre1.val = 0.0;
        pre1.err = 0.0;
        gsl_sf_exp_err_e(ln_pre2_val, ln_pre2_err, &pre2);
        pre2.val *= sgn2;
      }
      else {
        OVERFLOW_ERROR(result);
      }
    }
    else {
      pre1.val = 0.0;
      pre2.val = 0.0;
      UNDERFLOW_ERROR(result);
    }

    status_F1 = hyperg_2F1_series(  a,   b, 1.0-d, 1.0-x, &F1);
    status_F2 = hyperg_2F1_series(c-a, c-b, 1.0+d, 1.0-x, &F2);

    result->val  = pre1.val*F1.val + pre2.val*F2.val;
    result->err  = fabs(pre1.val*F1.err) + fabs(pre2.val*F2.err);
    result->err += fabs(pre1.err*F1.val) + fabs(pre2.err*F2.val);
    result->err += 2.0 * GSL_DBL_EPSILON * (fabs(pre1.val*F1.val) + fabs(pre2.val*F2.val));
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);

    return GSL_SUCCESS;
  }
}

static int pow_omx(const double x, const double p, gsl_sf_result * result)
{
  double ln_omx;
  double ln_result;
  if(fabs(x) < GSL_ROOT5_DBL_EPSILON) {
    ln_omx = -x*(1.0 + x*(1.0/2.0 + x*(1.0/3.0 + x/4.0 + x*x/5.0)));
  }
  else {
    ln_omx = log(1.0-x);
  }
  ln_result = p * ln_omx;
  return gsl_sf_exp_err_e(ln_result, GSL_DBL_EPSILON * fabs(ln_result), result);
}

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

int
gsl_sf_hyperg_2F1_e(double a, double b, const double c,
                       const double x,
                       gsl_sf_result * result)
{
  const double d = c - a - b;
  const double rinta = floor(a + 0.5);
  const double rintb = floor(b + 0.5);
  const double rintc = floor(c + 0.5);
  const int a_neg_integer = ( a < 0.0  &&  fabs(a - rinta) < locEPS );
  const int b_neg_integer = ( b < 0.0  &&  fabs(b - rintb) < locEPS );
  const int c_neg_integer = ( c < 0.0  &&  fabs(c - rintc) < locEPS );

  result->val = 0.0;
  result->err = 0.0;

   /* Handle x == 1.0 RJM */

  if (fabs (x - 1.0) < locEPS && (c - a - b) > 0 && c != 0 && !c_neg_integer) {
    gsl_sf_result lngamc, lngamcab, lngamca, lngamcb;
    double lngamc_sgn, lngamca_sgn, lngamcb_sgn;
    int status;
    int stat1 = gsl_sf_lngamma_sgn_e (c, &lngamc, &lngamc_sgn);
    int stat2 = gsl_sf_lngamma_e (c - a - b, &lngamcab);
    int stat3 = gsl_sf_lngamma_sgn_e (c - a, &lngamca, &lngamca_sgn);
    int stat4 = gsl_sf_lngamma_sgn_e (c - b, &lngamcb, &lngamcb_sgn);

    if (stat1 != GSL_SUCCESS || stat2 != GSL_SUCCESS
        || stat3 != GSL_SUCCESS || stat4 != GSL_SUCCESS)
      {
        DOMAIN_ERROR (result);
      }

    status =
      gsl_sf_exp_err_e (lngamc.val + lngamcab.val - lngamca.val - lngamcb.val,
                        lngamc.err + lngamcab.err + lngamca.err + lngamcb.err,
                        result);

    result->val *= lngamc_sgn / (lngamca_sgn * lngamcb_sgn);
      return status;
  }

  if(x < -1.0 || 1.0 <= x) {
    DOMAIN_ERROR(result);
  }

  if(c_neg_integer) {
    /* If c is a negative integer, then either a or b must be a
       negative integer of smaller magnitude than c to ensure
       cancellation of the series. */
    if(! (a_neg_integer && a > c + 0.1) && ! (b_neg_integer && b > c + 0.1)) {
      DOMAIN_ERROR(result);
    }
  }

  if(fabs(c-b) < locEPS || fabs(c-a) < locEPS) {
    return pow_omx(x, d, result);  /* (1-x)^(c-a-b) */
  }

  if(a >= 0.0 && b >= 0.0 && c >=0.0 && x >= 0.0 && x < 0.995) {
    /* Series has all positive definite
     * terms and x is not close to 1.
     */
    return hyperg_2F1_series(a, b, c, x, result);
  }

  if(fabs(a) < 10.0 && fabs(b) < 10.0) {
    /* a and b are not too large, so we attempt
     * variations on the series summation.
     */
    if(a_neg_integer) {
      return hyperg_2F1_series(rinta, b, c, x, result);
    }
    if(b_neg_integer) {
      return hyperg_2F1_series(a, rintb, c, x, result);
    }

    if(x < -0.25) {
      return hyperg_2F1_luke(a, b, c, x, result);
    }
    else if(x < 0.5) {
      return hyperg_2F1_series(a, b, c, x, result);
    }
    else {
      if(fabs(c) > 10.0) {
        return hyperg_2F1_series(a, b, c, x, result);
      }
      else {
        return hyperg_2F1_reflect(a, b, c, x, result);
      }
    }
  }
  else {
    /* Either a or b or both large.
     * Introduce some new variables ap,bp so that bp is
     * the larger in magnitude.
     */
    double ap, bp;
    if(fabs(a) > fabs(b)) {
      bp = a;
      ap = b;
    }
    else {
      bp = b;
      ap = a;
    }

    if(x < 0.0) {
      /* What the hell, maybe Luke will converge.
       */
      return hyperg_2F1_luke(a, b, c, x, result);
    }

    if(GSL_MAX_DBL(fabs(ap),1.0)*fabs(bp)*fabs(x) < 2.0*fabs(c)) {
      /* If c is large enough or x is small enough,
       * we can attempt the series anyway.
       */
      return hyperg_2F1_series(a, b, c, x, result);
    }

    if(fabs(bp*bp*x*x) < 0.001*fabs(bp) && fabs(ap) < 10.0) {
      /* The famous but nearly worthless "large b" asymptotic.
       */
      int stat = gsl_sf_hyperg_1F1_e(ap, c, bp*x, result);
      result->err = 0.001 * fabs(result->val);
      return stat;
    }

    /* We give up. */
    result->val = 0.0;
    result->err = 0.0;
    GSL_ERROR ("error", GSL_EUNIMPL);
  }
}

int
gsl_sf_hyperg_2F1_conj_e(const double aR, const double aI, const double c,
                            const double x,
                            gsl_sf_result * result)
{
  const double ax = fabs(x);
  const double rintc = floor(c + 0.5);
  const int c_neg_integer = ( c < 0.0  &&  fabs(c - rintc) < locEPS );

  result->val = 0.0;
  result->err = 0.0;

  if(ax >= 1.0 || c_neg_integer || c == 0.0) {
    DOMAIN_ERROR(result);
  }

  if(   (ax < 0.25 && fabs(aR) < 20.0 && fabs(aI) < 20.0)
     || (c > 0.0 && x > 0.0)
    ) {
    return hyperg_2F1_conj_series(aR, aI, c, x, result);
  }
  else if(fabs(aR) < 10.0 && fabs(aI) < 10.0) {
    if(x < -0.25) {
      return hyperg_2F1_conj_luke(aR, aI, c, x, result);
    }
    else {
      return hyperg_2F1_conj_series(aR, aI, c, x, result);
    }
  }
  else {
    if(x < 0.0) {
      /* What the hell, maybe Luke will converge.
       */
      return hyperg_2F1_conj_luke(aR, aI, c, x, result);
    }

    /* Give up. */
    result->val = 0.0;
    result->err = 0.0;
    GSL_ERROR ("error", GSL_EUNIMPL);
  }
}

int
gsl_sf_hyperg_2F1_renorm_e(const double a, const double b, const double c,
                              const double x,
                              gsl_sf_result * result
                              )
{
  const double rinta = floor(a + 0.5);
  const double rintb = floor(b + 0.5);
  const double rintc = floor(c + 0.5);
  const int a_neg_integer = ( a < 0.0  &&  fabs(a - rinta) < locEPS );
  const int b_neg_integer = ( b < 0.0  &&  fabs(b - rintb) < locEPS );
  const int c_neg_integer = ( c < 0.0  &&  fabs(c - rintc) < locEPS );

  if(c_neg_integer) {
    if((a_neg_integer && a > c+0.1) || (b_neg_integer && b > c+0.1)) {
      /* 2F1 terminates early */
      result->val = 0.0;
      result->err = 0.0;
      return GSL_SUCCESS;
    }
    else {
      /* 2F1 does not terminate early enough, so something survives */
      /* [Abramowitz+Stegun, 15.1.2] */
      gsl_sf_result g1, g2, g3, g4, g5;
      double s1, s2, s3, s4, s5;
      int stat = 0;
      stat += gsl_sf_lngamma_sgn_e(a-c+1, &g1, &s1);
      stat += gsl_sf_lngamma_sgn_e(b-c+1, &g2, &s2);
      stat += gsl_sf_lngamma_sgn_e(a, &g3, &s3);
      stat += gsl_sf_lngamma_sgn_e(b, &g4, &s4);
      stat += gsl_sf_lngamma_sgn_e(-c+2, &g5, &s5);
      if(stat != 0) {
        DOMAIN_ERROR(result);
      }
      else {
        gsl_sf_result F;
        int stat_F = gsl_sf_hyperg_2F1_e(a-c+1, b-c+1, -c+2, x, &F);
        double ln_pre_val = g1.val + g2.val - g3.val - g4.val - g5.val;
        double ln_pre_err = g1.err + g2.err + g3.err + g4.err + g5.err;
        double sg  = s1 * s2 * s3 * s4 * s5;
        int stat_e = gsl_sf_exp_mult_err_e(ln_pre_val, ln_pre_err,
                                              sg * F.val, F.err,
                                              result);
        return GSL_ERROR_SELECT_2(stat_e, stat_F);
      }
    }
  }
  else {
    /* generic c */
    gsl_sf_result F;
    gsl_sf_result lng;
    double sgn;
    int stat_g = gsl_sf_lngamma_sgn_e(c, &lng, &sgn);
    int stat_F = gsl_sf_hyperg_2F1_e(a, b, c, x, &F);
    int stat_e = gsl_sf_exp_mult_err_e(-lng.val, lng.err,
                                          sgn*F.val, F.err,
                                          result);
    return GSL_ERROR_SELECT_3(stat_e, stat_F, stat_g);
  }
}

int
gsl_sf_hyperg_2F1_conj_renorm_e(const double aR, const double aI, const double c,
                                   const double x,
                                   gsl_sf_result * result
                                   )
{
  const double rintc = floor(c  + 0.5);
  const double rinta = floor(aR + 0.5);
  const int a_neg_integer = ( aR < 0.0 && fabs(aR-rinta) < locEPS && aI == 0.0);
  const int c_neg_integer = (  c < 0.0 && fabs(c - rintc) < locEPS );

  if(c_neg_integer) {
    if(a_neg_integer && aR > c+0.1) {
      /* 2F1 terminates early */
      result->val = 0.0;
      result->err = 0.0;
      return GSL_SUCCESS;
    }
    else {
      /* 2F1 does not terminate early enough, so something survives */
      /* [Abramowitz+Stegun, 15.1.2] */
      gsl_sf_result g1, g2;
      gsl_sf_result g3;
      gsl_sf_result a1, a2;
      int stat = 0;
      stat += gsl_sf_lngamma_complex_e(aR-c+1, aI, &g1, &a1);
      stat += gsl_sf_lngamma_complex_e(aR, aI, &g2, &a2);
      stat += gsl_sf_lngamma_e(-c+2.0, &g3);
      if(stat != 0) {
        DOMAIN_ERROR(result);
      }
      else {
        gsl_sf_result F;
        int stat_F = gsl_sf_hyperg_2F1_conj_e(aR-c+1, aI, -c+2, x, &F);
        double ln_pre_val = 2.0*(g1.val - g2.val) - g3.val;
        double ln_pre_err = 2.0 * (g1.err + g2.err) + g3.err;
        int stat_e = gsl_sf_exp_mult_err_e(ln_pre_val, ln_pre_err,
                                              F.val, F.err,
                                              result);
        return GSL_ERROR_SELECT_2(stat_e, stat_F);
      }
    }
  }
  else {
    /* generic c */
    gsl_sf_result F;
    gsl_sf_result lng;
    double sgn;
    int stat_g = gsl_sf_lngamma_sgn_e(c, &lng, &sgn);
    int stat_F = gsl_sf_hyperg_2F1_conj_e(aR, aI, c, x, &F);
    int stat_e = gsl_sf_exp_mult_err_e(-lng.val, lng.err,
                                          sgn*F.val, F.err,
                                          result);
    return GSL_ERROR_SELECT_3(stat_e, stat_F, stat_g);
  }
}

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

#include "eval.h"

double gsl_sf_hyperg_2F1(double a, double b, double c, double x)
{
  EVAL_RESULT(gsl_sf_hyperg_2F1_e(a, b, c, x, &result));
}

double gsl_sf_hyperg_2F1_conj(double aR, double aI, double c, double x)
{
  EVAL_RESULT(gsl_sf_hyperg_2F1_conj_e(aR, aI, c, x, &result));
}

double gsl_sf_hyperg_2F1_renorm(double a, double b, double c, double x)
{
  EVAL_RESULT(gsl_sf_hyperg_2F1_renorm_e(a, b, c, x, &result));
}

double gsl_sf_hyperg_2F1_conj_renorm(double aR, double aI, double c, double x)
{
  EVAL_RESULT(gsl_sf_hyperg_2F1_conj_renorm_e(aR, aI, c, x, &result));
}


/* Copyright (c) 2014, Giuseppe Argentieri <giuseppe.argentieri@ts.infn.it>

 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 */
/*
 *
 *
 *       Filename:  polar.c
 *
 *    Description:  Cartesian <-> Polar coordinates transformation
 *
 *        Version:  1.0
 *        Created:  06/05/2014 14:21:40
 *       Revision:  none
 *        License:  BSD
 *
 *         Author:  Giuseppe Argentieri (ga), giuseppe.argentieri@ts.infn.it
 *   Organization:  Università degli Studi di Trieste
 *
 *
 */

#include <math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_math.h>
#include "funcs.h"

/*
 *      FUNCTION
 *         Name:  bloch_vector
 *  Description:  Return the Bloch vector corresponding to (r,theta,phi) polar coords
 *
 */
int bloch_vector ( gsl_vector* v, double r, double theta, double phi )
{
	/* Check the validity of coordinates */
	if ( r > 1 )                   /* Non physical state */
	{
		v = NULL ;
		return -1;
	}
	theta = fmod (theta, M_PI ) ;          /* Taking the modulo */
	phi   = fmod ( phi, 2*M_PI )  ;

	gsl_vector_set ( v, 1, r*sin(theta)*cos(phi) ) ;
	gsl_vector_set ( v, 2, r*sin(theta)*sin(phi) ) ;
	gsl_vector_set ( v, 3, r*cos(theta) ) ;

	return 0;
}		/* -----  end of function bloch_vector  ----- */

/*
 *      FUNCTION
 *         Name:  polars
 *  Description:  From the Bloch vector representation to the polar coordinates
 *
 */
int polars ( double* r, double* theta, double* phi, const gsl_vector* v )
{
	*r = gsl_hypot3( VECTOR(v,1), VECTOR(v,2), VECTOR(v,3) ) ;
	if ( *r > 1 )
		return -1 ;

	*theta = acos(VECTOR(v,3)) ;

	if ( gsl_fcmp(fabs(*theta),1,1e-9) )
		*phi = 0 ;
	else
		*phi = atan2(VECTOR(v,2),VECTOR(v,1)) ;

	return 0;
}		/* -----  end of function polars  ----- */