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

#include <config.h>
#include <math.h>
#include <stdlib.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_sys.h>

/* This is the CRAY RANF generator. The generator returns the
   upper 32 bits from each term of the sequence,

   x_{n+1} = (a x_n) mod m

   using 48-bit unsigned arithmetic, with a = 0x2875A2E7B175 and m =
   2^48. The seed specifies the lower 32 bits of the initial value,
   x_1, with the lowest bit set (to prevent the seed taking an even
   value), and the upper 16 bits set to 0.

   There is a subtlety in the implementation of the seed. The initial
   state is put one step back by multiplying by the modular inverse of
   a mod m. This is done for compatibility with the original CRAY
   implementation.

   Note, you can only seed the generator with integers up to 2^32,
   while the CRAY uses wide integers which can cover all 2^48 states
   of the generator.

   The theoretical value of x_{10001} is 141091827447341.

   The period of this generator is 2^{46}. */

static inline void ranf_advance (void *vstate);
static unsigned long int ranf_get (void *vstate);
static double ranf_get_double (void *vstate);
static void ranf_set (void *state, unsigned long int s);

static const unsigned short int a0 = 0xB175 ;
static const unsigned short int a1 = 0xA2E7 ;
static const unsigned short int a2 = 0x2875 ;

typedef struct
  {
    unsigned short int x0, x1, x2;
  }
ranf_state_t;

static inline void
ranf_advance (void *vstate)
{
  ranf_state_t *state = (ranf_state_t *) vstate;

  const unsigned long int x0 = (unsigned long int) state->x0 ;
  const unsigned long int x1 = (unsigned long int) state->x1 ;
  const unsigned long int x2 = (unsigned long int) state->x2 ;

  unsigned long int r ;

  r = a0 * x0 ;
  state->x0 = (r & 0xFFFF) ;

  r >>= 16 ;
  r += a0 * x1 + a1 * x0 ;
  state->x1 = (r & 0xFFFF) ;

  r >>= 16 ;
  r += a0 * x2 + a1 * x1 + a2 * x0 ;
  state->x2 = (r & 0xFFFF) ;
}

static unsigned long int
ranf_get (void *vstate)
{
  unsigned long int x1, x2;

  ranf_state_t *state = (ranf_state_t *) vstate;
  ranf_advance (state) ;

  x1 = (unsigned long int) state->x1;
  x2 = (unsigned long int) state->x2;

  return (x2 << 16) + x1;
}

static double
ranf_get_double (void * vstate)
{
  ranf_state_t *state = (ranf_state_t *) vstate;

  ranf_advance (state) ;

  return (ldexp((double) state->x2, -16)
          + ldexp((double) state->x1, -32)
          + ldexp((double) state->x0, -48)) ;
}

static void
ranf_set (void *vstate, unsigned long int s)
{
  ranf_state_t *state = (ranf_state_t *) vstate;

  unsigned short int x0, x1, x2 ;
  unsigned long int r ;

  unsigned long int b0 = 0xD6DD ;
  unsigned long int b1 = 0xB894 ;
  unsigned long int b2 = 0x5CEE ;

  if (s == 0)  /* default seed */
    {
      x0 = 0x9CD1 ;
      x1 = 0x53FC ;
      x2 = 0x9482 ;
    }
  else
    {
      x0 = (s | 1) & 0xFFFF ;
      x1 = s >> 16 & 0xFFFF ;
      x2 = 0 ;
    }

  r = b0 * x0 ;
  state->x0 = (r & 0xFFFF) ;

  r >>= 16 ;
  r += b0 * x1 + b1 * x0 ;
  state->x1 = (r & 0xFFFF) ;

  r >>= 16 ;
  r += b0 * x2 + b1 * x1 + b2 * x0 ;
  state->x2 = (r & 0xFFFF) ;

  return;
}

static const gsl_rng_type ranf_type =
{"ranf",                        /* name */
 0xffffffffUL,                  /* RAND_MAX */
 0,                             /* RAND_MIN */
 sizeof (ranf_state_t),
 &ranf_set,
 &ranf_get,
 &ranf_get_double
};

const gsl_rng_type *gsl_rng_ranf = &ranf_type;


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

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_multiroots.h>

int
gsl_multiroot_test_delta (const gsl_vector * dx, const gsl_vector * x,
                     double epsabs, double epsrel)
{
  size_t i;
  int ok = 1;
  const size_t n = x->size ;

  if (epsrel < 0.0)
    {
      GSL_ERROR ("relative tolerance is negative", GSL_EBADTOL);
    }

  for (i = 0 ; i < n ; i++)
    {
      double xi = gsl_vector_get(x,i);
      double dxi = gsl_vector_get(dx,i);
      double tolerance = epsabs + epsrel * fabs(xi)  ;

      if (fabs(dxi) < tolerance)
        {
          ok = 1;
        }
      else
        {
          ok = 0;
          break;
        }
    }

  if (ok)
    return GSL_SUCCESS ;

  return GSL_CONTINUE;
}

int
gsl_multiroot_test_residual (const gsl_vector * f, double epsabs)
{
  size_t i;

  double residual = 0;

  const size_t n = f->size;

  if (epsabs < 0.0)
    {
      GSL_ERROR ("absolute tolerance is negative", GSL_EBADTOL);
    }

  for (i = 0 ; i < n ; i++)
    {
      double fi = gsl_vector_get(f, i);

      residual += fabs(fi);
    }

  if (residual < epsabs)
    {
      return GSL_SUCCESS;
    }

  return GSL_CONTINUE ;
}


/** Function for dealiasing weather radar radial velocities.
 * @file libdealias.c
 * @author Adriaan Dokter, adapted from dealias.c RAVE functions by Gunther Haase
 * @date 2016-09-22
 */

#include "libdealias.h"
#include <stdio.h>
#include <gsl/gsl_multimin.h>
#include <gsl/gsl_vector.h>

void printDealias(const float *points, const int nDims, const float nyquist[],
    const float vradObs[], float vradDealias[], const int nPoints, const int iProfileType, const int iLayer, const int iPass){
    fprintf(stderr,"#iProfile iLayer iPass azim elev nyquist vrad vradd\n");
    for(int i=0; i<nPoints;i++){
        fprintf(stderr,"%i %i %i %3.1f %3.1f %3.1f %3.1f %3.1f\n",
        iProfileType,iLayer,iPass,points[i*nDims],points[i*nDims+1],nyquist[i],vradObs[i],vradDealias[i]);
    }
}

double test_field(float u, float v,const float *points, const float *pointsTrigon, const int nPoints, const int nDims, double x[], double y[], const float nyquist[]){
    double xt, yt, e, vm;
    double esum = 0;
    for (int iPoint=0; iPoint<nPoints; iPoint++) {
        // calculate the radial velocities for the test wind fields, eq 4 in Haase et al. 2004 jaot
        vm = (u*pointsTrigon[3*iPoint] + v*pointsTrigon[3*iPoint+1])*pointsTrigon[3*iPoint+2];
        // Eq. 7 for the test radial wind:
        xt = nyquist[iPoint]/M_PI * cos(vm*M_PI/nyquist[iPoint]);
        // Eq. 6 for the test radial wind:
        yt = nyquist[iPoint]/M_PI * sin(vm*M_PI/nyquist[iPoint]);

        // summed absolute differences between observed velocity field and test fields
        e = fabs(xt-x[iPoint]) + fabs(yt-y[iPoint]);
//        fprintf(stderr,"test_field: i=%i,u=%f,v=%f,vm=%f,xt=%f,yt=%f,e=%f,sin=%f,cos=%f,cos=%f\n",iPoint,u,v,vm,xt,yt,e,pointsTrigon[3*iPoint],pointsTrigon[3*iPoint+1],pointsTrigon[3*iPoint+2]);
        if (!isnan(e)) {
            esum = esum + e;
        }
    }
    return esum;
}

double test_field_gsl(const gsl_vector *uv, void* params){
    double u,v;
    float *points = ((void **) params)[0];
    float *pointsTrigon = ((void **) params)[1];
    int nPoints = *(int *) ((void **) params)[2];
    int nDims = *(int *) ((void **) params)[3];
    double *x = ((void **) params)[4];
    double *y = ((void **) params)[5];
    float *nyquist = ((void **) params)[6];

    u = gsl_vector_get(uv, 0);
    v = gsl_vector_get(uv, 1);

    return test_field(u, v, points, pointsTrigon, nPoints, nDims, x, y, nyquist);

}

int fit_field_gsl(gsl_vector *uv, void *params){
    gsl_vector *ss;

    // define which minimizer to use
    const gsl_multimin_fminimizer_type *T = gsl_multimin_fminimizer_nmsimplex2;
    gsl_multimin_fminimizer *s = NULL;

    int iter = 0;
    int status;
    double size;
    double u1 = 0;
    double v1 = 0;

    // Set initial step sizes to 1
    ss = gsl_vector_alloc(2);
    gsl_vector_set_all(ss, 1);

    // Initialize method
    gsl_multimin_function minex_func;
    minex_func.n = 2;
    minex_func.f = &test_field_gsl;
    minex_func.params = params;
    s = gsl_multimin_fminimizer_alloc (T, 2);
    gsl_multimin_fminimizer_set (s, &minex_func, uv, ss);

    // minimize by iteration
    do
    {
        iter++;
        status = gsl_multimin_fminimizer_iterate(s);

        if (status)
        break;

        size = gsl_multimin_fminimizer_size (s);
        status = gsl_multimin_test_size (size, 1e-2);

        if (status == GSL_SUCCESS)
        {
            #ifdef FPRINTFON
            printf ("converged to minimum on iteration ");
            printf ("%d at %10.3e %10.3e f() = %7.3f size = %.3f\n",
                iter,
                gsl_vector_get (s->x, 0),
                gsl_vector_get (s->x, 1),
                s->fval, size);
            #endif

            u1=gsl_vector_get(s->x, 0);
            v1=gsl_vector_get(s->x, 1);

            gsl_vector_set(uv, 0, u1);
            gsl_vector_set(uv, 1, v1);
        }
    }
    while (status == GSL_CONTINUE && iter < 100);

    #ifdef FPRINTFON
    fprintf(stdout,"Finished dealias at (x,y)=%f,%f at f()=%f ...\n",u1,v1,s->fval);
    #endif

    // clean up
    gsl_vector_free(ss);
    gsl_multimin_fminimizer_free (s);

    if (status != GSL_SUCCESS) return 0;
    return 1;
}

