//
// C++ Interface: %{MODULE}
//
// Description:
//
//
// Author: %{AUTHOR} <%{EMAIL}>, (C) %{YEAR}
//
// Copyright: See COPYING file that comes with this distribution
//
//

#ifndef STK_BasicVector_h
#define STK_BasicVector_h

#include "common.h"
#include "Error.h"

#include <cstddef>
#include <cstdlib>
#include <stdexcept>
#include <iostream>

#ifdef HAVE_ATLAS
extern "C"{
  #ifdef USE_MKL
    #include "mkl.h"
  #else
    #include <cblas.h>
    #include <atlas/clapack.h>
  #endif
}
#endif

namespace STK
{
  template<typename _ElemT> class BasicVector;
  template<typename _ElemT> class Matrix;

  // we need to declare the friend << operator here
  template<typename _ElemT>
    std::ostream & operator << (std::ostream & out, BasicVector<_ElemT> & m);

  /** **************************************************************************
   ** **************************************************************************
   *  @brief Provides a matrix abstraction class
   *
   *  This class provides a way to work with matrices in STK.
   *  It encapsulates basic operations and memory optimizations.
   *
   */
  template<typename _ElemT>
      class BasicVector
    {
    public:
      BasicVector(): mpData(NULL)
#ifdef STK_MEMALIGN_MANUAL
        ,mpFreeData(NULL)
#endif
        , mLength(0)
      {}

      /**
       * @brief Copy constructor
       * @param rV
       */
      BasicVector(const BasicVector<_ElemT>& rV);

      BasicVector(const size_t s);

      BasicVector(const _ElemT* pData, const size_t s);

      ~BasicVector();

      void
      Init(size_t length);

      /**
       * @brief Dealocates the window from memory and resets the dimensions to (0)
       */
      void
      Destroy();

      /**
       * @brief Returns @c true if vector is initialized
       */
      const bool
      IsInitialized() const
      { return mpData != NULL; }

      /**
       * @brief Sets all elements to 0
       */
      void
      Clear();

      const size_t
      Length() const
      { return mLength; }

      /**
       * @brief Returns size of matrix in memory (in bytes)
       */
      const size_t
      MSize() const
      {
        return (mLength + (((16 / sizeof(_ElemT)) - mLength%(16 / sizeof(_ElemT)))
                          % (16 / sizeof(_ElemT)))) * sizeof(_ElemT);
      }

      /**
       *  @brief Gives access to the vector memory area
       *  @return pointer to the first field
       */
      _ElemT*
      pData() const
      { return mpData; }

      /**
       *  @brief Gives access to the vector memory area
       *  @return pointer to the first field
       */
      const _ElemT* const
      cpData() const
      { return mpData; }

      /**
       *  @brief Gives access to a specified vector element without range check
       *  @return pointer to the first field of the row
       */
      _ElemT&
      operator [] (size_t i) const
      { return *(mpData + i);}

      //########################################################################
      //########################################################################
      BasicVector<_ElemT>&
      AddCVMul(const _ElemT c, const BasicVector<_ElemT>& rV);

      BasicVector<_ElemT>&
      AddCVMul(const _ElemT c, const _ElemT* pV);

      BasicVector<_ElemT>&
      AddCVVDotMul(const _ElemT c, const _ElemT* pA, const size_t nA,
                                   const _ElemT* pB, const size_t nB);

      /// this = this + c * rM * rV
      BasicVector<_ElemT>&
      AddCMVMul(const _ElemT c, const Matrix<_ElemT>& rM,
                const BasicVector<_ElemT>& rV);

      /// this = this + c * rM * pV
      BasicVector<_ElemT>&
      AddCMVMul(const _ElemT c, const Matrix<_ElemT>& rM,
                const _ElemT* pV);

      /// this = this + c * rM * rV
      BasicVector<_ElemT>&
      AddCMtVMul(const _ElemT c, const Matrix<_ElemT>& rM,
                 const BasicVector<_ElemT>& rV);

      /// this = this + c * rM * rV
      BasicVector<_ElemT>&
      AddCMtVMul(const _ElemT c, const Matrix<_ElemT>& rM,
                 const _ElemT* pV);

      /// Adds a diagonal after Matrix-Matrix Multiplication
      BasicVector<_ElemT>&
      AddDiagCMMMul(const _ElemT c, const Matrix<_ElemT>& rMa,
                    const Matrix<_ElemT>& rMb);

      _ElemT
      Dot(const BasicVector<_ElemT>& rV);

      _ElemT
      Dot(const _ElemT* pV);

      /// Performs a row stack of the matrix rMa
      BasicVector<_ElemT>&
      MatrixRowStack(const Matrix<_ElemT>& rMa);

      /// Returns sum of the elements
      _ElemT
      Sum() const;

      /// Returns sum of the elements
      BasicVector<_ElemT>&
      AddRowSum(const Matrix<_ElemT>& rM);

      /// Returns sum of the elements
      BasicVector<_ElemT>&
      AddColSum(const Matrix<_ElemT>& rM);

      /// Returns log(sum(exp())) without exp overflow
      _ElemT
      LogSumExp() const;

      //########################################################################
      //########################################################################

      friend std::ostream &
      operator << <> (
        std::ostream& rOut,
        BasicVector<_ElemT>& rV);

      //operator _ElemT* ()
      //{
      //  return mpData;
      //}

    //##########################################################################
    //##########################################################################
    //protected:
    public:
      /// data memory area
      _ElemT*   mpData;
#ifdef STK_MEMALIGN_MANUAL
      /// data to be freed (in case of manual memalignment use, see common.h)
      _ElemT*   mpFreeData;
#endif
      size_t  mLength;      ///< Number of elements
    }; // class BasicVector

}; // namespace STK

//*****************************************************************************
//*****************************************************************************
// we need to include the implementation
#include "BasicVector.tcc"
//*****************************************************************************
//*****************************************************************************

/******************************************************************************
 ******************************************************************************
 * The following section contains specialized template definitions
 * whose implementation is in BasicVector.cc
 */

namespace STK
{
  template<>
    BasicVector<float>&
    BasicVector<float>::
    AddCVMul(const float c, const BasicVector<float>& rV);

  template<>
    BasicVector<double>&
    BasicVector<double>::
    AddCVMul(const double c, const BasicVector<double>& rV);

  template<>
    BasicVector<float>&
    BasicVector<float>::
    AddCVMul(const float c, const float* pV);

  template<>
    BasicVector<double>&
    BasicVector<double>::
    AddCVMul(const double c, const double* pV);

  template<>
    BasicVector<float>&
    BasicVector<float>::
    AddCMVMul(const float c, const Matrix<float>& rV, const BasicVector<float>& rV2);

  template<>
    BasicVector<double>&
    BasicVector<double>::
    AddCMVMul(const double c, const Matrix<double>& rV, const BasicVector<double>& rV2);

  template<>
    BasicVector<float>&
    BasicVector<float>::
    AddCMtVMul(const float c, const Matrix<float>& rV, const BasicVector<float>& rV2);

  template<>
    BasicVector<double>&
    BasicVector<double>::
    AddCMtVMul(const double c, const Matrix<double>& rV, const BasicVector<double>& rV2);

  template<>
    BasicVector<float>&
    BasicVector<float>::
    AddCMtVMul(const float c, const Matrix<float>& rV, const float* pV);

  template<>
    BasicVector<double>&
    BasicVector<double>::
    AddCMtVMul(const double c, const Matrix<double>& rV, const double* pV);

  template<>
    float
    BasicVector<float>::
    Dot(const float* pV);

  template<>
    double
    BasicVector<double>::
    Dot(const double* pV);

} // namespace STK

#endif // #ifndef STK_BasicVector_h


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

#ifndef __GSL_RANDIST_H__
#define __GSL_RANDIST_H__
#include <gsl/gsl_rng.h>
#include <gsl/gsl_types.h>

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
# define __BEGIN_DECLS extern "C" {
# define __END_DECLS }
#else
# define __BEGIN_DECLS /* empty */
# define __END_DECLS /* empty */
#endif

__BEGIN_DECLS

GSL_EXPORT unsigned int gsl_ran_bernoulli (const gsl_rng * r, double p);
GSL_EXPORT double gsl_ran_bernoulli_pdf (const unsigned int k, double p);

GSL_EXPORT double gsl_ran_beta (const gsl_rng * r, const double a, const double b);
GSL_EXPORT double gsl_ran_beta_pdf (const double x, const double a, const double b);

GSL_EXPORT unsigned int gsl_ran_binomial (const gsl_rng * r, double p, unsigned int n);
GSL_EXPORT unsigned int gsl_ran_binomial_knuth (const gsl_rng * r, double p, unsigned int n);
GSL_EXPORT unsigned int gsl_ran_binomial_tpe (const gsl_rng * r, double p, unsigned int n);
GSL_EXPORT double gsl_ran_binomial_pdf (const unsigned int k, const double p, const unsigned int n);

GSL_EXPORT double gsl_ran_exponential (const gsl_rng * r, const double mu);
GSL_EXPORT double gsl_ran_exponential_pdf (const double x, const double mu);

GSL_EXPORT double gsl_ran_exppow (const gsl_rng * r, const double a, const double b);
GSL_EXPORT double gsl_ran_exppow_pdf (const double x, const double a, const double b);

GSL_EXPORT double gsl_ran_cauchy (const gsl_rng * r, const double a);
GSL_EXPORT double gsl_ran_cauchy_pdf (const double x, const double a);

GSL_EXPORT double gsl_ran_chisq (const gsl_rng * r, const double nu);
GSL_EXPORT double gsl_ran_chisq_pdf (const double x, const double nu);

GSL_EXPORT void gsl_ran_dirichlet (const gsl_rng * r, const size_t K, const double alpha[], double theta[]);
GSL_EXPORT double gsl_ran_dirichlet_pdf (const size_t K, const double alpha[], const double theta[]);
GSL_EXPORT double gsl_ran_dirichlet_lnpdf (const size_t K, const double alpha[], const double theta[]);

GSL_EXPORT double gsl_ran_erlang (const gsl_rng * r, const double a, const double n);
GSL_EXPORT double gsl_ran_erlang_pdf (const double x, const double a, const double n);

GSL_EXPORT double gsl_ran_fdist (const gsl_rng * r, const double nu1, const double nu2);
GSL_EXPORT double gsl_ran_fdist_pdf (const double x, const double nu1, const double nu2);

GSL_EXPORT double gsl_ran_flat (const gsl_rng * r, const double a, const double b);
GSL_EXPORT double gsl_ran_flat_pdf (double x, const double a, const double b);

GSL_EXPORT double gsl_ran_gamma (const gsl_rng * r, const double a, const double b);
GSL_EXPORT double gsl_ran_gamma_int (const gsl_rng * r, const unsigned int a);
GSL_EXPORT double gsl_ran_gamma_pdf (const double x, const double a, const double b);
GSL_EXPORT double gsl_ran_gamma_mt (const gsl_rng * r, const double a, const double b);

GSL_EXPORT double gsl_ran_gaussian (const gsl_rng * r, const double sigma);
GSL_EXPORT double gsl_ran_gaussian_ratio_method (const gsl_rng * r, const double sigma);
GSL_EXPORT double gsl_ran_gaussian_ziggurat (const gsl_rng * r, const double sigma);
GSL_EXPORT double gsl_ran_gaussian_pdf (const double x, const double sigma);

GSL_EXPORT double gsl_ran_ugaussian (const gsl_rng * r);
GSL_EXPORT double gsl_ran_ugaussian_ratio_method (const gsl_rng * r);
GSL_EXPORT double gsl_ran_ugaussian_pdf (const double x);

GSL_EXPORT double gsl_ran_gaussian_tail (const gsl_rng * r, const double a, const double sigma);
GSL_EXPORT double gsl_ran_gaussian_tail_pdf (const double x, const double a, const double sigma);

GSL_EXPORT double gsl_ran_ugaussian_tail (const gsl_rng * r, const double a);
GSL_EXPORT double gsl_ran_ugaussian_tail_pdf (const double x, const double a);

GSL_EXPORT void gsl_ran_bivariate_gaussian (const gsl_rng * r, double sigma_x, double sigma_y, double rho, double *x, double *y);
GSL_EXPORT double gsl_ran_bivariate_gaussian_pdf (const double x, const double y, const double sigma_x, const double sigma_y, const double rho);

GSL_EXPORT double gsl_ran_landau (const gsl_rng * r);
GSL_EXPORT double gsl_ran_landau_pdf (const double x);

GSL_EXPORT unsigned int gsl_ran_geometric (const gsl_rng * r, const double p);
GSL_EXPORT double gsl_ran_geometric_pdf (const unsigned int k, const double p);

