/*
** compute edge densities
**
** G.Lohmann, Feb 2015
*/

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

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_sort.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_sort_vector.h>
#include <gsl/gsl_histogram.h>

#include "viaio/Vlib.h"
#include "viaio/VImage.h"
#include "viaio/mu.h"

#ifdef _OPENMP
#include <omp.h>
#endif /*_OPENMP*/

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

extern void GetRank(float *data,gsl_vector *v,gsl_permutation *perm,gsl_permutation *rank);
extern float EdgeCorr(const float *data1,const float *data2,int,int);

int GetAddr(VImage mapimage,int bi,int ri,int ci,int m,int k,int l)
{
  int b = bi+m;
  int r = ri+k;
  int c = ci+l;
  if (b < 0 || r < 0 || c < 0) return -1;
  if (b >= VImageNBands(mapimage) || r >= VImageNRows(mapimage) || c >= VImageNColumns(mapimage)) return -1;
  int ii = (long)VPixel(mapimage,b,r,c,VInteger);
  return ii;
}

int VEdgeNeigbours(size_t id,VImage map,VImage mapimage,int adjdef,int *x)
{
  int nslices = VImageNBands(mapimage);
  int nrows = VImageNRows(mapimage);
  int ncols = VImageNColumns(mapimage);

  int bi = VPixel(map,0,0,id,VShort);
  int ri = VPixel(map,0,1,id,VShort);
  int ci = VPixel(map,0,2,id,VShort);
  if (bi < 1 || bi >= nslices-2) return 0;
  if (ri < 1 || ri >= nrows-2) return 0;
  if (ci < 1 || ci >= ncols-2) return 0;

  int wn=1,rad2=0;
  if (adjdef == 3) {
    wn = 2;
    rad2 = 2*2;
  }

  int n=0;
  int k,l,m;
  for (m=-wn; m<=wn; m++) {
    for (k=-wn; k<=wn; k++) {
      for (l=-wn; l<=wn; l++) {
      	int jj = GetAddr(mapimage,bi,ri,ci,m,k,l);
      	if (jj < 0) continue;  /* outside of brain mask */

      	if (adjdef == 0) {     /* 6 adjacency */
      	  if (ABS(m)+ABS(k)+ABS(l) > 1) continue;
      	}
      	if (adjdef == 1) {     /* 18-adjacency */
      	  if (ABS(m) > 0 && ABS(k) > 0 && ABS(l) > 0) continue;
      	}
	if (adjdef == 3) {     /* sphere */
	  if (m*m + k*k + l*l > rad2) continue;
	}
      	x[n] = jj;
      	n++;
      }
    }
  }
  return n;
}

void VCheckImage(VImage src)
{
  VAttrList out_list = VCreateAttrList();
  VAppendAttr(out_list,"image",NULL,VImageRepn,src);
  FILE *out_file = fopen("test.v","w");
  VWriteFile (out_file, out_list);
}

size_t EdgeDensity(float *E,int *I,int *J,size_t nedges_estimated,
		   gsl_histogram *TedHist,gsl_matrix_float *SNR1,gsl_matrix_float *SNR2,int n1,int n2,
		   VImage roi,VImage map,VImage mapimage,int adjdef,float elength,float zthreshold,
		   int nperm,int numperm,int step,float noise_cutoff,int metric)
{
  size_t i;
  size_t nvox=SNR1->size1;
  int nt=SNR1->size2;
  size_t progress=0;
  int rad2 = (int)(elength*elength);
  float tiny=1.0e-6;
  gsl_set_error_handler_off ();

  int iisel=4;

  /* size of neighbourhood */
  int nadj=0;
  if (adjdef == 0) nadj = 7;
  else if (adjdef == 1) nadj = 19;
  else if (adjdef == 2) nadj = 27;
  else if (adjdef == 3) nadj = 33;
  else VError(" illegal adjdef %d",adjdef);
  size_t nedges=0;
  int minadj = 1;

  /* histogram init */
  size_t nbins = gsl_histogram_bins (TedHist);
  double hmax = gsl_histogram_max (TedHist);
  double hmin = gsl_histogram_min (TedHist);

#pragma omp parallel for shared(nedges,progress,TedHist,E,I,J) schedule(dynamic)
  for (i=0; i<nvox; i+=step) {
    if (i%1000 == 0) fprintf(stderr," %ld000\r",(long)progress++);

    int bi = (int)VPixel(map,0,0,i,VShort);
    int ri = (int)VPixel(map,0,1,i,VShort);
    int ci = (int)VPixel(map,0,2,i,VShort);

    int iselect=0;

    int roiflagi = 0;
    if (roi != NULL) {
      if (VGetPixel(roi,bi,ri,ci) > 0.5) roiflagi = 1;
    }

    const float *datax1 = gsl_matrix_float_const_ptr(SNR1,i,0);
    const float *datax2 = gsl_matrix_float_const_ptr(SNR2,i,0);

    int *x = (int *) VCalloc(nadj,sizeof(int));
    int *y = (int *) VCalloc(nadj,sizeof(int));

    gsl_histogram *tmphist = gsl_histogram_alloc (nbins);
    gsl_histogram_set_ranges_uniform (tmphist,hmin,hmax);
    gsl_histogram_reset(tmphist);

    int nadjx = VEdgeNeigbours(i,map,mapimage,adjdef,x);
    if (nadjx < minadj) continue;

    size_t j=0;
    for (j=0; j<i; j+=step) {
      int bj = (int)VPixel(map,0,0,j,VShort);
      int rj = (int)VPixel(map,0,1,j,VShort);
      int cj = (int)VPixel(map,0,2,j,VShort);
      int d = SQR(bi-bj) + SQR(ri-rj) + SQR(ci-cj);
      if (d < rad2) continue;

      int roiflagj = 0;
      if (roi != NULL) {
	if (VGetPixel(roi,bj,rj,cj) > 0.5) roiflagj = 1;
	if (roiflagi + roiflagj != 1) continue;
      }

      const float *datay1 = gsl_matrix_float_const_ptr(SNR1,j,0);
      const float *datay2 = gsl_matrix_float_const_ptr(SNR2,j,0);

      /* edge z-value */
      double z1 = EdgeCorr(datax1,datay1,nt,metric);
      double z2 = EdgeCorr(datax2,datay2,nt,metric);
      double z = (z1-z2);
      if (z < zthreshold) continue;

      int nadjy = VEdgeNeigbours(j,map,mapimage,adjdef,y);
      if (nadjy < minadj) continue;

      /* use every 'step' value, only when estimating noise cutoff, i.e. step > 1 */
      if (iselect%iisel == 0 && step > 1) {
	iselect = 0;
      }
      else if (iselect%iisel != 0 && step > 1) {
	iselect++;
	continue;
      }

      /* inspect local neighbourhood */
      int r,s;
      double kx=0.0,nx=0.0;
      for (s=0; s<nadjx; s++) {
      	size_t ii = (size_t) x[s];
        int bii = (int)VPixel(map,0,0,ii,VShort);
        int rii = (int)VPixel(map,0,1,ii,VShort);
        int cii = (int)VPixel(map,0,2,ii,VShort);

	const float *dataxx1 = gsl_matrix_float_const_ptr(SNR1,ii,0);
	const float *dataxx2 = gsl_matrix_float_const_ptr(SNR2,ii,0);

      	for (r=0; r<nadjy; r++) {
      	  size_t jj = (size_t) y[r];
          int bjj = (int)VPixel(map,0,0,jj,VShort);
          int rjj = (int)VPixel(map,0,1,jj,VShort);
          int cjj = (int)VPixel(map,0,2,jj,VShort);

          int d2 = SQR(bii-bjj) + SQR(rii-rjj) + SQR(cii-cjj);
          if (d2 < rad2) continue;

	  const float *datayy1 = gsl_matrix_float_const_ptr(SNR1,jj,0);
	  const float *datayy2 = gsl_matrix_float_const_ptr(SNR2,jj,0);

	  /* edge z-value */
	  double z1 = EdgeCorr(dataxx1,datayy1,nt,metric);
	  double z2 = EdgeCorr(dataxx2,datayy2,nt,metric);
	  double z = (z1-z2);
          if (ABS(z) < tiny) continue;
      	  if (z > zthreshold) kx++;
          nx++;
     	}
      }
      if (nx < 1.0) continue;
      double ted = kx/nx;

      if (ted < hmin) ted = hmin;
      if (ted > hmax-tiny) ted = hmax-tiny;
      gsl_histogram_increment (tmphist,ted);

      if (E == NULL) continue;
      if (nperm != numperm) continue;
      if (ted < noise_cutoff) continue;

      #pragma omp critical
      {
	if (nedges >= nedges_estimated) {
	  VError(" alloc, nedges_estimated:  %lu",nedges_estimated);
	}
	E[nedges] = ted;    /* task-based edge density */
	I[nedges] = i;      /* voxel address i */
	J[nedges] = j;      /* voxel address j */
	nedges++;
      }
    }
#pragma omp critical
    {
      gsl_histogram_add (TedHist,tmphist);
    }
    gsl_histogram_free (tmphist);
    VFree(x);
    VFree(y);
  }
  return nedges;
}

/* estimate number of truly needed edges */
size_t EstimateEdges(float *E,int *I,int *J,size_t old_estimate,
		     gsl_histogram *TedHist,gsl_matrix_float *SNR1,gsl_matrix_float *SNR2,int n1,int n2,
		     VImage roi,VImage map,VImage mapimage,int adjdef,float elength,float zthr,
		     float noise_cutoff,int metric)
{
  float tmp_cutoff = -1.0;
  int step = 2;
  int nperm=0,numperm=1;

  gsl_histogram_reset(TedHist);
  EdgeDensity(E,I,J,old_estimate,TedHist,SNR1,SNR2,n1,n2,roi,map,mapimage,(int)adjdef,elength,zthr,
	      nperm,numperm,step,tmp_cutoff,metric);

  double sd = gsl_histogram_sigma (TedHist);
  if (sd < 1.0e-6) VError(" stdev of TedHist: %f",sd);

  size_t i0 = 0;
  if (gsl_histogram_find (TedHist,(double)noise_cutoff,&i0) != GSL_SUCCESS)
    VError (" err finding noise cutoff index, %f",noise_cutoff);

  size_t hbins = gsl_histogram_bins(TedHist);
  gsl_histogram_pdf *cdf = gsl_histogram_pdf_alloc(hbins);
  gsl_histogram_pdf_init (cdf,TedHist);

  double qx = 1.0 - cdf->sum[i0];
  if (qx > 1) qx = 1.0;
  size_t new_estimate = (size_t) (qx*(double)old_estimate);

  gsl_histogram_reset(TedHist);
  gsl_histogram_pdf_free (cdf);
  return new_estimate;
}


/**
 *
 * @file testing_dposv.c
 *
 *  PLASMA testing routines
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.6.0
 * @author Bilel Hadri, Hatem Ltaief
 * @date 2010-11-15
 * @generated d Tue Jan  7 11:45:18 2014
 *
 **/
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>

#include <plasma.h>
#include <cblas.h>
#include <lapacke.h>
#include <core_blas.h>
#include "testing_dmain.h"

enum blas_order_type {
            blas_rowmajor = 101,
            blas_colmajor = 102 };

enum blas_cmach_type {
            blas_base      = 151,
            blas_t         = 152,
            blas_rnd       = 153,
            blas_ieee      = 154,
            blas_emin      = 155,
            blas_emax      = 156,
            blas_eps       = 157,
            blas_prec      = 158,
            blas_underflow = 159,
            blas_overflow  = 160,
            blas_sfmin     = 161};

enum blas_norm_type {
            blas_one_norm       = 171,
            blas_real_one_norm  = 172,
            blas_two_norm       = 173,
            blas_frobenius_norm = 174,
            blas_inf_norm       = 175,
            blas_real_inf_norm  = 176,
            blas_max_norm       = 177,
            blas_real_max_norm  = 178 };

static void
BLAS_error(char *rname, int err, int val, int x) {
  fprintf( stderr, "%s %d %d %d\n", rname, err, val, x );
  abort();
}

static
void
BLAS_dge_norm(enum blas_order_type order, enum blas_norm_type norm,
  int m, int n, const double *a, int lda, double *res) {
  int i, j; float anorm, v;
  char rname[] = "BLAS_dge_norm";

  if (order != blas_colmajor) BLAS_error( rname, -1, order, 0 );

  if (norm == blas_frobenius_norm) {
    anorm = 0.0f;
    for (j = n; j; --j) {
      for (i = m; i; --i) {
        v = a[0];
        anorm += v * v;
        a++;
      }
      a += lda - m;
    }
    anorm = sqrt( anorm );
  } else if (norm == blas_inf_norm) {
    anorm = 0.0f;
    for (i = 0; i < m; ++i) {
      v = 0.0f;
      for (j = 0; j < n; ++j) {
        v += fabs( a[i + j * lda] );
      }
      if (v > anorm)
        anorm = v;
    }
  } else {
    BLAS_error( rname, -2, norm, 0 );
    return;
  }

  if (res) *res = anorm;
}

static
double
BLAS_dpow_di(double x, int n) {
  double rv = 1.0;

  if (n < 0) {
    n = -n;
    x = 1.0 / x;
  }

  for (; n; n >>= 1, x *= x) {
    if (n & 1)
      rv *= x;
  }

  return rv;
}

static
double
BLAS_dfpinfo(enum blas_cmach_type cmach) {
  double eps = 1.0, r = 1.0, o = 1.0, b = 2.0;
  int t = 53, l = 1024, m = -1021;
  char rname[] = "BLAS_dfpinfo";

  if ((sizeof eps) == sizeof(float)) {
    t = 24;
    l = 128;
    m = -125;
  } else {
    t = 53;
    l = 1024;
    m = -1021;
  }

  /* for (i = 0; i < t; ++i) eps *= half; */
  eps = BLAS_dpow_di( b, -t );
  /* for (i = 0; i >= m; --i) r *= half; */
  r = BLAS_dpow_di( b, m-1 );

  o -= eps;
  /* for (i = 0; i < l; ++i) o *= b; */
  o = (o * BLAS_dpow_di( b, l-1 )) * b;

  switch (cmach) {
    case blas_eps: return eps;
    case blas_sfmin: return r;
    default:
      BLAS_error( rname, -1, cmach, 0 );
      break;
  }
  return 0.0;
}

static int check_factorization(int, double*, double*, int, int , double);
static int check_solution(int, int, double*, int, double*, double*, int, double);
static int check_estimator(PLASMA_enum, int, double *, int,
                           double *, double, double,
                           double);