int dealias_points(const float *points, const int nDims, const float nyquist[],
    const double NI_MIN, const float vo[], float vradDealias[], const int nPoints){

    int i, j, n, m, eind, fitOk;
    double min1, esum, u1, v1, min2, dmy;

    // number of rows
    m = DEALIAS_VAF;
    // number of columns
    n = DEALIAS_NF;

    // max number of folds of nyquist interval to test for
    double MVA=2*ceil(DEALIAS_VMAX/(2*NI_MIN));

    // polarscan matrix, torus projected x coordinate, eq. 6 Haase et al. 2004 jaot
    double *x = RAVE_CALLOC ((size_t)nPoints, sizeof(double));
    // polarscan matrix, torus projected y coordinate, eq. 7 Haase et al. 2004 jaot
    double *y = RAVE_CALLOC ((size_t)nPoints, sizeof(double));
    // U-components of test velocity fields
    double *uh = RAVE_CALLOC ((size_t)(m*n), sizeof(double));
    // V-components of test velocity fields
    double *vh = RAVE_CALLOC ((size_t)(m*n), sizeof(double));
    // radial velocities of the best fitting test field
    double *vt1 = RAVE_CALLOC ((size_t)nPoints, sizeof(double));
    // array with trigonometric conversions of the points array
    float *pointsTrigon = RAVE_CALLOC ((size_t)(3*nPoints), sizeof(double));

    // map measured data to 3D
    for (i=0; i<nPoints; i++) {
        x[i] = nyquist[i]/M_PI * cos(vo[i]*M_PI/nyquist[i]);
        y[i] = nyquist[i]/M_PI * sin(vo[i]*M_PI/nyquist[i]);
    }

    // trigonometric conversion points array (compute once to speed up code)
    for (int iPoint=0; iPoint<nPoints; iPoint++) {
        pointsTrigon[3*iPoint+0] = sin(points[nDims*iPoint]*DEG2RAD);
        pointsTrigon[3*iPoint+1] = cos(points[nDims*iPoint]*DEG2RAD);
        pointsTrigon[3*iPoint+2] = cos(points[nDims*iPoint+1]*DEG2RAD);
    }

    // Setting up the u and v component of the test velocity fields:
    // index n=DEALIAS_NF gives number of azimuthal directions (default n=40, i.e. steps of 360/40=9 degrees)
    // index m=DEALIAS_VAF/NI_MIN*DEALIAS_VMAX gives number of speeds (maximum speed is DEALIAS_VMAX, steps of NI_MIN/DEALIAS_VAF)
    for (i=0; i<n; i++) {
        for (j=0; j<m; j++) {
            *(uh+i*m+j) = DEALIAS_VMAX/DEALIAS_VAF*(j+1) * sin(2*M_PI/DEALIAS_NF*i);
            *(vh+i*m+j) = DEALIAS_VMAX/DEALIAS_VAF*(j+1) * cos(2*M_PI/DEALIAS_NF*i);
        }
    }

    min1 = 1e32;
    eind = 0;
    u1 = 0;
    v1 = 0;

    gsl_vector *uv;
    uv = gsl_vector_alloc(2);

    void *params[7] = {(void *) points, (void *) pointsTrigon, (void *) &nPoints, (void *) &nDims, (void *) x, (void *) y, (void *) nyquist};

    // try several test velocity fields for use as starting point in GSL fit

    for (i=0; i<m*n; i++) {

        gsl_vector_set(uv, 0, *(uh+i));
        gsl_vector_set(uv, 1, *(vh+i));
        esum = test_field_gsl(uv, &params);

        if (esum<min1) {
            min1 = esum;
            eind = i;
        }
        u1 = *(uh+eind);
        v1 = *(vh+eind);
    }
    gsl_vector_set(uv, 0, u1);
    gsl_vector_set(uv, 1, v1);

    #ifdef FPRINTFON
    fprintf(stdout,"Start dealiasing at (x,y)=%f,%f at f()=%f ...\n",u1,v1,esum);
    #endif

    fitOk = fit_field_gsl(uv, &params);
    if(!fitOk) goto cleanup;

    // the radial velocity of the best fitting test velocity field:
    for (int iPoint=0; iPoint<nPoints; iPoint++) {
        *(vt1+iPoint) = (u1*sin(points[nDims*iPoint]*DEG2RAD) + v1*cos(points[nDims*iPoint]*DEG2RAD))
                        *cos(points[nDims*iPoint+1]*DEG2RAD);
    }

    // dealias the observed velocities using the best test velocity field
    for (int iPoint=0; iPoint<nPoints; iPoint++) {
        min2 = 1e32;
        dmy = 0;
        float diffVTest = (*(vt1+iPoint)-*(vo+iPoint));
        float dv = 0;
        for (i=0; i<MVA+1; i++) {
            // get a candidate velocity fold, i.e. integer multiple of nyquist velocity
            dv = nyquist[iPoint]*(2*i-MVA);
            // checking how many folds we have, vt1-vo is the residual between the real velocity
            // and the folded velocity; which equals a  multiple of the nyquist interval
            dmy = fabs(dv-diffVTest);
            if ((dmy<min2) && (!isnan(dmy))) {
                // add the aliased interval to the observed velocity field, and obtain dealiased velocity
                *(vradDealias+iPoint) = *(vo+iPoint) + dv;
                min2 = dmy;
            }
        } // loop MVA
    } // loop over points

    cleanup:
        RAVE_FREE(x);
        RAVE_FREE(y);
        RAVE_FREE(uh);
        RAVE_FREE(vh);
        RAVE_FREE(vt1);
        RAVE_FREE(pointsTrigon);
        gsl_vector_free(uv);

        if(fitOk) return 1;
        else return 0;
}


#ifndef VISCOUSVORTEXDOMAINSOLVER_H
#define VISCOUSVORTEXDOMAINSOLVER_H

#include <stdio.h>
#include <stdlib.h>
#include <common_functions.h>
#include <string.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>
#include <vector>
#include <algorithm>
//#include <parallel/algorithm>
#include "pstl/execution"
#include "pstl/algorithm"
#include "tbb/parallel_for_each.h"
#include <mutex>

#include <omp.h>

#include <streamline.h>
#include <vortex.h>
#include <vortexmotion.h>

//ToDo: add resumption of calculations mechanism
class ViscousVortexDomainSolver
{
    int NumberOfPanels;
    int MaxVortexes;
    int Niterations;

    double nu;
    double tau;
    double rho;
    double vx_inf;
    double vy_inf;

    double  *PanelLength;
    double  **PanelNodes,
            **PanelMids,
            **PanelNorms,
            **PanelTaus,
            **DeployPoints;

    gsl_matrix *OriginalVortexGenerationMatrix;
    gsl_matrix *VortexGenerationMatrix;
    gsl_matrix *VortexGenerationMatrix_Inversed;
    gsl_vector *VelocityProjections;
    gsl_vector *NewVorticities;
    gsl_permutation *Permutation;

    std::vector <Vortex> Flow;
    std::vector <VortexMotion> FlowEvolution;
    std::mutex SyncFlowEvolution;
    std::vector <Vortex> InBodyVortexes;

    std::vector <ControlPointOfFlow> StreamLine1;
    std::vector <ControlPointOfFlow> StreamLine2;
    void StepInTimeForStreamLines();
    void UpdateControlPointsOfStreamLine(std::vector<ControlPointOfFlow> &StreamLine);
    void MoveSingleControlPoint(ControlPointOfFlow &UpdatingPoint);

    int isVortexInBody(double x, double y); //ToDo: update to be virtual

    void DivideProfileToPanels();
    void CompletingGeneratingMatrix();
    void ComputeAssociatedVortexes();
    void AppendAssociatedVortexesToFlow();
    void UpdateVotexPositions();
    void ComputeVortexMotion(Vortex &Considered);
    double UpdateEpsilon(Vortex Considered, double InitialEpsilon);
    int LeakageControl(Vortex Checking);

public:

    ViscousVortexDomainSolver();
    ~ViscousVortexDomainSolver();

    void Solve();

    double xVelocityAt(double x, double y);
    double yVelocityAt(double x, double y);

    void Output_ParaView_Field(const char *FileName);
    void Output_ParaView_Field_with_exclusions(const char *FileName);
    void Output_ParaView_AllVortexes(const char *FileName);
    void Output_StreamLines(const char *FileName);
};

#endif // VISCOUSVORTEXDOMAINSOLVER_H


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

#include <config.h>
#include <stdlib.h>
#include <stdio.h>
#include <gsl/gsl_histogram.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_ieee_utils.h>

#define N 397
#define NR 10

void
test1d (void)
{
  double xr[NR + 1] =
  {0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0};

  gsl_histogram *h, *h1, *hr, *g;
  size_t i, j;

  gsl_ieee_env_setup ();

  h = gsl_histogram_calloc (N);
  h1 = gsl_histogram_calloc (N);
  g = gsl_histogram_calloc (N);

  gsl_test (h->range == 0, "gsl_histogram_alloc returns valid range pointer");
  gsl_test (h->bin == 0, "gsl_histogram_alloc returns valid bin pointer");
  gsl_test (h->n != N, "gsl_histogram_alloc returns valid size");

  hr = gsl_histogram_calloc_range (NR, xr);

  gsl_test (hr->range == 0, "gsl_histogram_calloc_range returns valid range pointer");
  gsl_test (hr->bin == 0, "gsl_histogram_calloc_range returns valid bin pointer");
  gsl_test (hr->n != NR, "gsl_histogram_calloc_range returns valid size");

  {
    int status = 0;
    for (i = 0; i <= NR; i++)
      {
        if (hr->range[i] != xr[i])
          {
            status = 1;
          }
      };

    gsl_test (status, "gsl_histogram_calloc_range creates range");
  }

  for (i = 0; i <= NR; i++)
    {
      hr->range[i] = 0.0;
    }

  {
    int status = gsl_histogram_set_ranges (hr, xr, NR+1);

    for (i = 0; i <= NR; i++)
      {
        if (hr->range[i] != xr[i])
          {
            status = 1;
          }
      };

    gsl_test (status, "gsl_histogram_set_range sets range");
  }

  for (i = 0; i < N; i++)
    {
      gsl_histogram_accumulate (h, (double) i, (double) i);
    };

  {
    int status = 0;

    for (i = 0; i < N; i++)
      {
        if (h->bin[i] != (double) i)
          {
            status = 1;
          }
      };

    gsl_test (status, "gsl_histogram_accumulate writes into array");
  }

  {
    int status = 0;

    for (i = 0; i < N; i++)
      {
        if (gsl_histogram_get (h, i) != i)
          status = 1;
      };
    gsl_test (status, "gsl_histogram_get reads from array");
  }

  for (i = 0; i <= N; i++)
    {
      h1->range[i] = 100.0 + i;
    }

  gsl_histogram_memcpy (h1, h);

  {
    int status = 0;
    for (i = 0; i <= N; i++)
      {
        if (h1->range[i] != h->range[i])
          status = 1;
      };
    gsl_test (status, "gsl_histogram_memcpy copies bin ranges");
  }

  {
    int status = 0;
    for (i = 0; i < N; i++)
      {
        if (gsl_histogram_get (h1, i) != gsl_histogram_get (h, i))
          status = 1;
      };
    gsl_test (status, "gsl_histogram_memcpy copies bin values");
  }

  gsl_histogram_free (h1);

  h1 = gsl_histogram_clone (h);

  {
    int status = 0;
    for (i = 0; i <= N; i++)
      {
        if (h1->range[i] != h->range[i])
          status = 1;
      };
    gsl_test (status, "gsl_histogram_clone copies bin ranges");
  }

  {
    int status = 0;
    for (i = 0; i < N; i++)
      {
        if (gsl_histogram_get (h1, i) != gsl_histogram_get (h, i))
          status = 1;
      };
    gsl_test (status, "gsl_histogram_clone copies bin values");
  }

  gsl_histogram_reset (h);

  {
    int status = 0;

    for (i = 0; i < N; i++)
      {
        if (h->bin[i] != 0)
          status = 1;
      }
    gsl_test (status, "gsl_histogram_reset zeros array");
  }

  {
    int status = 0;

    for (i = 0; i < N; i++)
      {
        gsl_histogram_increment (h, (double) i);

        for (j = 0; j <= i; j++)
          {
            if (h->bin[j] != 1)
              {
                status = 1;
              }
          }

        for (j = i + 1; j < N; j++)
          {
            if (h->bin[j] != 0)
              {
                status = 1;
              }
          }
      }

    gsl_test (status, "gsl_histogram_increment increases bin value");
  }

  {
    int status = 0;
    for (i = 0; i < N; i++)
      {
        double x0 = 0, x1 = 0;

        gsl_histogram_get_range (h, i, &x0, &x1);

        if (x0 != i || x1 != i + 1)
          {
            status = 1;
          }
      }
    gsl_test (status, "gsl_histogram_getbinrange returns bin range");
  }

  {
    int status = 0;
    if (gsl_histogram_max (h) != N)
      status = 1;
    gsl_test (status, "gsl_histogram_max returns maximum");
  }

  {
    int status = 0;
    if (gsl_histogram_min (h) != 0)
      status = 1;
    gsl_test (status, "gsl_histogram_min returns minimum");
  }

  {
    int status = 0;
    if (gsl_histogram_bins (h) != N)
      status = 1;
    gsl_test (status, "gsl_histogram_bins returns number of bins");
  }

  h->bin[2] = 123456.0;
  h->bin[4] = -654321;

  {
    double max = gsl_histogram_max_val (h);
    gsl_test (max != 123456.0, "gsl_histogram_max_val finds maximum value");
  }

  {
    double min = gsl_histogram_min_val (h);
    gsl_test (min != -654321.0, "gsl_histogram_min_val finds minimum value");
  }

  {
    size_t imax = gsl_histogram_max_bin (h);
    gsl_test (imax != 2, "gsl_histogram_max_bin finds maximum value bin");
  }

  {
    size_t imin = gsl_histogram_min_bin (h);
    gsl_test (imin != 4, "gsl_histogram_min_bin find minimum value bin");
  }

  for (i = 0; i < N; i++)
    {
      h->bin[i] = i + 27;
      g->bin[i] = (i + 27) * (i + 1);
    }

  {
    double sum=gsl_histogram_sum (h);
    gsl_test(sum != N*27+((N-1)*N)/2, "gsl_histogram_sum sums all bin values");
  }

  gsl_histogram_memcpy (h1, g);
  gsl_histogram_add (h1, h);

  {
    int status = 0;
    for (i = 0; i < N; i++)
      {
        if (h1->bin[i] != g->bin[i] + h->bin[i])
          status = 1;
      }
    gsl_test (status, "gsl_histogram_add histogram addition");
  }

  gsl_histogram_memcpy (h1, g);
  gsl_histogram_sub (h1, h);

  {
    int status = 0;
    for (i = 0; i < N; i++)
      {
        if (h1->bin[i] != g->bin[i] - h->bin[i])
          status = 1;
      }
    gsl_test (status, "gsl_histogram_sub histogram subtraction");
  }

  gsl_histogram_memcpy (h1, g);
  gsl_histogram_mul (h1, h);

  {
    int status = 0;
    for (i = 0; i < N; i++)
      {
        if (h1->bin[i] != g->bin[i] * h->bin[i])
          status = 1;
      }
    gsl_test (status, "gsl_histogram_mul histogram multiplication");
  }

  gsl_histogram_memcpy (h1, g);
  gsl_histogram_div (h1, h);

  {
    int status = 0;
    for (i = 0; i < N; i++)
      {
        if (h1->bin[i] != g->bin[i] / h->bin[i])
          status = 1;
      }
    gsl_test (status, "gsl_histogram_div histogram division");
  }

  gsl_histogram_memcpy (h1, g);
  gsl_histogram_scale (h1, 0.5);

  {
    int status = 0;
    for (i = 0; i < N; i++)
      {
        if (h1->bin[i] != 0.5 * g->bin[i])
          status = 1;
      }
    gsl_test (status, "gsl_histogram_scale histogram scaling");
  }

  gsl_histogram_memcpy (h1, g);
  gsl_histogram_shift (h1, 0.25);

  {
    int status = 0;
    for (i = 0; i < N; i++)
      {
        if (h1->bin[i] != 0.25 + g->bin[i])
          status = 1;
      }
    gsl_test (status, "gsl_histogram_shift histogram shift");
  }

  gsl_histogram_free (h);       /* free whatever is in h */

  h = gsl_histogram_calloc_uniform (N, 0.0, 1.0);

  gsl_test (h->range == 0,
            "gsl_histogram_calloc_uniform returns valid range pointer");
  gsl_test (h->bin == 0,
            "gsl_histogram_calloc_uniform returns valid bin pointer");
  gsl_test (h->n != N,
            "gsl_histogram_calloc_uniform returns valid size");

  gsl_histogram_accumulate (h, 0.0, 1.0);
  gsl_histogram_accumulate (h, 0.1, 2.0);
  gsl_histogram_accumulate (h, 0.2, 3.0);
  gsl_histogram_accumulate (h, 0.3, 4.0);

  {
    size_t i1, i2, i3, i4;
    double expected;
    int status = gsl_histogram_find (h, 0.0, &i1);
    status = gsl_histogram_find (h, 0.1, &i2);
    status = gsl_histogram_find (h, 0.2, &i3);
    status = gsl_histogram_find (h, 0.3, &i4);

    for (i = 0; i < N; i++)
      {
        if (i == i1)
          {
            expected = 1.0;
          }
        else if (i == i2)
          {
            expected = 2.0;
          }
        else if (i == i3)
          {
            expected = 3.0;
          }
        else if (i == i4)
          {
            expected = 4.0;
          }
        else
          {
            expected = 0.0;
          }

        if (h->bin[i] != expected)
          {
            status = 1;
          }

      }
    gsl_test (status, "gsl_histogram_find returns index");
  }

  {
    FILE *f = fopen ("test.txt", "w");
    gsl_histogram_fprintf (f, h, "%.19e", "%.19e");
    fclose (f);
  }

  {
    FILE *f = fopen ("test.txt", "r");
    gsl_histogram *hh = gsl_histogram_calloc (N);
    int status = 0;

    gsl_histogram_fscanf (f, hh);

    for (i = 0; i < N; i++)
      {
        if (h->range[i] != hh->range[i])
          status = 1;
        if (h->bin[i] != hh->bin[i])
          status = 1;
      }
    if (h->range[N] != hh->range[N])
      status = 1;

    gsl_test (status, "gsl_histogram_fprintf and fscanf");

    gsl_histogram_free (hh);
    fclose (f);
  }

  {
    FILE *f = fopen ("test.dat", "wb");
    gsl_histogram_fwrite (f, h);
    fclose (f);
  }

  {
    FILE *f = fopen ("test.dat", "rb");
    gsl_histogram *hh = gsl_histogram_calloc (N);
    int status = 0;

    gsl_histogram_fread (f, hh);

    for (i = 0; i < N; i++)
      {
        if (h->range[i] != hh->range[i])
          status = 1;
        if (h->bin[i] != hh->bin[i])
          status = 1;
      }
    if (h->range[N] != hh->range[N])
      status = 1;

    gsl_test (status, "gsl_histogram_fwrite and fread");

    gsl_histogram_free (hh);
    fclose (f);
  }

  gsl_histogram_free (h);
  gsl_histogram_free (g);
  gsl_histogram_free (h1);
  gsl_histogram_free (hr);
}