GSL_EXPORT unsigned int gsl_ran_hypergeometric (const gsl_rng * r, unsigned int n1, unsigned int n2, unsigned int t);
GSL_EXPORT double gsl_ran_hypergeometric_pdf (const unsigned int k, const unsigned int n1, const unsigned int n2, unsigned int t);

GSL_EXPORT double gsl_ran_gumbel1 (const gsl_rng * r, const double a, const double b);
GSL_EXPORT double gsl_ran_gumbel1_pdf (const double x, const double a, const double b);

GSL_EXPORT double gsl_ran_gumbel2 (const gsl_rng * r, const double a, const double b);
GSL_EXPORT double gsl_ran_gumbel2_pdf (const double x, const double a, const double b);

GSL_EXPORT double gsl_ran_logistic (const gsl_rng * r, const double a);
GSL_EXPORT double gsl_ran_logistic_pdf (const double x, const double a);

GSL_EXPORT double gsl_ran_lognormal (const gsl_rng * r, const double zeta, const double sigma);
GSL_EXPORT double gsl_ran_lognormal_pdf (const double x, const double zeta, const double sigma);

GSL_EXPORT unsigned int gsl_ran_logarithmic (const gsl_rng * r, const double p);
GSL_EXPORT double gsl_ran_logarithmic_pdf (const unsigned int k, const double p);

GSL_EXPORT void gsl_ran_multinomial (const gsl_rng * r, const size_t K,
                                     const unsigned int N, const double p[],
                                     unsigned int n[] );
GSL_EXPORT double gsl_ran_multinomial_pdf (const size_t K,
                                           const double p[], const unsigned int n[] );
GSL_EXPORT double gsl_ran_multinomial_lnpdf (const size_t K,
                                             const double p[], const unsigned int n[] );

GSL_EXPORT unsigned int gsl_ran_negative_binomial (const gsl_rng * r, double p, double n);
GSL_EXPORT double gsl_ran_negative_binomial_pdf (const unsigned int k, const double p, double n);

GSL_EXPORT unsigned int gsl_ran_pascal (const gsl_rng * r, double p, unsigned int n);
GSL_EXPORT double gsl_ran_pascal_pdf (const unsigned int k, const double p, unsigned int n);

GSL_EXPORT double gsl_ran_pareto (const gsl_rng * r, double a, const double b);
GSL_EXPORT double gsl_ran_pareto_pdf (const double x, const double a, const double b);

GSL_EXPORT unsigned int gsl_ran_poisson (const gsl_rng * r, double mu);
GSL_EXPORT void gsl_ran_poisson_array (const gsl_rng * r, size_t n, unsigned int array[],
                                       double mu);
GSL_EXPORT double gsl_ran_poisson_pdf (const unsigned int k, const double mu);

GSL_EXPORT double gsl_ran_rayleigh (const gsl_rng * r, const double sigma);
GSL_EXPORT double gsl_ran_rayleigh_pdf (const double x, const double sigma);

GSL_EXPORT double gsl_ran_rayleigh_tail (const gsl_rng * r, const double a, const double sigma);
GSL_EXPORT double gsl_ran_rayleigh_tail_pdf (const double x, const double a, const double sigma);

GSL_EXPORT double gsl_ran_tdist (const gsl_rng * r, const double nu);
GSL_EXPORT double gsl_ran_tdist_pdf (const double x, const double nu);

GSL_EXPORT double gsl_ran_laplace (const gsl_rng * r, const double a);
GSL_EXPORT double gsl_ran_laplace_pdf (const double x, const double a);

GSL_EXPORT double gsl_ran_levy (const gsl_rng * r, const double c, const double alpha);
GSL_EXPORT double gsl_ran_levy_skew (const gsl_rng * r, const double c, const double alpha, const double beta);

GSL_EXPORT double gsl_ran_weibull (const gsl_rng * r, const double a, const double b);
GSL_EXPORT double gsl_ran_weibull_pdf (const double x, const double a, const double b);

GSL_EXPORT void gsl_ran_dir_2d (const gsl_rng * r, double * x, double * y);
GSL_EXPORT void gsl_ran_dir_2d_trig_method (const gsl_rng * r, double * x, double * y);
GSL_EXPORT void gsl_ran_dir_3d (const gsl_rng * r, double * x, double * y, double * z);
GSL_EXPORT void gsl_ran_dir_nd (const gsl_rng * r, size_t n, double * x);

GSL_EXPORT void gsl_ran_shuffle (const gsl_rng * r, void * base, size_t nmembm, size_t size);
GSL_EXPORT int gsl_ran_choose (const gsl_rng * r, void * dest, size_t k, void * src, size_t n, size_t size) ;
GSL_EXPORT void gsl_ran_sample (const gsl_rng * r, void * dest, size_t k, void * src, size_t n, size_t size) ;

typedef struct {                /* struct for Walker algorithm */
    size_t K;
    size_t *A;
    double *F;
} gsl_ran_discrete_t;

GSL_EXPORT gsl_ran_discrete_t * gsl_ran_discrete_preproc (size_t K, const double *P);
GSL_EXPORT void gsl_ran_discrete_free(gsl_ran_discrete_t *g);
GSL_EXPORT size_t gsl_ran_discrete (const gsl_rng *r, const gsl_ran_discrete_t *g);
GSL_EXPORT double gsl_ran_discrete_pdf (size_t k, const gsl_ran_discrete_t *g);

__END_DECLS

#endif /* __GSL_RANDIST_H__ */


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

#include <gsl/gsl_matrix.h>

#include <gpc/gpc.h>
#include <egsl/egsl_macros.h>

#include "../csm_all.h"

#include "icp.h"

void sm_journal_open(const char* file) {
	file = 0;
/*	journal_open(file);*/
}

void ld_invalid_if_outside(LDP ld, double min_reading, double max_reading) {
	int i;
	for(i=0;i<ld->nrays;i++) {
		if(!ld_valid_ray(ld, i)) continue;
		double r = ld->readings[i];
		if( r <= min_reading || r > max_reading)
			ld->valid[i] = 0;
	}
}

void sm_icp(struct sm_params*params, struct sm_result*res) {
	res->valid = 0;

	LDP laser_ref  = params->laser_ref;
	LDP laser_sens = params->laser_sens;

	if(!ld_valid_fields(laser_ref) ||
	   !ld_valid_fields(laser_sens)) {
		return;
	}

	sm_debug("sm_icp: laser_sens has %d/%d; laser_ref has %d/%d rays valid\n",
		count_equal(laser_sens->valid, laser_sens->nrays, 1), laser_sens->nrays,
		count_equal(laser_ref->valid, laser_ref->nrays, 1), laser_ref->nrays);

	/** Mark as invalid the rays outside of (min_reading, max_reading] */
	ld_invalid_if_outside(laser_ref, params->min_reading, params->max_reading);
	ld_invalid_if_outside(laser_sens, params->min_reading, params->max_reading);

	sm_debug("sm_icp:  laser_sens has %d/%d; laser_ref has %d/%d rays valid (after removing outside interval [%f, %f])\n",
		count_equal(laser_sens->valid, laser_sens->nrays, 1), laser_sens->nrays,
		count_equal(laser_ref->valid, laser_ref->nrays, 1), laser_ref->nrays,
   	   params->min_reading, params->max_reading);

	if(JJ) jj_context_enter("sm_icp");

	egsl_push_named("sm_icp");

	if(params->use_corr_tricks || params->debug_verify_tricks)
		ld_create_jump_tables(laser_ref);

	ld_compute_cartesian(laser_ref);
	ld_compute_cartesian(laser_sens);

	if(params->do_alpha_test) {
		ld_simple_clustering(laser_ref, params->clustering_threshold);
		ld_compute_orientation(laser_ref, params->orientation_neighbourhood, params->sigma);
		ld_simple_clustering(laser_sens, params->clustering_threshold);
		ld_compute_orientation(laser_sens, params->orientation_neighbourhood, params->sigma);
	}

	if(JJ) jj_add("laser_ref",  ld_to_json(laser_ref));
	if(JJ) jj_add("laser_sens", ld_to_json(laser_sens));

	gsl_vector * x_new = gsl_vector_alloc(3);
	gsl_vector * x_old = vector_from_array(3, params->first_guess);

	if(params->do_visibility_test) {
		sm_debug("laser_ref:\n");
		visibilityTest(laser_ref, x_old);

		sm_debug("laser_sens:\n");
		gsl_vector * minus_x_old = gsl_vector_alloc(3);
		ominus(x_old,minus_x_old);
		visibilityTest(laser_sens, minus_x_old);
		gsl_vector_free(minus_x_old);
	}

	double error;
	int iterations;
	int nvalid;
	if(!icp_loop(params, x_old->data, x_new->data, &error, &nvalid, &iterations)) {
		sm_error("icp: ICP failed for some reason. \n");
		res->valid = 0;
		res->iterations = iterations;
		res->nvalid = 0;

	} else {
		/* It was succesfull */

		int restarted = 0;
		double best_error = error;
		gsl_vector * best_x = gsl_vector_alloc(3);
		gsl_vector_memcpy(best_x, x_new);

		if(params->restart &&
			(error/nvalid)>(params->restart_threshold_mean_error) ) {
			sm_debug("Restarting: %f > %f \n",(error/nvalid),(params->restart_threshold_mean_error));
			restarted = 1;
			double dt  = params->restart_dt;
			double dth = params->restart_dtheta;
			sm_debug("icp_loop: dt = %f dtheta= %f deg\n",dt,rad2deg(dth));

			double perturb[6][3] = {
				{dt,0,0}, {-dt,0,0},
				{0,dt,0}, {0,-dt,0},
				{0,0,dth}, {0,0,-dth}
			};

			int a; for(a=0;a<6;a++){
				sm_debug("-- Restarting with perturbation #%d\n", a);
				struct sm_params my_params = *params;
				gsl_vector * start = gsl_vector_alloc(3);
					gvs(start, 0, gvg(x_new,0)+perturb[a][0]);
					gvs(start, 1, gvg(x_new,1)+perturb[a][1]);
					gvs(start, 2, gvg(x_new,2)+perturb[a][2]);
				gsl_vector * x_a = gsl_vector_alloc(3);
				double my_error; int my_valid; int my_iterations;
				if(!icp_loop(&my_params, start->data, x_a->data, &my_error, &my_valid, &my_iterations)){
					sm_error("Error during restart #%d/%d. \n", a, 6);
					break;
				}
				iterations+=my_iterations;

				if(my_error < best_error) {
					sm_debug("--Perturbation #%d resulted in error %f < %f\n", a,my_error,best_error);
					gsl_vector_memcpy(best_x, x_a);
					best_error = my_error;
				}
				gsl_vector_free(x_a); gsl_vector_free(start);
			}
		}

		/* At last, we did it. */
		res->valid = 1;
		vector_to_array(best_x, res->x);
		sm_debug("icp: final x =  %s  \n", gsl_friendly_pose(best_x));

		if (restarted) { // recompute correspondences in case of restarts
			ld_compute_world_coords(laser_sens, res->x);
			if(params->use_corr_tricks)
				find_correspondences_tricks(params);
			else
				find_correspondences(params);
		}

		if(params->do_compute_covariance)  {

			val cov0_x, dx_dy1, dx_dy2;
			compute_covariance_exact(
				laser_ref, laser_sens, best_x,
				&cov0_x, &dx_dy1, &dx_dy2);

			val cov_x = sc(square(params->sigma), cov0_x);
/*			egsl_v2da(cov_x, res->cov_x); */

			res->cov_x_m = egsl_v2gslm(cov_x);
			res->dx_dy1_m = egsl_v2gslm(dx_dy1);
			res->dx_dy2_m = egsl_v2gslm(dx_dy2);

			if(0) {
				egsl_print("cov0_x", cov0_x);
				egsl_print_spectrum("cov0_x", cov0_x);

				val fim = ld_fisher0(laser_ref);
				val ifim = inv(fim);
				egsl_print("fim", fim);
				egsl_print_spectrum("ifim", ifim);
			}
		}

		res->error = best_error;
		res->iterations = iterations;
		res->nvalid = nvalid;

		gsl_vector_free(best_x);
	}
	gsl_vector_free(x_new);
	gsl_vector_free(x_old);

	egsl_pop_named("sm_icp");

	if(JJ) jj_context_exit();
}