int testing_dposv(int argc, char **argv)
{

    /* Check for number of arguments*/
    if (argc != 4){
        USAGE("POSV", "N LDA NRHS LDB",
              "   - N    : the size of the matrix\n"
              "   - LDA  : leading dimension of the matrix A\n"
              "   - NRHS : number of RHS\n"
              "   - LDB  : leading dimension of the RHS B\n");
        return -1;
    }

    int N     = atoi(argv[0]);
    int LDA   = atoi(argv[1]);
    int NRHS  = atoi(argv[2]);
    int LDB   = atoi(argv[3]);
    double eps;
    int info_solution, info_factorization;
    int u, trans1, trans2;

    double *A1   = (double *)malloc(LDA*N*sizeof(double));
    double *A2   = (double *)malloc(LDA*N*sizeof(double));
    double *B1   = (double *)malloc(LDB*NRHS*sizeof(double));
    double *B2   = (double *)malloc(LDB*NRHS*sizeof(double));

    /* Check if unable to allocate memory */
    if ((!A1)||(!A2)||(!B1)||(!B2)){
        printf("Out of Memory \n ");
        return -2;
    }

    eps = BLAS_dfpinfo( blas_eps );

    for(u=0; u<2; u++) {

        trans1 = uplo[u] == PlasmaUpper ? PlasmaTrans : PlasmaNoTrans;
        trans2 = uplo[u] == PlasmaUpper ? PlasmaNoTrans : PlasmaTrans;

        /*-------------------------------------------------------------
         *  TESTING DPOSV
         */

        /* Initialize A1 and A2 for Symmetric Positif Matrix */
        PLASMA_dplgsy( (double)N, N, A1, LDA, 51 );
        PLASMA_dlacpy( PlasmaUpperLower, N, N, A1, LDA, A2, LDA );

        /* Initialize B1 and B2 */
        PLASMA_dplrnt( N, NRHS, B1, LDB, 371 );
        PLASMA_dlacpy( PlasmaUpperLower, N, NRHS, B1, LDB, B2, LDB );

        printf("\n");
        printf("------ TESTS FOR PLASMA DPOSV ROUTINE -------  \n");
        printf("            Size of the Matrix %d by %d\n", N, N);
        printf("\n");
        printf(" The matrix A is randomly generated for each test.\n");
        printf("============\n");
        printf(" The relative machine precision (eps) is to be %e \n", eps);
        printf(" Computational tests pass if scaled residuals are less than 60.\n");

        /* PLASMA DPOSV */
        PLASMA_dposv(uplo[u], N, NRHS, A2, LDA, B2, LDB);

        /* Check the factorization and the solution */
        info_factorization = check_factorization( N, A1, A2, LDA, uplo[u], eps);
        info_solution = check_solution(N, NRHS, A1, LDA, B1, B2, LDB, eps);

        if ( (info_solution == 0) && (info_factorization == 0) ) {
            printf("***************************************************\n");
            printf(" ---- TESTING DPOSV(%s) ...................... PASSED !\n", uplostr[u]);
            printf("***************************************************\n");
        }
        else {
            printf("***************************************************\n");
            printf(" - TESTING DPOSV(%s) ... FAILED !\n", uplostr[u]);
            printf("***************************************************\n");
        }

        /*-------------------------------------------------------------
         *  TESTING DPOTRF + DPOTRS
         */

        /* Initialize A1 and A2 for Symmetric Positif Matrix */
        PLASMA_dplgsy( (double)N, N, A1, LDA, 51 );
        PLASMA_dlacpy( PlasmaUpperLower, N, N, A1, LDA, A2, LDA );

        /* Initialize B1 and B2 */
        PLASMA_dplrnt( N, NRHS, B1, LDB, 371 );
        PLASMA_dlacpy( PlasmaUpperLower, N, NRHS, B1, LDB, B2, LDB );

        /* Plasma routines */
        PLASMA_dpotrf(uplo[u], N, A2, LDA);
        PLASMA_dpotrs(uplo[u], N, NRHS, A2, LDA, B2, LDB);

        printf("\n");
        printf("------ TESTS FOR PLASMA DPOTRF + DPOTRS ROUTINE -------  \n");
        printf("            Size of the Matrix %d by %d\n", N, N);
        printf("\n");
        printf(" The matrix A is randomly generated for each test.\n");
        printf("============\n");
        printf(" The relative machine precision (eps) is to be %e \n", eps);
        printf(" Computational tests pass if scaled residuals are less than 60.\n");

        /* Check the factorization and the solution */
        info_factorization = check_factorization( N, A1, A2, LDA, uplo[u], eps);
        info_solution = check_solution(N, NRHS, A1, LDA, B1, B2, LDB, eps);

        if ((info_solution == 0)&(info_factorization == 0)){
            printf("***************************************************\n");
            printf(" ---- TESTING DPOTRF + DPOTRS (%s)............ PASSED !\n", uplostr[u]);
            printf("***************************************************\n");
        }
        else{
            printf("****************************************************\n");
            printf(" - TESTING DPOTRF + DPOTRS (%s)... FAILED !\n", uplostr[u]);
            printf("****************************************************\n");
        }

        /*-------------------------------------------------------------
         *  TESTING DPOTRF + ZPTRSM + DTRSM
         */

        /* Initialize A1 and A2 for Symmetric Positif Matrix */
        PLASMA_dplgsy( (double)N, N, A1, LDA, 51 );
        PLASMA_dlacpy( PlasmaUpperLower, N, N, A1, LDA, A2, LDA );

        /* Initialize B1 and B2 */
        PLASMA_dplrnt( N, NRHS, B1, LDB, 371 );
        PLASMA_dlacpy( PlasmaUpperLower, N, NRHS, B1, LDB, B2, LDB );

        /* PLASMA routines */
        PLASMA_dpotrf(uplo[u], N, A2, LDA);
        PLASMA_dtrsm(PlasmaLeft, uplo[u], trans1, PlasmaNonUnit,
                     N, NRHS, 1.0, A2, LDA, B2, LDB);
        PLASMA_dtrsm(PlasmaLeft, uplo[u], trans2, PlasmaNonUnit,
                     N, NRHS, 1.0, A2, LDA, B2, LDB);

        printf("\n");
        printf("------ TESTS FOR PLASMA DPOTRF + DTRSM + DTRSM  ROUTINE -------  \n");
        printf("            Size of the Matrix %d by %d\n", N, N);
        printf("\n");
        printf(" The matrix A is randomly generated for each test.\n");
        printf("============\n");
        printf(" The relative machine precision (eps) is to be %e \n", eps);
        printf(" Computational tests pass if scaled residuals are less than 60.\n");

        /* Check the factorization and the solution */
        info_factorization = check_factorization( N, A1, A2, LDA, uplo[u], eps);
        info_solution = check_solution(N, NRHS, A1, LDA, B1, B2, LDB, eps);

        if ((info_solution == 0)&(info_factorization == 0)){
            printf("***************************************************\n");
            printf(" ---- TESTING DPOTRF + DTRSM + DTRSM (%s)..... PASSED !\n", uplostr[u]);
            printf("***************************************************\n");
        }
        else{
            printf("***************************************************\n");
            printf(" - TESTING DPOTRF + DTRSM + DTRSM (%s)... FAILED !\n", uplostr[u]);
            printf("***************************************************\n");
        }

        /*-------------------------------------------------------------
         *  TESTING ZPOCON on the last call
         */
        {
            double Anorm = PLASMA_dlansy( PlasmaOneNorm, uplo[u], N, A1, LDA );
            double Acond;

            info_solution = PLASMA_dpocon(uplo[u], N, A2, LDA, Anorm, &Acond);
            if ( info_solution == 0 ) {
                info_solution = check_estimator(uplo[u], N, A1, LDA, A2, Anorm, Acond, eps);
            } else {
                printf(" PLASMA_dpocon returned info = %d\n", info_solution );
            }
            if ((info_solution == 0)){
                printf("***************************************************\n");
                printf(" ---- TESTING DPOTRF + ZPOCON (%s) ........... PASSED !\n", uplostr[u]);
                printf("***************************************************\n");
            }
            else{
                printf("**************************************************\n");
                printf(" - TESTING DPOTRF + ZPOCON (%s) ... FAILED !\n", uplostr[u]);
                printf("**************************************************\n");
            }
        }
    }

    free(A1); free(A2); free(B1); free(B2);

    return 0;
}

/*------------------------------------------------------------------------
 *  Check the factorization of the matrix A2
 */
static int check_factorization(int N, double *A1, double *A2, int LDA, int uplo, double eps)
{
    double Anorm, Rnorm;
    double alpha;
    int info_factorization;
    int i,j;

    double *Residual = (double *)malloc(N*N*sizeof(double));
    double *L1       = (double *)malloc(N*N*sizeof(double));
    double *L2       = (double *)malloc(N*N*sizeof(double));
    double *work              = (double *)malloc(N*sizeof(double));

    memset((void*)L1, 0, N*N*sizeof(double));
    memset((void*)L2, 0, N*N*sizeof(double));

    alpha= 1.0;

    LAPACKE_dlacpy_work(LAPACK_COL_MAJOR,' ', N, N, A1, LDA, Residual, N);

    /* Dealing with L'L or U'U  */
    if (uplo == PlasmaUpper){
        LAPACKE_dlacpy_work(LAPACK_COL_MAJOR,'u', N, N, A2, LDA, L1, N);
        LAPACKE_dlacpy_work(LAPACK_COL_MAJOR,'u', N, N, A2, LDA, L2, N);
        cblas_dtrmm(CblasColMajor, CblasLeft, CblasUpper, CblasTrans, CblasNonUnit, N, N, (alpha), L1, N, L2, N);
    }
    else{
        LAPACKE_dlacpy_work(LAPACK_COL_MAJOR,'l', N, N, A2, LDA, L1, N);
        LAPACKE_dlacpy_work(LAPACK_COL_MAJOR,'l', N, N, A2, LDA, L2, N);
        cblas_dtrmm(CblasColMajor, CblasRight, CblasLower, CblasTrans, CblasNonUnit, N, N, (alpha), L1, N, L2, N);
    }

    /* Compute the Residual || A -L'L|| */
    for (i = 0; i < N; i++)
        for (j = 0; j < N; j++)
           Residual[j*N+i] = L2[j*N+i] - Residual[j*N+i];

    BLAS_dge_norm( blas_colmajor, blas_inf_norm, N, N, Residual, N, &Rnorm );
    BLAS_dge_norm( blas_colmajor, blas_inf_norm, N, N, A1, LDA, &Anorm );

    printf("============\n");
    printf("Checking the Cholesky Factorization \n");
    printf("-- ||L'L-A||_oo/(||A||_oo.N.eps) = %e \n",Rnorm/(Anorm*N*eps));

    if ( isnan(Rnorm/(Anorm*N*eps)) || isinf(Rnorm/(Anorm*N*eps)) || (Rnorm/(Anorm*N*eps) > 60.0) ){
        printf("-- Factorization is suspicious ! \n");
        info_factorization = 1;
    }
    else{
        printf("-- Factorization is CORRECT ! \n");
        info_factorization = 0;
    }

    free(Residual); free(L1); free(L2); free(work);

    return info_factorization;
}

/*------------------------------------------------------------------------
 *  Check the accuracy of the solution of the linear system
 */

static int check_solution(int N, int NRHS, double *A1, int LDA, double *B1, double *B2, int LDB, double eps )
{
    int info_solution;
    double Rnorm, Anorm, Xnorm, Bnorm, result;
    double alpha, beta;
    double *work = (double *)malloc(N*sizeof(double));

    alpha = 1.0;
    beta  = -1.0;

    BLAS_dge_norm( blas_colmajor, blas_inf_norm, N, NRHS, B2, LDB, &Xnorm );
    BLAS_dge_norm( blas_colmajor, blas_inf_norm, N, N,    A1, LDA, &Anorm );
    BLAS_dge_norm( blas_colmajor, blas_inf_norm, N, NRHS, B1, LDB, &Bnorm );

    cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, N, NRHS, N, (alpha), A1, LDA, B2, LDB, (beta), B1, LDB);
    BLAS_dge_norm( blas_colmajor, blas_inf_norm, N, NRHS, B1, LDB, &Rnorm );

    if (getenv("PLASMA_TESTING_VERBOSE"))
      printf( "||A||_oo=%f\n||X||_oo=%f\n||B||_oo=%f\n||A X - B||_oo=%e\n", Anorm, Xnorm, Bnorm, Rnorm );

    result = Rnorm / ( (Anorm*Xnorm+Bnorm)*N*eps ) ;
    printf("============\n");
    printf("Checking the Residual of the solution \n");
    printf("-- ||Ax-B||_oo/((||A||_oo||x||_oo+||B||_oo).N.eps) = %e \n", result);

    if (  isnan(Xnorm) || isinf(Xnorm) || isnan(result) || isinf(result) || (result > 60.0) ) {
        printf("-- The solution is suspicious ! \n");
        info_solution = 1;
     }
    else{
        printf("-- The solution is CORRECT ! \n");
        info_solution = 0;
    }

    free(work);

    return info_solution;
}

/*------------------------------------------------------------------------
 *  Check the accuracy of the condition estimator
 */
static int check_estimator(PLASMA_enum uplo, int N, double *A1, int LDA,
                           double *A2, double Anorm, double Acond,
                           double eps)
{
    int info_solution;
    double result, Acond_lapack;
    double invcond, invcond_lapack;

    info_solution = LAPACKE_dpocon(LAPACK_COL_MAJOR, lapack_const(uplo), N, A2, LDA, Anorm, &Acond_lapack);

    if ( info_solution != 0 ) {
        printf(" PLASMA_dgecon returned info = %d\n", info_solution );
        return info_solution;
    }

    invcond_lapack = 1. / ( Acond_lapack );
    invcond        = 1. / ( Acond );

    printf("============\n");
    printf("Checking the condition number \n");
    printf("-- Acond_plasma = %e, Acond_lapack = %e \n"
           "-- Ainvcond_plasma = %e, Ainvcond_lapack = %e \n",
           Acond, Acond_lapack, invcond, invcond_lapack );

    result = fabs( Acond_lapack - Acond ) / eps;
    if ( result > 60. ) {
        printf("-- The solution is suspicious ! \n");
        info_solution = 1;
     }
    else{
        printf("-- The solution is CORRECT ! \n");
        info_solution = 0;
    }
    return info_solution;
}


/*
 * 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 Sun Nov  7 20:43:55 EST 1999 */

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

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

/*
 * This function contains 38 FP additions, 8 FP multiplications,
 * (or, 34 additions, 4 multiplications, 4 fused multiply/add),
 * 18 stack variables, and 24 memory accesses
 */
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.41 1999/05/26 15:44:14 fftw Exp $
 * $Id: fft.ml,v 1.43 1999/05/17 19:44:18 fftw Exp $
 * $Id: to_c.ml,v 1.25 1999/10/26 21:41:32 stevenj Exp $
 */