/**
  * Copyright 2019 José Manuel Abuín Mosquera <josemanuel.abuin@usc.es>
  *
  * This file is part of Matrix Market Suite.
  *
  * Matrix Market Suite is free software: you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
  * the Free Software Foundation, either version 3 of the License, or
  * (at your option) any later version.
  *
  * Matrix Market Suite 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 Matrix Market Suite. If not, see <http://www.gnu.org/licenses/>.
  */

#include <stdio.h>
#include <stdlib.h>
#include <mpi.h>
#include <unistd.h>

#include <cblas.h>

#include "DMxV-MPI.h"

void usageDMxVMPI(){

	fprintf(stderr, "\n");
	fprintf(stderr, "Usage: MM-Suite DMxV [options] <input-matrix> <input-vector>\n");
	fprintf(stderr, "\nInput/output options:\n\n");
	fprintf(stderr, "       -o STR        Output file name. Default: stdout\n");
	fprintf(stderr, "       -r            Input format is row per line. Default: False\n");
	fprintf(stderr, "\nParameters options:\n\n");
	fprintf(stderr, "       -a DOUBLE     Alpha. Default: 1.0\n");
	fprintf(stderr, "       -b DOUBLE     Beta. Default: 0.0\n");
	fprintf(stderr, "\nPerformance options:\n\n");
	fprintf(stderr, "       -t INT        Number of threads to use in OpenBLAS. Default: 1\n");
	fprintf(stderr, "\n");

}

int DMxVMPI(int argc, char *argv[], int numProcs, int myid) {

	int 			ret_code = 1;
	int			option;

	unsigned long 		*II;
	unsigned long 		*J;
	double 			*values;

	unsigned long 		M;
	unsigned long 		local_M;
	unsigned long 		N;
	unsigned long 		long nz;

	double 			*vectorValues;
	unsigned long 		M_Vector;
	unsigned long 		N_Vector;
	unsigned long long 	nz_vector;

	char			*outputFileName = NULL;

	char			*inputMatrixFile = NULL;
	char			*inputVectorFile = NULL;
	char			*outputVectorFile = NULL;

	int			inputFormatRow = 0;

	double			alpha = 1.0;
	double			beta = 0.0;

	int 		numThreads = 1;

	int			i, j;

	while ((option = getopt(argc, argv,"ro:a:b:t:")) >= 0) {
		switch (option) {
			case 'o' :
				//free(outputFileName);

				outputFileName = (char *) malloc(sizeof(char)*strlen(optarg)+1);
				strcpy(outputFileName,optarg);

				break;

			case 'r':
				inputFormatRow = 1;
				break;

			case 'b':
				beta = atof(optarg);
				break;

			case 'a':
				alpha = atof(optarg);
				break;

			case 't':
				numThreads = atoi(optarg);
				break;

			default: break;
		}

	}

	if ((optind + 3 != argc) && (optind + 2 != argc)) {
		if (myid == 0) {
			//fprintf(stderr,"[%s] Argc: %d, optind: %d\n",__func__, argc, optind);
			usageDMxVMPI();
		}
		return 0;
	}

	openblas_set_num_threads(numThreads);

	if(optind + 3 == argc) { //We have an output vector

		outputVectorFile = (char *)malloc(sizeof(char)*strlen(argv[optind+2])+1);
		strcpy(outputVectorFile,argv[optind+2]);
	}

	if(outputFileName == NULL) {
		outputFileName = (char *) malloc(sizeof(char)*6);
		sprintf(outputFileName,"stdout");
	}

	inputMatrixFile = (char *)malloc(sizeof(char)*strlen(argv[optind])+1);
	inputVectorFile = (char *)malloc(sizeof(char)*strlen(argv[optind+1])+1);

	strcpy(inputMatrixFile,argv[optind]);
	strcpy(inputVectorFile,argv[optind+1]);

	//Read matrix
	if(inputFormatRow) {
		if(!readDenseCoordinateMatrixMPIRowLine(inputMatrixFile,&II,&J,&values,&M,&local_M,&N,&nz,myid, numProcs)){
			fprintf(stderr, "[%s] Can not read Matrix\n",__func__);
			return 0;
		}
	}
	else{
		if(!readDenseCoordinateMatrixMPI(inputMatrixFile,&II,&J,&values,&M,&local_M,&N,&nz,myid, numProcs)){
			fprintf(stderr, "[%s] Can not read Matrix\n",__func__);
			return 0;
		}
	}

	//Read input vector
	if(!readDenseVector(inputVectorFile, &vectorValues,&M_Vector,&N_Vector,&nz_vector)){
		fprintf(stderr, "[%s] Can not read Vector\n",__func__);
		return 0;
	}

	/*
	void cblas_dgemv(const enum CBLAS_ORDER order,
                 const enum CBLAS_TRANSPOSE TransA, const int M, const int N,
                 const double alpha, const double *A, const int lda,
                 const double *X, const int incX, const double beta,
                 double *Y, const int incY);
                 */

        double *partial_result=(double *) calloc(local_M,sizeof(double));
        double* y = (double*)calloc(N,sizeof(double));

        if(y == NULL){
		fprintf(stderr,"[%s] Error reserving memory for final result vector in processor %d\n",__func__,myid);
		return 0;
	}

        //Read output vector if any
	if(outputVectorFile != NULL) {
		if(!readDenseVector(outputVectorFile, &y,&M_Vector,&N_Vector,&nz_vector)){
			fprintf(stderr, "[%s] Can not read Vector %s\n",__func__, outputVectorFile);
			return 0;
		}

		for( i = (local_M * myid), j = 0; i< (local_M * myid + local_M) && j< local_M; i++, j++) {
        		partial_result[j] = y [i];
        	}
	}

        double t_real = realtime();

        //y := alpha * A * x + beta * y

	//cblas_dgemv(CblasRowMajor,CblasNoTrans,local_M,N,1.0,values,N,vectorValues,1,0.0,partial_result,1);
	cblas_dgemv(CblasRowMajor,CblasNoTrans,local_M,N,alpha,values,N,vectorValues,1,beta,partial_result,1);

	MPI_Allgather (partial_result,local_M,MPI_DOUBLE,y,local_M,MPI_DOUBLE,MPI_COMM_WORLD);
	MPI_Barrier(MPI_COMM_WORLD);

	fprintf(stderr, "\n[%s] Time spent in DMxV: %.6f sec\n", __func__, realtime() - t_real);

	if (myid == 0){

		writeDenseVector(outputFileName, y,M_Vector,N_Vector,nz_vector);

	}
	return ret_code;
}


#include <stdio.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_odeiv.h>
#include "Effective_Potential.h"

#define PI 3.14159265
#define G 4.30091252525*pow(10,-3) //grav constant, in (parsec*km^2)/(Ms*sec^2)
#define c  300000 //speed of light, km/sec

void Geodesic_Orbit(int* time_size, double M, double periapsis, double e)
{
    int dimension = 2;		/* number of differential equations */

    double eps_abs = 1.e-8;	/* absolute error requested */
    double eps_rel = 1.e-10;	/* relative error requested */

    /* define the type of routine for making steps: */
    const gsl_odeiv_step_type *type_ptr = gsl_odeiv_step_rkf45;

    /*
       allocate/initialize the stepper, the control function, and the
       evolution function.
    */
    gsl_odeiv_step *step_ptr = gsl_odeiv_step_alloc (type_ptr, dimension);
    gsl_odeiv_control *control_ptr = gsl_odeiv_control_y_new (eps_abs, eps_rel);
    gsl_odeiv_evolve *evolve_ptr = gsl_odeiv_evolve_alloc (dimension);

    gsl_odeiv_system my_system;	/* structure with the rhs function, etc. */

    double E = E_r(M, periapsis, e);
    double L = L_r(e,periapsis,M);
    double r[*time_size];			/* current solution vector */

    double T, T_next;		/* current and next independent variable */
    double Tmin, Tmax, delta_t;	/* range of t and step size for output */

    double h = 1e-6;		/* starting step size for ode solver */
}


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <omp.h>
#include <float.h>
#include <math.h>
#include "sacio.h"
#include "parmt_utils.h"
#ifdef PARMT_USE_INTEL
#include <mkl.h>
#include <mkl_lapacke.h>
#include <mkl_cblas.h>
#else
#include <lapacke.h>
#include <cblas.h>
#endif
#include "parmt_mtsearch.h"
#include "iscl/array/array.h"
#include "iscl/memory/memory.h"
#include "iscl/random/random.h"
#include "iscl/signal/convolve.h"
#include "iscl/time/time.h"

int testBlockSize(void);