void sm_icp_xy(struct sm_params*params, struct sm_result*res)
{
	res->valid = 0;

	LDP laser_ref  = params->laser_ref;
	LDP laser_sens = params->laser_sens;

	if(!ld_valid_fields(laser_ref) ||
	   !ld_valid_fields(laser_sens)) {
		return;
	}

/*
	sm_debug("sm_icp: laser_sens has %d/%d; laser_ref has %d/%d rays valid\n",
		count_equal(laser_sens->valid, laser_sens->nrays, 1), laser_sens->nrays,
		count_equal(laser_ref->valid, laser_ref->nrays, 1), laser_ref->nrays);
	*/

	/** Mark as invalid the rays outside of (min_reading, max_reading] */
	ld_invalid_if_outside(laser_ref, params->min_reading, params->max_reading);
	ld_invalid_if_outside(laser_sens, params->min_reading, params->max_reading);

/*
	sm_debug("sm_icp:  laser_sens has %d/%d; laser_ref has %d/%d rays valid (after removing outside interval [%f, %f])\n",
		count_equal(laser_sens->valid, laser_sens->nrays, 1), laser_sens->nrays,
		count_equal(laser_ref->valid, laser_ref->nrays, 1), laser_ref->nrays,
   	   params->min_reading, params->max_reading);

	if(JJ) jj_context_enter("sm_icp");

	egsl_push_named("sm_icp");
	*/

	if(params->use_corr_tricks || params->debug_verify_tricks)
		ld_create_jump_tables(laser_ref);
/*
	ld_compute_cartesian(laser_ref);
	ld_compute_cartesian(laser_sens);
*/

	if(params->do_alpha_test) {
		ld_simple_clustering(laser_ref, params->clustering_threshold);
		ld_compute_orientation(laser_ref, params->orientation_neighbourhood, params->sigma);
		ld_simple_clustering(laser_sens, params->clustering_threshold);
		ld_compute_orientation(laser_sens, params->orientation_neighbourhood, params->sigma);
	}

	if(JJ) jj_add("laser_ref",  ld_to_json(laser_ref));
	if(JJ) jj_add("laser_sens", ld_to_json(laser_sens));

	gsl_vector * x_new = gsl_vector_alloc(3);
	gsl_vector * x_old = vector_from_array(3, params->first_guess);

	if(params->do_visibility_test) {
		sm_debug("laser_ref:\n");
		visibilityTest(laser_ref, x_old);

		sm_debug("laser_sens:\n");
		gsl_vector * minus_x_old = gsl_vector_alloc(3);
		ominus(x_old,minus_x_old);
		visibilityTest(laser_sens, minus_x_old);
		gsl_vector_free(minus_x_old);
	}

	double error;
	int iterations;
	int nvalid;
	if(!icp_loop(params, x_old->data, x_new->data, &error, &nvalid, &iterations)) {
		sm_error("icp: ICP failed for some reason. \n");
		res->valid = 0;
		res->iterations = iterations;
		res->nvalid = 0;

	} else {
		/* It was succesfull */

		double best_error = error;
		gsl_vector * best_x = gsl_vector_alloc(3);
		gsl_vector_memcpy(best_x, x_new);

		if(params->restart &&
			(error/nvalid)>(params->restart_threshold_mean_error) ) {
			sm_debug("Restarting: %f > %f \n",(error/nvalid),(params->restart_threshold_mean_error));
			double dt  = params->restart_dt;
			double dth = params->restart_dtheta;
			sm_debug("icp_loop: dt = %f dtheta= %f deg\n",dt,rad2deg(dth));

			double perturb[6][3] = {
				{dt,0,0}, {-dt,0,0},
				{0,dt,0}, {0,-dt,0},
				{0,0,dth}, {0,0,-dth}
			};

			int a; for(a=0;a<6;a++){
				sm_debug("-- Restarting with perturbation #%d\n", a);
				struct sm_params my_params = *params;
				gsl_vector * start = gsl_vector_alloc(3);
					gvs(start, 0, gvg(x_new,0)+perturb[a][0]);
					gvs(start, 1, gvg(x_new,1)+perturb[a][1]);
					gvs(start, 2, gvg(x_new,2)+perturb[a][2]);
				gsl_vector * x_a = gsl_vector_alloc(3);
				double my_error; int my_valid; int my_iterations;
				if(!icp_loop(&my_params, start->data, x_a->data, &my_error, &my_valid, &my_iterations)){
					sm_error("Error during restart #%d/%d. \n", a, 6);
					break;
				}
				iterations+=my_iterations;

				if(my_error < best_error) {
					sm_debug("--Perturbation #%d resulted in error %f < %f\n", a,my_error,best_error);
					gsl_vector_memcpy(best_x, x_a);
					best_error = my_error;
				}
				gsl_vector_free(x_a); gsl_vector_free(start);
			}
		}

		/* At last, we did it. */
		res->valid = 1;
		vector_to_array(best_x, res->x);
		sm_debug("icp: final x =  %s  \n", gsl_friendly_pose(best_x));

		if(params->do_compute_covariance)  {

			val cov0_x, dx_dy1, dx_dy2;
			compute_covariance_exact(
				laser_ref, laser_sens, best_x,
				&cov0_x, &dx_dy1, &dx_dy2);

			val cov_x = sc(square(params->sigma), cov0_x);
//			egsl_v2da(cov_x, res->cov_x);

			res->cov_x_m = egsl_v2gslm(cov_x);
			res->dx_dy1_m = egsl_v2gslm(dx_dy1);
			res->dx_dy2_m = egsl_v2gslm(dx_dy2);

			if(0) {
				egsl_print("cov0_x", cov0_x);
				egsl_print_spectrum("cov0_x", cov0_x);

				val fim = ld_fisher0(laser_ref);
				val ifim = inv(fim);
				egsl_print("fim", fim);
				egsl_print_spectrum("ifim", ifim);
			}
		}

		res->error = best_error;
		res->iterations = iterations;
		res->nvalid = nvalid;

		gsl_vector_free(x_new);
		gsl_vector_free(x_old);
		gsl_vector_free(best_x);
	}
/*
	egsl_pop_named("sm_icp");

	if(JJ) jj_context_exit();
*/
}


/*
 * Copyright (c) 1997-1999 Massachusetts Institute of Technology
 *
 * 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

/* This file was automatically generated --- DO NOT EDIT */
/* Generated on Mon Mar  8 17:44:40 EST 1999 */

#include <fftw-int.h>
#include <fftw.h>

/* Generated by: ./genfft -magic-alignment-check -magic-twiddle-load-all -magic-variables 4 -magic-loopi -notwiddle 9 */

/*
 * This function contains 80 FP additions, 40 FP multiplications,
 * (or, 60 additions, 20 multiplications, 20 fused multiply/add),
 * 30 stack variables, and 36 memory accesses
 */
static const fftw_real K939692620 = FFTW_KONST(+0.939692620785908384054109277324731469936208134);
static const fftw_real K342020143 = FFTW_KONST(+0.342020143325668733044099614682259580763083368);
static const fftw_real K984807753 = FFTW_KONST(+0.984807753012208059366743024589523013670643252);
static const fftw_real K173648177 = FFTW_KONST(+0.173648177666930348851716626769314796000375677);
static const fftw_real K642787609 = FFTW_KONST(+0.642787609686539326322643409907263432907559884);
static const fftw_real K766044443 = FFTW_KONST(+0.766044443118978035202392650555416673935832457);
static const fftw_real K866025403 = FFTW_KONST(+0.866025403784438646763723170752936183471402627);
static const fftw_real K500000000 = FFTW_KONST(+0.500000000000000000000000000000000000000000000);

/*
 * Generator Id's :
 * $Id: exprdag.ml,v 1.36 1999/02/19 17:22:11 athena Exp $
 * $Id: fft.ml,v 1.41 1999/02/19 17:22:13 athena Exp $
 * $Id: to_c.ml,v 1.24 1999/02/19 17:22:17 athena Exp $
 */

void fftw_no_twiddle_9(const fftw_complex *input, fftw_complex *output, int istride, int ostride)
{
     fftw_real tmp5;
     fftw_real tmp17;
     fftw_real tmp50;
     fftw_real tmp20;
     fftw_real tmp78;
     fftw_real tmp53;
     fftw_real tmp10;
     fftw_real tmp26;
     fftw_real tmp58;
     fftw_real tmp74;
     fftw_real tmp31;
     fftw_real tmp59;
     fftw_real tmp15;
     fftw_real tmp37;
     fftw_real tmp62;
     fftw_real tmp75;
     fftw_real tmp42;
     fftw_real tmp61;
     ASSERT_ALIGNED_DOUBLE();
     {
	  fftw_real tmp1;
	  fftw_real tmp2;
	  fftw_real tmp3;
	  fftw_real tmp4;
	  ASSERT_ALIGNED_DOUBLE();
	  tmp1 = c_re(input[0]);
	  tmp2 = c_re(input[3 * istride]);
	  tmp3 = c_re(input[6 * istride]);
	  tmp4 = tmp2 + tmp3;
	  tmp5 = tmp1 + tmp4;
	  tmp17 = tmp1 - (K500000000 * tmp4);
	  tmp50 = K866025403 * (tmp3 - tmp2);
     }
     {
	  fftw_real tmp51;
	  fftw_real tmp18;
	  fftw_real tmp19;
	  fftw_real tmp52;
	  ASSERT_ALIGNED_DOUBLE();
	  tmp51 = c_im(input[0]);
	  tmp18 = c_im(input[3 * istride]);
	  tmp19 = c_im(input[6 * istride]);
	  tmp52 = tmp18 + tmp19;
	  tmp20 = K866025403 * (tmp18 - tmp19);
	  tmp78 = tmp51 + tmp52;
	  tmp53 = tmp51 - (K500000000 * tmp52);
     }
     {
	  fftw_real tmp6;
	  fftw_real tmp28;
	  fftw_real tmp9;
	  fftw_real tmp27;
	  fftw_real tmp25;
	  fftw_real tmp29;
	  fftw_real tmp22;
	  fftw_real tmp30;
	  ASSERT_ALIGNED_DOUBLE();
	  tmp6 = c_re(input[istride]);
	  tmp28 = c_im(input[istride]);
	  {
	       fftw_real tmp7;
	       fftw_real tmp8;
	       fftw_real tmp23;
	       fftw_real tmp24;
	       ASSERT_ALIGNED_DOUBLE();
	       tmp7 = c_re(input[4 * istride]);
	       tmp8 = c_re(input[7 * istride]);
	       tmp9 = tmp7 + tmp8;
	       tmp27 = K866025403 * (tmp8 - tmp7);
	       tmp23 = c_im(input[4 * istride]);
	       tmp24 = c_im(input[7 * istride]);
	       tmp25 = K866025403 * (tmp23 - tmp24);
	       tmp29 = tmp23 + tmp24;
	  }
	  tmp10 = tmp6 + tmp9;
	  tmp22 = tmp6 - (K500000000 * tmp9);
	  tmp26 = tmp22 + tmp25;
	  tmp58 = tmp22 - tmp25;
	  tmp74 = tmp28 + tmp29;
	  tmp30 = tmp28 - (K500000000 * tmp29);
	  tmp31 = tmp27 + tmp30;
	  tmp59 = tmp30 - tmp27;
     }
     {
	  fftw_real tmp11;
	  fftw_real tmp39;
	  fftw_real tmp14;
	  fftw_real tmp38;
	  fftw_real tmp36;
	  fftw_real tmp40;
	  fftw_real tmp33;
	  fftw_real tmp41;
	  ASSERT_ALIGNED_DOUBLE();
	  tmp11 = c_re(input[2 * istride]);
	  tmp39 = c_im(input[2 * istride]);
	  {
	       fftw_real tmp12;
	       fftw_real tmp13;
	       fftw_real tmp34;
	       fftw_real tmp35;
	       ASSERT_ALIGNED_DOUBLE();
	       tmp12 = c_re(input[5 * istride]);
	       tmp13 = c_re(input[8 * istride]);
	       tmp14 = tmp12 + tmp13;
	       tmp38 = K866025403 * (tmp13 - tmp12);
	       tmp34 = c_im(input[5 * istride]);
	       tmp35 = c_im(input[8 * istride]);
	       tmp36 = K866025403 * (tmp34 - tmp35);
	       tmp40 = tmp34 + tmp35;
	  }
	  tmp15 = tmp11 + tmp14;
	  tmp33 = tmp11 - (K500000000 * tmp14);
	  tmp37 = tmp33 + tmp36;
	  tmp62 = tmp33 - tmp36;
	  tmp75 = tmp39 + tmp40;
	  tmp41 = tmp39 - (K500000000 * tmp40);
	  tmp42 = tmp38 + tmp41;
	  tmp61 = tmp41 - tmp38;
     }
     {
	  fftw_real tmp76;
	  fftw_real tmp16;
	  fftw_real tmp73;
	  fftw_real tmp77;
	  fftw_real tmp79;
	  fftw_real tmp80;
	  ASSERT_ALIGNED_DOUBLE();
	  tmp76 = K866025403 * (tmp74 - tmp75);
	  tmp16 = tmp10 + tmp15;
	  tmp73 = tmp5 - (K500000000 * tmp16);
	  c_re(output[0]) = tmp5 + tmp16;
	  c_re(output[3 * ostride]) = tmp73 + tmp76;
	  c_re(output[6 * ostride]) = tmp73 - tmp76;
	  tmp77 = K866025403 * (tmp15 - tmp10);
	  tmp79 = tmp74 + tmp75;
	  tmp80 = tmp78 - (K500000000 * tmp79);
	  c_im(output[3 * ostride]) = tmp77 + tmp80;
	  c_im(output[6 * ostride]) = tmp80 - tmp77;
	  c_im(output[0]) = tmp78 + tmp79;
     }
     {
	  fftw_real tmp21;
	  fftw_real tmp54;
	  fftw_real tmp44;
	  fftw_real tmp49;
	  fftw_real tmp48;
	  fftw_real tmp55;
	  fftw_real tmp45;
	  fftw_real tmp56;
	  ASSERT_ALIGNED_DOUBLE();
	  tmp21 = tmp17 + tmp20;
	  tmp54 = tmp50 + tmp53;
	  {
	       fftw_real tmp32;
	       fftw_real tmp43;
	       fftw_real tmp46;
	       fftw_real tmp47;
	       ASSERT_ALIGNED_DOUBLE();
	       tmp32 = (K766044443 * tmp26) + (K642787609 * tmp31);
	       tmp43 = (K173648177 * tmp37) + (K984807753 * tmp42);
	       tmp44 = tmp32 + tmp43;
	       tmp49 = K866025403 * (tmp43 - tmp32);
	       tmp46 = (K766044443 * tmp31) - (K642787609 * tmp26);
	       tmp47 = (K173648177 * tmp42) - (K984807753 * tmp37);
	       tmp48 = K866025403 * (tmp46 - tmp47);
	       tmp55 = tmp46 + tmp47;
	  }
	  c_re(output[ostride]) = tmp21 + tmp44;
	  tmp45 = tmp21 - (K500000000 * tmp44);
	  c_re(output[7 * ostride]) = tmp45 - tmp48;
	  c_re(output[4 * ostride]) = tmp45 + tmp48;
	  c_im(output[ostride]) = tmp54 + tmp55;
	  tmp56 = tmp54 - (K500000000 * tmp55);
	  c_im(output[4 * ostride]) = tmp49 + tmp56;
	  c_im(output[7 * ostride]) = tmp56 - tmp49;
     }
     {
	  fftw_real tmp57;
	  fftw_real tmp69;
	  fftw_real tmp72;
	  fftw_real tmp66;
	  fftw_real tmp64;
	  fftw_real tmp65;
	  fftw_real tmp71;
	  fftw_real tmp70;
	  ASSERT_ALIGNED_DOUBLE();
	  {
	       fftw_real tmp67;
	       fftw_real tmp68;
	       fftw_real tmp60;
	       fftw_real tmp63;
	       ASSERT_ALIGNED_DOUBLE();
	       tmp57 = tmp17 - tmp20;
	       tmp67 = (K173648177 * tmp59) - (K984807753 * tmp58);
	       tmp68 = (K342020143 * tmp62) + (K939692620 * tmp61);
	       tmp69 = tmp67 - tmp68;
	       tmp72 = K866025403 * (tmp67 + tmp68);
	       tmp66 = tmp53 - tmp50;
	       tmp60 = (K173648177 * tmp58) + (K984807753 * tmp59);
	       tmp63 = (K342020143 * tmp61) - (K939692620 * tmp62);
	       tmp64 = tmp60 + tmp63;
	       tmp65 = K866025403 * (tmp63 - tmp60);
	  }
	  c_re(output[2 * ostride]) = tmp57 + tmp64;
	  tmp71 = tmp57 - (K500000000 * tmp64);
	  c_re(output[8 * ostride]) = tmp71 - tmp72;
	  c_re(output[5 * ostride]) = tmp71 + tmp72;
	  c_im(output[2 * ostride]) = tmp66 + tmp69;
	  tmp70 = tmp66 - (K500000000 * tmp69);
	  c_im(output[5 * ostride]) = tmp65 + tmp70;
	  c_im(output[8 * ostride]) = tmp70 - tmp65;
     }
}