void fftw_real2hc_12(const fftw_real *input, fftw_real *real_output, fftw_real *imag_output, int istride, int real_ostride, int imag_ostride)
{
     fftw_real tmp5;
     fftw_real tmp25;
     fftw_real tmp11;
     fftw_real tmp23;
     fftw_real tmp30;
     fftw_real tmp35;
     fftw_real tmp10;
     fftw_real tmp26;
     fftw_real tmp12;
     fftw_real tmp18;
     fftw_real tmp29;
     fftw_real tmp34;
     fftw_real tmp31;
     fftw_real tmp32;
     ASSERT_ALIGNED_DOUBLE;
     {
	  fftw_real tmp1;
	  fftw_real tmp2;
	  fftw_real tmp3;
	  fftw_real tmp4;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp1 = input[0];
	  tmp2 = input[4 * istride];
	  tmp3 = input[8 * istride];
	  tmp4 = tmp2 + tmp3;
	  tmp5 = tmp1 + tmp4;
	  tmp25 = tmp1 - (K500000000 * tmp4);
	  tmp11 = tmp3 - tmp2;
     }
     {
	  fftw_real tmp19;
	  fftw_real tmp20;
	  fftw_real tmp21;
	  fftw_real tmp22;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp19 = input[9 * istride];
	  tmp20 = input[istride];
	  tmp21 = input[5 * istride];
	  tmp22 = tmp20 + tmp21;
	  tmp23 = tmp19 - (K500000000 * tmp22);
	  tmp30 = tmp19 + tmp22;
	  tmp35 = tmp21 - tmp20;
     }
     {
	  fftw_real tmp6;
	  fftw_real tmp7;
	  fftw_real tmp8;
	  fftw_real tmp9;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp6 = input[6 * istride];
	  tmp7 = input[10 * istride];
	  tmp8 = input[2 * istride];
	  tmp9 = tmp7 + tmp8;
	  tmp10 = tmp6 + tmp9;
	  tmp26 = tmp6 - (K500000000 * tmp9);
	  tmp12 = tmp8 - tmp7;
     }
     {
	  fftw_real tmp14;
	  fftw_real tmp15;
	  fftw_real tmp16;
	  fftw_real tmp17;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp14 = input[3 * istride];
	  tmp15 = input[7 * istride];
	  tmp16 = input[11 * istride];
	  tmp17 = tmp15 + tmp16;
	  tmp18 = tmp14 - (K500000000 * tmp17);
	  tmp29 = tmp14 + tmp17;
	  tmp34 = tmp16 - tmp15;
     }
     real_output[3 * real_ostride] = tmp5 - tmp10;
     imag_output[3 * imag_ostride] = tmp29 - tmp30;
     tmp31 = tmp5 + tmp10;
     tmp32 = tmp29 + tmp30;
     real_output[6 * real_ostride] = tmp31 - tmp32;
     real_output[0] = tmp31 + tmp32;
     {
	  fftw_real tmp37;
	  fftw_real tmp38;
	  fftw_real tmp33;
	  fftw_real tmp36;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp37 = tmp34 + tmp35;
	  tmp38 = tmp11 + tmp12;
	  imag_output[2 * imag_ostride] = K866025403 * (tmp37 - tmp38);
	  imag_output[4 * imag_ostride] = K866025403 * (tmp38 + tmp37);
	  tmp33 = tmp25 - tmp26;
	  tmp36 = K866025403 * (tmp34 - tmp35);
	  real_output[5 * real_ostride] = tmp33 - tmp36;
	  real_output[real_ostride] = tmp33 + tmp36;
     }
     {
	  fftw_real tmp27;
	  fftw_real tmp28;
	  fftw_real tmp13;
	  fftw_real tmp24;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp27 = tmp25 + tmp26;
	  tmp28 = tmp18 + tmp23;
	  real_output[2 * real_ostride] = tmp27 - tmp28;
	  real_output[4 * real_ostride] = tmp27 + tmp28;
	  tmp13 = K866025403 * (tmp11 - tmp12);
	  tmp24 = tmp18 - tmp23;
	  imag_output[imag_ostride] = tmp13 - tmp24;
	  imag_output[5 * imag_ostride] = -(tmp13 + tmp24);
     }
}

fftw_codelet_desc fftw_real2hc_12_desc =
{
     "fftw_real2hc_12",
     (void (*)()) fftw_real2hc_12,
     12,
     FFTW_FORWARD,
     FFTW_REAL2HC,
     266,
     0,
     (const int *) 0,
};


/*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
**                                                                                **
** This file forms part of the Underworld geophysics modelling application.         **
**                                                                                **
** For full license and copyright information, please refer to the LICENSE.md file  **
** located at the project root, or contact the authors.                             **
**                                                                                **
**~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*/
/*

Given a block system
A x = b,
or
( K G )(u) = (f)
( D C )(p)   (h)

We define a symmetrically scaled system,
L A R R^-1 x = L b,		A'x' = b'
where L = diag(L1, 1), R = diag(R1,1) are block diagonal
and L_i and R_i are both diagonal matrirces.

The scaling produces
( L1   )( K G )( R1   )( R1   )^-1(u) = ( L1   )(f)
Du + Cp = (h)
and we solve
( L1   )( K G )( R1   )(v) = ( L1   )(f)
 Du + Cp  = (h)
or
( L1*K*R1, L1*G )(v) = (L1*f)
( D*R1  ,  C    )(p) = ( h  )

The solution u,p is recovered via
(u) = (R1*v)
(p) = (p)

*/

#include <petsc.h>
#include <petscmat.h>
#include <petscvec.h>

#include <StGermain/libStGermain/src/StGermain.h>
#include <StgDomain/libStgDomain/src/StgDomain.h>

#include "common-driver-utils.h"
#include "stokes_Kblock_scaling.h"

/* private prototypes */
PetscErrorCode BSSCR_MatStokesKBlock_ApplyScaling( MatStokesBlockScaling BA, Mat A, Vec b, Vec x, Mat S, PetscTruth sym );
PetscErrorCode BSSCR_mat_kblock_invert_scalings( MatStokesBlockScaling BA );

/* A x = b -> A'x' = b' */
/*
Note this routine actually modifies the matrix and rhs b.
*/

// updated
PetscErrorCode BSSCR_MatStokesKBlock_ApplyScaling( MatStokesBlockScaling BA, Mat A, Vec b, Vec x, Mat S, PetscTruth sym )
{
	Mat K,G,D;
	Vec L1,R1;
	Vec f,u;

	/* Get the scalings out the block mat data */
	VecNestGetSubVec( BA->Lz, 0, &L1 );
	VecNestGetSubVec( BA->Rz, 0, &R1 );

	/* get the subblock solution and rhs */
	if( x != PETSC_NULL ) {
		VecNestGetSubVec( x, 0, &u );
		VecPointwiseDivide( u, u,R1); /* x <- x * 1/R1 */
	}
	if( b != PETSC_NULL ) {
		VecNestGetSubVec( b, 0, &f );
		VecPointwiseMult( f, f,L1); /* f <- f * L1 */
	}

	/* Scale matrices */
	MatNestGetSubMat( A, 0,0, &K );
	MatNestGetSubMat( A, 0,1, &G );
	MatNestGetSubMat( A, 1,0, &D );

	if( K != PETSC_NULL ) {		MatDiagonalScale( K, L1,R1 );		}
	if( G != PETSC_NULL ) {		MatDiagonalScale( G, L1,PETSC_NULL );		}
	if( D != PETSC_NULL && !sym ) {	MatDiagonalScale( D, PETSC_NULL,R1 );		}

	PetscFunctionReturn(0);
}

// updated
PetscErrorCode BSSCR_MatStokesKBlockScalingCreate( MatStokesBlockScaling *_BA )
{
	MatStokesBlockScaling BA;
	PetscErrorCode ierr;

	ierr = PetscMalloc( sizeof(struct _p_MatStokesBlockScaling), &BA );CHKERRQ(ierr);

	BA->Lz = PETSC_NULL;
	BA->Rz = PETSC_NULL;

	BA->scaling_exists              = PETSC_FALSE;
	BA->scalings_have_been_inverted = PETSC_FALSE;
	BA->system_has_been_scaled      = PETSC_FALSE;

	*_BA = BA;

	PetscFunctionReturn(0);
}

// updated
PetscErrorCode BSSCR_MatStokesKBlockScalingDestroy( MatStokesBlockScaling BA )
{

	if( BA->scaling_exists == PETSC_FALSE ) PetscFunctionReturn(0);

	if( BA->Lz != PETSC_NULL ) {
		Stg_VecDestroy(&BA->Lz );
		BA->Lz = PETSC_NULL;
	}

	if( BA->Rz != PETSC_NULL ) {
		Stg_VecDestroy(&BA->Rz );
		BA->Rz = PETSC_NULL;
	}

	PetscFree( BA );

	PetscFunctionReturn(0);
}

/*
A is 2x2 block matrix
b and x are 2x1 block vectors
*/
// updated
PetscErrorCode BSSCR_MatKBlock_ConstructScaling( MatStokesBlockScaling BA, Mat A, Vec b, Vec x, PetscTruth sym )
{

	if( BA->scaling_exists == PETSC_FALSE ) {
      //PetscInt M,N;
		//PetscTruth is_block;

		/* check A is 2x2 block matrix */
		/* Stg_PetscTypeCompare( (PetscObject)A, "block", &is_block ); */
		/* if (is_block==PETSC_FALSE) { */
		/* 	Stg_SETERRQ( PETSC_ERR_SUP, "Only valid for MatType = block" ); */
		/* } */
		/* MatGetSize( A, &M, &N ); */
		/* if ( (M!=2) || (N!=2) ) { */
		/* 	Stg_SETERRQ2( PETSC_ERR_SUP, "Only valid for 2x2 block. Yours has dimension %Dx%D", M,N ); */
		/* } */

		VecDuplicate( x, &BA->Lz );
		VecDuplicate( x, &BA->Rz );

		BA->scaling_exists = PETSC_TRUE;
	}

//	if( BA->user_build_scaling != PETSC_NULL ) {
//		BA->user_build_scaling( A,b,x,BA->scaling_ctx);
//	}
//	else {
	BSSCR_MatStokesKBlockDefaultBuildScaling(BA,A,b,x, sym);
//	}

	BA->scalings_have_been_inverted = PETSC_FALSE;

	PetscFunctionReturn(0);
}

// updated
PetscErrorCode BSSCR_mat_kblock_invert_scalings( MatStokesBlockScaling BA )
{
	Vec L1,R1;

	VecNestGetSubVec( BA->Lz, 0, &L1 );
	VecNestGetSubVec( BA->Rz, 0, &R1 );

	VecReciprocal(L1);
	VecReciprocal(R1);

	/* toggle inversion flag */
	if( BA->scalings_have_been_inverted == PETSC_TRUE ) {   BA->scalings_have_been_inverted = PETSC_FALSE;  }
	if( BA->scalings_have_been_inverted == PETSC_FALSE ) {  BA->scalings_have_been_inverted = PETSC_TRUE;   }

	PetscFunctionReturn(0);
}

/* updated */
PetscErrorCode BSSCR_MatStokesKBlockScaleSystem( MatStokesBlockScaling BA, Mat A, Vec b, Vec x, Mat S, PetscTruth sym )
{
	if( BA->scaling_exists == PETSC_FALSE ) {
	    BSSCR_MatKBlock_ConstructScaling( BA,A,b,x, sym );
	}

	if( BA->scalings_have_been_inverted == PETSC_TRUE ) {
		BSSCR_mat_kblock_invert_scalings(BA); /* to undo inversion */
	}

	BSSCR_MatStokesKBlock_ApplyScaling(BA,A,b,x,S,sym);
	BA->system_has_been_scaled = PETSC_TRUE;

	/*
	PetscPrintf( PETSC_COMM_WORLD, "Post Scaling \n");
	MatBlock_ReportOperatorScales(A);
	*/

	PetscFunctionReturn(0);
}

// updated
PetscErrorCode BSSCR_MatStokesKBlockUnScaleSystem( MatStokesBlockScaling BA, Mat A, Vec b, Vec x, Mat S, PetscTruth sym )
{

	if( BA->system_has_been_scaled == PETSC_FALSE ) {
		printf("Warning: MatBlock has not been scaled !! \n");
		PetscFunctionReturn(0);
	}

	if( BA->scalings_have_been_inverted == PETSC_FALSE ) {
		BSSCR_mat_kblock_invert_scalings(BA);
	}

	BSSCR_MatStokesKBlock_ApplyScaling(BA,A,b,x,S,sym);
	BA->system_has_been_scaled = PETSC_FALSE;

	PetscFunctionReturn(0);
}

PetscErrorCode BSSCR_MatStokesKBlockReportOperatorScales( Mat A, PetscTruth sym )
{
	Vec rA, rG;
	PetscInt loc;
	PetscReal min, max;
	Mat K,G,D,C;
	//PetscTruth is_block;

	/* check A is 2x2 block matrix */
	/* Stg_PetscTypeCompare( (PetscObject)A, "block", &is_block ); */
	/* if (is_block==PETSC_FALSE) { */
	/* 	Stg_SETERRQ( PETSC_ERR_SUP, "Only valid for MatType = block" ); */
	/* } */
	/* MatGetSize( A, &M, &N ); */
	/* if ( (M!=2) || (N!=2) ) { */
	/* 	Stg_SETERRQ2( PETSC_ERR_SUP, "Only valid for 2x2 block. Yours has dimension %Dx%D", M,N ); */
	/* } */

	MatNestGetSubMat( A, 0,0, &K );
	MatNestGetSubMat( A, 0,1, &G );
	MatNestGetSubMat( A, 1,0, &D );
	MatNestGetSubMat( A, 1,1, &C );

	MatGetVecs( K, PETSC_NULL, &rA );
	VecDuplicate( rA, &rG );

	/* Report the row max and mins */
	if (K!=PETSC_NULL) {
		MatGetRowMaxAbs( K, rA, PETSC_NULL );
		VecMax( rA, &loc, &max );
		PetscPrintf( PETSC_COMM_WORLD, "Sup_max(K) = %g \n", max );

		MatGetRowMinAbs( K, rA, PETSC_NULL );
		VecMin( rA, &loc, &min );
		PetscPrintf( PETSC_COMM_WORLD, "Sup_min(K) = %g \n\n", min );
	}

	if( G != PETSC_NULL ) {
		MatGetRowMaxAbs( G, rG, PETSC_NULL );
		VecMax( rG, &loc, &max );
		PetscPrintf( PETSC_COMM_WORLD, "Sup_max(G) = %g \n", max );

		MatGetRowMinAbs( G, rG, PETSC_NULL );
		VecMin( rG, &loc, &min );
		PetscPrintf( PETSC_COMM_WORLD, "Sup_min(G) = %g \n", min );
	}

	if( D != PETSC_NULL && !sym ) {
                Vec rD;

                MatGetVecs( D, PETSC_NULL, &rD );
		MatGetRowMaxAbs( D, rD, PETSC_NULL );
		VecMax( rD, &loc, &max );
		PetscPrintf( PETSC_COMM_WORLD, "Sup_max(D) = %g \n", max );

		MatGetRowMinAbs( D, rD, PETSC_NULL );
		VecMin( rD, &loc, &min );
		PetscPrintf( PETSC_COMM_WORLD, "Sup_min(D) = %g \n", min );

                Stg_VecDestroy(&rD );
	}

	if( C != PETSC_NULL ) {
		Vec cG;

		MatGetVecs( G, &cG, PETSC_NULL );
		MatGetRowMaxAbs( C, cG, PETSC_NULL );
		VecMax( cG, &loc, &max );
		PetscPrintf( PETSC_COMM_WORLD, "Sup_max(C) = %g \n", max );

		MatGetRowMinAbs( C, cG, PETSC_NULL );
		VecMin( cG, &loc, &min );
		PetscPrintf( PETSC_COMM_WORLD, "Sup_min(C) = %g \n\n", min );

		Stg_VecDestroy(&cG);
	}

	Stg_VecDestroy(&rA );
	Stg_VecDestroy(&rG );

	PetscFunctionReturn(0);
}