int main()
{
    char *ffList[10] = {"rotation_data/B00103ZDS.sac\0",
                        "rotation_data/B00106ZSS.sac\0",
                        "rotation_data/B00101ZDD.sac\0",
                        "rotation_data/B00109ZEX.sac\0",
                        "rotation_data/B00104RDS.sac\0",
                        "rotation_data/B00107RSS.sac\0",
                        "rotation_data/B00102RDD.sac\0",
                        "rotation_data/B00110REX.sac\0",
                        "rotation_data/B00105TDS.sac\0",
                        "rotation_data/B00108TSS.sac\0"};
    struct parmtNoiseBasis_struct basis;
    struct sacData_struct sacZDS, sacZSS, sacZDD, sacZEX,
                          sacRDS, sacRSS, sacRDD, sacREX,
                          sacTDS, sacTSS, sac;
    double *CeInv, *CeInvDiag, *d, *dn, *G, *R, *X,
           *mts, *betas, *gammas, *kappas, *M0s,
           *phi, *sigmas, *thetas, *xrand, *var, *eye;
    double baz, betaMax, betaMin, gammaMax, gammaMin, kappaMin, kappaMax,
           m0Min, m0Max, sigmaMax, sigmaMin, thetaMax, thetaMin;
    double cmpaz, cmpinc, sigma, xnorm;
    int *lags, i, icomp, ierr, imt, imtopt, j, ldc, ldz, ng, nb, nk,
        nlags, ns, nt, nm, nmt, npgrns, npts, rank;
    bool lrescale = false;
    const double oneDeg = M_PI/180.0;  // one degree for padding
    const double az = 25.0;
    const int ldm = 8;
    const int blockSize = 32;
    iscl_init();
    // Discretize the moment tensor space
    ng = 11; // longitudes
    nb = 11; // colatitudes
    nk = 11;  // strike angles
    ns = 11;  // rake
    nt = 11;  // dip
    nm = 4;  // magnitudes
    betaMin = 0.0 + oneDeg;
    betaMax = M_PI - oneDeg;
    gammaMin =-M_PI/6.0 + oneDeg;
    gammaMax = M_PI/6.0 - oneDeg;
    kappaMin = 0.0 + oneDeg;
    kappaMax = 2.0*M_PI - oneDeg;
    thetaMin = 0.0 + oneDeg;
    thetaMax = 0.5*M_PI - oneDeg;
    sigmaMin =-0.5*M_PI + oneDeg;
    sigmaMax = 0.5*M_PI - oneDeg;
    m0Min = 1.0/sqrt(2.0);
    m0Max = 2.0/sqrt(2.0);
    //m0Max = 1.e8/sqrt(2.0) + (double) (nm - 1)*1.e8/sqrt(2.0);
    //omp_set_num_threads(nthreads);
    mkl_set_num_threads(1);
    // Discretize the MT space
    M0s    = array_linspace64f(m0Min,    m0Max,    nm, &ierr);
    betas  = array_linspace64f(betaMin,  betaMax,  nb, &ierr);
    gammas = array_linspace64f(gammaMin, gammaMax, ng, &ierr);
    kappas = array_linspace64f(kappaMin, kappaMax, nk, &ierr);
    thetas = array_linspace64f(thetaMin, thetaMax, nt, &ierr);
    sigmas = array_linspace64f(sigmaMin, sigmaMax, ns, &ierr);
    nmt = ng*nb*nk*ns*nt*nm;
    mts = memory_calloc64f(nmt*ldm);
    ierr = parmt_discretizeMT64f(ng, gammas,
                                 nb, betas,
                                 nm, M0s,
                                 nk, kappas,
                                 nt, thetas,
                                 ns, sigmas,
                                 ldm, nmt, mts);
    if (ierr != 0)
    {
        printf("Failed to discretize MTs\n");
        return EXIT_FAILURE;
    }
    memory_free64f(&M0s);
    memory_free64f(&betas);
    memory_free64f(&gammas);
    memory_free64f(&kappas);
    memory_free64f(&thetas);
    memory_free64f(&sigmas);
    // Read the green's functions generated by CPS
    memset(&sacZDS, 0, sizeof(struct sacData_struct));
    memset(&sacZSS, 0, sizeof(struct sacData_struct));
    memset(&sacZDD, 0, sizeof(struct sacData_struct));
    memset(&sacZEX, 0, sizeof(struct sacData_struct));
    memset(&sacRDS, 0, sizeof(struct sacData_struct));
    memset(&sacRSS, 0, sizeof(struct sacData_struct));
    memset(&sacRDD, 0, sizeof(struct sacData_struct));
    memset(&sacREX, 0, sizeof(struct sacData_struct));
    memset(&sacTDS, 0, sizeof(struct sacData_struct));
    memset(&sacTSS, 0, sizeof(struct sacData_struct));
    ierr  = sacio_readTimeSeriesFile(ffList[0], &sacZDS);
    ierr += sacio_readTimeSeriesFile(ffList[1], &sacZSS);
    ierr += sacio_readTimeSeriesFile(ffList[2], &sacZDD);
    ierr += sacio_readTimeSeriesFile(ffList[3], &sacZEX);
    ierr += sacio_readTimeSeriesFile(ffList[4], &sacRDS);
    ierr += sacio_readTimeSeriesFile(ffList[5], &sacRSS);
    ierr += sacio_readTimeSeriesFile(ffList[6], &sacRDD);
    ierr += sacio_readTimeSeriesFile(ffList[7], &sacREX);
    ierr += sacio_readTimeSeriesFile(ffList[8], &sacTDS);
    ierr += sacio_readTimeSeriesFile(ffList[9], &sacTSS);
    if (ierr != 0)
    {
        printf("Failed to load fundmaental faults\n");
        return EXIT_FAILURE;
    }
    // Set the green's functions
    npgrns = MIN(64, sacZDS.npts);
    npts = npgrns;
    icomp = 1;     // vertical
    cmpaz = 0.0;   // channel azimuth
    cmpinc =-90.0; // channel orientation (-90+ up 0 is horizontal)
    G = memory_calloc64f(6*npgrns);
    printf("Setting Green's functions...\n");
    baz = fmod(az + 180, 360.0);
    ierr = parmt_utils_ff2mtGreens64f(npgrns, icomp,
                                      az, baz,
                                      cmpaz, cmpinc,
                                      sacZDS.data, sacZSS.data,
                                      sacZDD.data, sacZEX.data,
                                      sacRDS.data, sacRSS.data,
                                      sacRDD.data, sacREX.data,
                                      sacTDS.data, sacTSS.data,
                                      &G[0*npgrns], &G[1*npgrns], &G[2*npgrns],
                                      &G[3*npgrns], &G[4*npgrns], &G[5*npgrns]);
    if (ierr != 0)
    {
        printf("Failed to set Green's functions\n");
        return EXIT_FAILURE;
    }
    // Free intermediate memory
    sacio_free(&sacZDS);
    sacio_free(&sacZSS);
    sacio_free(&sacZDD);
    sacio_free(&sacZEX);
    sacio_free(&sacRDS);
    sacio_free(&sacRSS);
    sacio_free(&sacRDD);
    sacio_free(&sacREX);
    sacio_free(&sacTDS);
    sacio_free(&sacTSS);
    // make a noise matrix
    ldc = npts;
    CeInv = memory_calloc64f(npts*ldc);
    R = memory_calloc64f(npts*ldc);
    X = memory_calloc64f((2*npts-1)*ldc);
    //xrand = random_randn64f(npts, 0.0, 0.01, &ierr);
    xrand = random_rand64f(npts, &ierr);
    xrand[0] = 0.0;
    xrand[npts-1] = 0.0;
    xnorm = cblas_dnrm2(npts, xrand, 1);
    cblas_dscal(npts, 1.0/xnorm, xrand, 1);
    ierr = convolve_corrmtx64f_work(npts, xrand,
                                    npts-1, ldc, X,
                                    CORRMTX_AUTOCORRELATION,
                                    true, ldc, R);
FILE *fmat = fopen("xcmat.txt", "w");
for (i=0; i<npts; i++)
{
  for (j=0; j<npts; j++)
  {
   fprintf(fmat, "%d %d %e\n", i+1, npts-j, R[i*ldc+j]);
  }
fprintf(fmat,"\n");
}
fclose(fmat);
    memory_free64f(&X);
    cblas_dcopy(npts*ldc, R, 1, CeInv, 1);
    // Make a noise basis
    ierr = parmt_utils_getNoiseBasisFromCtC64f(npts, ldc, R, &basis);
    if (ierr != 0)
    {
        printf("Failed to compute eigenbasis\n");
        return EXIT_FAILURE;
    }
    // Compute inverse of R
    ierr = LAPACKE_dpotrf(LAPACK_ROW_MAJOR, 'U', npts, CeInv, ldc);
    if (ierr != 0)
    {
        printf("Failed to compute chol(CeInv)\n");
        return EXIT_FAILURE;
    }
    ierr = LAPACKE_dpotri(LAPACK_ROW_MAJOR, 'U', npts, CeInv, ldc);
    if (ierr != 0)
    {
        printf("Failed to compute CeInv %d\n", ierr);
        return EXIT_FAILURE;
    }
    // The KL basis uses sqrt(eig) but for consistency with inv(Ce) need
    // just the regular old eigenvalues
    for (i=0; i<basis.nvals; i++)
    {
        basis.sqrtEvals[i] = pow(basis.sqrtEvals[i], 2);
    }
    // Copy upper to lower
    for (i=0; i<npts; i++)
    {
        for (j=0; j<i; j++)
        {
            CeInv[i*ldc+j] = CeInv[j*ldc+i];
        }
    }
    CeInvDiag = memory_calloc64f(npts);
    for (i=0; i<npts; i++){CeInvDiag[i] = CeInv[ldc*i+i];}
    // Make a synthetic
    imtopt = MIN(nmt-1, MAX(0, (3*nmt)/8));
    d = memory_calloc64f(npts);
    cblas_dgemv(CblasColMajor, CblasNoTrans,
                npgrns, 6, 1.0, G, npgrns,
                &mts[ldm*imtopt], 1, 0.0, d, 1);
    // make a noisy synthetic so Ei doesn't blow up
double relError = 0.2;
rank = parmt_utils_getEffectiveRank(relError, basis);
printf("%d\n", rank);
//    rank = basis.npts/4;
//printf("%d\n", rank);
//getchar();
printf("Effective rank used: %d\n", rank);
    sigma = 1.0;
    double snr = 15.0;
    dn = memory_calloc64f(npts);
    parmt_utils_makeKLNoise(rank, npts, basis, dn);
    double xsum = array_sum64f(npts, dn, &ierr);
    for (i=0; i<npts; i++){dn[i] = dn[i] - xsum/(double) npts;}
    double signalEnergy = cblas_dnrm2(npts, d, 1); // signal variance
    double noiseEnergy = cblas_dnrm2(npts, dn, 1); // noise variance
printf("%f %f\n", signalEnergy, noiseEnergy);
    double xvar = 0.0;
    double xfact = (1.0/noiseEnergy)*signalEnergy/pow(10.0, 0.05*snr);
    for (i=0; i<npts; i++)
    {
        dn[i] = xfact*dn[i] + d[i];
    }
    sigma = 0.0;
    for (i=0; i<npts; i++)
    {
        sigma = sigma + pow(dn[i] - d[i], 2); // noise variance
    }
    sigma = sqrt(sigma);
    //sigma = pow(cblas_dnrm2(npts, dn, 1), 2);
printf("variance: %f\n", sigma);
//sigma = 0.002;
printf("%e %e %e %e\n", xfact, sigma, d[14], dn[14]);
    //------------------------------------------------------------------------//
    //                        unlagged conventional                           //
    //------------------------------------------------------------------------//
    // run the inversion for a diagonal matrix
    phi = memory_calloc64f(nmt);
    time_tic();
    nlags = 0;
    ierr = parmt_mtSearch64f(ldm, nlags, nmt,
                             npts, ldc,
                             true,
                             &G[0*npgrns], &G[1*npgrns], &G[2*npgrns],
                             &G[3*npgrns], &G[4*npgrns], &G[5*npgrns],
                             CeInvDiag, mts, d, phi);
    if (ierr != 0)
    {
        printf("Failed calling mtSearch64f 1\n");
        return EXIT_FAILURE;
    }
    imt = array_argmin64f(nmt, phi, &ierr);
    if (imt != imtopt)
    {
        printf("Failed to recover optimal index in test 1\n");
        return EXIT_FAILURE;
    }
    printf("Unlagged diagonal matrix time: %f (s)\n", time_toc());
    time_tic();
 printf("est + true optimal indices: %d %d %e\n", imt, imtopt, phi[imt]);

    ierr = parmt_mtSearch64f(ldm, nlags, nmt,
                             npts, ldc,
                             false,
                             &G[0*npgrns], &G[1*npgrns], &G[2*npgrns],
                             &G[3*npgrns], &G[4*npgrns], &G[5*npgrns],
                             CeInv, mts, dn, phi);
    if (ierr != 0)
    {
        printf("Failed calling mtSearch64f 2\n");
        return EXIT_FAILURE;
    }
    imt = array_argmin64f(nmt, phi, &ierr);
    if (imt != imtopt)
    {
        printf("Failed to recover optimal index in test 2 %d %d\n", imt, imtopt);
        //return EXIT_FAILURE;
    }
imt = array_argmin64f(nmt, phi, &ierr);
    printf("est + true optimal indices: %d %d %e\n", imt, imtopt, phi[imt]);
    printf("Unlagged time: %f (s)\n", time_toc());

    lags = memory_calloc32i(nmt);
    var = memory_calloc64f(npts);
    nlags = 0;
    eye = array_set64f(npts, 1.0, &ierr);
    time_tic();
    ierr = parmt_mtSearchL164f(ldm, nmt, npts, blockSize,
                               nlags, false,
                               lrescale,
                               &G[0*npgrns], &G[1*npgrns], &G[2*npgrns],
                               &G[3*npgrns], &G[4*npgrns], &G[5*npgrns],
                               eye, mts, d, phi, var, lags);
    imt = array_argmax64f(nmt, phi, &ierr);
    printf("L1 est + true optimal indices: %d %d %e %e\n",
           imt, imtopt, phi[imt], phi[imtopt]);
    printf("Unlagged general matrix time: %f (s)\n", time_toc());
    nlags = 4;
omp_set_num_threads(1);
    ierr = parmt_mtSearchL164f(ldm, nmt, npts, blockSize,
                               nlags, true,
                               lrescale,
                               &G[0*npgrns], &G[1*npgrns], &G[2*npgrns],
                               &G[3*npgrns], &G[4*npgrns], &G[5*npgrns],
                               eye, mts, d, phi, var, lags);
    imt = array_argmax64f(nmt, phi, &ierr);
    printf("L1 lagged est + true optimal indices: %d %d %d %e %e\n",
           imt, imtopt, lags[imt], phi[imt], phi[imtopt]);
    printf("Lagged time: %f (s)\n", time_toc());

    time_tic();
    ierr = parmt_mtSearchL164f(ldm, nmt, npts, blockSize,
                               nlags, true,
                               true, //lrescale,
                               &G[0*npgrns], &G[1*npgrns], &G[2*npgrns],
                               &G[3*npgrns], &G[4*npgrns], &G[5*npgrns],
                               eye, mts, d, phi, var, lags);
    int imtNew = array_argmax64f(nmt, phi, &ierr);
    if (imtNew != imt)
    {
        if (fabs(phi[imtNew] - phi[imt]) > 1.e-14)
        {
            printf("Failed to locate lrescaled script %e\n",
                   fabs(phi[imtNew] - phi[imt]));
            return EXIT_FAILURE;
        }
    }
    printf("L1 rescaled lagged est + true optimal indices: %d %d %d %e %e\n",
           imt, imtopt, lags[imt], phi[imt], phi[imtopt]);
    printf("Rescaled lagged time: %f (s)\n", time_toc());

    // repeat with lags
    ierr = testBlockSize();
/*
    nlags = 10;
    eye = array_set64f(npts, 1.0, &ierr);
    ierr = parmt_mtSearchL164f(ldm, nmt, npts, blockSize,
                               nlags, true,
                               &G[0*npgrns], &G[1*npgrns], &G[2*npgrns],
                               &G[3*npgrns], &G[4*npgrns], &G[5*npgrns],
                               eye, mts, d, phi, var, lags);
    imt = array_argmax64f(nmt, phi, &ierr);
    printf("L1 est + true optimal indices: %d %d %e %d\n", imt, imtopt, phi[imt], lags[imt]);
    printf("Unlagged general matrix time: %f (s)\n", time_toc());
*/
/*
    //------------------------------------------------------------------------//
    //                             unlagged kl                                //
    //------------------------------------------------------------------------//
    time_tic();
    ldz = basis.lde;
    ierr = parmt_mtSearchKL64f(nlags, nmt,
                               npts, ldz,
                               rank,
                               sigma,
                               &G[0*npgrns], &G[1*npgrns], &G[2*npgrns],
                               &G[3*npgrns], &G[4*npgrns], &G[5*npgrns],
                               basis.sqrtEvals,
                               basis.evects,
                               mts, dn, phi);
    if (ierr != 0)
    {
        printf("Failed calling mtSearchKL64f unlagged\n");
        return EXIT_FAILURE;
    }
    imt = array_argmax64f(nmt, phi, &ierr);
*/
/*
    if (imt != imtopt)
    {
        printf("Failed to locate unlagged kl optimum\n");
        return EXIT_FAILURE;
    }
*/
/*
    printf("est + true optimal indices: %d %d %e %e %e\n", imt, imtopt,
           phi[imt], phi[imtopt], phi[array_argmin64f(nmt, phi)]);
    printf("%e\n", array_sum64f(nmt, phi, &ierr));
    printf("Unlagged general kl time %f (s)\n", time_toc());
*/
//TEST:;
/*
double *est = memory_calloc64f(npts);
    cblas_dgemv(CblasColMajor, CblasNoTrans,
                npgrns, 6, 1.0, G, npgrns,
                &mts[ldm*imt], 1, 0.0, est, 1);
FILE *fn = fopen("noise.txt","w");
for (i=0; i<npts; i++)
{
 fprintf(fn, "%d %e %e %e %e\n", i, d[i], dn[i], est[i], d[i] - dn[i]);
}
fclose(fn);
*/
    // Free memory
    parmt_utils_freeNoiseBasis(&basis);
    memory_free64f(&G);
    memory_free64f(&CeInv);
    memory_free64f(&CeInvDiag);
    memory_free64f(&mts);
    memory_free64f(&d);
    memory_free64f(&phi);
    memory_free64f(&var);
    memory_free64f(&eye);
    iscl_finalize();
    return EXIT_SUCCESS;
}
//============================================================================//
int testPolarityMTSearch(void)
{
    const char *fcnm = "testPolarityMTSearch\0";
    const int blockSizes[4] = {1, 2, 32, 64};
    int ierr, nb, ng, nk, nm, ns, nt;
    double *betas, *gammas, *kappas, *mts, *M0s, *thetas, *sigmas;
    double betaMin, betaMax, gammaMin, gammaMax, kappaMin, kappaMax,
           m0Min, m0Max, sigmaMin, sigmaMax, thetaMin, thetaMax;
    const double oneDeg = M_PI/180.0;  // one degree for padding
    // Discretize the moment tensor space
    ng = 5; // longitudes
    nb = 6; // colatitudes
    nk = 7; // strike angles
    ns = 8; // rake
    nt = 9; // dip
    nm = 1; // magnitudes
    betaMin = 0.0 + oneDeg;
    betaMax = M_PI - oneDeg;
    gammaMin =-M_PI/6.0 + oneDeg;
    gammaMax = M_PI/6.0 - oneDeg;
    kappaMin = 0.0 + oneDeg;
    kappaMax = 2.0*M_PI - oneDeg;
    thetaMin = 0.0 + oneDeg;
    thetaMax = 0.5*M_PI - oneDeg;
    sigmaMin =-0.5*M_PI + oneDeg;
    sigmaMax = 0.5*M_PI - oneDeg;
    m0Min = 1.0/sqrt(2.0);
    m0Max = 2.0/sqrt(2.0);
    ierr = parmt_discretizeCells64f(nb, betaMin, betaMax,
                                    ng, gammaMin, gammaMax,
                                    nk, kappaMin, kappaMax,
                                    ns, sigmaMin, sigmaMax,
                                    nt, thetaMin, thetaMax,
                                    nm, m0Min, m0Max, false,
                                    &betas, &gammas, &kappas,
                                    &sigmas, &thetas, &M0s);
    if (ierr != 0)
    {
        printf("%s: Error making cell discretization\n", fcnm);
        return EXIT_FAILURE;
    }
    memory_free64f(&betas);
    memory_free64f(&gammas);
    memory_free64f(&kappas);
    memory_free64f(&sigmas);
    memory_free64f(&thetas);
    memory_free64f(&M0s);
    // Investigate different block sizes
    return EXIT_SUCCESS;
}
//============================================================================//
/*!
 * @brief Tests the effect of blocksize in gemm on performance
 */