fftw_codelet_desc fftw_no_twiddle_9_desc =
{
     "fftw_no_twiddle_9",
     (void (*)()) fftw_no_twiddle_9,
     9,
     FFTW_FORWARD,
     FFTW_NOTW,
     145,
     0,
     (const int *) 0,
};


/**
 *
 * @file qwrapper_zlaset.c
 *
 *  PLASMA core_blas quark wrapper
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.6.0
 * @author Hatem Ltaief
 * @date 2010-11-15
 * @precisions normal z -> c d s
 *
 **/
#include <lapacke.h>
#include "common.h"

/***************************************************************************//**
 *
 **/
void QUARK_CORE_zlaset(Quark *quark, Quark_Task_Flags *task_flags,
                       PLASMA_enum uplo, int M, int N,
                       PLASMA_Complex64_t alpha, PLASMA_Complex64_t beta,
                       PLASMA_Complex64_t *A, int LDA)
{
    DAG_CORE_LASET;
    QUARK_Insert_Task(quark, CORE_zlaset_quark, task_flags,
        sizeof(PLASMA_enum),                &uplo,  VALUE,
        sizeof(int),                        &M,     VALUE,
        sizeof(int),                        &N,     VALUE,
        sizeof(PLASMA_Complex64_t),         &alpha, VALUE,
        sizeof(PLASMA_Complex64_t),         &beta,  VALUE,
        sizeof(PLASMA_Complex64_t)*LDA*N,    A,      OUTPUT,
        sizeof(int),                        &LDA,   VALUE,
        0);
}

/***************************************************************************//**
 *
 **/
#if defined(PLASMA_HAVE_WEAK)
#pragma weak CORE_zlaset_quark = PCORE_zlaset_quark
#define CORE_zlaset_quark PCORE_zlaset_quark
#endif
void CORE_zlaset_quark(Quark *quark)
{
    PLASMA_enum uplo;
    int M;
    int N;
    PLASMA_Complex64_t alpha;
    PLASMA_Complex64_t beta;
    PLASMA_Complex64_t *A;
    int LDA;

    quark_unpack_args_7(quark, uplo, M, N, alpha, beta, A, LDA);
    LAPACKE_zlaset_work(
        LAPACK_COL_MAJOR,
        lapack_const(uplo),
        M, N, alpha, beta, A, LDA);
}


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

/************************************************************************************************************************************************************************/
/***********\\Global variable\\******************************************************************************************************************************************/

size_t limit = 100;								// Limit for the convergence of the integrale
size_t size = 100;
double 	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)
			 	q_v[3],									// unit vector q_i, q_j, q_k
			 	sig8;									// Value of sigma 8 (define in main function)

struct my_f_params {double a; double b; double c; double d;};
gsl_interp_accel *acc[25];
gsl_spline *spline[25];						//Use for interpolation

/************************************************************************************************************************************************************************/
/*******************\\ 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
  gl_int(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
  gl_int2(double a, double b, void (*f)(double, double, double*,double*), void *prms,double *int1,double *int2)
{
   double integral1 = 0.0;
   double integral2 = 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];
   double outi[2], outs[2];
   double R=* (double *)prms;

   for (; px >= x; pA--, px--) {
      dum = c * *px;
      (*f)(d - dum,R,&outi[0],&outi[1]);
      (*f)(d + dum,R,&outs[0],&outs[1]);
      integral1 += *pA * (outi[0] + outs[0]);
      integral2 += *pA * (outi[1] + outs[1]);
   }

   *int1 = c * integral1;
   *int2 = c * integral2;
}

void
  gl_int5(double a, double b, void (*f)(double, double, double*,double*,double*,double*,double*), void *prms, double *int1,double *int2, double *int3, double *int4, double *int5)
{
    double integral1 = 0.0;
    double integral2 = 0.0;
    double integral3 = 0.0;
  	double integral4 = 0.0;
   	double integral5 = 0.0;
    double c = 0.5 * (b - a);
    double d = 0.5 * (b + a);
    double dum;
    double outi[5],outs[5];
    const double *px = &x[NUM_OF_POSITIVE_ZEROS - 1];
    const double *pA = &A[NUM_OF_POSITIVE_ZEROS - 1];
	double R=* (double *)prms;

	for (; px >= x; pA--, px--) {
 		dum = c * *px;
    	(*f)(d - dum,R,&outi[0],&outi[1],&outi[2],&outi[3],&outi[4]);
    	(*f)(d + dum,R,&outs[0],&outs[1],&outs[2],&outs[3],&outs[4]);
      	integral1 += *pA * (outi[0] + outs[0]);
      	integral2 += *pA * (outi[1] + outs[1]);
        integral3 += *pA * (outi[2] + outs[2]);
      	integral4 += *pA * (outi[3] + outs[3]);
		integral5 += *pA * (outi[4] + outs[4]);
	}
   	*int1 = c * integral1;
   	*int2 = c * integral2;
   	*int3 = c * integral3;
   	*int4 = c * integral4;
   	*int5 = c * integral5;
}

void
  gl_int6(double a, double b, void (*f)(double, double, double []), void *prms,double result[], int n)
{
	int i;
   double integral[n];
   for (i=0; i<n; i++)
		integral[i] = 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];
	double outi[n], outs[n];
	double R=* (double *)prms;

    for (; px >= x; pA--, px--) {
		dum = c * *px;
		(*f)(d - dum,R,outi);
		(*f)(d + dum,R,outs);
		for (i=0; i<n; i++)
			integral[i] += *pA * (outi[i] + outs[i]);
	   }
		for (i=0; i<n; i++)
			result[i] = c*integral[i];
}

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

/************************************************************************************************************************************************************************/
/****************************\\gsl_cquad integration \\*****************************************************************************************************************/

double int_cquad (double func(double, void*), double alpha)
{
	double result, error;
    gsl_function F;
    F.function = func;
    F.params = α

    gsl_integration_cquad_workspace *w = gsl_integration_cquad_workspace_alloc(size);
    gsl_integration_cquad(&F, kmin, kmax, 0, 1e-9, w, &result, &error, NULL);
    gsl_integration_cquad_workspace_free(w);
    return result;
}

/************************************************************************************************************************************************************************/
/****************************\\Bessel function\\************************************************************************************************************************/

double func_bessel_0(double x)
{
	return sin(x)/x;
}

double func_bessel_1(double x)
{
	return sin(x)/(x*x)-cos(x)/x;
}

double func_bessel_2 (double x)
{
	return (3./(x*x)-1)*sin(x)/x-3*cos(x)/(x*x);
}

double func_bessel_3 (double x)
{
	return (15./pow(x,3)-6./(x))*sin(x)/x-(15./(x*x)-1)*cos(x)/x;
}

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

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

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

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

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

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

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

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

double iXi_L(double q)
{
    if (q>0 && q<=1999)  return gsl_spline_eval (spline[8], q, acc[8]);
    else return 0;
}

double iU_1(double q)
{
    if (q>0 && q<=1999)  return gsl_spline_eval (spline[9], q, acc[9]);
    else return 0;
}

double iU_3(double q)
{
    if (q>0 && q<=1999)  return gsl_spline_eval (spline[10], q, acc[10]);
    else return 0;
}

double iU_11(double q)
{
    if (q>0 && q<=1999)  return gsl_spline_eval (spline[11], q, acc[11]);
    else return 0;
}

double iU_20(double q)
{
    if (q>0 && q<=1999)  return gsl_spline_eval (spline[12], q, acc[12]);
    else return 0;
}

double iX_11(double q)
{
    if (q>0 && q<=1999)  return gsl_spline_eval (spline[13], q, acc[13]);
    else return 0;
}

double iX_13(double q)
{
    if (q>0 && q<=1999)  return gsl_spline_eval (spline[14], q, acc[14]);
    else return 0;
}

double iX_22(double q)
{
    if (q>0 && q<=1999)  return gsl_spline_eval (spline[15], q, acc[15]);
    else return 0;
}

double iX_10_12(double q)
{
    if (q>0 && q<=1999)  return gsl_spline_eval (spline[16], q, acc[16]);
    else return 0;
}