// updated
PetscErrorCode BSSCR_MatStokesKBlockDefaultBuildScaling( MatStokesBlockScaling BA, Mat A, Vec b, Vec x, PetscTruth sym )
{
	Mat K;
	Vec rA;
	Vec L1, R1;

	VecNestGetSubVec( BA->Lz, 0, &L1 );
	VecNestGetSubVec( BA->Rz, 0, &R1 );

	rA = L1;

	MatNestGetSubMat( A, 0,0, &K );

	/* Get diag of K */
	MatGetDiagonal( K, rA);
	VecSqrt( rA );
	VecReciprocal( rA );

	/* leave Shat alone as the Schur Compliment is unchanged by any consistent scaling of K */
	//MatGetDiagonal( Shat, rC);
	//VecSqrt( rC );
	//VecReciprocal( rC );

	/* no scaling for pressure */
	//VecSet( rC, 1.0 );

	VecCopy( L1, R1 );

	PetscFunctionReturn(0);
}


#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_histogram.h>
#include"mpi.h"

const double PI = 3.14159;

double rz(double red);

struct galaxy {
  double x, y, z, d, w;
};

void N2divide( int * n, int N, int Ntot);

int main (int argc, char **argv) {

  FILE * DD;
  FILE * PP;

  const int Nr = 150;
  const int Nm = 100;
  const double RSTEP = 1.0;
  const double MSTEP = 0.01;

  const int maxNgal = 40000000;
  const double RMIN = 0.0;
  const double RMAX = 150.0;

  int *n;

  char DDname[400];
  char PPname[400];

  int bit;
  int bittot;

  sprintf(DDname,argv[1]);

  MPI_Init (&argc, &argv);
  int numproc;
  int totproc;
  MPI_Comm_rank (MPI_COMM_WORLD, &numproc);
  MPI_Comm_size (MPI_COMM_WORLD, &totproc);

  n = (int *) calloc(totproc + 1, sizeof(int));

  sprintf(PPname,"%s.RR",argv[2]);

  DD = fopen(DDname,"r");
  printf("%s %d\n",DDname,numproc);
  struct galaxy * gal;
  double * DDcount;
  double * DDcount_in_place;
  gal = (struct galaxy*)malloc(maxNgal*sizeof(struct galaxy));
  DDcount = (double*)calloc(Nr*Nm,sizeof(double));
  DDcount_in_place = (double*)calloc(Nr*Nm,sizeof(double));
  struct galaxy * galp;
  galp = gal;

  int N = 0;
  int Ntest = 0;
  double sumW = 0;
  double sumW_in_place = 0;
  double ra, dec, red, dist;
  double weight = 1.0;
  double weight1;
  double weight2;
  double weight3;
  double weight4;
  double weightFKP;
  int indexFKP;
  double ddummy;
  char line[2000];
  for (int i = 0; i < 0; i ++) {
  fgets (line, 2000, DD);
  printf("%s\n", line);
  }
  int counter = 0;
  while (fscanf(DD,"%le %le %le\n",&ra,&dec,&red) != EOF) {
    counter ++;
    if (counter%3 != 0) {
      continue;
    }
    ra *= PI/180.0;
    dec *= PI/180.0;
    dist = rz(red);
    galp->x = dist*cos(dec)*cos(ra);
    galp->y = dist*cos(dec)*sin(ra);
    galp->z = dist*sin(dec);
    galp->d = dist;
//    galp->w = weight1 * weight2;
    galp++;
    N++;
  }
  fclose(DD);
  n[0] = 1;
  N2divide(n, totproc, N);
  n[totproc] = N;
  n[0] = 0;
  bit = n[numproc];
  bittot = n[numproc + 1];
  for (int i = 0; i < totproc + 1; i++) {
    printf("n[%d] = %d\n", i, n[i]);
  }
  double mu;
  double d1, d2, d3;
  double x1, y1, z1, x2, y2, z2, gd1, gd2, gw1, gw2;
  double r2, rat;
  double rr;
  double xb, yb, zb, db2;
  struct galaxy * galp1, * galp2;
  galp1 = gal;
  galp1 += bit;
  for (int i = bit; i < bittot; i++) {
    x1 = galp1->x; y1 = galp1->y; z1 = galp1->z; gd1 = galp1->d; //gw1 = galp1->w;
    galp2 = gal + i + 1;
    for (int j = i + 1; j < N; j++) {
       x2 = galp2->x; y2 = galp2->y; z2 = galp2->z; gd2 = galp2->d; //gw2 = galp2->w;
       //sumW += gw1*gw2;
       sumW += 1.0;
       d1 = x1 - x2;
       d2 = y1 - y2;
       d3 = z1 - z2;
       r2 = d1*d1 + d2*d2 + d3*d3;
       if (r2 > RMAX*RMAX || r2 < RMIN*RMIN) {
	   galp2++;
         continue;
       }
       rat = gd1/gd2;
       xb = x1 + x2*rat;
       yb = y1 + y2*rat;
       zb = z1 + z2*rat;
       db2 = xb*xb + yb*yb + zb*zb;
       mu = fabs((xb*d1 + yb*d2 + zb*d3)/sqrt(r2)/sqrt(db2));
       rr = sqrt(r2);
	int binr = (int)((rr - RMIN)/RSTEP);
	int binm = (int)(mu/MSTEP);
	if (binr >= 0 && binm >= 0 && binr < Nr && binm < Nm){
	int ind = binr + Nr*binm;
//	DDcount[ind] += gw1*gw2;
        DDcount[ind] += 1.0;
	}
       galp2++;
    }
    galp1++;
  }
  free(gal);

  MPI_Reduce(DDcount, DDcount_in_place, Nr*Nm, MPI_DOUBLE, MPI_SUM, 0, MPI_COMM_WORLD);
  MPI_Reduce(&sumW, &sumW_in_place, 1, MPI_DOUBLE, MPI_SUM, 0, MPI_COMM_WORLD);
  if (numproc == 0) {
    PP = fopen(PPname,"w");
    printf("%s\n", PPname);
    fprintf(PP,"# weighted number: %lf\n",sumW_in_place);
    fprintf(PP,"# RBINS: %d\n",Nr);
    fprintf(PP,"# MBINS: %d\n",Nm);
    int k, l;
    for (k = 0; k < Nm; k++) {
      for (l = 0; l < Nr; l++) {
      fprintf(PP,"%lf ",DDcount_in_place[k*Nr + l]);
      }
      fprintf(PP,"\n");
    }
    fclose(PP);
  }

  free(DDcount);
  free(DDcount_in_place);

  free(n);
  MPI_Finalize ();

  return 0;
}

double f (double x, void * p) {
  double Om = 0.310;
  double ff = 2997.92458/sqrt(Om*(1.0 + x)*(1.0 + x)*(1.0 + x) + 1.0 - Om);
  return ff;
}

double rz (double red) {
  gsl_integration_workspace * w = gsl_integration_workspace_alloc(1000);
  double result, error;
  gsl_function F;
  F.function = &f;
  gsl_integration_qags(&F, 0, red, 0, 1e-7, 1000, w, &result, &error);
  gsl_integration_workspace_free (w);
  return result;
}

void N2divide( int * n, int N, int Ntot) {
   double dummy;
   for (int i = 1; i < N; i++) {
     dummy = double(Ntot) - 0.5;
     dummy -=
sqrt((2*double(Ntot)-1)*(2*double(Ntot)-1)-4*((2*double(Ntot)-n[i-1])*(n[i-1]-1)+double(Ntot)*(double(Ntot)-1)/N))/2.0;
     n[i] = int(dummy);
   }

   return;

}


/*
 * Copyright 2020 Makani Technologies LLC
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef SIM_PHYSICS_AERO_H_
#define SIM_PHYSICS_AERO_H_

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <stdint.h>

#include <memory>
#include <string>
#include <vector>

#include "common/c_math/force_moment.h"
#include "common/c_math/linalg.h"
#include "common/c_math/vec3.h"
#include "common/macros.h"
#include "control/system_types.h"
#include "sim/physics/aero_types.h"
#include "sim/physics/avl_aero_database.h"
#include "sim/physics/delta_coeff_aero_database.h"
#include "sim/physics/dvl_aero_database.h"
#include "sim/sim_types.h"

class Aero {
  friend class AeroTest;

 public:
  explicit Aero(const AeroSimParams &aero_sim_params);
  ~Aero() {}

  // Calculates the force-moment coefficient for a given (alpha, beta,
  // omega_hat, Reynolds number, flap deflection) operating point,
  // taking into account information from multiple databases.
  void CalcForceMomentCoeff(double alpha, double beta, const Vec3 &omega_hat,
                            const Vec &flaps, double reynolds_number,
                            ForceMoment *force_moment_coeff,
                            double thrust_coeff) const {
    CalcForceMomentCoeff(alpha, beta, omega_hat, flaps, reynolds_number,
                         force_moment_coeff, thrust_coeff, nullptr);
  }

  // See function above.  Optionally returns the breakdown of the
  // force-moment coefficient into stability and control derivatives
  // if raw_aero_coeffs is not null.
  void CalcForceMomentCoeff(double alpha, double beta, const Vec3 &omega_hat,
                            const Vec &flaps, double reynolds_number,
                            ForceMoment *force_moment_coeff,
                            double thrust_coeff,
                            RawAeroCoeffs *raw_aero_coeffs) const;

  void CalcLowIncidenceCoeffs(double alpha, double beta, const Vec3 &omega_hat,
                              const Vec &flaps, double reynolds_number,
                              ForceMoment *force_moment_coeff,
                              double thrust_coeff) const;

  void CalcHighIncidenceCoeffs(double alpha, double beta, const Vec3 &omega_hat,
                               const Vec &flaps, double reynolds_number,
                               ForceMoment *force_moment_coeff,
                               double thrust_coeff) const;

  double CalcHingeCM(FlapLabel label, double alpha, double delta) const;

  double CalcEmpiricalPitchingMoment(double alpha) const;

 private:
  // Returns true if the parameters to the aerodynamic model pass
  // basic consistency checks.
  bool ValidateParams(const AeroSimParams &params) const;

  // Linearly interpolates the force-moment coefficients between two
  // aerodynamics databases based on Reynolds number.  Returns the
  // weight used to mix the databases of the lower Reynolds database.
  template <typename Database, typename DatabaseCoeffs>
  double InterpolateAeroDatabases(
      double alpha, double beta, const Vec3 &omega_hat, const Vec &flaps,
      double reynolds_number,
      const std::vector<std::unique_ptr<Database>> &databases,
      DatabaseCoeffs *lower_coeffs, DatabaseCoeffs *upper_coeffs,
      ForceMoment *cfm, double thrust_coeff) const;
  const AeroSimParams &aero_sim_params_;

  // Set of databases to use when the flaps are deflected in the
  // linear range.  These databases are typically used during
  // crosswind flight.
  std::vector<std::unique_ptr<AvlAeroDatabase>> small_deflection_databases_;

  // Set of databases to use when the flaps may be deflected a large
  // amount, outside the linear range.  These databases are typically
  // used during hover and transition when the elevator may be
  // feathered.
  std::vector<std::unique_ptr<DvlAeroDatabase>> large_deflection_databases_;

  // Set of databases to use when the spoilers are deflected to
  // max range.  These databases are typically used during
  // crosswind flight, during high speed events.
  std::unique_ptr<DeltaCoeffAeroDatabase> spoiler_offset_database_;

  DISALLOW_COPY_AND_ASSIGN(Aero);
};

#endif  // SIM_PHYSICS_AERO_H_


/*
 * 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 Sun Nov  7 20:44:04 EST 1999 */

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

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

/*
 * This function contains 32 FP additions, 12 FP multiplications,
 * (or, 26 additions, 6 multiplications, 6 fused multiply/add),
 * 16 stack variables, and 20 memory accesses
 */
static const fftw_real K250000000 = FFTW_KONST(+0.250000000000000000000000000000000000000000000);
static const fftw_real K951056516 = FFTW_KONST(+0.951056516295153572116439333379382143405698634);
static const fftw_real K587785252 = FFTW_KONST(+0.587785252292473129168705954639072768597652438);
static const fftw_real K559016994 = FFTW_KONST(+0.559016994374947424102293417182819058860154590);

/*
 * Generator Id's :
 * $Id: exprdag.ml,v 1.41 1999/05/26 15:44:14 fftw Exp $
 * $Id: fft.ml,v 1.43 1999/05/17 19:44:18 fftw Exp $
 * $Id: to_c.ml,v 1.25 1999/10/26 21:41:32 stevenj Exp $
 */