int testBlockSize(void)
{
    const char *fcnm = "testBlockSize\0";
    FILE *fout;
    char fname[PATH_MAX];
    double *G, *d, *eye, *phi, *phiOri, *var, time, dnorm, xmin;
    int *lags = NULL;
    int i, ierr, imt, nmt, nt0, npmax, npts, npgrns, pSize;
    const int nblocks = 10;
    const int nps = 6;
    int pSizes[6] = {32, 64, 128, 256, 512, 1024};
    int blockSizes[10] = {1, 2, 4, 8, 16, 32, 64, 128, 256, 512};
    const int nlags = 0;
    const int ldm = 6;
    const bool lwantLags = false;
    int imtopt, ng, nb, nk, ns, nt, nm;
    const bool lrescale = false;
    double *betas, *gammas, *kappas, *mts, *M0s, *thetas, *sigmas;
    double betaMin, betaMax, gammaMin, gammaMax, kappaMin, kappaMax,
           m0Min, m0Max, sigmaMin, sigmaMax, thetaMin, thetaMax;
    const double oneDeg = M_PI/180.0;  // one degree for padding
    // disretize the moment tensor
    // Discretize the moment tensor space
    ng = 5; // longitudes
    nb = 5; // colatitudes
    nk = 7; // strike angles
    ns = 7; // rake
    nt = 7; // dip
    nm = 1; // magnitudes
    betaMin = 0.0 + oneDeg;
    betaMax = M_PI - oneDeg;
    gammaMin =-M_PI/6.0 + oneDeg;
    gammaMax = M_PI/6.0 - oneDeg;
    kappaMin = 0.0 + oneDeg;
    kappaMax = 2.0*M_PI - oneDeg;
    thetaMin = 0.0 + oneDeg;
    thetaMax = 0.5*M_PI - oneDeg;
    sigmaMin =-0.5*M_PI + oneDeg;
    sigmaMax = 0.5*M_PI - oneDeg;
    m0Min = 1.0/sqrt(2.0);
    m0Max = 2.0/sqrt(2.0);
    // Discretize the MT space
    M0s    = array_linspace64f(m0Min,    m0Max,    nm, &ierr);
    betas  = array_linspace64f(betaMin,  betaMax,  nb, &ierr);
    gammas = array_linspace64f(gammaMin, gammaMax, ng, &ierr);
    kappas = array_linspace64f(kappaMin, kappaMax, nk, &ierr);
    thetas = array_linspace64f(thetaMin, thetaMax, nt, &ierr);
    sigmas = array_linspace64f(sigmaMin, sigmaMax, ns, &ierr);
    nmt = ng*nb*nk*ns*nt*nm;
    imtopt = nmt/2;
    mts = memory_calloc64f(nmt*ldm);
    ierr = parmt_discretizeMT64f(ng, gammas,
                                 nb, betas,
                                 nm, M0s,
                                 nk, kappas,
                                 nt, thetas,
                                 ns, sigmas,
                                 ldm, nmt, mts);
    if (ierr != 0)
    {
        printf("Failed to discretize MTs\n");
        return EXIT_FAILURE;
    }
    memory_free64f(&M0s);
    memory_free64f(&betas);
    memory_free64f(&gammas);
    memory_free64f(&kappas);
    memory_free64f(&thetas);
    memory_free64f(&sigmas);
    // make some random data for testing
    npmax = array_max32i(nps, pSizes, &ierr);
    G = random_rand64f(6*npmax, &ierr);
    // set space
    phiOri = memory_calloc64f(nmt);
    phi = memory_calloc64f(nmt);
    var = memory_calloc64f(npmax);
    d = memory_calloc64f(npmax);
    eye = array_set64f(npmax, 1.0, &ierr);
    // fix the number of threads
    nt0 = omp_get_num_threads();
    omp_set_num_threads(1);
    // loop on point sizes
    for (pSize=0; pSize<nps; pSize++)
    {
        // make synthetic data
        memset(fname, 0, PATH_MAX*sizeof(char));
        sprintf(fname, "blockSize_%d.txt", pSizes[pSize]);
        fout = fopen(fname, "w");
        npts = pSizes[pSize];
        npgrns = npts;
        cblas_dgemv(CblasColMajor, CblasNoTrans,
                    npgrns, 6, 1.0, G, npgrns, &mts[ldm*imtopt], 1, 0.0, d, 1);
        for (i=0; i<nblocks; i++)
        {
            time_tic();
            ierr = parmt_mtSearchL164f(ldm, nmt, npts, blockSizes[i],
                                       nlags, lwantLags,
                                       lrescale,
                                       &G[0*npgrns], &G[1*npgrns], &G[2*npgrns],
                                       &G[3*npgrns], &G[4*npgrns], &G[5*npgrns],
                                       eye, mts, d, phi, var, lags);
            time = time_toc();
            fprintf(fout, "%d %d %e %e %d\n",
                    npts, blockSizes[i], 1.e6*time/(double) nmt, time/(double) nmt, nmt);
            if (i == 0)
            {
                ierr = array_copy64f_work(nmt, phi, phiOri);
            }
            dnorm = array_normDiff64f(nmt, phi, phiOri, ONE_NORM, 1.0, &ierr);
            xmin = array_min64f(nmt, phi, &ierr);
            if (dnorm > DBL_EPSILON*(double) nmt)
            {
                printf("Error computing blocksize\n");
                ierr = 1;
                goto ERROR;
            }
            imt = array_argmax64f(nmt, phi, &ierr);
            if (imt != imtopt)
            {
                printf("Failed to locate optimum\n");
                ierr = 1;
                goto ERROR;
            }
        }
        fclose(fout);
    }
ERROR:;
    // restore number of threads
    omp_set_num_threads(nt0);
    // clear memory
    memory_free64f(&d);
    memory_free64f(&phiOri);
    memory_free64f(&phi);
    memory_free64f(&eye);
    memory_free64f(&var);
    memory_free64f(&mts);
    memory_free64f(&G);
    return 0;
}


/******************************************************************************
CosmoLike Configuration Space Covariances for Projected Galaxy 2-Point Statistics
https://github.com/CosmoLike/CosmoCov
by CosmoLike developers
******************************************************************************/

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