double iY_11(double q)
{
    if (q>0 && q<=1999)  return gsl_spline_eval (spline[17], q, acc[17]);
    else return 0;
}

double iY_13(double q)
{
    if (q>0 && q<=1999)  return gsl_spline_eval (spline[18], q, acc[18]);
    else return 0;
}

double iY_22(double q)
{
    if (q>0 && q<=1999)  return gsl_spline_eval (spline[19], q, acc[19]);
    else return 0;
}

double iY_10_12(double q)
{
    if (q>0 && q<=1999)  return gsl_spline_eval (spline[20], q, acc[20]);
    else return 0;
}

double iW_V1(double q)
{
    if (q>0 && q<=1999)  return gsl_spline_eval (spline[21], q, acc[21]);
    else return 0;
}

double iW_V3(double q)
{
    if (q>0 && q<=1999)  return gsl_spline_eval (spline[22], q, acc[22]);
    else return 0;
}

double iW_T(double q)
{
    if (q>0 && q<=1999)  return gsl_spline_eval (spline[23], q, acc[23]);
    else return 0;
}

double iXi_R(double r)
{
	if (r>0 && r<=129)  return gsl_spline_eval (spline[24], r, acc[24]);
	else return 0;
}

/************************************************************************************************************************************************************************/
/*****************\\delta kronecker\\***********************************************************************************************************************************/

int delta_K (int i, int j)
{
	if (i == j) return 1;
	else return 0;
}

/************************************************************************************************************************************************************************/
/***************************************************\\Sigma 8\\*********************************************************************************************************/

double fS8 (double k, void * params)
{
  double x = k*8;
  return P_L(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, 0, 130, 0, 1e-12, w, &result, &error, NULL);
    gsl_integration_cquad_workspace_free(w);

    return 9./(2.*M_PI*M_PI)*result;
}
/*
double P_L (double k)
{
	return Pm(k)/sig8;
}
*/
/************************************************************************************************************************************************************************/
/****************************************\\ R_n FUNCTIONS \\*********************************************************************************************************/

//	R_1_tilde function
double fRt1 (double x, void *p)
{
	double r = *(double *) p;
	return (r*r*(1.-x*x)*(1.-x*x))/(1.+r*r-2.*x*r);
}

double Rt_1 (double r)
{
	return gl_int(-1, 1, &fRt1, &r);
}

//	R_2_tilde function
double fRt2 (double x, void *p)
{
	double r = *(double *) p;
	return ((1.-x*x)*r*x*(1.-x*r))/(1.+r*r-2.*r*x);
}

double Rt_2 (double r)
{
	return gl_int(-1, 1, &fRt2, &r);
}

// R1 function
double fR_1 (double r, void *p)
{
	double k = *(double *) p;
	return P_L(k*r)*Rt_1(r);
}

double R1(double k)
{
	double result, error;
    gsl_function F;
    F.function = &fR_1;
    F.params = &k;

    gsl_integration_workspace *w = gsl_integration_workspace_alloc(200);
    gsl_integration_qag(&F, 0, kmax/k, 0, 1e-3, 200, 6, w, &result, &error);			// Kmax/k car k*r<=! kmax sinon Pk = 0
    gsl_integration_workspace_free(w);
	return (k*k*k)/(4.*M_PI*M_PI)*P_L(k)*result;
}

// R2 function
double fR_2 (double r, void *p)
{
	double k = *(double *) p;
	return P_L(k*r)*Rt_2(r);
}

double R2(double k)
{
	double result, error;
    gsl_function F;
    F.function = &fR_2;
    F.params = &k;

    gsl_integration_workspace *w = gsl_integration_workspace_alloc(200);
    gsl_integration_qag(&F, 0, kmax/k, 0, 5e-3, 200, 6, w, &result, &error);
    gsl_integration_workspace_free(w);
	return (k*k*k)/(4.*M_PI*M_PI)*P_L(k)*result;//gl_int(0, 800, &fR_2, &k);
}

/*****************Compute R1 and R2 in text files for interpolation*************************************************************************************************/

void calc_R1 ()
{
	printf("getting R function 1/2 ...\n");
	FILE* f;
	double k;
	f = fopen("data/func_R1.dat","w+");
	for (k=kmin; k<1; k+=0.001)
		fprintf(f, "%le %le\n", k, R1(k));
	fclose(f);
	f = fopen("data/func_R1.dat","a+");
	for (k=1; k<(kmax+9.9e-3); k+=0.01)
		fprintf(f, "%le %le\n", k, R1(k));
	fclose(f);
	return;
}
void calc_R2 ()
{
	printf("getting R function 2/2 ...\n");
	FILE* f;
	double k;
	f = fopen("data/func_R2.dat","w+");
	for (k=kmin; k<1; k+=0.001)
		fprintf(f, "%le %le\n", k, R2(k));
	fclose(f);
	f = fopen("data/func_R2.dat","a+");
	for (k=1; k<(kmax+9.9e-3); k+=0.01)
		fprintf(f, "%le %le\n", k, R2(k));
	fclose(f);
	return;
}

/***********************************************************************************************************************************************************************/
/******************************\\FUNCTION Q\\***********************************************************************************************************************/

// Compute Qn(r,x)
double Qt_1 (double r, double x)
{
	return (r*r*(1.-x*x)*(1.-x*x))/((1.+r*r-2.*x*r)*(1.+r*r-2.*x*r));
}

double Qt_2 (double r, double x)
{
	return ((1.-x*x)*r*x*(1.-x*r))/((1.+r*r-2.*r*x)*(1.+r*r-2.*r*x));
}

double Qt_5 (double r, double x)
{
	return (r*x*(1.-x*x))/(1.+r*r-2.*x*r);
}

double Qt_8 (double r, double x)
{
	return (r*r*(1.-x*x))	/(1.+r*r-2.*x*r);
}

/***********\\Integration to compute the inner integral of Qn(k)\\**************************************************************************************************/

double lg_Q_n( double x, void *p)
{
	struct my_f_params * params = (struct my_f_params *)p;
	double k = (params->a);
    double r = (params->b);
    double n = (params->c);
    double y_p = 1.+r*r-2.*x*r;
    if  (y_p < 0)	return 0;
    else {
		if (n == 1)	return P_L(k*sqrt(y_p))*Qt_1(r,x);
		if (n == 2)	return P_L(k*sqrt(y_p))*Qt_2(r,x);
		if (n == 5)	return P_L(k*sqrt(y_p))*Qt_5(r,x);
		if (n == 8)	return P_L(k*sqrt(y_p))*Qt_8(r,x);
		else return -1;
	}

}
/***********\\Compute Qn(k)\\***************************************************************************************************************************************/

double fQ_n (double r, void *p)
{
	struct my_f_params * params = (struct my_f_params *)p;
	double k = (params->a);
    double n = (params->b);
	struct my_f_params par = {k, r, n};
	return P_L(k*r)*gl_int(-1, 1, &lg_Q_n, &par);
}

double Q_n (int n, double k)
{
	struct my_f_params p = {k, n};
	double result, error;
    gsl_function F;
    F.function = &fQ_n;
    F.params = &p;

    gsl_integration_workspace *w = gsl_integration_workspace_alloc(200);
    gsl_integration_qag(&F, 0, kmax/k, 0, 1e-3, 200, 6, w, &result, &error);
    gsl_integration_workspace_free(w);

	return (k*k*k)/(4.*M_PI*M_PI)*result;
}

/*************Compute Qn in text file for interpolation*************************************************************************************************************/
void calc_Qn ()
{
	FILE* f;
	int n;
	double k;
	for (n=1; n<9; n++){
		printf("getting Q function %d/8...\n",n);
		switch (n){
		case 1:
			f = fopen("data/func_Q1.dat","w+");
			for (k=kmin; k<1; k+=0.001)
				fprintf(f, "%le %le\n", k, Q_n(1,k));
			fclose(f);
			f = fopen("data/func_Q1.dat","a+");
			for (k=1; k<kmax+100; k+=0.1)
				fprintf(f, "%le %le\n", k, Q_n(1,k));
			fclose(f);
			break;
		case 2:
			f = fopen("data/func_Q2.dat","w+");
			for (k=kmin; k<1; k+=0.001)
				fprintf(f, "%le %le\n", k, Q_n(2,k));
			fclose(f);
			f = fopen("data/func_Q2.dat","a+");
			for (k=1; k<kmax+100; k+=0.1)
				fprintf(f, "%le %le\n", k, Q_n(2,k));
			fclose(f);
			break;
		case 5:
			f = fopen("data/func_Q5.dat","w+");
			for (k=kmin; k<1; k+=0.001)
				fprintf(f, "%le %le\n", k, Q_n(5,k));
			fclose(f);
			f = fopen("data/func_Q5.dat","a+");
			for (k=1; k<kmax+100; k+=0.1)
				fprintf(f, "%le %le\n", k, Q_n(5,k));
			fclose(f);
			break;
		case 8:
			f = fopen("data/func_Q8.dat","w+");
			for (k=kmin; k<1; k+=0.001)
				fprintf(f, "%le %le\n", k, Q_n(8,k));
			fclose(f);
			f = fopen("data/func_Q8.dat","a+");
			for (k=1; k<kmax+100; k+=0.1)
				fprintf(f, "%le %le\n", k, Q_n(8,k));
			fclose(f);
			break;
		default:
			break;
		}
	}
	return;
}

/*******************************************************************************************************************************************************************/
/************************\\Compute Xi linear\\********************************************************************************************************************/

double fXi_L (double k, void * p)
{
	double q = *(double *) p;
    return P_L(k)*k*k*func_bessel_0(k*q);
}

double Xi_L (double q)
{
    return 1./(2.*M_PI*M_PI)*int_cquad(fXi_L, q);
}

/*******************************************************************************************************************************************************************/
/**************\\U function\\**************************************************************************************************************************************/
// U^1(q)
double fU_1 (double k, void *p)
{
	double q = *(double *) p;
	return -P_L(k)*k*func_bessel_1(k*q);
}

double U_1 (double q)
{
	return 1./(2.*M_PI*M_PI)*int_cquad(fU_1, q);
}

// U^3(q)
double fU_3 (double k, void *p)
{
	double q = *(double *) p;
	return -5./21.*R_1(k)*k*func_bessel_1(k*q);
}

double U_3 (double q)
{
	return 1./(2.*M_PI*M_PI)*int_cquad(fU_3, q);
}

// U_11(q)
double fU_11 (double k, void *p)
{
	double q = *(double *) p;
	return -6./7.*k*(R_1(k)+R_2(k))*func_bessel_1(k*q);
}

double U_11 (double q)
{
	return 1./(2.*M_PI*M_PI)*int_cquad(fU_11, q);
}

// U_20(q)
double fU_20 (double k, void *p)
{
	double q = *(double *) p;
	return -3./7.*k*Q_8(k)*func_bessel_1(k*q);
}

double U_20 (double q)
{
	return 1./(2.*M_PI*M_PI)*int_cquad(fU_20, q);
}

//Ui(q)
double get_U (double q)
{
	return iU_1(q)+iU_3(q);
}

/*******************************************************************************************************************************************************************/
/*******************\\X FUNCTIONS\\*****************************************************************************************************************************/
// X^11(q)
double fX_11 (double k, void *p)
{
	double q = *(double *) p;
	double x = k*q;
	return P_L(k)*(2./3.-2.*func_bessel_1(x)/x);
}

double X_11 (double q)
{
	return 1./(2.*M_PI*M_PI)*int_cquad(fX_11, q);
}

// X^22(q)
double fX_22 (double k, void *p)
{
		double q = *(double *) p;
		double x = k*q;
	return 9./98.*Q_1(k)*(2./3.-2.*func_bessel_1(x)/(x));
}

double X_22 (double q)
{
	return 1./(2.*M_PI*M_PI)*int_cquad(fX_22, q);
}

// X^13(q)
double fX_13 (double k, void *p)
{
	double q = *(double *) p;
	double x = k*q;
	return 5./21.*R_1(k)*(2./3.-2.*func_bessel_1(x)/x);
}

double X_13 (double q)
{
	return 1./(2.*M_PI*M_PI)*int_cquad(fX_13, q);
}

// X_10^12(q)
double fX_10_12 (double k, void *p)
{
	double q = *(double *) p;
	double x = k*q;
	return 1./14.*(2.*(R_1(k)-R_2(k))+3.*R_1(k)*sin(k*q)/(k*q)-3.*(3.*R_1(k)+4.*R_2(k)+2.*Q_5(k))*func_bessel_1(x)/x);
}

double X_10_12 (double q)
{
	return 1./(2.*M_PI*M_PI)*int_cquad(fX_10_12, q);
}