void fftwi_no_twiddle_5(const fftw_complex *input, fftw_complex *output, int istride, int ostride)
{
     fftw_real tmp1;
     fftw_real tmp27;
     fftw_real tmp8;
     fftw_real tmp10;
     fftw_real tmp21;
     fftw_real tmp22;
     fftw_real tmp14;
     fftw_real tmp28;
     fftw_real tmp26;
     fftw_real tmp17;
     ASSERT_ALIGNED_DOUBLE;
     tmp1 = c_re(input[0]);
     tmp27 = c_im(input[0]);
     {
	  fftw_real tmp2;
	  fftw_real tmp3;
	  fftw_real tmp4;
	  fftw_real tmp5;
	  fftw_real tmp6;
	  fftw_real tmp7;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp2 = c_re(input[istride]);
	  tmp3 = c_re(input[4 * istride]);
	  tmp4 = tmp2 + tmp3;
	  tmp5 = c_re(input[2 * istride]);
	  tmp6 = c_re(input[3 * istride]);
	  tmp7 = tmp5 + tmp6;
	  tmp8 = tmp4 + tmp7;
	  tmp10 = K559016994 * (tmp4 - tmp7);
	  tmp21 = tmp2 - tmp3;
	  tmp22 = tmp5 - tmp6;
     }
     {
	  fftw_real tmp12;
	  fftw_real tmp13;
	  fftw_real tmp24;
	  fftw_real tmp15;
	  fftw_real tmp16;
	  fftw_real tmp25;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp12 = c_im(input[istride]);
	  tmp13 = c_im(input[4 * istride]);
	  tmp24 = tmp12 + tmp13;
	  tmp15 = c_im(input[2 * istride]);
	  tmp16 = c_im(input[3 * istride]);
	  tmp25 = tmp15 + tmp16;
	  tmp14 = tmp12 - tmp13;
	  tmp28 = tmp24 + tmp25;
	  tmp26 = K559016994 * (tmp24 - tmp25);
	  tmp17 = tmp15 - tmp16;
     }
     c_re(output[0]) = tmp1 + tmp8;
     {
	  fftw_real tmp18;
	  fftw_real tmp20;
	  fftw_real tmp11;
	  fftw_real tmp19;
	  fftw_real tmp9;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp18 = (K587785252 * tmp14) - (K951056516 * tmp17);
	  tmp20 = (K951056516 * tmp14) + (K587785252 * tmp17);
	  tmp9 = tmp1 - (K250000000 * tmp8);
	  tmp11 = tmp9 - tmp10;
	  tmp19 = tmp10 + tmp9;
	  c_re(output[2 * ostride]) = tmp11 - tmp18;
	  c_re(output[3 * ostride]) = tmp11 + tmp18;
	  c_re(output[ostride]) = tmp19 - tmp20;
	  c_re(output[4 * ostride]) = tmp19 + tmp20;
     }
     c_im(output[0]) = tmp27 + tmp28;
     {
	  fftw_real tmp23;
	  fftw_real tmp31;
	  fftw_real tmp30;
	  fftw_real tmp32;
	  fftw_real tmp29;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp23 = (K951056516 * tmp21) + (K587785252 * tmp22);
	  tmp31 = (K587785252 * tmp21) - (K951056516 * tmp22);
	  tmp29 = tmp27 - (K250000000 * tmp28);
	  tmp30 = tmp26 + tmp29;
	  tmp32 = tmp29 - tmp26;
	  c_im(output[ostride]) = tmp23 + tmp30;
	  c_im(output[4 * ostride]) = tmp30 - tmp23;
	  c_im(output[2 * ostride]) = tmp31 + tmp32;
	  c_im(output[3 * ostride]) = tmp32 - tmp31;
     }
}

fftw_codelet_desc fftwi_no_twiddle_5_desc =
{
     "fftwi_no_twiddle_5",
     (void (*)()) fftwi_no_twiddle_5,
     5,
     FFTW_BACKWARD,
     FFTW_NOTW,
     122,
     0,
     (const int *) 0,
};


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

#include <gsl/gsl_errno.h>
#include <gsl/gsl_integration.h>

#include "ccl.h"

#ifdef HAVE_ANGPOW
#include "Angpow/angpow_ccl.h"
#endif

#define CCL_FRAC_RELEVANT 5E-4
//#define CCL_FRAC_RELEVANT 1E-3
//Gets the x-interval where the values of y are relevant
//(meaning, that the values of y for those x are at least above a fraction frac of its maximum)
static void get_support_interval(int n,double *x,double *y,double frac,
				 double *xmin_out,double *xmax_out)
{
  int ix;
  double ythr=-1000;

  //Initialize as the original edges in case we don't find an interval
  *xmin_out=x[0];
  *xmax_out=x[n-1];

  //Find threshold
  for(ix=0;ix<n;ix++) {
    if(y[ix]>ythr) ythr=y[ix];
  }
  ythr*=frac;

  //Find minimum
  for(ix=0;ix<n;ix++) {
    if(y[ix]>=ythr) {
      *xmin_out=x[ix];
      break;
    }
  }

  //Find maximum
  for(ix=n-1;ix>=0;ix--) {
    if(y[ix]>=ythr) {
      *xmax_out=x[ix];
      break;
    }
  }
}

//Wrapper around spline_eval with GSL function syntax
static double speval_bis(double x,void *params)
{
  return ccl_spline_eval(x,(SplPar *)params);
}

void ccl_cl_workspace_free(CCL_ClWorkspace *w)
{
  free(w->l_arr);
  free(w);
}

CCL_ClWorkspace *ccl_cl_workspace_default(int lmax,int l_limber,int non_limber_method,
					  double l_logstep,int l_linstep,
					  double dchi,double dlk,double zmin,int *status)
{
  CCL_ClWorkspace *w=(CCL_ClWorkspace *)malloc(sizeof(CCL_ClWorkspace));
  if(w==NULL) {
    *status=CCL_ERROR_MEMORY;
    //Can't access cosmology object
    //    ccl_cosmology_set_status_message(cosmo, "ccl_cls.c: ccl_cl_workspace_default(); memory allocation\n");
    return NULL;
  }

  //Set params
  w->dchi=dchi;
  w->dlk =dlk ;
  if(zmin<=0) { //We should make sure that zmin is always strictly positive
    free(w);
    *status=CCL_ERROR_INCONSISTENT;
    return NULL;
  }
  w->zmin=zmin;
  w->lmax=lmax;
  if((non_limber_method!=CCL_NONLIMBER_METHOD_NATIVE) && (non_limber_method!=CCL_NONLIMBER_METHOD_ANGPOW)) {
    free(w);
    *status=CCL_ERROR_INCONSISTENT;
    //Can't access cosmology object
    //    ccl_cosmology_set_status_message(cosmo, "ccl_cls.c: ccl_cl_workspace_default(); unknown non-limber method\n");
    return NULL;
  }
  w->nlimb_method=non_limber_method;
  w->l_limber=l_limber;
  w->l_logstep=l_logstep;
  w->l_linstep=l_linstep;

  //Compute number of multipoles
  int i_l=0,l0=0;
  int increment=CCL_MAX(((int)(l0*(w->l_logstep-1.))),1);
  while((l0 < w->lmax) && (increment < w->l_linstep)) {
    i_l++;
    l0+=increment;
    increment=CCL_MAX(((int)(l0*(w->l_logstep-1))),1);
  }
  increment=w->l_linstep;
  while(l0 < w->lmax) {
    i_l++;
    l0+=increment;
  }

  //Allocate array of multipoles
  w->n_ls=i_l+1;
  w->l_arr=(int *)malloc(w->n_ls*sizeof(int));
  if(w->l_arr==NULL) {
    free(w);
    *status=CCL_ERROR_MEMORY;
    //Can't access cosmology object
    //    ccl_cosmology_set_status_message(cosmo, "ccl_cls.c: ccl_cl_workspace_default(); memory allocation\n");
    return NULL;
  }

  //Redo the computation above and store values of ell
  i_l=0; l0=0;
  increment=CCL_MAX(((int)(l0*(w->l_logstep-1.))),1);
  while((l0 < w->lmax) && (increment < w->l_linstep)) {
    w->l_arr[i_l]=l0;
    i_l++;
    l0+=increment;
    increment=CCL_MAX(((int)(l0*(w->l_logstep-1))),1);
  }
  increment=w->l_linstep;
  while(l0 < w->lmax) {
    w->l_arr[i_l]=l0;
    i_l++;
    l0+=increment;
  }
  //Don't go further than lmaw
  w->l_arr[w->n_ls-1]=w->lmax;

  return w;
}

CCL_ClWorkspace *ccl_cl_workspace_default_limber(int lmax,double l_logstep,int l_linstep,
						 double dlk,int *status)
{
  return ccl_cl_workspace_default(lmax,-1,CCL_NONLIMBER_METHOD_NATIVE,l_logstep,l_linstep,3.,dlk,0.05,status);
}

//Params for lensing kernel integrand
typedef struct {
  double chi;
  SplPar *spl_pz;
  ccl_cosmology *cosmo;
  int *status;
} IntLensPar;

//Integrand for lensing kernel
static double integrand_wl(double chip,void *params)
{
  IntLensPar *p=(IntLensPar *)params;
  double chi=p->chi;
  double a=ccl_scale_factor_of_chi(p->cosmo,chip, p->status);
  double z=1./a-1;
  double pz=ccl_spline_eval(z,p->spl_pz);
  double h=p->cosmo->params.h*ccl_h_over_h0(p->cosmo,a, p->status)/CLIGHT_HMPC;

  if(chi==0)
    return h*pz;
  else
    return h*pz*ccl_sinn(p->cosmo,chip-chi,p->status)/ccl_sinn(p->cosmo,chip,p->status);
}

//Integral to compute lensing window function
//chi     -> comoving distance
//cosmo   -> ccl_cosmology object
//spl_pz  -> normalized N(z) spline
//chi_max -> maximum comoving distance to which the integral is computed
//win     -> result is stored here
static int window_lensing(double chi,ccl_cosmology *cosmo,SplPar *spl_pz,double chi_max,double *win)
{
  int gslstatus =0, status =0;
  double result,eresult;
  IntLensPar ip;
  gsl_function F;
  gsl_integration_workspace *w=gsl_integration_workspace_alloc(ccl_gsl->N_ITERATION);

  ip.chi=chi;
  ip.cosmo=cosmo;
  ip.spl_pz=spl_pz;
  ip.status = &status;
  F.function=&integrand_wl;
  F.params=&ip;
  gslstatus=gsl_integration_qag(&F, chi, chi_max, 0,
                                ccl_gsl->INTEGRATION_EPSREL, ccl_gsl->N_ITERATION,
                                ccl_gsl->INTEGRATION_GAUSS_KRONROD_POINTS,
                                w, &result, &eresult);
  *win=result;
  gsl_integration_workspace_free(w);
  if(gslstatus!=GSL_SUCCESS || *ip.status) {
    ccl_raise_gsl_warning(gslstatus, "ccl_cls.c: window_lensing():");
    return 1;
  }
  //TODO: chi_max should be changed to chi_horizon
  //we should precompute this quantity and store it in cosmo by default

  return 0;
}

//Params for lensing kernel integrand
typedef struct {
  double chi;
  SplPar *spl_pz;
  SplPar *spl_sz;
  ccl_cosmology *cosmo;
  int *status;
} IntMagPar;

//Integrand for magnification kernel
static double integrand_mag(double chip,void *params)
{
  IntMagPar *p=(IntMagPar *)params;
//EK: added local status here as the status testing is done in routines called from this function
  double chi=p->chi;
  double a=ccl_scale_factor_of_chi(p->cosmo,chip, p->status);
  double z=1./a-1;
  double pz=ccl_spline_eval(z,p->spl_pz);
  double sz=ccl_spline_eval(z,p->spl_sz);
  double h=p->cosmo->params.h*ccl_h_over_h0(p->cosmo,a, p->status)/CLIGHT_HMPC;

  if(chi==0)
    return h*pz*(1-2.5*sz);
  else
    return h*pz*(1-2.5*sz)*ccl_sinn(p->cosmo,chip-chi,p->status)/ccl_sinn(p->cosmo,chip,p->status);
}

//Integral to compute magnification window function
//chi     -> comoving distance
//cosmo   -> ccl_cosmology object
//spl_pz  -> normalized N(z) spline
//spl_pz  -> magnification bias s(z)
//chi_max -> maximum comoving distance to which the integral is computed
//win     -> result is stored here
static int window_magnification(double chi,ccl_cosmology *cosmo,SplPar *spl_pz,SplPar *spl_sz,
				double chi_max,double *win)
{
  int gslstatus =0, status =0;
  double result,eresult;
  IntMagPar ip;
  gsl_function F;
  gsl_integration_workspace *w=gsl_integration_workspace_alloc(ccl_gsl->N_ITERATION);

  ip.chi=chi;
  ip.cosmo=cosmo;
  ip.spl_pz=spl_pz;
  ip.spl_sz=spl_sz;
  ip.status = &status;
  F.function=&integrand_mag;
  F.params=&ip;
  gslstatus=gsl_integration_qag(&F, chi, chi_max, 0,
                                ccl_gsl->INTEGRATION_EPSREL, ccl_gsl->N_ITERATION,
                                ccl_gsl->INTEGRATION_GAUSS_KRONROD_POINTS,
                                w, &result, &eresult);
  *win=result;
  gsl_integration_workspace_free(w);
  if(gslstatus!=GSL_SUCCESS || *ip.status) {
    ccl_raise_gsl_warning(gslstatus, "ccl_cls.c: window_magnification():");
    return 1;
  }
  //TODO: chi_max should be changed to chi_horizon
  //we should precompute this quantity and store it in cosmo by default

  return 0;
}