#include <gsl/gsl_interp2d.h>
#include <gsl/gsl_spline2d.h>

#include <time.h>
#include <stdio.h>

void cov_G_cl_cl_real_fft_binned(double *theta, int Ntheta, double *dtheta, int z1, int z2, int z3, int z4, double **cov_g_interp);
void cov_NG_cl_cl_real_fft_binned(double *theta, int Ntheta, double *dtheta, int z1, int z2, int z3, int z4, double **cov_ng_interp);
void cov_G_cl_gl_real_fft_binned(double *theta, int Ntheta, double *dtheta, int z1, int z2, int zl, int zs, double **cov_g_interp);
void cov_NG_cl_gl_real_fft_binned(double *theta, int Ntheta, double *dtheta, int z1, int z2, int zl, int zs, double **cov_ng_interp);
void cov_G_cl_shear_real_fft_binned(double *theta, int Ntheta, double *dtheta, int z1, int z2, int z3, int z4, int pm, double **cov_g_interp);
void cov_NG_cl_shear_real_fft_binned(double *theta, int Ntheta, double *dtheta, int z1, int z2, int z3, int z4, int pm, double **cov_ng_interp);
void cov_G_gl_gl_real_fft_binned(double *theta, int Ntheta, double *dtheta, int zl1, int zs1, int zl2, int zs2, double **cov_g_interp);
void cov_NG_gl_gl_real_fft_binned(double *theta, int Ntheta, double *dtheta, int zl1, int zs1, int zl2, int zs2, double **cov_ng_interp);
void cov_G_gl_shear_real_fft_binned(double *theta, int Ntheta, double *dtheta, int zl, int zs, int z3, int z4, int pm, double **cov_g_interp);
void cov_NG_gl_shear_real_fft_binned(double *theta, int Ntheta, double *dtheta, int zl, int zs, int z3, int z4, int pm, double **cov_ng_interp);
void cov_G_shear_shear_real_fft_binned(double *theta, int Ntheta, double *dtheta, int z1, int z2, int z3, int z4, int pm1, int pm2, double **cov_g_interp);
void cov_NG_shear_shear_real_fft_binned(double *theta, int Ntheta, double *dtheta, int z1, int z2, int z3, int z4, int pm1, int pm2, double **cov_ng_interp);

void cov_G_real_fft_bin_template(double *theta, int Ntheta, double **cov_g_interp, config my_config, double *N, int *array, double *task_type);
void cov_NG_real_fft_bin_template(double *theta, int Ntheta, double **cov_ng_interp, config my_config, int *array, double *task_type);

double func_for_cov_G_shear(double l, int *ar);
double func_for_cov_NG_shear(double l1, double l2, int *ar);
double func_for_cov_G_cl(double l, int *ar);
double func_for_cov_NG_cl(double l1, double l2, int *ar);
double func_for_cov_G_cl_gl(double l, int *ar);
double func_for_cov_NG_cl_gl(double l1, double l2, int *ar);
double func_for_cov_G_cl_shear(double l, int *ar);
double func_for_cov_NG_cl_shear(double l1, double l2, int *ar);
double func_for_cov_G_gl(double l, int *ar);
double func_for_cov_NG_gl(double l1, double l2, int *ar);
double func_for_cov_G_gl_shear(double l, int *ar);
double func_for_cov_NG_gl_shear(double l1, double l2, int *ar);

void cov_G_shear_shear_real_fft_binned(double *theta, int Ntheta, double *dtheta, int z1, int z2, int z3, int z4, int pm1, int pm2, double **cov_g_interp) {

  // pure noise term
  double N[Ntheta],res = 0.;
  int array[5];
  array[1] = z1; array[2] = z2; array[3] = z3; array[4] = z4;

  int i,j;
  for(i=0;i<Ntheta;i++) {N[i] = 0.;}

  if (z1 ==z3 && z2 ==z4 && pm1 == pm2){ //&& pm1 == pm2 required as C+- doesn't have the diagonal shot noise term
    for(i=0;i<Ntheta;i++) {
      N[i] += pow(survey.sigma_e,4.0)/(M_PI*(theta[i+1]*theta[i+1]-theta[i]*theta[i])*4.*nsource(z1)*nsource(z2)*pow(survey.n_gal_conversion_factor,2.0)*survey.area*survey.area_conversion_factor);
    } //(number of galaxy pairs in the survey contributing to annulus of width Dtheta centered at theta1)^-1
  }
  if (z1 ==z4 && z2 ==z3 && pm1 == pm2){ //&& pm1 == pm2 required as C+- doesn't have the diagonal shot noise term
    for(i=0;i<Ntheta;i++) {
      N[i] += pow(survey.sigma_e,4.0)/(M_PI*(theta[i+1]*theta[i+1]-theta[i]*theta[i])*4.*nsource(z1)*nsource(z2)*pow(survey.n_gal_conversion_factor,2.0)*survey.area*survey.area_conversion_factor);
    }
  }
  for(i=0;i<Ntheta;i++) {
    if(N[i]) {N[i]/= w_mask(theta[i]); printf("N[i]: %lg\n", N[i]);}
  }

  // get big cov_G matrix
  config my_config;
  if (pm1 == 1){my_config.l1 = -0.5;}
  else{my_config.l1 = 3.5;}

  if (pm2 == 1){my_config.l2 = -0.5;}
  else{my_config.l2 = 3.5;}

  // my_config.nu1 = 1.01-my_config.l1/2.;
  // my_config.nu2 = 1.01-my_config.l2/2.;

  my_config.nu1 = 1.01;
  my_config.nu2 = 1.01;

  double task_type[] = {2,2};
  cov_G_real_fft_bin_template(theta, Ntheta, cov_g_interp, my_config, N, array, task_type);
}

void cov_NG_shear_shear_real_fft_binned(double *theta, int Ntheta, double *dtheta, int z1, int z2, int z3, int z4, int pm1, int pm2, double **cov_ng_interp) {
  int array[5];
  array[1] = z1; array[2] = z2; array[3] = z3; array[4] = z4;
  int i,j;

  // get big cov_NG matrix
  config my_config;
  if (pm1 == 1){my_config.l1 = -0.5;}
  else{my_config.l1 = 3.5;}

  if (pm2 == 1){my_config.l2 = -0.5;}
  else{my_config.l2 = 3.5;}

  my_config.nu1 = 1.01;
  my_config.nu2 = 1.01;

  double task_type[] = {2,2};
  cov_NG_real_fft_bin_template(theta, Ntheta, cov_ng_interp, my_config, array, task_type);

}

void cov_G_cl_cl_real_fft_binned(double *theta, int Ntheta, double *dtheta, int z1, int z2, int z3, int z4, double **cov_g_interp) {

  // pure noise term
  double N[Ntheta],res = 0.;
  int array[5];
  array[1] = z1; array[2] = z2; array[3] = z3; array[4] = z4;

  int i,j;
  for(i=0;i<Ntheta;i++) {N[i] = 0.;}

  if (z1 ==z3 && z2 ==z4){
    for(i=0;i<Ntheta;i++) {
      N[i] += 1./(M_PI*(theta[i+1]*theta[i+1]-theta[i]*theta[i])*nlens(z1)*nlens(z2)*pow(survey.n_gal_conversion_factor,2.0)*survey.area*survey.area_conversion_factor); //(number of galaxy pairs in the survey contributing to annulus of width Dtheta centered at theta1)^-1
    }
  }
  if (z1 ==z4 && z2 ==z3){
    for(i=0;i<Ntheta;i++) {
      N[i] += 1./(M_PI*(theta[i+1]*theta[i+1]-theta[i]*theta[i])*nlens(z1)*nlens(z2)*pow(survey.n_gal_conversion_factor,2.0)*survey.area*survey.area_conversion_factor);
    }
  }
  for(i=0;i<Ntheta;i++) {
    if(N[i]) N[i]/= w_mask(theta[i]);
  }

  // get big cov_G matrix
  config my_config;
  my_config.l1 = -0.5;
  my_config.l2 = -0.5;

  my_config.nu1 = 1.01;
  my_config.nu2 = 1.01;

  double task_type[] = {0,0};
  cov_G_real_fft_bin_template(theta, Ntheta, cov_g_interp, my_config, N, array, task_type);
}

void cov_NG_cl_cl_real_fft_binned(double *theta, int Ntheta, double *dtheta, int z1, int z2, int z3, int z4, double **cov_ng_interp) {
  int array[5];
  array[1] = z1; array[2] = z2; array[3] = z3; array[4] = z4;
  int i,j;

  // get big cov_NG matrix
  config my_config;
  my_config.l1 = -0.5;
  my_config.l2 = -0.5;
  my_config.nu1 = 1.01;
  my_config.nu2 = 1.01;
  double task_type[] = {0,0};
  cov_NG_real_fft_bin_template(theta, Ntheta, cov_ng_interp, my_config, array, task_type);
}

void cov_G_cl_gl_real_fft_binned(double *theta, int Ntheta, double *dtheta, int z1, int z2, int zl, int zs, double **cov_g_interp) {

  double N[Ntheta],res = 0.;
  int array[5];
  array[1] = z1; array[2] = z2; array[3] = zl; array[4] = zs;

  int i,j;

  // get big cov_G matrix
  config my_config;
  my_config.l1 = -0.5;
  my_config.l2 = 1.5;
  my_config.nu1 = 1.01;
  my_config.nu2 = 1.01;

  double task_type[] = {0,1};
  cov_G_real_fft_bin_template(theta, Ntheta, cov_g_interp, my_config, N, array, task_type);
}

void cov_NG_cl_gl_real_fft_binned(double *theta, int Ntheta, double *dtheta, int z1, int z2, int zl, int zs, double **cov_ng_interp) {
  int array[5];
  array[1] = z1; array[2] = z2; array[3] = zl; array[4] = zs;
  int i,j;

  // get big cov_NG matrix
  config my_config;
  my_config.l1 = -0.5;
  my_config.l2 = 1.5;
  my_config.nu1 = 1.01;
  my_config.nu2 = 1.01;

  double task_type[] = {0,1};
  cov_NG_real_fft_bin_template(theta, Ntheta, cov_ng_interp, my_config, array, task_type);
}

void cov_G_cl_shear_real_fft_binned(double *theta, int Ntheta, double *dtheta, int z1, int z2, int z3, int z4, int pm, double **cov_g_interp) {

  double N[Ntheta],res = 0.;
  int array[5];
  array[1] = z1; array[2] = z2; array[3] = z3; array[4] = z4;

  int i,j;

  // get big cov_G matrix
  config my_config;
  my_config.l1 = -0.5;
  if (pm == 1){my_config.l2 = -0.5;}
  else{my_config.l2 = 3.5;}
  my_config.nu1 = 1.01;
  my_config.nu2 = 1.01;

  double task_type[] = {0,2};
  cov_G_real_fft_bin_template(theta, Ntheta, cov_g_interp, my_config, N, array, task_type);
}

void cov_NG_cl_shear_real_fft_binned(double *theta, int Ntheta, double *dtheta, int z1, int z2, int z3, int z4, int pm, double **cov_ng_interp) {
  int array[5];
  array[1] = z1; array[2] = z2; array[3] = z3; array[4] = z4;
  int i,j;

  // get big cov_NG matrix
  config my_config;
  my_config.l1 = -0.5;
  if (pm == 1){my_config.l2 = -0.5;}
  else{my_config.l2 = 3.5;}
  my_config.nu1 = 1.01;
  my_config.nu2 = 1.01;

  double task_type[] = {0,2};
  cov_NG_real_fft_bin_template(theta, Ntheta, cov_ng_interp, my_config, array, task_type);
}

void cov_G_gl_gl_real_fft_binned(double *theta, int Ntheta, double *dtheta, int zl1, int zs1, int zl2, int zs2, double **cov_g_interp) {

  // pure noise term
  double N[Ntheta],res = 0.;
  int array[5];
  array[1] = zl1; array[2] = zs1; array[3] = zl2; array[4] = zs2;

  int i,j;
  for(i=0;i<Ntheta;i++) {N[i] = 0.;}

  if (zl1 ==zl2 && zs1 ==zs2){
    for(i=0;i<Ntheta;i++) {
      N[i] += pow(survey.sigma_e,2.0)/(2.0*M_PI*(theta[i+1]*theta[i+1]-theta[i]*theta[i])*nlens(zl1)*nsource(zs2)*pow(survey.n_gal_conversion_factor,2.0)*survey.area*survey.area_conversion_factor);
      if(N[i]) N[i]/= w_mask(theta[i]);
    }
  }

  // get big cov_G matrix
  config my_config;
  my_config.l1 = 1.5;
  my_config.l2 = 1.5;
  my_config.nu1 = 1.01;
  my_config.nu2 = 1.01;

  double task_type[] = {1,1};
  cov_G_real_fft_bin_template(theta, Ntheta, cov_g_interp, my_config, N, array, task_type);
}