/*******************************************************************************************************************************************************************/
/*******************Y FUNCTIONS*****************************************************************************************************************************/
// Y^11(q)
double fY_11 (double k, void *p)
{
	double q = *(double *) p;
	double x = k*q;
	return P_L(k)*(-2.*sin(k*q)/(k*q)+6.*func_bessel_1(x)/x);
}

double Y_11 (double q)
{
	return 1./(2.*M_PI*M_PI)*int_cquad(fY_11, q);
}

// Y^22(q)
double fY_22 (double k, void *p)
{
	double q = *(double *) p;
	double x = k*q;
	return 9./98.*Q_1(k)*(-2.*sin(k*q)/(k*q)+6.*func_bessel_1(x)/x);
}

double Y_22 (double q)
{
	return 1./(2.*M_PI*M_PI)*int_cquad(fY_22, q);
}

// Y^13(q)
double fY_13 (double k, void *p)
{
	double q = *(double *) p;
	double x = k*q;
	return 5./21.*R_1(k)*(-2.*sin(k*q)/(k*q)+6.*func_bessel_1(x)/x);
}

double Y_13 (double q)
{
	return 1./(2.*M_PI*M_PI)*int_cquad(fY_13, q);
}

// Y^10_12(q)
double fY_10_12 (double k, void *p)
{
	double q = *(double *) p;
	double x = q*k;
	return -3./14.*(3.*R_1(k)+4.*R_2(k)+2.*Q_5(k))*(sin(k*q)/(k*q)-3.*func_bessel_1(x)/x);
}

double Y_10_12 (double q)
{
	return 1./(2.*M_PI*M_PI)*int_cquad(fY_10_12, q);
}

/************************************************************************************************************************************************************************/
/**************************\\V+S et T function\\************************************************************************************************************************/
// V^112_1(q) + S^112	(q)
double fV1_112 (double k, void*p)
{
	double q = *(double *) p;
	double x = k*q;
	return -3./7.*R_1(k)*func_bessel_1(x)/k + 3./(7.*k)*(2.*R_1(k)+4.*R_2(k)+Q_1(k)+2.*Q_2(k))*func_bessel_2(x)/x;
}								// V^112_1								+								S^112

double V1_112 (double q)
{
	return 1./(2.*M_PI*M_PI)*int_cquad(fV1_112, q);
}

// V^112_3(q)+ S^112	(q)
double fV3_112 (double k, void *p)
{
	double q = *(double *) p;
	double x = k*q;
	return -3./7.*Q_1(k)*func_bessel_1(x)/k + 3./(7.*k)*(2.*R_1(k)+4.*R_2(k)+Q_1(k)+2.*Q_2(k))*func_bessel_2(x)/x;
}								// V^112_3								+								S^112

double V3_112 (double q)
{
	return 1./(2.*M_PI*M_PI)*int_cquad(fV3_112, q);
}

// T^112(q)
double fT_112 (double k, void *p)
{
	double q = *(double *) p;
	double x = k*q;
	return -3./7.*(2.*R_1(k)+4.*R_2(k)+Q_1(k)+2.*Q_2(k))*func_bessel_3(x)/k;
}

double T_112 (double q)
{
	return 1./(2.*M_PI*M_PI)*int_cquad(fT_112, q);
}

/************************************************************************************************************************************************************************/
/***************************\\Compute U, X, Y, V and T (q) in text file for interpolation\\******************************************************************************/

void q_function ()
 {
 	FILE *fx, *fy, *fw, *fu, *fxi;
 	double q;

 	printf("getting Xi_L function...\n");
 	fxi = fopen("data/Xi_L_func.dat","w+");
	for (q=0; q<10; q+=.01)
		fprintf(fxi,"%lf %lf\n",q, Xi_L(q));
	fclose(fxi);
	fxi = fopen("data/Xi_L_func.dat","a+");
	for (q=11; q<2000; q+=1)
		fprintf(fxi,"%lf %lf\n",q, Xi_L(q));
	fclose(fxi);

 	printf("getting U function...\n");
 	fu = fopen("data/U_func.dat","w+");
	for (q=0; q<10; q+=.01)
		fprintf(fu,"%lf %lf %lf %lf %lf\n",q, U_1(q), U_3(q), U_11(q), U_20(q));
	fclose(fu);
	fu = fopen("data/U_func.dat","a+");
	for (q=11; q<2000; q+=1)
		fprintf(fu,"%lf %lf %lf %lf %lf\n",q, U_1(q), U_3(q), U_11(q), U_20(q));
	fclose(fu);

	printf("getting X function...\n");
	fx = fopen("data/X_func.dat","w+");
	for (q=0; q<10; q+=.01)
		fprintf(fx,"%lf %lf %lf %lf %lf\n",q, X_11(q), X_13(q), X_22(q), X_10_12(q));
	fclose(fx);
	fx = fopen("data/X_func.dat","a+");
	for (q=11; q<2000; q+=1)
		fprintf(fx,"%lf %lf %lf %lf %lf\n",q, X_11(q), X_13(q), X_22(q), X_10_12(q));
	fclose(fx);

	printf("getting Y function...\n");
	fy = fopen("data/Y_func.dat","w+");
	for (q=0; q<10; q+=.01)
		fprintf(fy,"%lf %lf %lf %lf %lf\n",q, Y_11(q), Y_13(q), Y_22(q), Y_10_12(q));
	fclose(fy);
	fy = fopen("data/Y_func.dat","a+");
	for (q=11; q<2000; q+=1)
		fprintf(fy,"%lf %lf %lf %lf %lf\n",q, Y_11(q), Y_13(q), Y_22(q), Y_10_12(q));
	fclose(fy);

	printf("getting W function...\n");
	fw = fopen("data/W_func.dat","w+");
	for (q=0; q<10; q+=.01)
		fprintf(fw,"%lf %lf %lf %lf\n",q, V1_112(q), V3_112(q), T_112(q));
	fclose(fw);
	fw = fopen("data/W_func.dat","a+");
	for (q=11; q<2000; q+=1)
		fprintf(fw,"%lf %lf %lf %lf\n",q, V1_112(q), V3_112(q), T_112(q));
	fclose(fw);
	return;
}

/************************************************************************************************************************************************************************/
/*********************\\Interpolation functions\\***********************************************************************************************************************/

void interpole(int n, char ficher[100],int val)
{
	double T_x[val];
	double T_y[val];
	FILE* f;
	int i = 0;
	f = fopen(ficher, "r");
	for(i=0; i < val; 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, val);
    gsl_spline_init (spline[n], T_x, T_y, val);
    fclose(f);
}

void interp_qfunc()
{
	FILE *fu, *fx, *fy, *fw;
	int i;
	const int val = 2990;
	double X_q[val], X_x11[val], X_x13[val], X_x22[val], X_x1012[val], Y_q[val], Y_y11[val], Y_y13[val], Y_y22[val], Y_y1012[val], U_q[val], U_u1[val], U_u3[val], U_u11[val], U_u20[val], W_q[val], W_v1[val], W_v3[val], W_t[val];

	fu = fopen("data/U_func.dat", "r");
	for(i=0; i < val; i++) fscanf(fu, "%lf %lf %lf %lf %lf\n", &U_q[i], &U_u1[i], &U_u3[i], &U_u11[i], &U_u20[i]);
	acc[9] = gsl_interp_accel_alloc ();
    spline[9] = gsl_spline_alloc(gsl_interp_cspline, val);
    gsl_spline_init (spline[9], U_q, U_u1, val);
    acc[10] = gsl_interp_accel_alloc ();
    spline[10] = gsl_spline_alloc(gsl_interp_cspline, val);
    gsl_spline_init (spline[10], U_q, U_u3, val);
   	acc[11] = gsl_interp_accel_alloc ();
    spline[11] = gsl_spline_alloc(gsl_interp_cspline, val);
    gsl_spline_init (spline[11], U_q, U_u11, val);
    acc[12] = gsl_interp_accel_alloc ();
    spline[12] = gsl_spline_alloc(gsl_interp_cspline, val);
    gsl_spline_init (spline[12], U_q, U_u20, val);
    fclose(fu);

   	fx = fopen("data/X_func.dat", "r");
	for(i=0; i < val; i++) fscanf(fx, "%lf %lf %lf %lf %lf\n", &X_q[i], &X_x11[i], &X_x13[i], &X_x22[i], &X_x1012[i]);
	acc[13] = gsl_interp_accel_alloc ();
    spline[13] = gsl_spline_alloc(gsl_interp_cspline, val);
    gsl_spline_init (spline[13], X_q, X_x11, val);
    acc[14] = gsl_interp_accel_alloc ();
    spline[14] = gsl_spline_alloc(gsl_interp_cspline, val);
    gsl_spline_init (spline[14], X_q, X_x13, val);
   	acc[15] = gsl_interp_accel_alloc ();
    spline[15] = gsl_spline_alloc(gsl_interp_cspline, val);
    gsl_spline_init (spline[15], X_q, X_x22, val);
    acc[16] = gsl_interp_accel_alloc ();
    spline[16] = gsl_spline_alloc(gsl_interp_cspline, val);
    gsl_spline_init (spline[16], X_q, X_x1012, val);
    fclose(fx);

    fy = fopen("data/Y_func.dat", "r");
	for(i=0; i < val; i++) fscanf(fy, "%lf %lf %lf %lf %lf\n", &Y_q[i], &Y_y11[i], &Y_y13[i], &Y_y22[i], &Y_y1012[i]);
	acc[17] = gsl_interp_accel_alloc ();
    spline[17] = gsl_spline_alloc(gsl_interp_cspline, val);
    gsl_spline_init (spline[17], Y_q, Y_y11, val);
    acc[18] = gsl_interp_accel_alloc ();
    spline[18] = gsl_spline_alloc(gsl_interp_cspline, val);
    gsl_spline_init (spline[18], Y_q, Y_y13, val);
   	acc[19] = gsl_interp_accel_alloc ();
    spline[19] = gsl_spline_alloc(gsl_interp_cspline, val);
    gsl_spline_init (spline[19], Y_q, Y_y22, val);
    acc[20] = gsl_interp_accel_alloc ();
    spline[20] = gsl_spline_alloc(gsl_interp_cspline, val);
    gsl_spline_init (spline[20], Y_q, Y_y1012, val);
    fclose(fy);

    fw = fopen("data/W_func.dat", "r");
	for(i=0; i < val; i++) fscanf(fw, "%lf %lf %lf %lf\n", &W_q[i], &W_v1[i], &W_v3[i], &W_t[i]);
	acc[21] = gsl_interp_accel_alloc ();
    spline[21] = gsl_spline_alloc(gsl_interp_cspline, val);
    gsl_spline_init (spline[21], W_q, W_v1, val);
    acc[22] = gsl_interp_accel_alloc ();
    spline[22] = gsl_spline_alloc(gsl_interp_cspline, val);
    gsl_spline_init (spline[22], W_q, W_v3, val);
   	acc[23] = gsl_interp_accel_alloc ();
    spline[23] = gsl_spline_alloc(gsl_interp_cspline, val);
    gsl_spline_init (spline[23], W_q, W_t, val);
    fclose(fw);
}

/************************************************************************************************************************************************************************/
/*****************\\Matrix A \\ *****************************************************************************************************************************************/

				/*A_ij(q) = X(q)*delta_k(i,j) + Y(q)*q_v[i]*q_v[j]*/

double Aij_13 (int i, int j, double q)
{
	return iX_13(q)*delta_K(i,j)+iY_13(q)*q_v[i]*q_v[j];		// q_v[i] unit vector over i component
}

double Aij_22 (int i, int j, double q)
{
	return iX_22(q)*delta_K(i,j)+iY_22(q)*q_v[i]*q_v[j];
}

double Aij_11 (int i, int j, double q)
{
	return iX_11(q)*delta_K(i,j)+iY_11(q)*q_v[i]*q_v[j];
}

double Aij (int i, int j, double q)		// Compute the ij component of matrix A
{
	return Aij_11(i, j, q)+Aij_22(i, j, q)+2.*Aij_13(i, j, q);
}

double A10_ij (int i, int j, double q)
{
		return 2.*(iX_10_12(q)*delta_K(i,j)+iY_10_12(q)*q_v[i]*q_v[j]);
}

// Initialize A matrix
gsl_matrix *get_M (double q)
{
	int i, j;
	gsl_matrix *M = gsl_matrix_calloc (3,3);
	for (i=0; i<3; i++){
		for (j=0; j<3;j++)
			gsl_matrix_set(M, i, j, Aij(i,j,q));
	}
	return M;
}