//CCL_ClTracer creator
//cosmo   -> ccl_cosmology object
//tracer_type -> type of tracer. Supported: CL_TRACER_NC, CL_TRACER_WL
//nz_n -> number of points for N(z)
//z_n  -> array of z-values for N(z)
//n    -> corresponding N(z)-values. Normalization is irrelevant
//        N(z) will be set to zero outside the range covered by z_n
//nz_b -> number of points for b(z)
//z_b  -> array of z-values for b(z)
//b    -> corresponding b(z)-values.
//        b(z) will be assumed constant outside the range covered by z_n
static CCL_ClTracer *cl_tracer(ccl_cosmology *cosmo,int tracer_type,
				   int has_rsd,int has_magnification,int has_intrinsic_alignment,
				   int nz_n,double *z_n,double *n,
				   int nz_b,double *z_b,double *b,
				   int nz_s,double *z_s,double *s,
				   int nz_ba,double *z_ba,double *ba,
				   int nz_rf,double *z_rf,double *rf,
				   double z_source, int * status)
{
  int clstatus=0, gslstatus;
  CCL_ClTracer *clt=(CCL_ClTracer *)malloc(sizeof(CCL_ClTracer));
  if(clt==NULL) {

    *status=CCL_ERROR_MEMORY;
    ccl_cosmology_set_status_message(cosmo, "ccl_cls.c: ccl_cl_tracer(): memory allocation\n");
    return NULL;
  }

  if ( ((cosmo->params.N_nu_mass)>0) && tracer_type==CL_TRACER_NC && has_rsd){
	  free(clt);
	  *status=CCL_ERROR_NOT_IMPLEMENTED;
	  ccl_cosmology_set_status_message(cosmo, "ccl_cls.c: ccl_cl_tracer_new(): Number counts tracers with rsd not yet implemented in cosmologies with massive neutrinos.");
	  return NULL;
  }

  clt->tracer_type=tracer_type;
  clt->computed_transfer=0;

  double hub=cosmo->params.h*ccl_h_over_h0(cosmo,1.,status)/CLIGHT_HMPC;
  clt->prefac_lensing=1.5*hub*hub*cosmo->params.Omega_m;

  if((tracer_type==CL_TRACER_NC)||(tracer_type==CL_TRACER_WL)) {
    get_support_interval(nz_n,z_n,n,CCL_FRAC_RELEVANT,&(clt->zmin),&(clt->zmax));
    clt->chimax=ccl_comoving_radial_distance(cosmo,1./(1+clt->zmax),status);
    clt->chimin=ccl_comoving_radial_distance(cosmo,1./(1+clt->zmin),status);
    clt->spl_nz=ccl_spline_init(nz_n,z_n,n,0,0);
    if(clt->spl_nz==NULL) {
      free(clt);
      *status=CCL_ERROR_SPLINE;
      ccl_cosmology_set_status_message(cosmo, "ccl_cls.c: ccl_cl_tracer(): error initializing spline for N(z)\n");
      return NULL;
    }

    //Normalize n(z)
    gsl_function F;
    double nz_norm,nz_enorm;
    double *nz_normalized=(double *)malloc(nz_n*sizeof(double));
    if(nz_normalized==NULL) {
      ccl_spline_free(clt->spl_nz);
      free(clt);
      *status=CCL_ERROR_MEMORY;
      ccl_cosmology_set_status_message(cosmo, "ccl_cls.c: ccl_cl_tracer(): memory allocation\n");
      return NULL;
    }

    gsl_integration_workspace *w=gsl_integration_workspace_alloc(ccl_gsl->N_ITERATION);
    F.function=&speval_bis;
    F.params=clt->spl_nz;
    gslstatus=gsl_integration_qag(&F, z_n[0], z_n[nz_n-1], 0,
                                  ccl_gsl->INTEGRATION_EPSREL, ccl_gsl->N_ITERATION,
                                  ccl_gsl->INTEGRATION_GAUSS_KRONROD_POINTS,
                                  w, &nz_norm, &nz_enorm);
    gsl_integration_workspace_free(w);
    if(gslstatus!=GSL_SUCCESS) {
      ccl_raise_gsl_warning(gslstatus, "ccl_cls.c: cl_tracer():");
      ccl_spline_free(clt->spl_nz);
      free(clt);
      *status=CCL_ERROR_INTEG;
      ccl_cosmology_set_status_message(cosmo, "ccl_cls.c: ccl_cl_tracer(): integration error when normalizing N(z)\n");
      return NULL;
    }
    for(int ii=0;ii<nz_n;ii++)
      nz_normalized[ii]=n[ii]/nz_norm;
    ccl_spline_free(clt->spl_nz);
    clt->spl_nz=ccl_spline_init(nz_n,z_n,nz_normalized,0,0);
    free(nz_normalized);
    if(clt->spl_nz==NULL) {
      free(clt);
      *status=CCL_ERROR_SPLINE;
      ccl_cosmology_set_status_message(cosmo, "ccl_cls.c: ccl_cl_tracer(): error initializing normalized spline for N(z)\n");
      return NULL;
    }

    if(tracer_type==CL_TRACER_NC) {
      //Initialize bias spline
      clt->spl_bz=ccl_spline_init(nz_b,z_b,b,b[0],b[nz_b-1]);
      if(clt->spl_bz==NULL) {
	ccl_spline_free(clt->spl_nz);
	free(clt);
	*status=CCL_ERROR_SPLINE;
	ccl_cosmology_set_status_message(cosmo, "ccl_cls.c: ccl_cl_tracer(): error initializing spline for b(z)\n");
	return NULL;
      }
      clt->has_rsd=has_rsd;
      clt->has_magnification=has_magnification;
      if(clt->has_magnification) {
	//Compute weak lensing kernel
	int nchi;
	double *x,*y;
	double dchi_here=5.;
	double zmax=clt->spl_nz->xf;
	double chimax=ccl_comoving_radial_distance(cosmo,1./(1+zmax),status);
	//TODO: The interval in chi (5. Mpc) should be made a macro

	//In this case we need to integrate all the way to z=0. Reset zmin and chimin
	clt->zmin=0;
	clt->chimin=0;
	clt->spl_sz=ccl_spline_init(nz_s,z_s,s,s[0],s[nz_s-1]);
	if(clt->spl_sz==NULL) {
	  ccl_spline_free(clt->spl_nz);
	  ccl_spline_free(clt->spl_bz);
	  free(clt);
	  *status=CCL_ERROR_SPLINE;
	  ccl_cosmology_set_status_message(cosmo,
		 "ccl_cls.c: ccl_cl_tracer(): error initializing spline for s(z)\n");
	  return NULL;
	}

	nchi=(int)(chimax/dchi_here)+1;
	x=ccl_linear_spacing(0.,chimax,nchi);
	dchi_here=chimax/nchi;
	if(x==NULL || (fabs(x[0]-0)>1E-5) || (fabs(x[nchi-1]-chimax)>1e-5)) {
	  ccl_spline_free(clt->spl_nz);
	  ccl_spline_free(clt->spl_bz);
	  ccl_spline_free(clt->spl_sz);
	  free(clt);
	  *status=CCL_ERROR_LINSPACE;
	  ccl_cosmology_set_status_message(cosmo,
		 "ccl_cls.c: ccl_cl_tracer(): Error creating linear spacing in chi\n");
	  return NULL;
	}
	y=(double *)malloc(nchi*sizeof(double));
	if(y==NULL) {
	  free(x);
	  ccl_spline_free(clt->spl_nz);
	  ccl_spline_free(clt->spl_bz);
	  ccl_spline_free(clt->spl_sz);
	  free(clt);
	  *status=CCL_ERROR_MEMORY;
	  ccl_cosmology_set_status_message(cosmo, "ccl_cls.c: ccl_cl_tracer(): memory allocation\n");
	  return NULL;
	}

	for(int j=0;j<nchi;j++)
	  clstatus|=window_magnification(x[j],cosmo,clt->spl_nz,clt->spl_sz,chimax,&(y[j]));
	if(clstatus) {
	  free(y);
	  free(x);
	  ccl_spline_free(clt->spl_nz);
	  ccl_spline_free(clt->spl_bz);
	  ccl_spline_free(clt->spl_sz);
	  free(clt);
	  *status=CCL_ERROR_INTEG;
	  ccl_cosmology_set_status_message(cosmo, "ccl_cls.c: ccl_cl_tracer(): error computing lensing window\n");
	  return NULL;
	}

	clt->spl_wM=ccl_spline_init(nchi,x,y,y[0],0);
	if(clt->spl_wM==NULL) {
	  free(y);
	  free(x);
	  ccl_spline_free(clt->spl_nz);
	  ccl_spline_free(clt->spl_bz);
	  ccl_spline_free(clt->spl_sz);
	  free(clt);
	  *status=CCL_ERROR_SPLINE;
	  ccl_cosmology_set_status_message(cosmo,
		 "ccl_cls.c: ccl_cl_tracer(): error initializing spline for lensing window\n");
	  return NULL;
	}
	free(x); free(y);
      }
    }
    else if(tracer_type==CL_TRACER_WL) {
      //Compute weak lensing kernel
      int nchi;
      double *x,*y;
      double dchi_here=5.;
      double zmax=clt->spl_nz->xf;
      double chimax=ccl_comoving_radial_distance(cosmo,1./(1+zmax),status);
      //TODO: The interval in chi (5. Mpc) should be made a macro

      //In this case we need to integrate all the way to z=0. Reset zmin and chimin
      clt->zmin=0;
      clt->chimin=0;
      nchi=(int)(chimax/dchi_here)+1;
      x=ccl_linear_spacing(0.,chimax,nchi);
      dchi_here=chimax/nchi;
      if(x==NULL || (fabs(x[0]-0)>1E-5) || (fabs(x[nchi-1]-chimax)>1e-5)) {
	ccl_spline_free(clt->spl_nz);
	free(clt);
	*status=CCL_ERROR_LINSPACE;
	ccl_cosmology_set_status_message(cosmo,
	       "ccl_cls.c: ccl_cl_tracer(): Error creating linear spacing in chi\n");
	return NULL;
      }
      y=(double *)malloc(nchi*sizeof(double));
      if(y==NULL) {
	free(x);
	ccl_spline_free(clt->spl_nz);
	free(clt);
	*status=CCL_ERROR_MEMORY;
	ccl_cosmology_set_status_message(cosmo, "ccl_cls.c: ccl_cl_tracer(): memory allocation\n");
	return NULL;
      }

      for(int j=0;j<nchi;j++)
	clstatus|=window_lensing(x[j],cosmo,clt->spl_nz,chimax,&(y[j]));
      if(clstatus) {
	free(y);
	free(x);
	ccl_spline_free(clt->spl_nz);
	free(clt);
	*status=CCL_ERROR_INTEG;
	ccl_cosmology_set_status_message(cosmo, "ccl_cls.c: ccl_cl_tracer(): error computing lensing window\n");
	return NULL;
      }

      clt->spl_wL=ccl_spline_init(nchi,x,y,y[0],0);
      if(clt->spl_wL==NULL) {
	free(y);
	free(x);
	ccl_spline_free(clt->spl_nz);
	free(clt);
	*status=CCL_ERROR_SPLINE;
	ccl_cosmology_set_status_message(cosmo,
	       "ccl_cls.c: ccl_cl_tracer(): error initializing spline for lensing window\n");
	return NULL;
      }
      free(x); free(y);

      clt->has_intrinsic_alignment=has_intrinsic_alignment;
      if(clt->has_intrinsic_alignment) {
	clt->spl_rf=ccl_spline_init(nz_rf,z_rf,rf,rf[0],rf[nz_rf-1]);
	if(clt->spl_rf==NULL) {
	  ccl_spline_free(clt->spl_nz);
	  free(clt);
	  *status=CCL_ERROR_SPLINE;
	  ccl_cosmology_set_status_message(cosmo,
		 "ccl_cls.c: ccl_cl_tracer(): error initializing spline for rf(z)\n");
	  return NULL;
	}
	clt->spl_ba=ccl_spline_init(nz_ba,z_ba,ba,ba[0],ba[nz_ba-1]);
	if(clt->spl_ba==NULL) {
	  ccl_spline_free(clt->spl_rf);
	  ccl_spline_free(clt->spl_nz);
	  free(clt);
	  *status=CCL_ERROR_SPLINE;
	  ccl_cosmology_set_status_message(cosmo,
		 "ccl_cls.c: ccl_cl_tracer(): error initializing spline for ba(z)\n");
	  return NULL;
	}
      }
    }
  }
  else if(tracer_type==CL_TRACER_CL) {
    clt->chi_source=ccl_comoving_radial_distance(cosmo,1./(1+z_source),status);
    clt->chimax=clt->chi_source;
    clt->chimin=0;
  }
  else {
    *status=CCL_ERROR_INCONSISTENT;
    ccl_cosmology_set_status_message(cosmo, "ccl_cls.c: ccl_cl_tracer(): unknown tracer type\n");
    return NULL;
  }

  return clt;
}

//CCL_ClTracer constructor with error checking
//cosmo   -> ccl_cosmology object
//tracer_type -> type of tracer. Supported: CL_TRACER_NC, CL_TRACER_WL
//nz_n -> number of points for N(z)
//z_n  -> array of z-values for N(z)
//n    -> corresponding N(z)-values. Normalization is irrelevant
//        N(z) will be set to zero outside the range covered by z_n
//nz_b -> number of points for b(z)
//z_b  -> array of z-values for b(z)
//b    -> corresponding b(z)-values.
//        b(z) will be assumed constant outside the range covered by z_n
CCL_ClTracer *ccl_cl_tracer(ccl_cosmology *cosmo,int tracer_type,
				int has_rsd,int has_magnification,int has_intrinsic_alignment,
				int nz_n,double *z_n,double *n,
				int nz_b,double *z_b,double *b,
				int nz_s,double *z_s,double *s,
				int nz_ba,double *z_ba,double *ba,
				int nz_rf,double *z_rf,double *rf,
				double z_source, int * status)
{
  CCL_ClTracer *clt=cl_tracer(cosmo,tracer_type,has_rsd,has_magnification,has_intrinsic_alignment,
			      nz_n,z_n,n,nz_b,z_b,b,nz_s,z_s,s,
			      nz_ba,z_ba,ba,nz_rf,z_rf,rf,z_source,status);
  ccl_check_status(cosmo,status);
  return clt;
}

//CCL_ClTracer destructor
void ccl_cl_tracer_free(CCL_ClTracer *clt)
{
  if((clt->tracer_type==CL_TRACER_NC) || (clt->tracer_type==CL_TRACER_WL))
    ccl_spline_free(clt->spl_nz);

  if(clt->tracer_type==CL_TRACER_NC) {
    ccl_spline_free(clt->spl_bz);
    if(clt->has_magnification) {
      ccl_spline_free(clt->spl_sz);
      ccl_spline_free(clt->spl_wM);
    }
  }
  else if(clt->tracer_type==CL_TRACER_WL) {
    ccl_spline_free(clt->spl_wL);
    if(clt->has_intrinsic_alignment) {
      ccl_spline_free(clt->spl_ba);
      ccl_spline_free(clt->spl_rf);
    }
  }
  if(clt->computed_transfer) {
    int il;
    free(clt->n_k);
    for(il=0;il<clt->n_ls;il++)
      ccl_spline_free(clt->spl_transfer[il]);
    free(clt->spl_transfer);
  }
  free(clt);
}

CCL_ClTracer *ccl_cl_tracer_cmblens(ccl_cosmology *cosmo,double z_source,int *status)
{
  return ccl_cl_tracer(cosmo,CL_TRACER_CL,
			   0,0,0,
			   0,NULL,NULL,0,NULL,NULL,0,NULL,NULL,
			   0,NULL,NULL,0,NULL,NULL,z_source,status);
}