void cov_NG_gl_gl_real_fft_binned(double *theta, int Ntheta, double *dtheta, int zl1, int zs1, int zl2, int zs2, double **cov_ng_interp) {
  int array[5];
  array[1] = zl1; array[2] = zs1; array[3] = zl2; array[4] = zs2;
  int i,j;

  // get big cov_NG matrix
  config my_config;
  my_config.l1 = 1.5;
  my_config.l2 = 1.5;
  my_config.nu1 = 1.01;
  my_config.nu2 = 1.01;

  double task_type[] = {1,1};
  cov_NG_real_fft_bin_template(theta, Ntheta, cov_ng_interp, my_config, array, task_type);
}

void cov_G_gl_shear_real_fft_binned(double *theta, int Ntheta, double *dtheta, int zl, int zs, int z3, int z4, int pm, double **cov_g_interp) {

  double N[Ntheta],res = 0.;
  int array[5];
  array[1] = zl; array[2] = zs; array[3] = z3; array[4] = z4;

  int i,j;

  // get big cov_G matrix
  config my_config;
  my_config.l1 = 1.5;

  if (pm == 1){my_config.l2 = -0.5;}
  else{my_config.l2 = 3.5;}
  my_config.nu1 = 1.01;
  my_config.nu2 = 1.01;

  double task_type[] = {1,2};
  cov_G_real_fft_bin_template(theta, Ntheta, cov_g_interp, my_config, N, array, task_type);
}

void cov_NG_gl_shear_real_fft_binned(double *theta, int Ntheta, double *dtheta, int zl, int zs, int z3, int z4, int pm, double **cov_ng_interp) {
  int array[5];
  array[1] = zl; array[2] = zs; array[3] = z3; array[4] = z4;
  int i,j;

  // get big cov_NG matrix
  config my_config;
  my_config.l1 = 1.5;

  if (pm == 1){my_config.l2 = -0.5;}
  else{my_config.l2 = 3.5;}
  my_config.nu1 = 1.01;
  my_config.nu2 = 1.01;

  double task_type[] = {1,2};
  cov_NG_real_fft_bin_template(theta, Ntheta, cov_ng_interp, my_config, array, task_type);
}

/*********** Templates *************************************/

void cov_G_real_fft_bin_template(double *theta, int Ntheta, double **cov_g_interp, config my_config, double *N, int *array, double *task_type) {

  my_config.sym_Flag = 0;
  my_config.c_window_width = 0.25;

  double res = 0.;
  int i,j;
  // get input func log-sampled in ell
  const int Nsub = 20; // split each (theta[i],theta[i+1]) range into Nsub sub-ranges, i.e. fill in (Nsub-1) sampling points within the range

  // clock_t start_t, end_t;
  // start_t = clock();

  double ellmax = 50000.;
  double ell_first = 1./theta[Ntheta]; // include the last bin edge
  double ell_last = 1./theta[0];
  double dlnell = log(theta[1]/theta[0])/Nsub;
  int ind_first = floor(log(ell_first)/dlnell); // index of ell_first in log-sampled array (from range [1,ellmax]) containing our orginal desired array (1/theta)
  int Nell = floor(log(ellmax/ell_first)/dlnell) + ind_first + 1; // total sampling points in range [1,ellmax]
  double ell[Nell], func[Nell];
  // printf("ell_first:%lg\n", ell_first);
  // printf("Nell:%d\n", Nell);

  // end_t = clock();
  // printf("Total time taken by CPU: %f\n", (double)(end_t - start_t) / CLOCKS_PER_SEC  );

  double noise_factor = 0.;
  double (*func_for_cov_G)(double, int*);
  if(task_type[0]==0) {
    if(task_type[1]==0) {func_for_cov_G = &func_for_cov_G_cl; noise_factor=1.;}
    else if(task_type[1]==1) {func_for_cov_G = &func_for_cov_G_cl_gl;}
    else if(task_type[1]==2) {func_for_cov_G = &func_for_cov_G_cl_shear;}
  }
  else if(task_type[0]==1) {
    if(task_type[1]==1) {func_for_cov_G = &func_for_cov_G_gl; noise_factor=1.;}
    else if(task_type[1]==2) {func_for_cov_G = &func_for_cov_G_gl_shear;}
  }
  else {
    if(task_type[1]==2) {func_for_cov_G = &func_for_cov_G_shear; noise_factor=2.;}
  }

  for(i=0; i<Nell; i++) {
    ell[i] = exp(dlnell * (i-ind_first)) * ell_first;
    func[i] = func_for_cov_G(ell[i], array);
  }

  // end_t = clock();
  // printf("Total time taken by CPU: %f\n", (double)(end_t - start_t) / CLOCKS_PER_SEC  );

  // extrapolate
  // const int N_extra = 800-246;
  const int N_extra = 800;
  int Ntot = N_extra*2+Nell;
  double large_ell[Ntot], large_fl[Ntot];
  extrap_log_linear_cfastcov(ell, Nell, N_extra, large_ell);
  extrap_log_linear_cfastcov(func, Nell, N_extra, large_fl);

  double **fk1k2;
  fk1k2 = malloc(Ntot*sizeof(double *));
  for(i=0;i<Ntot;i++) {
    fk1k2[i] = malloc(Ntot*sizeof(double));
  }
  mk_diag_g_to_ng(large_fl, Ntot, dlnell, fk1k2);

  // end_t = clock();
  // printf("Total time taken by CPU: %f\n", (double)(end_t - start_t) / CLOCKS_PER_SEC  );

  // do integration
  double theta_fine[Ntot];
  double **result;
  result = malloc(Ntot*sizeof(double *));
  for(i=0;i<Ntot;i++) {
    result[i] = malloc(Ntot*sizeof(double));
  }
  // twobessel(large_ell, large_ell, fk1k2, Ntot, Ntot, &my_config, theta_fine, theta_fine, result);
  double smooth_dlnr = dlnell*Nsub;
  printf("Ntot,%d\n",Ntot);
  twobessel_binave(large_ell, large_ell, fk1k2, Ntot, Ntot, &my_config, smooth_dlnr, 2.5, theta_fine, theta_fine, result);
  // printf("Ntot,%d\n",Ntot);
  // end_t = clock();
  // printf("Total time taken by CPU: %f\n", (double)(end_t - start_t) / CLOCKS_PER_SEC  );

  int ind_theta_first = Ntot - 1 - N_extra-ind_first - Nsub*(Ntheta); // different from nobin case because ind_first is set different!
  for(i=0; i<Ntheta; i++) {
    for(j=0; j<Ntheta; j++) {
      cov_g_interp[i][j] = result[ind_theta_first+i*Nsub][ind_theta_first+j*Nsub];
      cov_g_interp[i][j] /= ( (theta[i+1]*theta[i+1]-theta[i]*theta[i])*(theta[j+1]*theta[j+1]-theta[j]*theta[j]) *M_PI*M_PI*M_PI*survey.area/41253.);
    }
    // printf("theta[%d]=%lg, cov_g_interp[%d][%d]: %lg\n",i, theta[i], i,i, cov_g_interp[i][i]);
    cov_g_interp[i][i] += noise_factor * N[i];
  }

  // end_t = clock();
  // printf("Total time taken by CPU: %f\n", (double)(end_t - start_t) / CLOCKS_PER_SEC  );

  free(result);free(fk1k2);

}

void cov_NG_real_fft_bin_template(double *theta, int Ntheta, double **cov_ng_interp, config my_config, int *array, double *task_type) {
  int i,j;
  my_config.sym_Flag = 0;
  my_config.c_window_width = 0.25;

  // get input func log-sampled in ell
  const int Nsub = 20; // split each (theta[i],theta[i+1]) range into Nsub sub-ranges, i.e. fill in (Nsub-1) sampling points within the range

  // clock_t start_t, end_t;
  // start_t = clock();

  double ellmax = 50000.;
  double ell_first = 1./theta[Ntheta]; // include the last bin edge
  double ell_last = 1./theta[0];
  double dlnell = log(theta[1]/theta[0])/Nsub;
  int ind_first = floor(log(ell_first)/dlnell); // index of ell_first in log-sampled array (from range [1,ellmax]) containing our orginal desired array (1/theta)
  int Nell = floor(log(ellmax/ell_first)/dlnell) + ind_first + 1; // total sampling points in range [1,ellmax]
  double ell[Nell];
  // printf("ell_first:%lg\n", ell_first);
  // printf("Nell:%d\n", Nell);
  for(i=0; i<Nell; i++) {
    ell[i] = exp(dlnell * (i-ind_first)) * ell_first;
  }

  double (*func_for_cov_NG)(double, double, int*);
  if(task_type[0]==0) {
    if(task_type[1]==0) {func_for_cov_NG = &func_for_cov_NG_cl;}
    else if(task_type[1]==1) {func_for_cov_NG = &func_for_cov_NG_cl_gl;}
    else if(task_type[1]==2) {func_for_cov_NG = &func_for_cov_NG_cl_shear;}
  }
  else if(task_type[0]==1) {
    if(task_type[1]==1) {func_for_cov_NG = &func_for_cov_NG_gl;}
    else if(task_type[1]==2) {func_for_cov_NG = &func_for_cov_NG_gl_shear;}
  }
  else {
    if(task_type[1]==2) {func_for_cov_NG = &func_for_cov_NG_shear;}
  }

  double **func;
  func = malloc(Nell*sizeof(double *));
  for(i=0; i<Nell; i++) {
    func[i] = malloc(Nell * sizeof(double));
    for(j=0; j<Nell; j++) {
      func[i][j] = func_for_cov_NG(ell[i], ell[j], array);
    }
  }

  // FILE * output = fopen("func_ng.txt", "w");
  // for(i=0;i<Nell;i++){
  //   for(j=0;j<Nell;j++){
  //   fprintf(output, "%lg ", func[i][j]);
  // }
  // fprintf(output, "\n");
  // }
  // fclose(output);
  // exit(0);

  // extrapolate
  const int N_extra = 800;
  int Ntot = N_extra*2+Nell;
  double large_ell[Ntot];
  double **fk1k2;
  fk1k2 = malloc(Ntot*sizeof(double *));
  for(i=0;i<Ntot;i++) {
    fk1k2[i] = malloc(Ntot*sizeof(double));
  }

  extrap_log_linear_cfastcov(ell, Nell, N_extra, large_ell);
  extrap_2dzeros(func, Nell, N_extra, fk1k2);

  // do integration
  double theta_fine[Ntot];
  double **result;
  result = malloc(Ntot*sizeof(double *));
  for(i=0;i<Ntot;i++) {
    result[i] = malloc(Ntot*sizeof(double));
  }
  // twobessel(large_ell, large_ell, fk1k2, Ntot, Ntot, &my_config, theta_fine, theta_fine, result);
  double smooth_dlnr = dlnell*Nsub;
  twobessel_binave(large_ell, large_ell, fk1k2, Ntot, Ntot, &my_config, smooth_dlnr, 2.5, theta_fine, theta_fine, result);
  printf("Ntot,%d\n",Ntot);

  int ind_theta_first = Ntot - 1 - N_extra-ind_first - Nsub*(Ntheta); // different from nobin case because ind_first is set different!
  for(i=0; i<Ntheta; i++) {
    for(j=0; j<Ntheta; j++) {
      cov_ng_interp[i][j] = result[ind_theta_first+i*Nsub][ind_theta_first+j*Nsub];
      cov_ng_interp[i][j] /= ( (theta[i+1]*theta[i+1]-theta[i]*theta[i])*(theta[j+1]*theta[j+1]-theta[j]*theta[j]) /2. *M_PI*M_PI*M_PI);
    }
  }

  free(result);free(fk1k2);free(func);
}

/********** Functions for differnt covariances ***************************************/
double func_for_cov_G_shear(double l, int *ar){
  double C13, C14, C23, C24, N13=0.0, N14=0.0, N23=0.0, N24=0.0;
  int n1,n2,n3,n4;
  n1 = ar[1]; n2 = ar[2]; n3 = ar[3]; n4 = ar[4];
  //printf("n1=%d n2=%d n3=%d n4=%d l=%le\n",n1,n2,n3,n4,l);
  C13 = C_shear_shear_IA_tab(l,n1,n3);
  C24 = C_shear_shear_IA_tab(l,n2,n4);
  C14 = C_shear_shear_IA_tab(l,n1,n4);
  C23 = C_shear_shear_IA_tab(l,n2,n3);

  if (n1 == n3){N13= pow(survey.sigma_e,2.0)/(2.*nsource(n1)*survey.n_gal_conversion_factor);}
  if (n1 == n4){N14= pow(survey.sigma_e,2.0)/(2.*nsource(n1)*survey.n_gal_conversion_factor);}
  if (n2 == n3){N23= pow(survey.sigma_e,2.0)/(2.*nsource(n2)*survey.n_gal_conversion_factor);}
  if (n2 == n4){N24= pow(survey.sigma_e,2.0)/(2.*nsource(n2)*survey.n_gal_conversion_factor);}

  return (C13*C24+C13*N24+N13*C24 + C14*C23+C14*N23+N14*C23)*l*l*l;
}

double func_for_cov_NG_shear(double l1, double l2, int *ar){
  double tri = 0.,res =0;
  int n1,n2,n3,n4;
  n1 = ar[1]; n2 = ar[2]; n3 = ar[3]; n4 = ar[4];
  tri= bin_cov_NG_shear_shear_tomo(l1,l2,n1,n2,n3,n4);
  // if(isnan(tri)) {tri=0.;}
  return tri * pow(l1, 2.5) * pow(l2, 2.5);

}