/************************************************************************************************************************************************************************/
/******************\\ g_i, G_ij and Gamma_ijk functions\\**************************************************************************************************************/

double g_i (int i, int j, gsl_matrix *M_inv, double y[])
{
	return gsl_matrix_get(M_inv, i, j)*y[j];
}

double G_ij (int i, int j, gsl_matrix *M_inv, double g[])
{
	return gsl_matrix_get(M_inv, i, j)-g[i]*g[j];
}

double Gamma_ijk (int i, int j, int k, gsl_matrix *M_inv, double g[])
{
	return 	gsl_matrix_get(M_inv, i, j)*g[k]+ gsl_matrix_get(M_inv, k, i)*g[j] + gsl_matrix_get(M_inv, j, k)*g[i] - g[i]*g[j]*g[k];
}

/************************************************************************************************************************************************************************/
/******************\\Compute W^112_ijk(q)\\**************************************************************************************************************************/

double W_112 (int i, int j, int k, double q)
{
	return 	iW_V1(q)*delta_K(j,k)*q_v[i] +
					iW_V1(q)*delta_K(k,i)*q_v[j] +
					iW_V3(q)*delta_K(i,j)*q_v[k] +
					iW_T(q)*q_v[i]*q_v[j]*q_v[k];
}

/************************************************************************************************************************************************************************/
/*************\\Dot functions\\*****************************************************************************************************************************************/
//Udot_n
double fU_dot (double q)
{
	return iU_1(q) + 3.*iU_3(q);
}

//Adot_in
double fA_dot (int i, int n, double q)
{
	return Aij_11(i,n,q)+4.*Aij_13(i,n,q)+2.*Aij_22(i,n,q);
}

//Adot^10_in
double fA10_dot (int i, int n, double q)
{
	return 3.*(iX_10_12(q)*delta_K(i,n)+iY_10_12(q)*q_v[i]*q_v[n]);
}

//Wdot_ijn
double fW_dot (int i, int j, int n, double q)
{
	return 2.*W_112(i,j,n,q)+W_112(n,i,j,q)+W_112(j,n,i,q);
}

/************************************************************************************************************************************************************************/
/*********\\Double dot functions\\*************************************************************************************************************************************/

//A2dot_nm
double fA_2dot (int n, int m, double q)
{
	return Aij_11(n,m,q)+6.*Aij_13(n,m,q)+4.*Aij_22(n,m,q);
}

//A2dot^10_nm
double fA10_2dot (int n, int m, double q)
{
	return 4.*(iX_10_12(q)*delta_K(n,m)+iY_10_12(q)*q_v[n]*q_v[m]);	//A_nm = A_mn
}

// W2dot_inm
double fW_2dot (int i, int n, int m, double q)
{
	return 2.*W_112(i,n,m,q)+2.*W_112(m,i,n,q)+W_112(n,m,i,q);		//W^121_inm = W^112_min and W^211_inm = W^112_nmi
}

/************************************************************************************************************************************************************************/
/**************\\M_0 integral\\*****************************************************************************************************************************************/
 void M_0(double y, double R, double M0_fin[])
{
	int l, m,o;
    double mu, q_n, det_A, y_t[3], q[3], U[3], G[3][3], U11[3], U20[3], g[3], Gamma[3][3][3], W[3][3][3], W_fin[3][3][3], A10[3][3], Xi, s, fac, F_b[6], M0_tab[6];
    double dmu = 0.001;
    int signum = 0;

	for (l = 0; l<6; l++)				// Inizialise at each step
		M0_tab[l] = 0;

	for (mu=-1; mu <= 1; mu +=dmu){					// Decomp integrale dtheta en somme de riemann int=somme f(x_i)*delta_x
	    gsl_matrix *A = gsl_matrix_calloc (3, 3);
		gsl_matrix *A_inv = gsl_matrix_calloc (3, 3);		// Declaration of matrix A (3x3)

//		change variable y = q-r
		y_t[0] = y*sqrt(1-mu*mu);
		y_t[1] = 0;
		y_t[2] = y*mu;

// 	Compute vector q = y+r with r= (0,0,1) along the LOS
		q[0] = y_t[0];
		q[1] = y_t[1];
		q[2] = y_t[2]+R;
	    q_n = sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]); 	// norm of q

// 	unit vector
	    for (l=0; l<3; l++)
	    	q_v[l] = q[l]/q_n;

/********Compute function ****************************************/
// Xi linear
	    Xi = iXi_L(q_n);

//  	U functions
	    for (l = 0; l<3 ; l++){
	   		U[l] = get_U(q_n)*q_v[l];
	   		U11[l] = iU_11(q_n)*q_v[l];
	   		U20[l] = iU_20(q_n)*q_v[l];
	    }

// 	Matix A
		gsl_permutation *p = gsl_permutation_alloc (3);
	  	A = get_M(q_n);															//Initialize matix A
	    gsl_linalg_LU_decomp(A, p, &signum);
	    det_A =	gsl_linalg_LU_det(A, signum);					// Determinant of A
		gsl_linalg_LU_invert(A, p, A_inv); 	    					// Inverse of matrix A

// 	g_i components
		for (l=0; l<3; l++){
			g[l] = 0;
	   		for (m=0; m<3; m++)
	    		g[l] += g_i(l, m, A_inv, y_t);
	    }

// 	G_ij components
	    for (l=0; l<3; l++){
	    	for (m=0; m<3; m++)
	    		G[l][m] = G_ij(l, m, A_inv, g);
	    }

// Gamma_ijk
		for (l=0; l<3; l++){
			for	(m=0; m<3; m++){
				for (o=0; o<3; o++)
					Gamma[l][m][o] = Gamma_ijk(l, m, o, A_inv, g);
			}
		}

// 	W_ijk
		for (l=0; l<3; l++){
			for	(m=0; m<3; m++){
				for (o=0; o<3; o++)
					W[l][m][o] = W_112(l, m, o, q_n);
			}
		}
		for (l=0; l<3; l++){
			for	(m=0; m<3; m++){
				for (o=0; o<3; o++)
					W_fin[l][m][o] = W[l][m][o]+ W[o][l][m]+ W[m][o][ l ];
			}
		}

// 	A^10_ij
		for (l=0; l<3; l++){
			for	(m=0; m<3; m++)
				A10[l][m] = A10_ij(l, m, q_n);
		}

//		Sum over all the bias component
		for(l=0; l<6; l++)
			F_b[l] = 0;

//		<F'>
		s=0;
		for (l=0; l<3; l++)
			s += U[l]*g[l];
		F_b[1] -= 2.*s;
		F_b[4] -= 2.*Xi*s; 	//  <F'F''>
		s=0;
		for (l=0; l<3; l++){
			for	(m=0; m<3; m++)
				s += A10[l][m]*G[l][m];
		}
		F_b[1] -= s;

//		<F'>²
		F_b[3] += Xi;
		s=0;
		for (l=0; l<3; l++)
			s += U11[l]*g[l];
		F_b[3] -= s;
		s=0;
		for (l=0; l<3; l++){
			for	(m=0; m<3; m++)
				s += U[l]*U[m]*G[l][m];
		}
		F_b[3] -= s;

//		<F''>
		s=0;
		for (l=0; l<3; l++)
			s += U20[l]*g[l];
		F_b[2] -= s;
		s=0;
		for (l=0; l<3; l++){
			for	(m=0; m<3; m++)
				s += U[l]*U[m]*G[l][m];
		}
		F_b[2] -= s;

//		<F''>²
		F_b[5] = 0.5*Xi*Xi;

// 	F0
		s=0;
		for (l=0; l<3; l++){
			for	(m=0; m<3; m++){
				for (o=0; o<3; o++)
					s += Gamma[l][m][o]*W_fin[l][m][o];
			}
		}
		F_b[0] += s/6.;
		F_b[0] += 1.;

// 	gaussian factor of M
	    s = 0;
		for (l=0; l<3; l++)
			s += g[l]*y_t[l];
		fac = exp(-0.5*s)/(sqrt(2.*M_PI)*2.*M_PI*sqrt(det_A));

//		Sum over all component
		for (l=0; l<6; l++)
			M0_tab[l] += F_b[l]*fac;

		gsl_permutation_free (p);
		gsl_matrix_free(A);
		gsl_matrix_free(A_inv);
	}
	s = y*y*dmu;
	for (l=0; l<6; l++)
			M0_fin[l] = M0_tab[l]*s;
}

/********Generate correlation function in real space Xi_R(r) ************************************************************************************************************/

void get_Xi_R ()
{
	printf("Calculating Xi_r...\n");
	int i, n =6;
	double r, M0[n];
	FILE *fi;
	fi = fopen("data/Xi_r_CLPT.dat","w+");
	for (r=1; r<=160; r+=1){
		for (i=0; i<n; i++)
			M0[i] = 0;
		gl_int6(0, 50, &M_0, &r, M0, n);
		for (i=0; i<n; i++)
			M0[i] *= 2.*M_PI;
		M0[0] -= 1;
		fprintf(fi,"%lf %lf %lf %lf %lf %lf %lf\n", r, M0[0], M0[1], M0[2], M0[3], M0[4], M0[5]);
	}
	fclose(fi);
	return ;
}
/************************************************************************************************************************************************************************/
/**************\\M_1 integral\\*****************************************************************************************************************************************/
 void M_1(double y, double R, double M1_fin[])
{
	const double rn[ 3 ] = { 0, 0, 1 }; // unit vector to project over the LOS
	int l, m,o;
    double mu, q_n, det_A, y_t[3], q[3], U[3], U_dot[3], G[3][3], U11[3], U20[3], g[3], W[3][3][3], W_dot[3][3][3], A_dot[3][3], A10_dot[3][3], Xi, s, fac, F_b[5], M1_tab[5];
	double dmu = 0.005;
    int signum = 0;					// Use for matrix inversion

	for (l = 0; l<5 ; l++) 			// Initialize at each step
		M1_tab[l] = 0;

	for (mu=-1; mu <= 1; mu +=dmu){					// Decomp integrale dtheta en somme de riemann int=somme f(x_i)*delta_x
	    gsl_matrix *A = gsl_matrix_calloc (3, 3);
		gsl_matrix *A_inv = gsl_matrix_calloc (3, 3);		// Declaration of matrix A (3x3)

//		change variable y = q-r
		y_t[0] = y*sqrt(1-mu*mu);
		y_t[1] = 0;
		y_t[2] = y*mu;

// 	Compute vector q = y+r with r= (0,0,1) along the LOS
		q[0] = y_t[0];
		q[1] = y_t[1];
		q[2] = y_t[2]+R;
	    q_n = sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]); 	// norm of q

// 	unit vector
	    for (l=0; l<3; l++)
	    	q_v[l] = q[l]/q_n;

/********Compute function ****************************************/
// Xi linear
	    Xi = iXi_L(q_n);

//  	U functions
	    for (l = 0; l<3 ; l++){
	   		U_dot[l] = fU_dot(q_n)*q_v[l];
	   		U[l] = get_U(q_n)*q_v[l];
	   		U11[l] = 2.*iU_11(q_n)*q_v[l];			// Pq 2 ?????
	   		U20[l] = 2.*iU_20(q_n)*q_v[l];
	    }

// 	Matix A
		gsl_permutation *p = gsl_permutation_alloc (3);
	  	A = get_M(q_n);															//Initialize matix A
	    gsl_linalg_LU_decomp(A, p, &signum);
	    det_A =	gsl_linalg_LU_det(A, signum);					// Determinant of A
		gsl_linalg_LU_invert(A, p, A_inv); 	    					// Inverse of matrix A

// 	g_i components
		for (l=0; l<3; l++){
			g[l] = 0;
	   		for (m=0; m<3; m++)
	    		g[l] += g_i(l, m, A_inv, y_t);
	    }

// 	G_ij components
	    for (l=0; l<3; l++){
	    	for (m=0; m<3; m++)
	    		G[l][m] = G_ij(l, m, A_inv, g);
	    }

//		 Adot_in
		for (l=0; l<3; l++){
			for	(m=0; m<3; m++)
				A_dot[l][m] = fA_dot(l, m, q_n);
		}

// 	Adot^10_in
		for (l=0; l<3; l++){
			for	(m=0; m<3; m++)
				A10_dot[l][m] = fA10_dot(l, m, q_n);
		}
//		 W_ijn
		for (l=0; l<3; l++){
			for	(m=0; m<3; m++){
				for (o=0; o<3; o++)
					W[l][m][o] = W_112(l, m, o, q_n);
			}
		}
		for (l=0; l<3; l++){
			for	(m=0; m<3; m++){
				for (o=0; o<3; o++)
					W_dot[l][m][o] = 2.*W[l][m][o]+ W[o][l][m]+ W[m][o][ l ];
			}
		}