CCL_ClTracer *ccl_cl_tracer_number_counts(ccl_cosmology *cosmo,
					      int has_rsd,int has_magnification,
					      int nz_n,double *z_n,double *n,
					      int nz_b,double *z_b,double *b,
					      int nz_s,double *z_s,double *s, int * status)
{
  return ccl_cl_tracer(cosmo,CL_TRACER_NC,has_rsd,has_magnification,0,
			   nz_n,z_n,n,nz_b,z_b,b,nz_s,z_s,s,
			   -1,NULL,NULL,-1,NULL,NULL,0, status);
}

CCL_ClTracer *ccl_cl_tracer_number_counts_simple(ccl_cosmology *cosmo,
						     int nz_n,double *z_n,double *n,
						     int nz_b,double *z_b,double *b, int * status)
{
  return ccl_cl_tracer(cosmo,CL_TRACER_NC,0,0,0,
			   nz_n,z_n,n,nz_b,z_b,b,-1,NULL,NULL,
			   -1,NULL,NULL,-1,NULL,NULL,0, status);
}

CCL_ClTracer *ccl_cl_tracer_lensing(ccl_cosmology *cosmo,
					int has_alignment,
					int nz_n,double *z_n,double *n,
					int nz_ba,double *z_ba,double *ba,
					int nz_rf,double *z_rf,double *rf, int * status)
{
  return ccl_cl_tracer(cosmo,CL_TRACER_WL,0,0,has_alignment,
			   nz_n,z_n,n,-1,NULL,NULL,-1,NULL,NULL,
			   nz_ba,z_ba,ba,nz_rf,z_rf,rf,0, status);
}

CCL_ClTracer *ccl_cl_tracer_lensing_simple(ccl_cosmology *cosmo,
					       int nz_n,double *z_n,double *n, int * status)
{
  return ccl_cl_tracer(cosmo,CL_TRACER_WL,0,0,0,
			   nz_n,z_n,n,-1,NULL,NULL,-1,NULL,NULL,
			   -1,NULL,NULL,-1,NULL,NULL,0, status);
}

static void limits_bessel(double l,double thr,double *xmin,double *xmax)
{
  double thrb=thr*0.5635/pow(l+0.53,0.834);
  *xmax=1./thrb;
  if(l<=0)
    *xmin=0;
  else {
    double logxmin=((l+1.)*(log(l+1.)+M_LN2-1.)+0.5*M_LN2+log(thrb))/l;
    *xmin=exp(logxmin);
  }
}

static double j_bessel_limber(int l,double k)
{
  return sqrt(M_PI/(2*l+1.))/k;
}

static double f_dens(double a,ccl_cosmology *cosmo,CCL_ClTracer *clt, int * status)
{
  double z=1./a-1;
  double pz=ccl_spline_eval(z,clt->spl_nz);
  double bz=ccl_spline_eval(z,clt->spl_bz);
  double h=cosmo->params.h*ccl_h_over_h0(cosmo,a,status)/CLIGHT_HMPC;

  return pz*bz*h;
}

static double f_rsd(double a,ccl_cosmology *cosmo,CCL_ClTracer *clt, int * status)
{
  double z=1./a-1;
  double pz=ccl_spline_eval(z,clt->spl_nz);
  double fg=ccl_growth_rate(cosmo,a,status);
  double h=cosmo->params.h*ccl_h_over_h0(cosmo,a,status)/CLIGHT_HMPC;

  return pz*fg*h;
}

static double f_mag(double a,double chi,ccl_cosmology *cosmo,CCL_ClTracer *clt, int * status)
{
  double wM=ccl_spline_eval(chi,clt->spl_wM);

  if(wM<=0)
    return 0;
  else
    return wM/(a*chi);
}

//Transfer function for number counts
//l -> angular multipole
//k -> wavenumber modulus
//cosmo -> ccl_cosmology object
//w -> CCL_ClWorskpace object
//clt -> CCL_ClTracer object (must be of the CL_TRACER_NC type)
static double transfer_nc(int l,double k,
			  ccl_cosmology *cosmo,CCL_ClWorkspace *w,CCL_ClTracer *clt, int * status)
{
  double ret=0;
  if(l>w->l_limber) {
    double x0=(l+0.5);
    double chi0=x0/k;
    if(chi0<=clt->chimax) {
      double a0=ccl_scale_factor_of_chi(cosmo,chi0,status);
      double pk0=ccl_nonlin_matter_power(cosmo,k,a0,status);
      double jl0=j_bessel_limber(l,k);
      double f_all=f_dens(a0,cosmo,clt,status)*jl0;
      if(clt->has_rsd) {
	double x1=(l+1.5);
	double chi1=x1/k;
	if(chi1<=clt->chimax) {
	  double a1=ccl_scale_factor_of_chi(cosmo,chi1,status);
	  double pk1=ccl_nonlin_matter_power(cosmo,k,a1,status);
	  double fg0=f_rsd(a0,cosmo,clt,status);
	  double fg1=f_rsd(a1,cosmo,clt,status);
	  double jl1=j_bessel_limber(l+1,k);
	  f_all+=fg0*(1.-l*(l-1.)/(x0*x0))*jl0-fg1*2.*jl1*sqrt(pk1/pk0)/x1;
	}
      }
      if(clt->has_magnification)
      	f_all+=-2*clt->prefac_lensing*l*(l+1)*f_mag(a0,chi0,cosmo,clt,status)*jl0/(k*k);
      ret=f_all*sqrt(pk0);
    }
  }
  else {
    int i,nchi=(int)((clt->chimax-clt->chimin)/w->dchi)+1;
    for(i=0;i<nchi;i++) {
      double chi=clt->chimin+w->dchi*(i+0.5);
      if(chi<=clt->chimax) {
	double a=ccl_scale_factor_of_chi(cosmo,chi,status);
	double pk=ccl_nonlin_matter_power(cosmo,k,a,status);
	double jl=ccl_j_bessel(l,k*chi);
	double f_all=f_dens(a,cosmo,clt,status)*jl;
	if(clt->has_rsd) {
	  double ddjl,x=k*chi;
	  if(x<1E-10) {
	    if(l==0) ddjl=0.3333-0.1*x*x;
	    else if(l==2) ddjl=-0.13333333333+0.05714285714285714*x*x;
	    else ddjl=0;
          }
	  else {
	    double jlp1=ccl_j_bessel(l+1,x);
	    ddjl=((x*x-l*(l-1))*jl-2*x*jlp1)/(x*x);
	  }
	  f_all+=f_rsd(a,cosmo,clt,status)*ddjl;
	}
	if(clt->has_magnification)
	  f_all+=-2*clt->prefac_lensing*l*(l+1)*f_mag(a,chi,cosmo,clt,status)*jl/(k*k);

	ret+=f_all*sqrt(pk); //TODO: is it worth splining this sqrt?
      }
    }
    ret*=w->dchi;
  }

  return ret;
}

static double f_lensing(double a,double chi,ccl_cosmology *cosmo,CCL_ClTracer *clt, int * status)
{
  double wL=ccl_spline_eval(chi,clt->spl_wL);

  if(wL<=0)
    return 0;
  else
    return clt->prefac_lensing*wL/(a*chi);
}

static double f_IA_NLA(double a,double chi,ccl_cosmology *cosmo,CCL_ClTracer *clt, int * status)
{
  if(chi<=1E-10)
    return 0;
  else {
    double a=ccl_scale_factor_of_chi(cosmo,chi, status);
    double z=1./a-1;
    double pz=ccl_spline_eval(z,clt->spl_nz);
    double ba=ccl_spline_eval(z,clt->spl_ba);
    double rf=ccl_spline_eval(z,clt->spl_rf);
    double h=cosmo->params.h*ccl_h_over_h0(cosmo,a,status)/CLIGHT_HMPC;

    return pz*ba*rf*h/(chi*chi);
  }
}

//Transfer function for shear
//l -> angular multipole
//k -> wavenumber modulus
//cosmo -> ccl_cosmology object
//w -> CCL_ClWorskpace object
//clt -> CCL_ClTracer object (must be of the CL_TRACER_WL type)
static double transfer_wl(int l,double k,
			  ccl_cosmology *cosmo,CCL_ClWorkspace *w,CCL_ClTracer *clt, int * status)
{
  double ret=0;
  if(l>w->l_limber) {
    double chi=(l+0.5)/k;
    if(chi<=clt->chimax) {
      double a=ccl_scale_factor_of_chi(cosmo,chi,status);
      double pk=ccl_nonlin_matter_power(cosmo,k,a,status);
      double jl=j_bessel_limber(l,k);
      double f_all=f_lensing(a,chi,cosmo,clt,status)*jl;
      if(clt->has_intrinsic_alignment)
	f_all+=f_IA_NLA(a,chi,cosmo,clt,status)*jl;

      ret=f_all*sqrt(pk);
    }
  }
  else {
    int i,nchi=(int)((clt->chimax-clt->chimin)/w->dchi)+1;
    for(i=0;i<nchi;i++) {
      double chi=clt->chimin+w->dchi*(i+0.5);
      if(chi<=clt->chimax) {
	double a=ccl_scale_factor_of_chi(cosmo,chi,status);
	double pk=ccl_nonlin_matter_power(cosmo,k,a,status);
	double jl=ccl_j_bessel(l,k*chi);
	double f_all=f_lensing(a,chi,cosmo,clt,status)*jl;
	if(clt->has_intrinsic_alignment)
	  f_all+=f_IA_NLA(a,chi,cosmo,clt,status)*jl;

	ret+=f_all*sqrt(pk); //TODO: is it worth splining this sqrt?
      }
    }
    ret*=w->dchi;
  }

  return sqrt((l+2.)*(l+1.)*l*(l-1.))*ret/(k*k);
  //return (l+1.)*l*ret/(k*k);
}

static double transfer_cmblens(int l,double k,ccl_cosmology *cosmo,CCL_ClTracer *clt,int *status)
{
  double chi=(l+0.5)/k;
  if(chi>=clt->chi_source)
    return 0;

  if(chi<=clt->chimax) {
    double a=ccl_scale_factor_of_chi(cosmo,chi,status);
    double w=1-chi/clt->chi_source;
    double jl=j_bessel_limber(l,k);
    double pk=ccl_nonlin_matter_power(cosmo,k,a,status);
    return clt->prefac_lensing*l*(l+1.)*w*sqrt(pk)*jl/(a*chi*k*k);
  }
  return 0;
}

//Wrapper for transfer function
//l -> angular multipole
//k -> wavenumber modulus
//cosmo -> ccl_cosmology object
//clt -> CCL_ClTracer object
static double transfer_wrap(int il,double lk,ccl_cosmology *cosmo,
			    CCL_ClWorkspace *w,CCL_ClTracer *clt, int * status)
{
  double transfer_out=0;
  double k=pow(10.,lk);

  if(clt->tracer_type==CL_TRACER_NC)
    transfer_out=transfer_nc(w->l_arr[il],k,cosmo,w,clt,status);
  else if(clt->tracer_type==CL_TRACER_WL)
    transfer_out=transfer_wl(w->l_arr[il],k,cosmo,w,clt,status);
  else if(clt->tracer_type==CL_TRACER_CL)
    transfer_out=transfer_cmblens(w->l_arr[il],k,cosmo,clt,status);
  else
    transfer_out=-1;
  return transfer_out;
}

static double *get_lkarr(ccl_cosmology *cosmo,CCL_ClWorkspace *w,
			 double l,double chimin,double chimax,int *nk,
			 int *status)
{
  int ik;

  //First compute relevant k-range for this ell
  double kmin,kmax,lkmin,lkmax;
  if(l>w->l_limber) {
    kmin=CCL_MAX(ccl_splines->K_MIN,0.8*(l+0.5)/chimax);
    kmax=CCL_MIN(ccl_splines->K_MAX,1.2*(l+0.5)/chimin);
  }
  else {
    double xmin,xmax;
    limits_bessel(l,CCL_FRAC_RELEVANT,&xmin,&xmax);
    kmin=CCL_MAX(ccl_splines->K_MIN,xmin/chimax);
    kmax=CCL_MIN(ccl_splines->K_MAX,xmax/chimin);
    //Cap by maximum meaningful argument of the Bessel function
    kmax=CCL_MIN(kmax,2*(w->l_arr[w->n_ls-1]+0.5)/chimin); //Cap by 2 x inverse scale corresponding to l_max
  }
  lkmin=log10(kmin);
  lkmax=log10(kmax);

  //Allocate memory for transfer function
  double *lkarr;
  double lknew=lkmin;
  double k_period=2*M_PI/chimax;
  double dklin=0.45;
  ik=0;
  while(lknew<=lkmax) {
    double kk=pow(10.,lknew);
    double dk1=k_period*dklin;
    double dk2=kk*(pow(10.,w->dlk)-1.);
    double dk=CCL_MIN(dk1,dk2);
    lknew=log10(kk+dk);
    ik++;
  }

  *nk=CCL_MAX(10,ik+1);
  lkarr=(double *)malloc((*nk)*sizeof(double));
  if(lkarr==NULL) {
    return NULL;
  }

  if((*nk)==10) {
    for(ik=0;ik<(*nk);ik++)
      lkarr[ik]=lkmin+(lkmax-lkmin)*ik/((*nk)-1.);
  }
  else {
    ik=0;
    lkarr[ik]=lkmin;
    while(lkarr[ik]<=lkmax) {
      double kk=pow(10.,lkarr[ik]);
      double dk1=k_period*dklin;
      double dk2=kk*(pow(10.,w->dlk)-1.);
      double dk=CCL_MIN(dk1,dk2);
      ik++;
      lkarr[ik]=log10(kk+dk);
    }
  }

  return lkarr;
}