double func_for_cov_G_cl(double l, int *ar){
  double C13, C14, C23, C24, N13=0.0, N14=0.0, N23=0.0, N24=0.0;
  int n1,n2,n3,n4;
  n1 = ar[1]; n2 = ar[2]; n3 = ar[3]; n4 = ar[4];
  //printf("n1=%d n2=%d n3=%d n4=%d l=%le\n",n1,n2,n3,n4,l);
  C13 = C_cl_tomo(l,n1,n3);
  C24 = C_cl_tomo(l,n2,n4);
  C14 = C_cl_tomo(l,n1,n4);
  C23 = C_cl_tomo(l,n2,n3);

  N13= tomo.n_lens_ij[n1][n3]/(nlens(n1)*nlens(n3)*survey.n_gal_conversion_factor);
  N14= tomo.n_lens_ij[n1][n4]/(nlens(n1)*nlens(n4)*survey.n_gal_conversion_factor);
  N23= tomo.n_lens_ij[n2][n3]/(nlens(n2)*nlens(n3)*survey.n_gal_conversion_factor);
  N24= tomo.n_lens_ij[n2][n4]/(nlens(n2)*nlens(n4)*survey.n_gal_conversion_factor);

  return (C13*C24+C13*N24+N13*C24 + C14*C23+C14*N23+N14*C23)*l*l*l;
}

double func_for_cov_NG_cl(double l1, double l2, int *ar){
  double tri = 0.,res =0;
  int n1,n2,n3,n4;
  n1 = ar[1]; n2 = ar[2]; n3 = ar[3]; n4 = ar[4];
  tri= bin_cov_NG_cl_cl_tomo(l1,l2,n1,n2,n3,n4);
  // if(isnan(tri)) {tri=0.;}
  return tri * pow(l1, 2.5) * pow(l2, 2.5);
}

double func_for_cov_G_cl_gl(double l, int *ar){
  double C13, C14, C23, C24, N13=0.0, N14=0.0, N23=0.0, N24=0.0;
  int n1,n2,n3,n4;
  n1 = ar[1]; n2 = ar[2]; n3 = ar[3]; n4 = ar[4];
  //printf("n1=%d n2=%d n3=%d n4=%d l=%le\n",n1,n2,n3,n4,l);
  C13 = C_cl_tomo(l,n1,n3);
  C24 = C_ggl_IA_tab(l,n2,n4);
  C14 = C_ggl_IA_tab(l,n1,n4);
  C23 = C_cl_tomo(l,n2,n3);

  N13= tomo.n_lens_ij[n1][n3]/(nlens(n1)*nlens(n3)*survey.n_gal_conversion_factor);
  N23= tomo.n_lens_ij[n2][n3]/(nlens(n2)*nlens(n3)*survey.n_gal_conversion_factor);
  //printf("%lg, %lg, %lg, %lg, %lg, %lg, %lg\n", l, C13, C24, C14, C23, N13, N23);
  return (C13*C24+C13*N24+C24*N13+C14*C23+C14*N23+C23*N14)*l*l*l;
}

double func_for_cov_NG_cl_gl(double l1, double l2, int *ar){
  double tri = 0.,res =0;
  int n1,n2,n3,n4;
  n1 = ar[1]; n2 = ar[2]; n3 = ar[3]; n4 = ar[4];
  tri= bin_cov_NG_cl_gl_tomo(l1,l2,n1,n2,n3,n4);
  // if(isnan(tri)) {tri=0.;}
  return tri * pow(l1, 2.5) * pow(l2, 2.5);
}

double func_for_cov_G_cl_shear(double l, int *ar){
  double C13, C14, C23, C24, N13=0.0, N14=0.0, N23=0.0, N24=0.0;
  int n1,n2,n3,n4;
  n1 = ar[1]; n2 = ar[2]; n3 = ar[3]; n4 = ar[4];
  //printf("n1=%d n2=%d n3=%d n4=%d l=%le\n",n1,n2,n3,n4,l);
  C13 = C_ggl_IA_tab(l,n1,n3);
  C24 = C_ggl_IA_tab(l,n2,n4);
  C14 = C_ggl_IA_tab(l,n1,n4);
  C23 = C_ggl_IA_tab(l,n2,n3);

  return (C13*C24+ C14*C23)*l*l*l;
}

double func_for_cov_NG_cl_shear(double l1, double l2, int *ar){
  double tri = 0.,res =0;
  int n1,n2,n3,n4;
  n1 = ar[1]; n2 = ar[2]; n3 = ar[3]; n4 = ar[4];
  tri= bin_cov_NG_cl_shear_tomo(l1,l2,n1,n2,n3,n4);
  // if(isnan(tri)) {tri=0.;}
  return tri * pow(l1, 2.5) * pow(l2, 2.5);
}

double func_for_cov_G_gl(double l, int *ar){
  double C13, C14, C23, C24, N13=0.0, N14=0.0, N23=0.0, N24=0.0;
  int n1,n2,n3,n4;
  n1 = ar[1]; n2 = ar[2]; n3 = ar[3]; n4 = ar[4];
  //printf("n1=%d n2=%d n3=%d n4=%d l=%le\n",n1,n2,n3,n4,l);
  C13 = C_cl_tomo(l,n1,n3);
  C24 = C_shear_shear_IA_tab(l,n2,n4);
  C14 = C_ggl_IA_tab(l,n1,n4);
  C23 = C_ggl_IA_tab(l,n3,n2);

  N13= tomo.n_lens_ij[n1][n3]/(nlens(n1)*nlens(n3)*survey.n_gal_conversion_factor);
  if (n2 == n4){N24= pow(survey.sigma_e,2.0)/(2.0*nsource(n2)*survey.n_gal_conversion_factor);}
  // printf("%lg, %lg, %lg, %lg, %lg, %lg, %lg\n", l, C13, C24, C14, C23, N13, N24);
  return (C13*C24+C13*N24+N13*C24 + C14*C23+C14*N23+N14*C23)*l*l*l;
}

double func_for_cov_NG_gl(double l1, double l2, int *ar){
  double tri = 0.,res =0;
  int n1,n2,n3,n4;
  n1 = ar[1]; n2 = ar[2]; n3 = ar[3]; n4 = ar[4];
  tri= bin_cov_NG_gl_gl_tomo(l1,l2,n1,n2,n3,n4);
  // if(isnan(tri)) {tri=0.;}
  return tri * pow(l1, 2.5) * pow(l2, 2.5);
}

double func_for_cov_G_gl_shear(double l, int *ar){
  double C13, C14, C23, C24, N13=0.0, N14=0.0, N23=0.0, N24=0.0;
  int n1,n2,n3,n4;
  n1 = ar[1]; n2 = ar[2]; n3 = ar[3]; n4 = ar[4];
  //printf("n1=%d n2=%d n3=%d n4=%d l=%le\n",n1,n2,n3,n4,l);
  C13 = C_ggl_IA_tab(l,n1,n3);
  C24 = C_shear_shear_IA_tab(l,n2,n4);
  C14 = C_ggl_IA_tab(l,n1,n4);
  C23 = C_shear_shear_IA_tab(l,n2,n3);
  if (n2 == n4){N24= pow(survey.sigma_e,2.0)/(2.0*nsource(n2)*survey.n_gal_conversion_factor);}
  if (n2 == n3){N23= pow(survey.sigma_e,2.0)/(2.0*nsource(n2)*survey.n_gal_conversion_factor);}

  return (C13*C24+C13*N24+N13*C24 + C14*C23+C14*N23+N14*C23)*l*l*l;
}

double func_for_cov_NG_gl_shear(double l1, double l2, int *ar){
  double tri = 0.,res =0;
  int n1,n2,n3,n4;
  n1 = ar[1]; n2 = ar[2]; n3 = ar[3]; n4 = ar[4];
  tri= bin_cov_NG_gl_shear_tomo(l1,l2,n1,n2,n3,n4);
  // if(isnan(tri)) {tri=0.;}
  return tri * pow(l1, 2.5) * pow(l2, 2.5);
}


#include <gsl/gsl_math.h>
#include <gsl/gsl_cblas.h>
#include "cblas.h"

void
cblas_sgbmv (const enum CBLAS_ORDER order, const enum CBLAS_TRANSPOSE TransA,
             const int M, const int N, const int KL, const int KU,
             const float alpha, const float *A, const int lda, const float *X,
             const int incX, const float beta, float *Y, const int incY)
{
#define BASE float
#include "source_gbmv_r.h"
#undef BASE
}


#include <stdio.h>
#include <stdarg.h>
#include <string.h>
#include <math.h>
#include <gbpLib.h>
#include <gbpRNG.h>
#include <gbpMCMC.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_fit.h>
#include <gsl/gsl_interp.h>

void read_MCMC_covariance(MCMC_info *MCMC, char *filename) {
    FILE *  fp;
    double *V;
    int     n_P, i_P, j_P;
    if(MCMC->V == NULL)
        SID_log("Initializing the covariance matrix from file {%s}...", SID_LOG_OPEN, filename);
    else
        SID_log("Updating the covariance matrix from file {%s}...", SID_LOG_OPEN, filename);
    fp = fopen(filename, "r");
    SID_fread_verify(&n_P, sizeof(int), 1, fp);
    V = (double *)SID_malloc(sizeof(double) * n_P * n_P);
    SID_fread_verify(V, sizeof(double), n_P * n_P, fp);
    fclose(fp);
    set_MCMC_covariance(MCMC, V);
    SID_free(SID_FARG V);
    SID_log("Done.", SID_LOG_CLOSE);
}


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

#include <config.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_combination.h>

size_t
gsl_combination_n (const gsl_combination * c)
{
  return c->n ;
}

size_t
gsl_combination_k (const gsl_combination * c)
{
  return c->k ;
}

size_t *
gsl_combination_data (const gsl_combination * c)
{
  return c->data ;
}

#ifndef HIDE_INLINE_STATIC
size_t
gsl_combination_get (const gsl_combination * c, const size_t i)
{
  if (gsl_check_range)
    {
      if (i >= c->k)            /* size_t is unsigned, can't be negative */
        {
          GSL_ERROR_VAL ("index out of range", GSL_EINVAL, 0);
        }
    }

  return c->data[i];
}
#endif

int
gsl_combination_valid (gsl_combination * c)
{
  const size_t n = c->n ;
  const size_t k = c->k ;

  size_t i, j ;

  if( k > n )
    {
      GSL_ERROR("combination has k greater than n", GSL_FAILURE) ;
    }
  for (i = 0; i < k; i++)
    {
      const size_t ci = c->data[i];

      if (ci >= n)
        {
          GSL_ERROR("combination index outside range", GSL_FAILURE) ;
        }

      for (j = 0; j < i; j++)
        {
          if (c->data[j] == ci)
            {
              GSL_ERROR("duplicate combination index", GSL_FAILURE) ;
            }
          if (c->data[j] > ci)
            {
              GSL_ERROR("combination indices not in increasing order",
                        GSL_FAILURE) ;
            }
        }
    }

  return GSL_SUCCESS;
}

int
gsl_combination_next (gsl_combination * c)
{
  /* Replaces c with the next combination (in the standard lexicographical
   * ordering).  Returns GSL_FAILURE if there is no next combination.
   */
  const size_t n = c->n;
  const size_t k = c->k;
  size_t *data = c->data;
  size_t i;

  if(k == 0)
    {
      return GSL_FAILURE;
    }
  i = k - 1;

  while(i > 0 && data[i] == n - k + i)
    {
      i--;
    }
  if(i == 0 && data[i] == n - k)
    {
      return GSL_FAILURE;
    }
  data[i]++;
  for(; i < k - 1; i++)
    {
      data[i + 1] = data[i] + 1;
    }
  return GSL_SUCCESS;
}

int
gsl_combination_prev (gsl_combination * c)
{
  /* Replaces c with the previous combination (in the standard
   * lexicographical ordering).  Returns GSL_FAILURE if there is no
   * previous combination.
   */
  const size_t n = c->n;
  const size_t k = c->k;
  size_t *data = c->data;
  size_t i;

  if(k == 0)
    {
      return GSL_FAILURE;
    }
  i = k - 1;

  while(i > 0 && data[i] == data[i-1] + 1)
    {
      i--;
    }
  if(i == 0 && data[i] == 0)
    {
      return GSL_FAILURE;
    }
  data[i++]--;
  for(; i < k; i++)
    {
      data[i] = n - k + i;
    }
  return GSL_SUCCESS;
}

int
gsl_combination_memcpy (gsl_combination * dest, const gsl_combination * src)
{
   const size_t src_n = src->n;
   const size_t src_k = src->k;
   const size_t dest_n = dest->n;
   const size_t dest_k = dest->k;

   if (src_n != dest_n || src_k != dest_k)
     {
       GSL_ERROR ("combination lengths are not equal", GSL_EBADLEN);
     }

   {
     size_t j;

     for (j = 0; j < src_k; j++)
       {
         dest->data[j] = src->data[j];
       }
   }

   return GSL_SUCCESS;
}