//		Sum over all the bias component
		for(l=0; l<5; l++)
			F_b[l] = 0;
// 	<F'> 	F_b[1]
		s = 0;
		for (l=0; l<3; l++)
			s += U_dot[l]*rn[l];
	    F_b[1] += 2.*s;
		s = 0;
		for (l=0; l<3; l++){
			for (o=0; o<3; o++)
				s += g[l]*A10_dot[l][o]*rn[o];
		}
	    F_b[1] -= 2.*s;
		s = 0;
		for (l=0; l<3; l++){
			for (m=0; m<3; m++){
				for (o=0; o<3; o++)
					s += G[l][m]*U[l]*A_dot[m][o]*rn[o];
			}
		}
	    F_b[1] -= 2.*s;

//		<F''>
		s = 0;
		for (l=0; l<3; l++)
			s += U20[l]*rn[l];
		F_b[2] += s;
		s = 0;
		for (l=0; l<3; l++){
			for	(m=0; m<3; m++)
				s += U[l]*g[l]*U_dot[m]*rn[m];
		}
		F_b[2] -= 2.*s;

//		<F'>²
		s=0;
		for (l=0; l<3; l++)
			s += U11[l]*rn[l];
	    F_b[3] += s;
		s=0;
		for (l=0; l<3; l++){
			for	(m=0; m<3; m++)
				s += U[l]*g[l]*U_dot[m]*rn[m];
		}
	    F_b[3] -= 2.*s;
		s=0;
		for (l=0; l<3; l++){
			for	(m=0; m<3; m++)
				s += g[l]*A_dot[l][m]*rn[m];
		}
	    F_b[3] -= Xi*s;

//		<F'F''>
		s=0;
		for (l=0; l<3; l++)
			s += U_dot[l]*rn[l];
		F_b[4] += 2.*Xi*s;

//		F0
		s=0;
		for (l=0; l<3; l++){
			for	(m=0; m<3; m++){
				for (o=0; o<3; o++)
					s += G[l][m]*W_dot[l][m][o]*rn[o];
			}
		}
		F_b[0] -= s/2.;
		s=0;
		for (l=0; l<3; l++){
			for	(m=0; m<3; m++)
				s += g[l]*A_dot[l][m]*rn[m];
		}
		F_b[0] -= s;
// 	gaussian factor of M
		s = 0;
		for (l=0; l<3; l++)
			s += g[l]*y_t[l];
		fac = exp(-0.5*s)/(sqrt(2.*M_PI)*2.*M_PI*sqrt(det_A));

//		Sum over all component
		for (l=0; l<5; l++)
			M1_tab[l] += F_b[l]*fac;

		gsl_matrix_free(A);
		gsl_matrix_free(A_inv);
		gsl_permutation_free (p);
	}
	s = y*y*dmu;
	for (l=0; l<5; l++)
			M1_fin[l] = M1_tab[l]*s;
}
/********Generate V_12(r) **********************************************************************************************************************************************/
/*To compare with Wang +13 we do not divise by 1+Xi_R*/
void get_V12 ()
{
	printf("Calculating V12...\n");
	int i, n=5;
	double r, result	[n];
	FILE *fi;
	fi = fopen("data/V_12_CLPT.dat","w+");
	for (r=1; r<=160; r+=1){
		gl_int6(0, 50, &M_1, &r, result, n);
		for (i=0; i<n; i++)
			result[i] *= 2.*M_PI;
		fprintf(fi,"%lf %lf %lf %lf %lf %lf\n", r, result[0], result[1], result[2], result[3], result[4]);
	}
	fclose(fi);
	return ;
}

/************************************************************************************************************************************************************************/
/************************************************************************************************************************************************************************/
//Calcul de M2
void M_2 (double y, double R, double M2_fin[])
{
	const double rn[ 3 ] = { 0, 0, 1 }; // unit vector to project on n and m
	int l, m,o, n;
    double mu, q_n, det_A, y_t[3], q[3], U[3], U_dot[3], G[3][3], g[3], W[3][3][3], W_2dot[3][3][3], A_dot[3][3], A_2dot[3][3], A10_2dot[3][3], Xi, s[3][3], f, fac, M2_tab[8];;
    double	F_par[4], 			//Bias factor for sigma_parallel
    				F_per[4];				//Bias factor for sigma_perpendicular
    double dmu = 0.005;
    int signum = 0; 				// use for matrix inversion

	for (l=0; l<8; l++)				//Initialize at each step
		M2_tab[l] = 0;

	for (mu=-1; mu <= 1; mu +=dmu){					// Decomp integrale dtheta en somme de riemann int=somme f(x_i)*delta_x
	    gsl_matrix *A = gsl_matrix_calloc (3, 3);
		gsl_matrix *A_inv = gsl_matrix_calloc (3, 3);		// Declaration of matrix A (3x3)

//		change variable y = q-r
		y_t[0] = y*sqrt(1-mu*mu);
		y_t[1] = 0;
		y_t[2] = y*mu;

// 	Compute vector q = y+r with r= (0,0,1) along the LOS
		q[0] = y_t[0];
		q[1] = y_t[1];
		q[2] = y_t[2]+R;
	    q_n = sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]); 	// norm of q

// 	unit vector
	    for (l=0; l<3; l++)
	    	q_v[l] = q[l]/q_n;

/********Compute function ****************************************/
// Xi linear
	    Xi = iXi_L(q_n);

// 	U functions
	    for (l = 0; l<3 ; l++){
	    	U[l] = get_U(q_n)*q_v[l];
	   		U_dot[l] = fU_dot(q_n)*q_v[l];
	    }

// 	Matix A
		gsl_permutation *p = gsl_permutation_alloc (3);
	  	A = get_M(q_n);															//Initialize matix A
	    gsl_linalg_LU_decomp(A, p, &signum);
	    det_A =	gsl_linalg_LU_det(A, signum);					// Determinant of A
		gsl_linalg_LU_invert(A, p, A_inv); 	    					// Inverse of matrix A

// 	g_i components
		for (l=0; l<3; l++){
			g[l] = 0;
	   		for (m=0; m<3; m++)
	    		g[l] += g_i(l, m, A_inv, y_t);
	    }

// 	G_ij components
	    for (l=0; l<3; l++){
	    	for (m=0; m<3; m++)
	    		G[l][m] = G_ij(l, m, A_inv, g);
	    }

// 	Adot_nm
		for (l=0; l<3; l++){
			for	(m=0; m<3; m++)
				A_dot[l][m] = fA_dot(l, m, q_n);
		}

//		A2dot_nm
		for (l=0; l<3; l++){
			for	(m=0; m<3; m++)
				A_2dot[l][m] = fA_2dot(l, m, q_n);
		}

//		A2dot^10_nm
		for (l=0; l<3; l++){
			for	(m=0; m<3; m++)
				A10_2dot[l][m] = fA10_2dot(l, m, q_n);
		}
	// W_inm
		for (l=0; l<3; l++){
			for	(m=0; m<3; m++){
				for (o=0; o<3; o++)
					W[l][m][o] = W_112(l, m, o, q_n);
			}
		}
		for (l=0; l<3; l++){
			for	(m=0; m<3; m++){
				for (o=0; o<3; o++)
					W_2dot[l][m][o] = 2.*W[l][m][o]+ W[o][l][m]+ 2.*W[m][o][ l ];
			}
		}

//		Sum over all the bias component
		for (l=0; l<4; l++){			// Initialize at each step
			F_par[l]=0;
			F_per[l]=0;
		}

// 	<F'>
		for (l=0; l<3; l++){
			for (m=0; m<3; m++)
				s[l][m] = A10_2dot[l][m];
		}
		for (l=0; l<3; l++){
			for (m=0; m<3; m++){
				for (o=0; o<3; o++){
					s[m][o] -= (A_dot[l][m]*g[l]*U_dot[o]+A_dot[l][o]*g[l]*U_dot[m]);
					s[m][o] -= U[l]*g[l]*A_2dot[m][o];
				}
			}
		}
		for (l=0; l<3; l++){
			for (m=0; m<3; m++){
				F_par[1] += 2.*s[l][m]*rn[l]*rn[m];
				F_per[1] += 2.*s[l][m]*delta_K(l,m);
			}
		}
//		<F''>
		for (l=0; l<3; l++){
			for	(m=0; m<3; m++)
				s[l][m] = U_dot[l]*U_dot[m];
		}
		for (l=0; l<3; l++){
			for (m=0; m<3; m++){
				F_par[2] += 2.*s[l][m]*rn[l]*rn[m];
				F_per[2] += 2.*s[l][m]*delta_K(l,m);
			}
		}

//		<F'>²
		for (l=0; l<3; l++){
			for	(m=0; m<3; m++){
				s[l][m] = Xi*A_2dot[l][m];
				s[l][m] += 2.*U_dot[l]*U_dot[m];
			}
		}
		for (l=0; l<3; l++){
			for (m=0; m<3; m++){
				F_par[3] += s[l][m]*rn[l]*rn[m];
				F_per[3] += s[l][m]*delta_K(l,m);
			}
		}

//		F0
		for (l=0; l<3; l++){
			for (m=0; m<3; m++)
				s[l][m] = A_2dot[l][m];
		}
		for (l=0; l<3; l++){
			for (m=0; m<3; m++){
				for(n=0; n<3; n++){
					for (o=0; o<3; o++)
						s[n][o] -= A_dot[l][n]*A_dot[m][o]*G[l][m];
				}
			}
		}
		for (l=0; l<3; l++){
			for (m=0; m<3; m++){
				for (o=0; o<3; o++)
					s[m][o] -= W_2dot[l][m][o]*g[l];
			}
		}
		for (l=0; l<3; l++){
			for (m=0; m<3; m++){
				F_per[0] += s[l][m]*delta_K(l,m);
				F_par[0] += s[l][m]*rn[l]*rn[m];
			}
		}

// 	gaussian factor of M
		f = 0;
		for (l=0; l<3; l++)
			f += g[l]*y_t[l];
		fac = exp(-0.5*f)/(sqrt(2.*M_PI)*2.*M_PI*sqrt(fabs(det_A)));

//		Sum over all component
		for (l=0; l<4; l++){
			M2_tab[l] += F_par[l]*fac;
			M2_tab[l+4] += F_per[l]*fac;
		}
		gsl_permutation_free (p);
	}
	f = y*y*dmu;
	for (l=0; l<4; l++){
		M2_fin[l] = M2_tab[l]*f;
		M2_fin[l+4] = 0.5*f*(M2_tab[l+4]-M2_tab[l]);
	}
}

/********Generate Sigma_12(r) perp and par*******************************************************************************************************************************/
//To compare with Wang +13 we do not divise by 1+Xi_R
void get_Sigma ()
{
	int i, n=8;
	double r, result[n];
	FILE *fi;
	printf("Calculating Sigma...\n");
	fi = fopen("data/Sigma_12_CLPT.dat","w+");
	for (r=1; r<=160; r+=1){
		gl_int6(0, 50, &M_2, &r, result, n);
		for (i=0; i<n; i++)
			result[i] *= 2.*M_PI;
		fprintf(fi,"%lf %lf %lf %lf %lf %lf %lf %lf %lf\n", r, result[0], result[1], result[2], result[3], result[4], result[5], result[6], result[7]);
	}
	fclose(fi);
	return ;
}

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

/************************************************************************************************************************************************************************/
/**********\\MAIN FUNCTION\\***************************************************************************************************************************************/
int
main (int argc, char *argv[])
{
	int i;
	double nLignes;
	FILE *ps;
// Count number of lines
	ps = fopen(argv[1], "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 val;
		fscanf(ps, "%lf %*f\n", &val);
		if (val<kmin) kmin=val;
		if (val>kmax) kmax=val;
	}
	fclose(ps);
	lk = kmax-kmin;

	interpole(1,argv[1], nLignes);

	calc_R1();
	calc_R2();
	calc_Qn();

	interpole(2,"data/func_R1.dat", 10901);
	interpole(3,"data/func_R2.dat", 10901);
	interpole(4,"data/func_Q1.dat", 2991);
	interpole(5,"data/func_Q2.dat", 2991);
	interpole(6,"data/func_Q5.dat", 2991);
	interpole(7,"data/func_Q8.dat", 2991);

	q_function();

	interpole(8,"data/Xi_L_func.dat", 2900);
	interp_qfunc();

	get_Xi_R();
	get_V12();
	get_Sigma();

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

	return 0;
}