static void compute_transfer(CCL_ClTracer *clt,ccl_cosmology *cosmo,CCL_ClWorkspace *w,int *status)
{
  int il;
  double zmin=CCL_MAX(w->zmin,clt->zmin);
  double chimin=ccl_comoving_radial_distance(cosmo,1./(1+zmin),status);
  double chimax=clt->chimax;

  //Get how many multipoles and allocate info for each of them
  clt->n_ls=w->n_ls;
  clt->n_k=(int *)malloc(clt->n_ls*sizeof(int));
  if(clt->n_k==NULL) {
    *status=CCL_ERROR_MEMORY;
    ccl_cosmology_set_status_message(cosmo, "ccl_cls.c: compute_transfer(): memory allocation\n");
    return;
  }
  clt->spl_transfer=(SplPar **)malloc(clt->n_ls*sizeof(SplPar *));
  if(clt->spl_transfer==NULL) {
    free(clt->n_k);
    *status=CCL_ERROR_MEMORY;
    ccl_cosmology_set_status_message(cosmo, "ccl_cls.c: compute_transfer(): memory allocation\n");
    return;
  }

  //Loop over multipoles and compute transfer function for each
  for(il=0;il<clt->n_ls;il++) {
    int ik,nk;
    double l=(double)(w->l_arr[il]);
    double *lkarr=get_lkarr(cosmo,w,l,chimin,chimax,&nk,status);
    if(lkarr==NULL) {
      *status=CCL_ERROR_MEMORY;
      ccl_cosmology_set_status_message(cosmo, "ccl_cls.c: compute_transfer(): memory allocation\n");
      break;
    }

    double *tkarr=(double *)malloc(nk*sizeof(double));
    if(tkarr==NULL) {
      free(lkarr);
      *status=CCL_ERROR_MEMORY;
      ccl_cosmology_set_status_message(cosmo, "ccl_cls.c: compute_transfer(): memory allocation\n");
      break;
    }
    clt->n_k[il]=nk;

    //Loop over k and compute transfer function
    for(ik=0;ik<nk;ik++)
      tkarr[ik]=transfer_wrap(il,lkarr[ik],cosmo,w,clt,status);
    if(*status) {
      free(clt->n_k);
      free(lkarr);
      free(tkarr);
      break;
    }

    //Initialize spline for this ell
    clt->spl_transfer[il]=ccl_spline_init(nk,lkarr,tkarr,0,0);
    if(clt->spl_transfer[il]==NULL) {
      free(lkarr);
      free(tkarr);
      *status=CCL_ERROR_MEMORY;
      ccl_cosmology_set_status_message(cosmo, "ccl_cls.c: compute_transfer(): memory allocation\n");
      break;
    }
    free(lkarr);
    free(tkarr);
  }
  if(*status) {
    free(clt->n_k);
    int ill;
    for(ill=0;ill<il;ill++)
      ccl_spline_free(clt->spl_transfer[il]);
    free(clt->spl_transfer);
  }

  clt->computed_transfer=1;
}

static double transfer(int il,double lk,ccl_cosmology *cosmo,
		       CCL_ClWorkspace *w,CCL_ClTracer *clt,int *status)
{
  if(il<w->l_limber) {
    if(!(clt->computed_transfer))
      compute_transfer(clt,cosmo,w,status);

    return ccl_spline_eval(lk,clt->spl_transfer[il]);
  } else {
    return transfer_wrap(il,lk,cosmo,w,clt,status);
  }
}

//Params for power spectrum integrand
typedef struct {
  int il;
  ccl_cosmology *cosmo;
  CCL_ClWorkspace *w;
  CCL_ClTracer *clt1;
  CCL_ClTracer *clt2;
  int *status;
} IntClPar;

//Integrand for integral power spectrum
static double cl_integrand(double lk,void *params)
{
  double d1,d2;
  IntClPar *p=(IntClPar *)params;
  d1=transfer(p->il,lk,p->cosmo,p->w,p->clt1,p->status);
  d2=transfer(p->il,lk,p->cosmo,p->w,p->clt2,p->status);

  return pow(10.,3*lk)*d1*d2;
}

//Figure out k intervals where the Limber kernel has support
//clt1 -> tracer #1
//clt2 -> tracer #2
//l    -> angular multipole
//lkmin, lkmax -> log10 of the range of scales where the transfer functions have support
static void get_k_interval(ccl_cosmology *cosmo,CCL_ClWorkspace *w,
			   CCL_ClTracer *clt1,CCL_ClTracer *clt2,int l,
			   double *lkmin,double *lkmax)
{
  double chimin,chimax;
  int cut_low_1=0,cut_low_2=0;

  //Define a minimum distance only if no lensing is needed
  if((clt1->tracer_type==CL_TRACER_NC) && (clt1->has_magnification==0)) cut_low_1=1;
  if((clt2->tracer_type==CL_TRACER_NC) && (clt2->has_magnification==0)) cut_low_2=1;

  if(l<w->l_limber) {
    chimin=2*(l+0.5)/ccl_splines->K_MAX;
    chimax=0.5*(l+0.5)/ccl_splines->K_MIN;
  }
  else {
    if(cut_low_1) {
      if(cut_low_2) {
	chimin=fmax(clt1->chimin,clt2->chimin);
	chimax=fmin(clt1->chimax,clt2->chimax);
      }
      else {
	chimin=clt1->chimin;
	chimax=clt1->chimax;
      }
    }
    else if(cut_low_2) {
      chimin=clt2->chimin;
      chimax=clt2->chimax;
    }
    else {
      chimin=0.5*(l+0.5)/ccl_splines->K_MAX;
      chimax=2*(l+0.5)/ccl_splines->K_MIN;
    }
  }

  if(chimin<=0)
    chimin=0.5*(l+0.5)/ccl_splines->K_MAX;

  *lkmax=fmin( 2,log10(2  *(l+0.5)/chimin));
  *lkmin=fmax(-4,log10(0.5*(l+0.5)/chimax));
}

//Compute angular power spectrum between two bins
//cosmo -> ccl_cosmology object
//il -> index in angular multipole array
//clt1 -> tracer #1
//clt2 -> tracer #2
static double ccl_angular_cl_native(ccl_cosmology *cosmo,CCL_ClWorkspace *cw,int il,
				    CCL_ClTracer *clt1,CCL_ClTracer *clt2,int * status)
{
  int clastatus=0, gslstatus;
  IntClPar ipar;
  double result=0,eresult;
  double lkmin,lkmax;
  gsl_function F;
  gsl_integration_workspace *w=gsl_integration_workspace_alloc(ccl_gsl->N_ITERATION);

  ipar.il=il;
  ipar.cosmo=cosmo;
  ipar.w=cw;
  ipar.clt1=clt1;
  ipar.clt2=clt2;
  ipar.status = &clastatus;
  F.function=&cl_integrand;
  F.params=&ipar;
  get_k_interval(cosmo,cw,clt1,clt2,cw->l_arr[il],&lkmin,&lkmax);
  gslstatus=gsl_integration_qag(&F, lkmin, lkmax, 0,
                                ccl_gsl->INTEGRATION_LIMBER_EPSREL, ccl_gsl->N_ITERATION,
                                ccl_gsl->INTEGRATION_LIMBER_GAUSS_KRONROD_POINTS,
                                w, &result, &eresult);
  gsl_integration_workspace_free(w);

  // Test if a round-off error occured in the evaluation of the integral
  // If so, try another integration function, more robust but potentially slower
  if(gslstatus == GSL_EROUND) {
    ccl_raise_gsl_warning(gslstatus, "ccl_cls.c: ccl_angular_cl_native(): Default GSL integration failure, attempting backup method.");
    gsl_integration_cquad_workspace *w_cquad= gsl_integration_cquad_workspace_alloc (ccl_gsl->N_ITERATION);
    size_t nevals=0;
    gslstatus=gsl_integration_cquad(&F, lkmin, lkmax, 0,
				    ccl_gsl->INTEGRATION_LIMBER_EPSREL,
				    w_cquad, &result, &eresult, &nevals);
    gsl_integration_cquad_workspace_free(w_cquad);
  }
  if(gslstatus!=GSL_SUCCESS || *ipar.status) {
    ccl_raise_gsl_warning(gslstatus, "ccl_cls.c: ccl_angular_cl_native():");
    // If an error status was already set, don't overwrite it.
    if(*status == 0){
        *status=CCL_ERROR_INTEG;
        ccl_cosmology_set_status_message(cosmo, "ccl_cls.c: ccl_angular_cl_native(): error integrating over k\n");
    }
    return -1;
  }
  ccl_check_status(cosmo,status);

  return result*M_LN10*2./M_PI;
}

void ccl_angular_cls(ccl_cosmology *cosmo,CCL_ClWorkspace *w,
		     CCL_ClTracer *clt1,CCL_ClTracer *clt2,
		     int nl_out,int *l_out,double *cl_out,int *status)
{
  int ii;
  //First check if ell range is within workspace
  for(ii=0;ii<nl_out;ii++) {
    if(l_out[ii]>w->lmax) {
      *status=CCL_ERROR_SPLINE_EV;
      ccl_cosmology_set_status_message(cosmo, "ccl_cls.c: ccl_angular_cls(); "
	     "requested l beyond range allowed by workspace\n");
      return;
    }
  }

  //Allocate array for power spectrum at interpolation nodes
  double *l_nodes=(double *)malloc(w->n_ls*sizeof(double));
  if(l_nodes==NULL) {
    *status=CCL_ERROR_MEMORY;
    ccl_cosmology_set_status_message(cosmo, "ccl_cls.c: ccl_angular_cls(); memory allocation\n");
    return;
  }
  double *cl_nodes=(double *)malloc(w->n_ls*sizeof(double));
  if(cl_nodes==NULL) {
    free(l_nodes);
    *status=CCL_ERROR_MEMORY;
    ccl_cosmology_set_status_message(cosmo, "ccl_cls.c: ccl_cl_angular_cls(); memory allocation\n");
    return;
  }
  for(ii=0;ii<w->n_ls;ii++)
    l_nodes[ii]=(double)(w->l_arr[ii]);

  //Now check if angpow is needed at all
  int method_use=w->nlimb_method;
  if(method_use==CCL_NONLIMBER_METHOD_ANGPOW) {
    int do_angpow=0;
    for(ii=0;ii<w->n_ls;ii++) {
      if(w->l_arr[ii]<=w->l_limber)
	do_angpow=1;
    }
    //Resort to native method if we have lensing (this will hopefully only be temporary)
    if(clt1->tracer_type==CL_TRACER_WL || clt2->tracer_type==CL_TRACER_WL ||
       clt1->has_magnification || clt2->has_magnification) {
      do_angpow=0;
      method_use=CCL_NONLIMBER_METHOD_NATIVE;
    }
    //Use angpow if non-limber is needed
#ifdef HAVE_ANGPOW
    if(do_angpow)
      ccl_angular_cls_angpow(cosmo,w,clt1,clt2,cl_nodes,status);
    ccl_check_status(cosmo,status);
#else
    do_angpow=0;
    method_use=CCL_NONLIMBER_METHOD_NATIVE;
#endif
  }

  //Compute limber nodes
  for(ii=0;ii<w->n_ls;ii++) {
    if((method_use==CCL_NONLIMBER_METHOD_NATIVE) || (w->l_arr[ii]>w->l_limber))
      cl_nodes[ii]=ccl_angular_cl_native(cosmo,w,ii,clt1,clt2,status);
  }

  //Interpolate into ells requested by user
  SplPar *spcl_nodes=ccl_spline_init(w->n_ls,l_nodes,cl_nodes,0,0);
  if(spcl_nodes==NULL) {
    free(cl_nodes);
    *status=CCL_ERROR_MEMORY;
    ccl_cosmology_set_status_message(cosmo, "ccl_cls.c: ccl_cl_angular_cls(); memory allocation\n");
    return;
  }
  for(ii=0;ii<nl_out;ii++)
    cl_out[ii]=ccl_spline_eval((double)(l_out[ii]),spcl_nodes);

  //Cleanup
  ccl_spline_free(spcl_nodes);
  free(cl_nodes);
  free(l_nodes);
}

static int check_clt_fa_inconsistency(CCL_ClTracer *clt,int func_code)
{
  if(((func_code==CCL_CLT_NZ) && (clt->tracer_type==CL_TRACER_CL)) || //Lensing has no N(z)
     (((func_code==CCL_CLT_BZ) || (func_code==CCL_CLT_SZ) || (func_code==CCL_CLT_WM)) &&
      (clt->tracer_type!=CL_TRACER_NC)) || //bias and magnification only for clustering
     (((func_code==CCL_CLT_RF) || (func_code==CCL_CLT_BA) || (func_code==CCL_CLT_WL)) &&
      (clt->tracer_type!=CL_TRACER_WL))) //IAs only for weak lensing
    return 1;
  if((((func_code==CCL_CLT_SZ) || (func_code==CCL_CLT_WM)) &&
      (clt->has_magnification==0)) || //Correct combination, but no magnification
     (((func_code==CCL_CLT_RF) || (func_code==CCL_CLT_BA)) &&
      (clt->has_intrinsic_alignment==0))) //Correct combination, but no IAs
    return 1;
  return 0;
}

double ccl_get_tracer_fa(ccl_cosmology *cosmo,CCL_ClTracer *clt,double a,int func_code,int *status)
{
  SplPar *spl;
  double x=1./a-1; //x-variable is redshift by default

  if(check_clt_fa_inconsistency(clt,func_code)) {
    *status=CCL_ERROR_INCONSISTENT;
    ccl_cosmology_set_status_message(cosmo, "ccl_cls.c: inconsistent combination of tracer and internal function to be evaluated");
    return -1;
  }

  if(func_code==CCL_CLT_NZ)
    spl=clt->spl_nz;
  if(func_code==CCL_CLT_BZ)
    spl=clt->spl_bz;
  if(func_code==CCL_CLT_SZ)
    spl=clt->spl_sz;
  if(func_code==CCL_CLT_RF)
    spl=clt->spl_rf;
  if(func_code==CCL_CLT_BA)
    spl=clt->spl_ba;
  if((func_code==CCL_CLT_WL) || (func_code==CCL_CLT_WM)) {
    x=ccl_comoving_radial_distance(cosmo,a,status);
    if(func_code==CCL_CLT_WL)
      spl=clt->spl_wL;
    if(func_code==CCL_CLT_WM)
      spl=clt->spl_wM;
  }

  return ccl_spline_eval(x,spl);
}

int ccl_get_tracer_fas(ccl_cosmology *cosmo,CCL_ClTracer *clt,int na,double *a,double *fa,
		       int func_code,int *status)
{
  SplPar *spl;

  if(check_clt_fa_inconsistency(clt,func_code)) {
    *status=CCL_ERROR_INCONSISTENT;
    ccl_cosmology_set_status_message(cosmo, "ccl_cls.c: inconsistent combination of tracer and internal function to be evaluated");
    return -1;
  }

  if(func_code==CCL_CLT_NZ)
    spl=clt->spl_nz;
  if(func_code==CCL_CLT_BZ)
    spl=clt->spl_bz;
  if(func_code==CCL_CLT_SZ)
    spl=clt->spl_sz;
  if(func_code==CCL_CLT_RF)
    spl=clt->spl_rf;
  if(func_code==CCL_CLT_BA)
    spl=clt->spl_ba;
  if(func_code==CCL_CLT_WL)
    spl=clt->spl_wL;
  if(func_code==CCL_CLT_WM)
    spl=clt->spl_wM;

  int compchi=0;
  if((func_code==CCL_CLT_WL) || (func_code==CCL_CLT_WM))
    compchi=1;

  int ia;
  for(ia=0;ia<na;ia++) {
    double x;
    if(compchi)
      x=ccl_comoving_radial_distance(cosmo,a[ia],status);
    else
      x=1./a[ia]-1;
    fa[ia]=ccl_spline_eval(x,spl);
  }

  return 0;
}