/* ===============================================================================*/
/*   Version 1.0.             Cullan Howlett                                      */
/*   Copyright (c) 2017       International Centre for Radio Astronomy Research,  */
/*   The MIT License (MIT)    University of Western Australia                     */
/*                                                                                */
/* Permission is hereby granted, free of charge, to any person obtaining a copy   */
/* of this software and associated documentation files (the "Software"), to deal  */
/* in the Software without restriction, including without limitation the rights   */
/* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell      */
/* copies of the Software, and to permit persons to whom the Software is          */
/* furnished to do so, subject to the following conditions:                       */
/*                                                                                */
/* The above copyright notice and this permission notice shall be included in     */
/* all copies or substantial portions of the Software.                            */
/*                                                                                */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR     */
/* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,       */
/* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE    */
/* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER         */
/* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,  */
/* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN      */
/* THE SOFTWARE.                                                                  */
/* ===============================================================================*/

#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_integration.h>

// Fisher matrix calculation for surveys with velocity and density field measurements. This extended version includes the extra parameters
// associated with primordial non-Gaussianity, scale-dependent density and velocity bias, and zero-point offsets, as per Howlett 2017a.
// ASSUMPTIONS:
//  -  Uncorrelated shot-noise between the density and velocity fields
//  -  I use the trapezium rule to integrate over r. There will be some error due to this, but this makes the most sense as
//     we are binning the number density anyway, and it makes hella difference in the speed of the code.
//  -  The redshift dependence of the non-linear matter and velocity divergence power spectra is captured using linear interpolation.
//  -  The PV error scales as a fixed pecentage of H0*r.
//  -  Flat LCDM cosmology (but not necessarily GR as gammaval can be changed).
//  -  The damping of the velocity and density fields due to non-linear RSD is redshift independent
//  -  The scale dependence of the scale-dependent bias is redshift independent

// The parameters necessary for the calculation
static int nparams = 2;           // The number of free parameters (we can use any of beta, fsigma8, r_g, sigma_g, sigma_u, fnl, Rv, Rd, zp_err)
static int Data[2] = {0,1};     // A vector of flags for the parameters we are interested in (0=beta, 1=fsigma8, 2=r_g, 3=sigma_g, 4=sigma_u, 5=fnl, 6=Rv, 7=Rd, 8=zp_err). MAKE SURE THE LENGTH OF THIS VECTOR, NPARAMS AND THE ENTRIES AGREE/MAKE SENSE, OR YOU MIGHT GET NONSENSE RESULTS!!
static int nziter = 1;            // Now many bins in redshift between zmin and zmax we are considering
static double zmin = 0.0;         // The minimum redshift to consider (You must have power spectra that are within this range or GSL spline will error out)
static double zmax = 0.4;         // The maximum redshift to consider (You must have power spectra that are within this range or GSL spline will error out)
static double Om = 0.3121;        // The matter density at z=0
static double c = 299792.458;     // The speed of light in km/s
static double gammaval = 0.55;    // The value of gammaval to use in the forecasts (where f(z) = Om(z)^gammaval)
static double r_g = 1.0;          // The cross correlation coefficient between the velocity and density fields
static double beta0 = 0.493;      // The value of beta (at z=0, we'll modify this by the redshift dependent value of bias and f as required)
static double sigma80 = 0.8150;   // The value of sigma8 at z=0
static double sigma_u = 13.00;    // The value of the velocity damping parameter in Mpc/h. I use the values from Jun Koda's paper
static double sigma_g = 4.24;     // The value of the density damping parameter in Mpc/h. I use the values from Jun Koda's paper
static double fnl = 0.0;          // The value of fnl contributing to primordial non-Gaussianity
static double Rd = 0.0;           // Scale-dependent bias following the parameterisation in Howlett2017a
static double Rv = 0.0;           // Velocity bias following the parameterisation in Howlett2017a
static double zp_err = 0.05;      // The error in the zeropoint offset if we are interested in the systematic bias caused by such an error (sigma_m in Eq. 34 of Howlett 2017a)
static double do_bias = 0;        // Whether or not to calculate the systematic parameter bias due to neglecting scale-dependent biases.
static double do_zp_bias = 1;     // Whether or not to calculate the systematic parameter bias due to having a zero-point offset.
static double kmax = 0.2;         // The maximum k to evaluate for dd, dv and vv correlations (Typical values are 0.1 - 0.2, on smaller scales the models are likely to break down).
static double survey_area[3] = {0.0, 0.0, 1.65};   // We need to know the survey area for each survey and the overlap area between the surveys (redshift survey only first, then PV survey only, then overlap.
                                                    // For fully overlapping we would have {0, 0, size_overlap}. For redshift larger than PV, we would have {size_red-size_overlap, 0, size_overlap}). Units are pi steradians, such that full sky is 4.0, half sky is 2.0 etc.
static double error_rand = 300.0;    // The observational error due to random non-linear velocities (I normally use 300km/s as in Jun Koda's paper)
static double error_dist = 0.20;     // The percentage error on the distance indicator (Typically 0.05 - 0.10 for SNe IA, 0.2 or more for Tully-Fisher or Fundamental Plane)
static double verbosity = 2;         // How much output to give: 0 = only percentage errors on fsigma8, 1 = other useful info and nuisance parameters, 2 = full fisher and covariance matrices

// The number of redshifts and the redshifts themselves of the input matter and velocity divergence power spectra.
// These numbers are multiplied by 100, converted to ints and written in the form _z0p%02d which is then appended to the filename Pvel_file. See routine read_power.
static double nzin = 11;
static double zin[11] = {0.00, 0.05, 0.10, 0.15, 0.20, 0.25, 0.30, 0.35, 0.40, 0.45, 0.50};
char * Pvel_file = "./example_files/example_pk";          // The file containing the velocity divergence power spectrum. Don't include .dat as we'll append the redshifts on read in
char * Trans_file = "./example_files/example_transfer.dat";   // The file containing the transfer function. Only used for primordial non-Gaussianity and must be normalised to 1 as k->0

// The files containing the number density of the surveys. First is the PV survey, then the redshift survey. These files MUST have the same binning and redshift range,
// so that the sum over redshift bins works (would be fine if we used splines), i.e., if one survey is shallower then that file must contain rows with n(z)=0.
// I also typically save nbar x 10^6 in the input file to make sure I don't lose precision when outputting small nbar values to files. This is corrected when the nbar file
// is read in, so see the read_nz() routine!
char * nbar_file[300] = {"./example_files/taipan_vel_nbar_final_reformat.dat",
                         "./example_files/taipan_red_nbar_final_reformat.dat"};

// Other global parameters and arrays
int NK, * NRED;
double pkkmin;         // The minimum kmin to integrate over, based on the input power spectrum file
double pkkmax;         // The maximum k in the input power spectrum. The maximum k to integrate over is the smallest of this or kmax
double * zarray;
double * rarray;
double * ezarray;
double * deltararray;
double * growtharray;
double ** nbararray;
double * karray, * deltakarray;
double ** pmmarray, ** pmtarray, ** pttarray;
gsl_spline * growth_spline, * r_spline;
gsl_interp_accel * growth_acc, * r_acc;
gsl_spline * Trans_spline, * growth_spline, * r_spline;
gsl_interp_accel * Trans_acc, * growth_acc, * r_acc;

// Prototypes
double zeff_integrand(double mu, void * pin);
double mu_integrand(double mu, void * pin);
double ezinv(double x, void *p);
double rz(double red);
double growthfunc(double x, void *p);
double growthz(double red);
void read_nz();
void read_power();
void read_transfer();
double calc_zeff(double kmin, double zmin_iter, double zmax_iter);
void calc_fisher(double kmin, double zmin_iter, double zmax_iter, double z_eff, double bias_flag, gsl_matrix * Covariance, double * Sys);

// Calculates the fished matrix for a velocity survey.
int main(int argc, char **argv) {

    int i, j;

    // Read in the velocity divergence power spectrum output from the COPTER code (Carlson 2009)
    read_power();

    // Read in the transfer function
    read_transfer();

    // Read in the number densities of the surveys
    read_nz();

    // Run some checks
    if (!((survey_area[0] > 0.0) || (survey_area[2] > 0.0))) {
        for (i=0; i<nparams; i++) {
            if (Data[i] == 2) {
                printf("ERROR: r_g is a free parameter, but there is no information in the density field (Fisher matrix will be singular)\n");
                exit(0);
            }
            if (Data[i] == 3) {
                printf("ERROR: sigma_g is a free parameter, but there is no information in the density field (Fisher matrix will be singular)\n");
                exit(0);
            }
        }
    }
    if (!((survey_area[1] > 0.0) || (survey_area[2] > 0.0))) {
        for (i=0; i<nparams; i++) {
            if (Data[i] == 4) {
                printf("ERROR: sigma_u is a free parameter, but there is no information in the velocity field (Fisher matrix will be singular)\n");
                exit(0);
            }
        }
    }
    if ((sizeof(Data)/sizeof(*Data)) != nparams) {
        printf("ERROR: Size of Data vector for parameters of interest must be equal to nparams\n");
        exit(0);
    }

    printf("Evaluating the Fisher Matrix for %d bins between [z_min = %lf, z_max = %lf]\n", nziter, zmin, zmax);

    if (verbosity == 0) {
        if (do_bias || do_zp_bias) {
            printf("#     zmin         zmax         zeff      fsigma8(z_eff)   TRUE percentage error(z_eff)     percentage bias(z_eff)       INCORRECT percentage error(z_eff)\n");
        } else {
            printf("#     zmin         zmax         zeff      fsigma8(z_eff)   percentage error(z_eff)\n");
        }
    }

    int ziter;
    double * Sys = (double *)malloc(nparams*sizeof(double*));
    gsl_matrix * Covariance = gsl_matrix_alloc(nparams, nparams);
    for (ziter = 0; ziter<nziter; ziter++) {

        double zbinwidth = (zmax-zmin)/(nziter);
        double zmin_iter = ziter*zbinwidth + zmin;
        double zmax_iter = (ziter+1.0)*zbinwidth + zmin;

        double rzmax = gsl_spline_eval(r_spline, zmax_iter, r_acc);
        double kmin = M_PI/rzmax;

        if (verbosity > 0) printf("Evaluating the Fisher Matrix for [k_min = %lf, k_max = %lf] and [z_min = %lf, z_max = %lf]\n", kmin, kmax, zmin_iter, zmax_iter);

        // Calculate the effective redshift (which I base on the sum of the S/N for the density and velocity fields)
        // *********************************************************************************************************
        double z_eff = calc_zeff(kmin, zmin_iter, zmax_iter);

        // Fisher matrix using the TRUE covariance matrix (i.e., with scale-dependent biases). This routine changes based on whether do_zp_bias is true or false.
        // In the former case, this Fisher Matrix DOESN'T include a zero-point offset, as we compute the systematically biased case later. If do_zp_bias is false
        // we are not interested in the bias caused by an offset, and so we treat whatever value is in zp_err as real and the Fisher Matrix DOES include a potential zero-point offset
        // ******************************************************************************************************************
        calc_fisher(kmin, zmin_iter, zmax_iter, z_eff, 0, Covariance, Sys);

        // Bias vector (i.e., Eq. 50 from Howlett 2017a, which uses computes the parameter offset due to neglecting scale-dependent biases or adding a zero-point offset)
        // ******************************************************************************************************************
        if (do_bias || do_zp_bias) calc_fisher(kmin, zmin_iter, zmax_iter, z_eff, 1, Covariance, Sys);

        // Fisher matrix using the INCORRECT covariance matrix (i.e., without scale-dependent biases, and with zero-point offsets). This is used for comparing the
        // magnitude of any bias with respect to the errors (as the errors could also be biased too small!)
        // ******************************************************************************************************************
        if (do_bias || do_zp_bias) calc_fisher(kmin, zmin_iter, zmax_iter, z_eff, 2, Covariance, Sys);

    }

    // Now the full Fisher matrix over all redshifts if we had more than 1 redshift bin. This is equivalent to assuming a single measurement from all data
    // and NOT the same as summing the individual matrices above (which is equivalent to making separate measurements at each redshift, then marginalising over
    // all redshift-dependent quantities, and so may be different for things like fnl).
    if (nziter > 1) {
        double rzmax = gsl_spline_eval(r_spline, zmax, r_acc);
        double kmin = M_PI/rzmax;

        if (verbosity > 0) printf("Finally, evaluating the Fisher Matrix for [k_min = %lf, k_max = %lf] and [z_min = %lf, z_max = %lf]\n", kmin, kmax, zmin, zmax);

        // Calculate the effective redshift (which I base on the sum of the S/N for the density and velocity fields)
        // *********************************************************************************************************
        double z_eff = calc_zeff(kmin, zmin, zmax);

        // Fisher matrix using the TRUE covariance matrix (i.e., with scale-dependent biases). This routine changes based on whether do_zp_bias is true or false.
        // In the former case, this Fisher Matrix DOESN'T include a zero-point offset, as we compute the systematically biased case later. If do_zp_bias is false
        // we are not interested in the bias caused by an offset, and so we treat whatever value is in zp_err as real and the Fisher Matrix DOES include a potential zero-point offset
        // ******************************************************************************************************************
        calc_fisher(kmin, zmin, zmax, z_eff, 0, Covariance, Sys);

        // Bias vector (i.e., Eq. 50 from Howlett 2017a, which uses computes the parameter offset due to neglecting scale-dependent biases or adding a zero-point offset)
        // ******************************************************************************************************************
        if (do_bias || do_zp_bias) calc_fisher(kmin, zmin, zmax, z_eff, 1, Covariance, Sys);

        // Fisher matrix using the INCORRECT covariance matrix (i.e., without scale-dependent biases, and with zero-point offsets). This is used for comparing the
        // magnitude of any bias with respect to the errors (as the errors could also be biased too small!)
        // ******************************************************************************************************************
        if (do_bias || do_zp_bias) calc_fisher(kmin, zmin, zmax, z_eff, 2, Covariance, Sys);

    }

    free(Sys);
    gsl_matrix_free(Covariance);
    gsl_spline_free(growth_spline);
    gsl_interp_accel_free(growth_acc);
    gsl_spline_free(Trans_spline);
    gsl_interp_accel_free(Trans_acc);

    return 0;
}

double calc_zeff(double kmin, double zmin_iter, double zmax_iter) {

    int numk;
    double k_sum1 = 0.0, k_sum2 = 0.0;
    for (numk=0; numk<NK; numk++) {

        double k = karray[numk]+0.5*deltakarray[numk];
        double deltak = deltakarray[numk];
        if (k < kmin) continue;
        if (k > kmax) continue;

        double result, error;
        double params[4] = {numk, k, zmin_iter, zmax_iter};

        size_t nevals = 1000;
        gsl_function F;
        F.function = &zeff_integrand;
        F.params = &params;
        gsl_integration_workspace * w = gsl_integration_workspace_alloc(1000);

        gsl_integration_qags(&F, 0.0, 1.0, 0, 5e-3, nevals, w, &result, &error);
        gsl_integration_workspace_free(w);

        k_sum1 += k*k*deltak*result;
        k_sum2 += k*k*deltak;

    }
    double z_eff = k_sum1/k_sum2;
    if (verbosity > 0) printf("Effective redshift z_eff = %lf\n", z_eff);

    return z_eff;

}

// Calculate the fisher matrix, integrating over k, then mu, then r (r is last as it means we are effectively integrating over effective volume).
// As the input spectra are tabulated we'll just use the trapezium rule to integrate over k
void calc_fisher(double kmin, double zmin_iter, double zmax_iter, double z_eff, double bias_flag, gsl_matrix * Covariance, double * Sys) {

    int i, j, numk;

    double growth_eff = gsl_spline_eval(growth_spline, z_eff, growth_acc);

    double sigma8 = sigma80 * growth_eff;
    double Omz = Om*ezinv(z_eff,NULL)*ezinv(z_eff,NULL)*(1.0+z_eff)*(1.0+z_eff)*(1.0+z_eff);
    double f = pow(Omz, gammaval);
    double beta = f*beta0*growth_eff/pow(Om,0.55);

    if (verbosity > 0) {
        if (do_zp_bias) {
            if (bias_flag == 0) {
                printf("TRUE Fisher matrix without zero-point offset:\n");
            } else if (bias_flag == 1) {
                printf("BIAS vector:\n");
            } else if (bias_flag == 2) {
                printf("INCORRECT Fisher matrix:\n");
            }
        } else {
            if (bias_flag == 0) {
                printf("TRUE Fisher matrix with potential zero-point offset set by zp_err:\n");
            } else if (bias_flag == 1) {
                printf("BIAS vector:\n");
            } else if (bias_flag == 2) {
                printf("INCORRECT Fisher matrix:\n");
            }
        }
    }

    double * Bias = (double *)malloc(nparams*sizeof(double*));
    gsl_matrix * Fisher = gsl_matrix_alloc(nparams, nparams);
    for (i=0; i<nparams; i++) {
        for (j=i; j<nparams; j++) {

            double k_sum = 0.0;
            for (numk=0; numk<NK; numk++) {

                double k = karray[numk]+0.5*deltakarray[numk];
                double deltak = deltakarray[numk];
                if (k < kmin) continue;
                if (k > kmax) continue;

                double result, error;
                double params[7] = {numk, k, Data[i], Data[j], zmin_iter, zmax_iter, bias_flag};

                size_t nevals = 1000;
                gsl_function F;
                F.function = &mu_integrand;
                F.params = &params;
                gsl_integration_workspace * w = gsl_integration_workspace_alloc(1000);

                gsl_integration_qags(&F, 0.0, 1.0, 0, 5e-3, nevals, w, &result, &error);
                gsl_integration_workspace_free(w);

                k_sum += k*k*deltak*result;

            }
            //printf("%d, %d, %lf\n", i, j, k_sum/(4.0*M_PI));
            if (bias_flag == 1) {
                Bias[i] = k_sum/(4.0*M_PI);
            } else {
                gsl_matrix_set(Fisher, i, j, k_sum/(4.0*M_PI));
                gsl_matrix_set(Fisher, j, i, k_sum/(4.0*M_PI));
            }
        }
    }

    if (bias_flag == 1) {

        for (i=0; i<nparams; i++) {
            Sys[i] = 0.0;
            for (j=0; j<nparams; j++) {
                Sys[i] += gsl_matrix_get(Covariance, i, j)*Bias[j];
            }
        }

        if (verbosity > 0) {
            for (i=0; i<nparams; i++) {
                if (Data[i] == 0) {
                    printf("%12.6lf bias on beta = %12.6lf\n", Sys[i], beta);
                }
                if (Data[i] == 1) {
                    printf("%12.6lf bias on fsigma8 = %12.6lf\n", Sys[i], f*sigma8);
                }
                if (Data[i] == 2) {
                    printf("%12.6lf bias on r_g = %12.6lf\n", Sys[i], r_g);
                }
                if (Data[i] == 3) {
                    printf("%12.6lf bias on sigma_g = %12.6lf\n", Sys[i], sigma_g);
                }
                if (Data[i] == 4) {
                    printf("%12.6lf bias on sigma_u = %12.6lf\n", Sys[i], sigma_u);
                }
                if (Data[i] == 5) {
                    printf("%12.6lf bias on fnl = %12.6lf\n", Sys[i], fnl);
                }
            }
        }

    } else {

        if (verbosity == 2) {
            printf("Fisher Matrix\n======================\n");
            for (i=0; i<nparams; i++) {
                printf("[");
                for (j=0; j<nparams; j++) printf("%15.6lf,\t", gsl_matrix_get(Fisher, i, j));
                printf("],\n");
            }
        }

        // Now invert the Fisher matrix
        int s;
        gsl_permutation * p;
        p = gsl_permutation_alloc(nparams);
        gsl_linalg_LU_decomp(Fisher, p, &s);
        gsl_linalg_LU_invert(Fisher, p, Covariance);
        gsl_permutation_free(p);

        if (verbosity == 0) {
            for (i=0; i<nparams; i++) {
                if (Data[i] == 1) {
                    if (do_bias || do_zp_bias) {
                        if (bias_flag == 0) {
                            printf("%12.6lf  %12.6lf  %12.6lf  %12.6lf        %12.6lf        ", zmin_iter, zmax_iter, z_eff, f*sigma8, 100.0*sqrt(gsl_matrix_get(Covariance, i, i))/(f*sigma8));
                        } else if (bias_flag == 2) {
                            printf("          %12.6lf          %12.6lf\n", 100.0*Sys[i]/sqrt(gsl_matrix_get(Covariance, i, i)), 100.0*sqrt(gsl_matrix_get(Covariance, i, i))/(f*sigma8));
                        }
                    } else {
                        printf("%12.6lf  %12.6lf  %12.6lf  %12.6lf  %12.6lf\n", zmin_iter, zmax_iter, z_eff, f*sigma8, 100.0*sqrt(gsl_matrix_get(Covariance, i, i))/(f*sigma8));
                    }
                }
            }
        }

        if (verbosity > 0) {
            printf("Parameter constraints\n======================================================\n");
            for (i=0; i<nparams; i++) {
                if (Data[i] == 0) {
                    printf("beta = %12.6lf +/- %12.6lf\n", beta, sqrt(gsl_matrix_get(Covariance, i, i)));
                    printf("%4.2lf percent error on beta\n", 100.0*sqrt(gsl_matrix_get(Covariance, i, i))/beta);
                    if (bias_flag == 2) printf("%4.2lf percent bias on beta\n", 100.0*Sys[i]/sqrt(gsl_matrix_get(Covariance, i, i)));
                }
                if (Data[i] == 1) {
                    printf("fsigma8 = %12.6lf +/- %12.6lf\n", f*sigma8, sqrt(gsl_matrix_get(Covariance, i, i)));
                    printf("%4.2lf percent error on fsigma8\n", 100.0*sqrt(gsl_matrix_get(Covariance, i, i))/(f*sigma8));
                    if (bias_flag == 2) printf("%4.2lf percent bias on fsigma8\n", 100.0*Sys[i]/sqrt(gsl_matrix_get(Covariance, i, i)));
                }
                if (Data[i] == 2) {
                    printf("r_g = %12.6lf +/- %12.6lf\n", r_g, sqrt(gsl_matrix_get(Covariance, i, i)));
                    printf("%4.2lf percent error on r_g\n", 100.0*sqrt(gsl_matrix_get(Covariance, i, i))/r_g);
                    if (bias_flag == 2) printf("%4.2lf percent bias on r_g\n", 100.0*Sys[i]/sqrt(gsl_matrix_get(Covariance, i, i)));
                }
                if (Data[i] == 3) {
                    printf("sigma_g = %12.6lf +/- %12.6lf\n", sigma_g, sqrt(gsl_matrix_get(Covariance, i, i)));
                    printf("%4.2lf percent error on sigma_g\n", 100.0*sqrt(gsl_matrix_get(Covariance, i, i))/sigma_g);
                    if (bias_flag == 2) printf("%4.2lf percent bias on sigma_g\n", 100.0*Sys[i]/sqrt(gsl_matrix_get(Covariance, i, i)));
                }
                if (Data[i] == 4) {
                    printf("sigma_u = %12.6lf +/- %12.6lf\n", sigma_u, sqrt(gsl_matrix_get(Covariance, i, i)));
                    printf("%4.2lf percent error on sigma_u\n", 100.0*sqrt(gsl_matrix_get(Covariance, i, i))/sigma_u);
                    if (bias_flag == 2) printf("%4.2lf percent bias on sigma_u\n", 100.0*Sys[i]/sqrt(gsl_matrix_get(Covariance, i, i)));
                }
                if (Data[i] == 5) {
                    printf("fnl = %12.6lf +/- %12.6lf\n", fnl, sqrt(gsl_matrix_get(Covariance, i, i)));
                    printf("%4.2lf absolute error on fnl\n", sqrt(gsl_matrix_get(Covariance, i, i)));
                    if (bias_flag == 2) printf("%4.2lf percent bias on fnl\n", 100.0*Sys[i]/sqrt(gsl_matrix_get(Covariance, i, i)));
                }
                if (Data[i] == 6) {
                    printf("Rv = %12.6lf +/- %12.6lf\n", Rv, sqrt(gsl_matrix_get(Covariance, i, i)));
                    printf("%4.2lf percent error on Rv\n", 100.0*sqrt(gsl_matrix_get(Covariance, i, i))/Rv);
                }
                if (Data[i] == 7) {
                    printf("Rd = %12.6lf +/- %12.6lf\n", Rd, sqrt(gsl_matrix_get(Covariance, i, i)));
                    printf("%4.2lf percent error on Rd\n", 100.0*sqrt(gsl_matrix_get(Covariance, i, i))/Rd);
                }
                if (Data[i] == 8) {
                    printf("zp_err = %12.6lf +/- %12.6lf\n", zp_err, sqrt(gsl_matrix_get(Covariance, i, i)));
                    printf("%4.2lf percent error on zp_err\n", 100.0*sqrt(gsl_matrix_get(Covariance, i, i))/zp_err);
                }
            }
        }

        if (verbosity == 2) {
            printf("Covariance Matrix\n======================\n");
            for (i=0; i<nparams; i++) {
                printf("[");
                for (j=0; j<nparams; j++) printf("%15.6lf,\t", gsl_matrix_get(Covariance, i, j));
                printf("],\n");
            }
        }

    }

    free(Bias);
    gsl_matrix_free(Fisher);

}

// The integrand to calculate the effective redshift. I'm not actually sure how this is done in the case of
// density and velocity field measurements, but it seems logical to base it on the sum of the integral of the density spectra and the velocity power spectra
// weighted by their effective signal to noise. In this way the effective redshift is calculated in the same way as a redshift survey, but there is some dependence
// on the S/N in the velocity power spectrum too. In any case, the S/N of the density field measurement is always much higher (because there are
// no errors and the number density is higher) and so this dominates the effective redshift calculation.
double zeff_integrand(double mu, void * pin) {

    int i, j, m, q, u, surv;
    double * p = (double *)pin;

    int numk = (int)p[0];
    double k = p[1];
    double zminval = p[2];
    double zmaxval = p[3];
    gsl_interp_accel * Pmm_acc, * Pmt_acc, * Ptt_acc;
    gsl_spline * Pmm_spline, * Pmt_spline, * Ptt_spline;
    if (nzin > 1) {
        double * Pmm_array = (double *)malloc(nzin*sizeof(double));
        double * Pmt_array = (double *)malloc(nzin*sizeof(double));
        double * Ptt_array = (double *)malloc(nzin*sizeof(double));
        for (j=0; j<nzin; j++) {
            Pmm_array[j] = pmmarray[j][numk];
            Pmt_array[j] = pmtarray[j][numk];
            Ptt_array[j] = pttarray[j][numk];
        }
        Pmm_acc    = gsl_interp_accel_alloc();
        Pmm_spline = gsl_spline_alloc(gsl_interp_cspline, nzin);
        gsl_spline_init(Pmm_spline, zin, Pmm_array, nzin);
        free(Pmm_array);

        Pmt_acc    = gsl_interp_accel_alloc();
        Pmt_spline = gsl_spline_alloc(gsl_interp_cspline, nzin);
        gsl_spline_init(Pmt_spline, zin, Pmt_array, nzin);
        free(Pmt_array);

        Ptt_acc    = gsl_interp_accel_alloc();
        Ptt_spline = gsl_spline_alloc(gsl_interp_cspline, nzin);
        gsl_spline_init(Ptt_spline, zin, Ptt_array, nzin);
        free(Ptt_array);
    }

    double Tk = gsl_spline_eval(Trans_spline, k, Trans_acc);
    double Ak0 = 3.0*1.686*Om*1.0e4/(k*k*Tk*c*c);                         // The prefactor for the scale dependent bias induced by primordial non-Gaussianity
    double dendamp = sqrt(1.0/(1.0+0.5*(k*k*mu*mu*sigma_g*sigma_g)));     // This is unitless
    double veldamp = sin(k*sigma_u)/(k*sigma_u);                          // This is unitless

    double dVeff = 0.0, zdVeff = 0.0;
    for (i=0; i<NRED[0]; i++) {

        double zval = zarray[i];
        if (zval < zminval) continue;
        if (zval > zmaxval) break;

        double r_sum = 0.0;
        double r = rarray[i];
        double deltar = deltararray[i];

        double dd_prefac=0.0, vv_prefac=0.0;
        double P_gg=0.0, P_uu=0.0;

        double Ak = Ak0/growtharray[i];
        double sigma8 = sigma80 * growtharray[i];

        // First lets calculate the relevant power spectra. Interpolate the power spectra linearly in redshift
        double Pmm, Pmt, Ptt;
        Pmm = gsl_spline_eval(Pmm_spline, zval, Pmm_acc);
        Pmt = gsl_spline_eval(Pmt_spline, zval, Pmm_acc);
        Ptt = gsl_spline_eval(Ptt_spline, zval, Pmm_acc);

        double Omz = Om*ezinv(zval,NULL)*ezinv(zval,NULL)*(1.0+zval)*(1.0+zval)*(1.0+zval);
        double f = pow(Omz, gammaval);
        double beta = f*beta0*growtharray[i]/pow(Om,0.55);
        double bv = 1.0 - Rv*Rv*k*k;
        double bsd = Rd*k*k;
        double beta_sd = 1.0/beta + bsd/f;

        vv_prefac  = 1.0e2*bv*f*mu*veldamp/k;
        dd_prefac = ((1.0+fnl*Ak)*(1.0+fnl*Ak)*(beta_sd*beta_sd) + 2.0*bv*r_g*mu*mu*(1.0+fnl*Ak)*beta_sd + bv*bv*mu*mu*mu*mu - 2.0*fnl*Ak*(1.0+fnl*Ak)*beta_sd/f - 2.0*bv*r_g*mu*mu*fnl*Ak/f + fnl*fnl*Ak*Ak/(f*f))*f*f*dendamp*dendamp;
        P_gg = dd_prefac*Pmm;
        P_uu = vv_prefac*vv_prefac*Ptt;

        // We need to do the overlapping and non-overlapping parts of the redshifts and PV surveys separately
        for (surv=0; surv<3; surv++) {
            double surv_sum = 0.0;
            if (survey_area[surv] > 0.0) {
                double error_obs, error_noise, n_g = 0.0, n_u = 0.0;

                // Set the nbar for each section.
                if (surv == 0) {
                    n_g = nbararray[1][i];
                } else if (surv == 1) {
                    error_obs = 100.0*error_dist*r;                              // Percentage error * distance * H0 in km/s (factor of 100.0 comes from hubble parameter)
                    error_noise = error_rand*error_rand + error_obs*error_obs;   // Error_noise is in km^{2}s^{-2}
                    n_u = nbararray[0][i]/error_noise;
                } else {
                    error_obs = 100.0*error_dist*r;                              // Percentage error * distance * H0 in km/s (factor of 100.0 comes from hubble parameter)
                    error_noise = error_rand*error_rand + error_obs*error_obs;   // Error_noise is in km^{2}s^{-2}
                    n_u = nbararray[0][i]/error_noise;
                    n_g = nbararray[1][i];
                }

                double value1 = n_g/(1.0 + n_g*P_gg);
                double value2 = n_u/(1.0 + n_u*P_uu);
                surv_sum += value1*value1 + value2*value2;

                surv_sum *= survey_area[surv];
                r_sum += surv_sum;
            }
        }

        dVeff += r*r*deltar*r_sum;
        zdVeff += zval*r*r*deltar*r_sum;

    }

    gsl_spline_free(Pmm_spline);
    gsl_spline_free(Pmt_spline);
    gsl_spline_free(Ptt_spline);
    gsl_interp_accel_free(Pmm_acc);
    gsl_interp_accel_free(Pmt_acc);
    gsl_interp_accel_free(Ptt_acc);

    return zdVeff/dVeff;

}

// The integrand for the integral over mu in the Fisher matrix calculation.
// For each mu we need to create a 2x2 matrix of the relevant power spectra derivatives and the inverse of the power spectrum matrix.
// Because there are some regions where the number density goes to zero we have to work directly with the inverse as it is difficult to invert numerically
// but if we deal with the inverse only then we can just set the relevant parts to zero when the number density is zero.
double mu_integrand(double mu, void * pin) {

    int i, j, m, q, u, surv;
    double * p = (double *)pin;
    double result, error;

    int numk = (int)p[0];
    double k = p[1];
    double zminval = p[4];
    double zmaxval = p[5];
    gsl_interp_accel * Pmm_acc, * Pmt_acc, * Ptt_acc;
    gsl_spline * Pmm_spline, * Pmt_spline, * Ptt_spline;
    double * Pmm_array = (double *)malloc(nzin*sizeof(double));
    double * Pmt_array = (double *)malloc(nzin*sizeof(double));
    double * Ptt_array = (double *)malloc(nzin*sizeof(double));
    for (j=0; j<nzin; j++) {
        Pmm_array[j] = pmmarray[j][numk];
        Pmt_array[j] = pmtarray[j][numk];
        Ptt_array[j] = pttarray[j][numk];
    }
    Pmm_acc    = gsl_interp_accel_alloc();
    Pmm_spline = gsl_spline_alloc(gsl_interp_cspline, nzin);
    gsl_spline_init(Pmm_spline, zin, Pmm_array, nzin);
    free(Pmm_array);

    Pmt_acc    = gsl_interp_accel_alloc();
    Pmt_spline = gsl_spline_alloc(gsl_interp_cspline, nzin);
    gsl_spline_init(Pmt_spline, zin, Pmt_array, nzin);
    free(Pmt_array);

    Ptt_acc    = gsl_interp_accel_alloc();
    Ptt_spline = gsl_spline_alloc(gsl_interp_cspline, nzin);
    gsl_spline_init(Ptt_spline, zin, Ptt_array, nzin);
    free(Ptt_array);

    double Tk = gsl_spline_eval(Trans_spline, k, Trans_acc);
    double Ak0 = 3.0*1.686*Om*1.0e4/(k*k*Tk*c*c);                         // The prefactor for the scale dependent bias induced by primordial non-Gaussianity
    double dendamp = sqrt(1.0/(1.0+0.5*(k*k*mu*mu*sigma_g*sigma_g)));     // This is unitless
    double veldamp = sin(k*sigma_u)/(k*sigma_u);                          // This is unitless

    double result_sum = 0.0;
    for (i=0; i<NRED[0]; i++) {

        double zval = zarray[i];
        double r_sum = 0.0;
        double r = rarray[i];
        double ez = ezarray[i];
        double deltar = deltararray[i];

        if (zval < zminval) continue;
        if (zval > zmaxval) break;

        double dd_prefac=0.0, dv_prefac=0.0, vv_prefac=0.0;
        double dd_prefac_bias=0.0, dv_prefac_bias=0.0, vv_prefac_bias=0.0;
        double P_gg=0.0, P_ug=0.0, P_uu=0.0;
        double P_gg_bias=0.0, P_ug_bias=0.0, P_uu_bias=0.0;

        double Ak = Ak0/growtharray[i];
        double sigma8 = sigma80 * growtharray[i];

        // First lets calculate the relevant power spectra. Interpolate the power spectra linearly in redshift
        double Pmm, Pmt, Ptt;
        Pmm = gsl_spline_eval(Pmm_spline, zval, Pmm_acc);
        Pmt = gsl_spline_eval(Pmt_spline, zval, Pmm_acc);
        Ptt = gsl_spline_eval(Ptt_spline, zval, Pmm_acc);

        double Omz = Om*ezinv(zval,NULL)*ezinv(zval,NULL)*(1.0+zval)*(1.0+zval)*(1.0+zval);
        double f = pow(Omz, gammaval);
        double beta = f*beta0*growtharray[i]/pow(Om,0.55);
        double bv = 1.0 - Rv*Rv*k*k;
        double bsd = Rd*k*k;
        double beta_sd = 1.0/beta + bsd/f;

        // If p[6] (bias_flag) == 2 this means we are computing the Fisher matrix INCLUDING systematics, and so we need the covariance matrix without scale-dependent bias
        if ((int)p[6] == 2) {
            bv = 1.0;
            beta_sd = 1.0/beta;
        }

        // In the presence of a zero-point offset we have to calculate the additional velocity recieved.
        // This is redshift dependent if we assume a fixed offset in the logarithmic distance ratio
        double zp_err_prefac = c*log(10.0)/(1.0 - ((c*(1.0 + zval)*(1.0 + zval))/(100.0*ez*r)))/5.0;
        double zp_err_v = zp_err*zp_err_prefac;

        vv_prefac  = 1.0e2*bv*f*mu*veldamp/k;
        dd_prefac = ((1.0+fnl*Ak)*(1.0+fnl*Ak)*(beta_sd*beta_sd) + 2.0*bv*r_g*mu*mu*(1.0+fnl*Ak)*beta_sd + bv*bv*mu*mu*mu*mu - 2.0*fnl*Ak*(1.0+fnl*Ak)*beta_sd/f - 2.0*bv*r_g*mu*mu*fnl*Ak/f + fnl*fnl*Ak*Ak/(f*f))*f*f*dendamp*dendamp;
        dv_prefac = (r_g*(1.0+fnl*Ak)*beta_sd - r_g*fnl*Ak/f + bv*mu*mu)*f*dendamp;

        P_gg = dd_prefac*Pmm;
        P_ug = vv_prefac*dv_prefac*Pmt;
        P_uu = vv_prefac*vv_prefac*Ptt;

        // And now the derivatives. Need to create a matrix of derivatives for each of the two parameters of interest
        gsl_matrix * dPdt1 = gsl_matrix_calloc(2, 2);
        gsl_matrix * dPdt2 = gsl_matrix_calloc(2, 2);
        double value;
        switch((int)p[2]) {
            // Differential w.r.t betaA
            case 0:
                value = -2.0*((1.0+fnl*Ak)*(1.0+fnl*Ak)*beta_sd + bv*r_g*mu*mu*(1.0+fnl*Ak) - fnl*Ak*(1.0+fnl*Ak)/f)*f*f*dendamp*dendamp*Pmm/(beta*beta);
                gsl_matrix_set(dPdt1, 0, 0, value);
                value = -(vv_prefac*f*r_g*(1.0+fnl*Ak)*dendamp*Pmt)/(beta*beta);
                gsl_matrix_set(dPdt1, 0, 1, value);
                gsl_matrix_set(dPdt1, 1, 0, value);
                break;
            // Differential w.r.t fsigma8
            case 1:
                value = 2.0*(f*(1.0+fnl*Ak)*(1.0+fnl*Ak)*beta_sd/beta + f*r_g*bv*mu*mu*(1.0+fnl*Ak)*(2.0/beta + bsd/f) + f*bv*bv*mu*mu*mu*mu - fnl*Ak*(1.0+fnl*Ak)/beta - bv*r_g*mu*mu*fnl*Ak)*dendamp*dendamp*Pmm/sigma8;
                gsl_matrix_set(dPdt1, 0, 0, value);
                value = vv_prefac*(r_g*(1.0+fnl*Ak)*(2.0/beta + bsd/f) - r_g*fnl*Ak/f + bv*mu*mu)*dendamp*Pmt/sigma8;
                gsl_matrix_set(dPdt1, 0, 1, value);
                gsl_matrix_set(dPdt1, 1, 0, value);
                value = (2.0*P_uu)/(f*sigma8);
                gsl_matrix_set(dPdt1, 1, 1, value);
                break;
            // Differential w.r.t r_g
            case 2:
                value = 2.0*bv*mu*mu*((1.0+fnl*Ak)*beta_sd - fnl*Ak/f)*f*f*dendamp*dendamp*Pmm;
                gsl_matrix_set(dPdt1, 0, 0, value);
                value = vv_prefac*((1.0+fnl*Ak)*beta_sd - fnl*Ak/f)*f*dendamp*Pmt;
                gsl_matrix_set(dPdt1, 0, 1, value);
                gsl_matrix_set(dPdt1, 1, 0, value);
                break;
            // Differential w.r.t sigma_g
            case 3:
                value = -k*k*mu*mu*dendamp*dendamp*sigma_g*P_gg;
                gsl_matrix_set(dPdt1, 0, 0, value);
                value = -0.5*k*k*mu*mu*dendamp*dendamp*sigma_g*P_ug;
                gsl_matrix_set(dPdt1, 0, 1, value);
                gsl_matrix_set(dPdt1, 1, 0, value);
                break;
            // Differential w.r.t sigma_u
            case 4:
                value = P_ug*(k*cos(k*sigma_u)/sin(k*sigma_u) - 1.0/sigma_u);
                gsl_matrix_set(dPdt1, 0, 1, value);
                gsl_matrix_set(dPdt1, 1, 0, value);
                value = 2.0*P_uu*(k*cos(k*sigma_u)/sin(k*sigma_u) - 1.0/sigma_u);
                gsl_matrix_set(dPdt1, 1, 1, value);
                break;
            // Differential w.r.t fnl
            case 5:
                value = 2.0*Ak*((1.0+fnl*Ak)*beta_sd*beta_sd + bv*r_g*mu*mu*(beta_sd - 1.0/f) - (1.0+2.0*fnl*Ak)*beta_sd/f + fnl*Ak/(f*f))*f*f*dendamp*dendamp*Pmm;
                gsl_matrix_set(dPdt1, 0, 0, value);
                value = vv_prefac*r_g*Ak*(beta_sd - 1.0/f)*f*dendamp*Pmt;
                gsl_matrix_set(dPdt1, 0, 1, value);
                gsl_matrix_set(dPdt1, 1, 0, value);
                break;
            // Differential w.r.t Rv
            case 6:
                value = -4.0*Rv*k*k*(r_g*mu*mu*((1.0+fnl*Ak)*beta_sd - fnl*Ak/f) + bv*mu*mu*mu*mu)*f*f*dendamp*dendamp*Pmm;
                gsl_matrix_set(dPdt1, 0, 0, value);
                value = -2.0*Rv*k*k*vv_prefac*(r_g*(1.0+fnl*Ak)*beta_sd/bv - r_g*fnl*Ak/(f*bv) + 2.0*mu*mu)*f*dendamp*Pmt;
                gsl_matrix_set(dPdt1, 0, 1, value);
                gsl_matrix_set(dPdt1, 1, 0, value);
                value = -4.0*Rv*k*k*P_uu/bv;
                gsl_matrix_set(dPdt1, 1, 1, value);
                break;
            // Differential w.r.t Rd
            case 7:
                value = 2.0*k*k*(f*(1.0+fnl*Ak)*(1.0+fnl*Ak)*beta_sd + bv*r_g*f*mu*mu*(1.0+fnl*Ak) - fnl*Ak*(1.0+fnl*Ak))*dendamp*dendamp*Pmm;
                gsl_matrix_set(dPdt1, 0, 0, value);
                value = vv_prefac*k*k*r_g*(1.0+fnl*Ak)*dendamp*Pmt;
                gsl_matrix_set(dPdt1, 0, 1, value);
                gsl_matrix_set(dPdt1, 1, 0, value);
                break;
            // Differential w.r.t. zp_err
            case 8:
                if (nbararray[0][i] > 0.0) {
                    value = 2.0*zp_err_v*zp_err_prefac/nbararray[0][i];
                    gsl_matrix_set(dPdt1, 1, 1, value);
                }
                break;
            default:
                break;
        }
        // If we want to calculate the Bias vector and look at systematics from scale-dependent bias or zero-point offsets, we simply have to replace dPdt2 with the difference between the models with and without bias.
        // The subscript "bias" is the systematically biased covariance matrix, which, in the most confusing way possible, is the one which doesn't have the velocity/scale-dependent bias included or does have the zero-point offset
        if ((int)p[6] == 1) {
            // Calculate the model without scale-dependent biases
            if (do_bias) {
                double bv_bias = 1.0;
                double beta_sd_bias = 1.0/beta;
                vv_prefac_bias  = 1.0e2*bv_bias*f*mu*veldamp/k;
                dd_prefac_bias = ((1.0+fnl*Ak)*(1.0+fnl*Ak)*(beta_sd_bias*beta_sd_bias) + 2.0*bv_bias*r_g*mu*mu*(1.0+fnl*Ak)*beta_sd_bias + bv_bias*bv_bias*mu*mu*mu*mu - 2.0*fnl*Ak*(1.0+fnl*Ak)*beta_sd_bias/f - 2.0*bv_bias*r_g*mu*mu*fnl*Ak/f + fnl*fnl*Ak*Ak/(f*f))*f*f*dendamp*dendamp;
                dv_prefac_bias = (r_g*(1.0+fnl*Ak)*beta_sd_bias - r_g*fnl*Ak/f + bv_bias*mu*mu)*f*dendamp;
                P_gg_bias = dd_prefac_bias*Pmm;
                P_ug_bias = vv_prefac_bias*dv_prefac_bias*Pmt;
                P_uu_bias = vv_prefac_bias*vv_prefac_bias*Ptt;
                gsl_matrix_set(dPdt2, 0, 0, P_gg - P_gg_bias);
                gsl_matrix_set(dPdt2, 0, 1, P_ug - P_ug_bias);
                gsl_matrix_set(dPdt2, 1, 0, P_ug - P_ug_bias);
                gsl_matrix_set(dPdt2, 1, 1, P_uu - P_uu_bias);
                //printf("%lf, %lf, %lf, %lf\n",bvA, P_uuAA, P_uuAA_bias, gsl_matrix_get(dPdt2, 1, 1));
            }
            if (do_zp_bias) {
                if (nbararray[0][i] > 0.0) {
                    gsl_matrix_set(dPdt2, 1, 1, zp_err_v*zp_err_v/nbararray[0][i]);
                }
            }
        } else {
            switch((int)p[3]) {
                // Differential w.r.t betaA
                case 0:
                    value = -2.0*((1.0+fnl*Ak)*(1.0+fnl*Ak)*beta_sd + bv*r_g*mu*mu*(1.0+fnl*Ak) - fnl*Ak*(1.0+fnl*Ak)/f)*f*f*dendamp*dendamp*Pmm/(beta*beta);
                    gsl_matrix_set(dPdt2, 0, 0, value);
                    value = -(vv_prefac*f*r_g*(1.0+fnl*Ak)*dendamp*Pmt)/(beta*beta);
                    gsl_matrix_set(dPdt2, 0, 1, value);
                    gsl_matrix_set(dPdt2, 1, 0, value);
                    break;
                // Differential w.r.t fsigma8
                case 1:
                    value = 2.0*(f*(1.0+fnl*Ak)*(1.0+fnl*Ak)*beta_sd/beta + f*r_g*bv*mu*mu*(1.0+fnl*Ak)*(2.0/beta + bsd/f) + f*bv*bv*mu*mu*mu*mu - fnl*Ak*(1.0+fnl*Ak)/beta - bv*r_g*mu*mu*fnl*Ak)*dendamp*dendamp*Pmm/sigma8;
                    gsl_matrix_set(dPdt2, 0, 0, value);
                    value = vv_prefac*(r_g*(1.0+fnl*Ak)*(2.0/beta + bsd/f) - r_g*fnl*Ak/f + bv*mu*mu)*dendamp*Pmt/sigma8;
                    gsl_matrix_set(dPdt2, 0, 1, value);
                    gsl_matrix_set(dPdt2, 1, 0, value);
                    value = (2.0*P_uu)/(f*sigma8);
                    gsl_matrix_set(dPdt2, 1, 1, value);
                    break;
                // Differential w.r.t r_g
                case 2:
                    value = 2.0*bv*mu*mu*((1.0+fnl*Ak)*beta_sd - fnl*Ak/f)*f*f*dendamp*dendamp*Pmm;
                    gsl_matrix_set(dPdt2, 0, 0, value);
                    value = vv_prefac*((1.0+fnl*Ak)*beta_sd - fnl*Ak/f)*f*dendamp*Pmt;
                    gsl_matrix_set(dPdt2, 0, 1, value);
                    gsl_matrix_set(dPdt2, 1, 0, value);
                    break;
                // Differential w.r.t sigma_g
                case 3:
                    value = -k*k*mu*mu*dendamp*dendamp*sigma_g*P_gg;
                    gsl_matrix_set(dPdt2, 0, 0, value);
                    value = -0.5*k*k*mu*mu*dendamp*dendamp*sigma_g*P_ug;
                    gsl_matrix_set(dPdt2, 0, 1, value);
                    gsl_matrix_set(dPdt2, 1, 0, value);
                    break;
                // Differential w.r.t sigma_u
                case 4:
                    value = P_ug*(k*cos(k*sigma_u)/sin(k*sigma_u) - 1.0/sigma_u);
                    gsl_matrix_set(dPdt2, 0, 1, value);
                    gsl_matrix_set(dPdt2, 1, 0, value);
                    value = 2.0*P_uu*(k*cos(k*sigma_u)/sin(k*sigma_u) - 1.0/sigma_u);
                    gsl_matrix_set(dPdt2, 1, 1, value);
                    break;
                // Differential w.r.t fnl
                case 5:
                    value = 2.0*Ak*((1.0+fnl*Ak)*beta_sd*beta_sd + bv*r_g*mu*mu*(beta_sd - 1.0/f) - (1.0+2.0*fnl*Ak)*beta_sd/f + fnl*Ak/(f*f))*f*f*dendamp*dendamp*Pmm;
                    gsl_matrix_set(dPdt2, 0, 0, value);
                    value = vv_prefac*r_g*Ak*(beta_sd - 1.0/f)*f*dendamp*Pmt;
                    gsl_matrix_set(dPdt2, 0, 1, value);
                    gsl_matrix_set(dPdt2, 1, 0, value);
                    break;
                // Differential w.r.t Rv
                case 6:
                    value = -4.0*Rv*k*k*(r_g*mu*mu*((1.0+fnl*Ak)*beta_sd - fnl*Ak/f) + bv*mu*mu*mu*mu)*f*f*dendamp*dendamp*Pmm;
                    gsl_matrix_set(dPdt2, 0, 0, value);
                    value = -2.0*Rv*k*k*vv_prefac*(r_g*(1.0+fnl*Ak)*beta_sd/bv - r_g*fnl*Ak/(f*bv) + 2.0*mu*mu)*f*dendamp*Pmt;
                    gsl_matrix_set(dPdt2, 0, 1, value);
                    gsl_matrix_set(dPdt2, 1, 0, value);
                    value = -4.0*Rv*k*k*P_uu/bv;
                    gsl_matrix_set(dPdt2, 1, 1, value);
                    break;
                // Differential w.r.t Rd
                case 7:
                    value = 2.0*k*k*(f*(1.0+fnl*Ak)*(1.0+fnl*Ak)*beta_sd + bv*r_g*f*mu*mu*(1.0+fnl*Ak) - fnl*Ak*(1.0+fnl*Ak))*dendamp*dendamp*Pmm;
                    gsl_matrix_set(dPdt2, 0, 0, value);
                    value = vv_prefac*k*k*r_g*(1.0+fnl*Ak)*dendamp*Pmt;
                    gsl_matrix_set(dPdt2, 0, 1, value);
                    gsl_matrix_set(dPdt2, 1, 0, value);
                    break;
                // Differential w.r.t. zp_err
                case 8:
                    if (nbararray[0][i] > 0.0) {
                        value = 2.0*zp_err_v*zp_err_prefac/nbararray[0][i];
                        gsl_matrix_set(dPdt2, 1, 1, value);
                    }
                    break;
                default:
                    break;
            }
        }

        // If we are looking at the bias from the zero-point error, then once we have calculated dPdt2 we want the covariance matrix without
        // systematics and so without a zero-point offset. The same is true if we want the TRUE covariance matrix
        if (do_zp_bias) {
            if (((int)p[6] == 0) || ((int)p[6] == 1)) zp_err_v = 0.0;
        }

        // We need to do the overlapping and non-overlapping parts of the surveys separately
        for (surv=0; surv<3; surv++) {
            double surv_sum = 0.0;
            if (survey_area[surv] > 0.0) {
                double error_obs, error_noise, n_g = 0.0, n_u = 0.0;

                // Set the nbar for each section.
                if (surv == 0) {
                    n_g = nbararray[1][i];
                } else if (surv == 1) {
                    error_obs = 100.0*error_dist*r;                                                  // Percentage error * distance * H0 in km/s (factor of 100.0 comes from hubble parameter)
                    error_noise = error_rand*error_rand + error_obs*error_obs + zp_err_v*zp_err_v;   // Error_noise is in km^{2}s^{-2}
                    n_u = nbararray[0][i]/error_noise;
                } else {
                    error_obs = 100.0*error_dist*r;                                                  // Percentage error * distance * H0 in km/s (factor of 100.0 comes from hubble parameter)
                    error_noise = error_rand*error_rand + error_obs*error_obs + zp_err_v*zp_err_v;   // Error_noise is in km^{2}s^{-2}
                    n_u = nbararray[0][i]/error_noise;
                    n_g = nbararray[1][i];
                }

                //printf("%lf, %lf, %lf\n", r, n_g, 1.0e6*n_u);

                if (!((n_u > 0.0) || (n_g > 0.0))) continue;

                // First we need the determinant.
                double det = 1.0 + n_u*n_g*(P_gg*P_uu - P_ug*P_ug) + n_u*P_uu + n_g*P_gg;

                // Now the inverse matrix.
                gsl_matrix * iP = gsl_matrix_calloc(2, 2);
                value = n_u*n_g*P_uu + n_g;
                gsl_matrix_set(iP, 0, 0, value);
                value = n_g*n_u*P_gg + n_u;
                gsl_matrix_set(iP, 1, 1, value);
                value = - n_g*n_u*P_ug;
                gsl_matrix_set(iP, 0, 1, value);
                gsl_matrix_set(iP, 1, 0, value);

                // Finally we need to compute the Fisher integrand by summing over the inverse and differential matrices
                for (j=0; j<2; j++) {
                    for (m=0; m<2; m++) {
                        for (u=0; u<2; u++) {
                            for (q=0; q<2; q++) {
                                value = gsl_matrix_get(dPdt1, j, q)*gsl_matrix_get(iP, q, u)*gsl_matrix_get(dPdt2, u, m)*gsl_matrix_get(iP, m, j);
                                surv_sum += value;
                            }
                        }
                    }
                }
                surv_sum /= det*det;
                surv_sum *= survey_area[surv];
                r_sum += surv_sum;
                gsl_matrix_free(iP);
                //printf("%d, %lf, %lf, %lf, %lf, %lf\n", surv, k, mu, r, r_sum, surv_sum);

            }
        }
        //printf("%lf, %lf, %lf, %lf\n", k, mu, r, r_sum);

        result_sum += r*r*deltar*r_sum;

        gsl_matrix_free(dPdt1);
        gsl_matrix_free(dPdt2);
    }

    gsl_spline_free(Pmm_spline);
    gsl_spline_free(Pmt_spline);
    gsl_spline_free(Ptt_spline);
    gsl_interp_accel_free(Pmm_acc);
    gsl_interp_accel_free(Pmt_acc);
    gsl_interp_accel_free(Ptt_acc);

    return result_sum;
}

// Routine to read in the number density as a function of redshift. We need a file containing the left-most edge of each redshift bin and teh number density in that bin.
// From this we create arrays to store the bin centre, the bin width, the comoving distance and growth factor at the bin centre and the number density.
// The last bin width and bin centre is constructed from the last row of the input and the value of zmax at the top of the code.
// ITS VERY IMPORTANT THAT THE NUMBER OF ROWS AND THE REDSHIFTS OF BOTH THE DENSITY AND PV NUMBER DENSITIES MATCH AS THE INTEGRATION OVER Z IS DONE USING THE TRAPEZIUM RULE.
// ALSO MAKE NOTE OF THE FACTOR OF 1.0e-6 ON LINE 827. THIS IS BECAUSE I TYPICALLY SAVE THE VALUE OF NBAR x 10^6 IN THE INPUT FILES< SO THAT I DON'T LOSE PRECISION
// WHEN SMALL VALUES OF THE NUMBER DENSITY ARE WRITTEN TO A FILE!
void read_nz() {

    FILE * fp;
    char buf[500];
    int i, nsamp;

    NRED = (int *)calloc(2, sizeof(int));
    nbararray = (double **)calloc(2, sizeof(double*));
    double * zinarray;

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

        if(!(fp = fopen(nbar_file[nsamp], "r"))) {
            printf("\nERROR: Can't open nbar file '%s'.\n\n", nbar_file[nsamp]);
            exit(0);
        }

        NRED[nsamp] = 0;
        while(fgets(buf,500,fp)) {
            if(strncmp(buf,"#",1)!=0) {
                double tz, tnbar;
                if(sscanf(buf, "%lf %lf\n", &tz, &tnbar) != 2) {printf("nbar read error\n"); exit(0);};
                if (tz > zmax) break;
                NRED[nsamp]++;
            }
        }
        fclose(fp);

        if (nsamp == 0) zinarray = (double *)calloc(NRED[nsamp], sizeof(double));
        nbararray[nsamp] = (double *)calloc(NRED[nsamp], sizeof(double));

        NRED[nsamp] = 0;
        fp = fopen(nbar_file[nsamp], "r");
        while(fgets(buf,500,fp)) {
            if(strncmp(buf,"#",1)!=0) {
                double tz, tnbar;
                if(sscanf(buf, "%lf %lf\n", &tz, &tnbar) != 2) {printf("nbar read error\n"); exit(0);};
                if (tz > zmax) break;
                if (nsamp == 0) zinarray[NRED[nsamp]] = tz;
                nbararray[nsamp][NRED[nsamp]] = 1.0e-6*tnbar;
                NRED[nsamp]++;
            }
        }
        fclose(fp);
    }

    if (NRED[1] != NRED[0]) {
        printf("ERROR: The number of redshift bins for each sample must match\n");
        exit(0);
    }

    zarray = (double *)calloc(NRED[0], sizeof(double));
    rarray = (double *)calloc(NRED[0], sizeof(double));
    ezarray = (double *)calloc(NRED[0], sizeof(double));
    deltararray = (double *)calloc(NRED[0], sizeof(double));
    growtharray = (double *)calloc(NRED[0], sizeof(double));

    for (i=0; i<NRED[0]-1; i++) {
        zarray[i] = (zinarray[i+1]+zinarray[i])/2.0;
        rarray[i] = rz(zarray[i]);
        ezarray[i] = 1.0/ezinv(zarray[i], NULL);
        deltararray[i] = rz(zinarray[i+1]) - rz(zinarray[i]);
        growtharray[i] = growthz(zarray[i])/growthz(0.0);
        //printf("%12.6lf %12.6lf %12.6lf %12.6lf %12.6lf %12.6lf\n", zarray[i], rarray[i], deltararray[i], growtharray[i], nbararray[0][i], nbararray[1][i]);
    }
    zarray[NRED[0]-1] = (zmax+zinarray[NRED[0]-1])/2.0;
    rarray[NRED[0]-1] = rz(zarray[NRED[0]-1]);
    ezarray[NRED[0]-1] = 1.0/ezinv(zarray[NRED[0]-1], NULL);
    deltararray[NRED[0]-1] = rz(zmax) - rz(zinarray[NRED[0]-1]);
    growtharray[NRED[0]-1] = growthz(zarray[NRED[0]-1])/growthz(0.0);
    //printf("%12.6lf %12.6lf %12.6lf %12.6lf %12.6lf %12.6lf\n", zarray[NRED[0]-1], rarray[NRED[0]-1], deltararray[NRED[0]-1], growtharray[NRED[0]-1], nbararray[0][NRED[0]-1], nbararray[1][NRED[0]-1]);

    growth_acc    = gsl_interp_accel_alloc();
    growth_spline = gsl_spline_alloc(gsl_interp_cspline, NRED[0]);
    gsl_spline_init(growth_spline, zarray, growtharray, NRED[0]);

    free(zinarray);

    // Also create a simple redshift-distance spline
    int nbins = 400;
    double REDMIN = 0.0;
    double REDMAX = 2.0;
    double redbinwidth = (REDMAX-REDMIN)/(double)(nbins-1);
    double RMIN = rz(REDMIN);
    double RMAX = rz(REDMAX);
    double * ztemp = (double *)malloc(nbins*sizeof(double));
    double * rtemp = (double *)malloc(nbins*sizeof(double));
    for (i=0;i<nbins;i++) {
        ztemp[i] = i*redbinwidth+REDMIN;
        rtemp[i] = rz(ztemp[i]);
    }
    r_acc = gsl_interp_accel_alloc();
    r_spline = gsl_spline_alloc(gsl_interp_cspline, nbins);
    gsl_spline_init(r_spline, ztemp, rtemp, nbins);

    free(ztemp);
    free(rtemp);

    return;
}

// Routine to read in the velocity power spectrum.
void read_power() {

    FILE * fp;
    char buf[500];
    int i, j;

    pmmarray = (double**)malloc(nzin*sizeof(double*));
    pmtarray = (double**)malloc(nzin*sizeof(double*));
    pttarray = (double**)malloc(nzin*sizeof(double*));

    for (i = 0; i<nzin; i++) {

        char Pvel_file_in[500];
        sprintf(Pvel_file_in, "%s_z0p%02d.dat", Pvel_file, (int)(100.0*zin[i]));

        if(!(fp = fopen(Pvel_file_in, "r"))) {
            printf("\nERROR: Can't open power file '%s'.\n\n", Pvel_file_in);
            exit(0);
        }

        NK = 0;
        while(fgets(buf,500,fp)) {
            if(strncmp(buf,"#",1)!=0) {
                double tk, pkdelta, pkdeltavel, pkvel;
                if(sscanf(buf, "%lf %lf %lf %lf\n", &tk, &pkdelta, &pkdeltavel, &pkvel) != 4) {printf("Pvel read error\n"); exit(0);};
                NK++;
            }
        }
        fclose(fp);

        if (i == 0) {
            karray = (double *)calloc(NK, sizeof(double));
            deltakarray = (double *)calloc(NK-1, sizeof(double));
        }
        pmmarray[i] = (double *)calloc(NK, sizeof(double));
        pmtarray[i] = (double *)calloc(NK, sizeof(double));
        pttarray[i] = (double *)calloc(NK, sizeof(double));

        NK = 0;
        fp = fopen(Pvel_file_in, "r");
        while(fgets(buf,500,fp)) {
            if(strncmp(buf,"#",1)!=0) {
                double tk, pkdelta, pkdeltavel, pkvel;
                if(sscanf(buf, "%lf %lf %lf %lf\n", &tk, &pkdelta, &pkdeltavel, &pkvel) != 4) {printf("Pvel read error\n"); exit(0);};
                if (i == 0) karray[NK] = tk;
                pttarray[i][NK] = pkvel;
                pmmarray[i][NK] = pkdelta;
                pmtarray[i][NK] = pkdeltavel;
                NK++;
            }
        }
        fclose(fp);
    }

    for (i=0; i<NK-1; i++) deltakarray[i] = karray[i+1]-karray[i];

    pkkmin = karray[0];
    pkkmax = karray[NK-1];

    if (pkkmax < kmax) {
        printf("ERROR: The maximum k in the input power spectra is less than k_max\n");
        exit(0);
    }

    return;
}

// Routine to read in the transfer function.
void read_transfer() {

    FILE * fp;
    char buf[500];
    int i;

    if(!(fp = fopen(Trans_file, "r"))) {
        printf("\nERROR: Can't open transfer function file '%s'.\n\n", Trans_file);
        exit(0);
    }

    int NTK = 0;
    while(fgets(buf,500,fp)) {
        if(strncmp(buf,"#",1)!=0) {
            double tk, ttk;
            if(sscanf(buf, "%lf %lf\n", &tk, &ttk) != 2) {printf("Transfer read error\n"); exit(0);};
            NTK++;
        }
    }
    fclose(fp);

    double * tkarray = (double *)calloc(NTK, sizeof(double));
    double * ttkarray = (double *)calloc(NTK, sizeof(double));

    NTK = 0;
    fp = fopen(Trans_file, "r");
    while(fgets(buf,500,fp)) {
        if(strncmp(buf,"#",1)!=0) {
            double tk, ttk;
            if(sscanf(buf, "%lf %lf\n", &tk, &ttk) != 2) {printf("Transfer read error\n"); exit(0);};
            tkarray[NTK] = tk;
            ttkarray[NTK] = ttk;
            NTK++;
        }
    }
    fclose(fp);

    // Spline the transfer function
    Trans_acc    = gsl_interp_accel_alloc();
    Trans_spline = gsl_spline_alloc(gsl_interp_cspline, NTK);
    gsl_spline_init(Trans_spline, tkarray, ttkarray, NTK);

    free(tkarray);
    free(ttkarray);

    return;
}

// Integrand for the comoving distance
double ezinv(double x, void *p) {
  return 1.0/sqrt(Om*(1.0+x)*(1.0+x)*(1.0+x)+(1.0-Om));
}

// Calculates the comoving distance from the redshift
double rz(double red) {
  double result, error;
  gsl_function F;
  gsl_integration_workspace * w = gsl_integration_workspace_alloc(1000);
  F.function = &ezinv;
  gsl_integration_qags(&F, 0.0, red, 0, 1e-7, 1000, w, &result, &error);
  gsl_integration_workspace_free(w);
  return c*result/100.0;
}

// The integrand for the normalised growth factor
double growthfunc(double x, void *p) {
    double red = 1.0/x - 1.0;
    double Omz = Om*ezinv(red,NULL)*ezinv(red,NULL)/(x*x*x);
    double f = pow(Omz, gammaval);
    return f/x;
}

// Calculates the normalised growth factor as a function of redshift given a value of gammaval
double growthz(double red) {
  double result, error;
  gsl_function F;
  gsl_integration_workspace * w = gsl_integration_workspace_alloc(1000);
  F.function = &growthfunc;
  double a = 1.0/(1.0+red);
  gsl_integration_qags(&F, a, 1.0, 0, 1e-7, 1000, w, &result, &error);
  gsl_integration_workspace_free(w);
  return exp(-result);
}


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include<time.h>
#include <gsl/gsl_blas.h>

double epsl = 0.01;

typedef struct {
    double x, y, z;
} position;

typedef struct _vertex {
    position pos;
    gsl_matrix_view *C;
    struct _vertex *p,*l,*r;
} vertex;

void usage(char *exec) {
    printf("%s -s <input_file> <count_vertex>\n", exec);
    printf("%s -c <input_file>\n", exec);
    printf("%s -show <input_file>\n", exec);
}

int* ddd(char *fileName, int len){
    FILE *file = NULL;
    if ((file = fopen(fileName, "r")) == NULL) {
        /* error openning the file */
        perror("fopen: ");
        return NULL;
    }
    int n, i, j = 0, k = 0;
    char buff[6], buffFloat[17];
    memset(buff, 0, sizeof(buff));
    memset(buffFloat, 0, sizeof(buffFloat));
    if (fread(buff, sizeof(char), 5, file) < 5 || strcmp(buff, "E = [")) {
        perror("file out of params: ");
        return NULL;
    }

    double **mat = (double **)calloc(len, sizeof(double *));
    for (i = 0; i < len; i++) {
        mat[i] = (double *)calloc(3, sizeof(double *));
    }
    i = 0;
    while ((n = fread(buff, sizeof(char), 1, file)) == 1) {
        if (buff[0] == ' ') {
            if (k > 0) {
                mat[i][j] = atof(buffFloat);
                memset(buffFloat, 0, sizeof(buffFloat));
                k = 0;
                j++;
            }
            continue;
        }
        else if (buff[0] == ';') {
            if (k) {
                mat[i][j] = atof(buffFloat);
                memset(buffFloat, 0, sizeof(buffFloat));
                k = 0;
            }
            j = 0;
            i++;
        }
        else {
            buffFloat[k++] = buff[0];
        }
    }
    fclose(file);

    int current = 0;
    for (i = 0; i < len; i++) {

        for (j = 0; j < len; j++) {
            printf(" %f ", sqrt(pow(mat[i][0] - mat[j][0], 2) + pow(mat[i][1] - mat[j][1], 2) + pow(mat[i][2] - mat[j][2], 2)));
            fflush(stdout);
        }
        printf("\n");
    }
}

void printMat(double **mat, int len) {
    int i, j;
    for (i = 0; i < len; i++) {
        for (j = 0; j < len; j++) {
            //if (mat[i][j] != 0) {
                printf(" %f ", mat[i][j]);
                fflush(stdout);
            //}
        }
        printf("\n");
    }
}

double **matrixConstruct(char *fileName, int len){
    FILE *file = NULL;
    if ((file = fopen(fileName, "r")) == NULL) {
        /* error openning the file */
        perror("fopen: ");
        return NULL;
    }
    int n, i, j = 0, k = 0;
    char buff[6], buffFloat[17];
    memset(buff, 0, sizeof(buff));
    memset(buffFloat, 0, sizeof(buffFloat));
    if (fread(buff, sizeof(char), 5, file) < 5 || strcmp(buff, "E = [")) {
        perror("file out of params: ");
        return NULL;
    }

    double **mat = (double **)calloc(len, sizeof(double *));
    for (i = 0; i < len; i++) {
        mat[i] = (double *)calloc(len, sizeof(double *));
    }
    i = 0;
    while ((n = fread(buff, sizeof(char), 1, file)) == 1) {
        if (buff[0] == ' ') {
            if (k > 0) {
                mat[i][j] = atof(buffFloat);
                memset(buffFloat, 0, sizeof(buffFloat));
                k = 0;
                j++;
            }
            continue;
        }
        else if (buff[0] == ';') {
            if (k) {
                mat[i][j] = atof(buffFloat);
                memset(buffFloat, 0, sizeof(buffFloat));
                k = 0;
            }
            j = 0;
            i++;
        }
        else {
            buffFloat[k++] = buff[0];
        }
    }
    fclose(file);
    for (i = 0; i < len; i++)
    {
        for (j = 0; j < len; j++)
        {
            if(pow(mat[i][j] - mat[j][i],2) > 0.0000001) return NULL;
        }
    }

    return mat;
}

double dis(double **m, int i){
    if (i-1 < 0) return -1;
    return m[i][i-1];
}

double theta(double **m, int i){
    if (i-1 < 0) return -1000;
    return acos((pow(m[i][i+1],2) + pow(m[i][i-1],2) - pow(m[i+1][i-1],2))/(2*m[i][i+1]*m[i][i-1]));
}

double cosO(double **m, int i){
    //printf("%f %f\n", pow(m[i][i+1], 2) + pow(m[i+1][i+3],2) - 2*m[i][i+1]*m[i+1][i+3]*cos(theta(m, i+1))*cos(theta(m, i+2)) - pow(m[i][i+3],2), 2*m[i][i+1]*m[i+1][i+3]*sin(theta(m, i+1))*sin(theta(m, i+2)) );
    double theta2 = acos((pow(m[i+1][i+2],2) + pow(m[i+1][i+3],2) - pow(m[i+2][i+3],2))/(2*m[i+1][i+3]*m[i+1][i+2]));
    double res = (pow(m[i][i+1], 2) + pow(m[i+1][i+3],2) - 2*m[i][i+1]*m[i+1][i+3]*cos(theta(m, i+1))*cos(theta2) - pow(m[i][i+3],2))/
                    (2*m[i][i+1]*m[i+1][i+3]*sin(theta(m, i+1))*sin(theta2));
    if (res > 1) return 1;
    if (res < -1) return -1;
    return res;
}

double cosOmega(double **m, int i){
    if (i-3 < 0) return 1000;

    double Ai = (2*pow(m[i-2][i-1],2))*(pow(m[i-3][i-2],2) + pow(m[i-2][i] ,2) - pow(m[i-3][i] ,2));
    double Bi = pow(m[i-3][i-2],2) + pow(m[i-2][i-1],2) - pow(m[i-3][i-1],2);
    double Ci = pow(m[i-2][i-1],2) + pow(m[i-2][i],2) - pow(m[i-1][i],2);
    double Di = sqrt(
        4*pow(m[i-3][i-2],2)
        *pow(m[i-2][i-1],2)
        -pow(Bi,2) );
    double Ei = sqrt(4*pow(m[i-2][i-1],2)*pow(m[i-2][i],2)-pow(Ci,2) );
    double res = (Ai - Bi*Ci)/(Di*Ei);
    if (res > 1) return 1;
    if (res < -1) return -1;
    return res;
}

int isFeasible(double **mat, int len, vertex *v, int i){
    if (!v) return 0;
    int j = i-1;
    double pos[i][3];
    vertex *aux = v->p;
    while (aux)
    {
        //printf("\n\tj = %d {%f, %f, %f}\n", j, aux->pos.x, aux->pos.y, aux->pos.z);
        pos[j][0] = aux->pos.x;
        pos[j][1] = aux->pos.y;
        pos[j--][2] = aux->pos.z;
        aux = aux->p;
    }
    for (j = 0; j < i-3; j++)
    {
        if (mat[i][j] > 0)
        {
            double dij = pow(v->pos.x - pos[j][0],2)+pow(v->pos.y - pos[j][1],2)+pow(v->pos.z - pos[j][2],2);
            double diff = pow(mat[i][j],2)- dij;
            if (diff < 0) diff = diff*(-1);
            double dist = sqrt(diff);//pow(diff,2);
            //printf("{%d,%d} {%f, %f, %f}-{%f, %f, %f} \n\t%.20f- %.20f = %.20f \n %.20f \n", i, j, v->pos.x, v->pos.y, v->pos.z, pos[j][0], pos[j][1], pos[j][2], mat[i][j], dij, diff, dist);
            if (dist > epsl)
            {
               return 0;
            }
        }
    }
    return 1;
}

double LDE(double **mat, double **m, int len){
    int i, j, count = 0;
    double dij = 0;
    for (i = 0; i < len; i++)
    {
        for (j = i+1; j < len; j++)
        {
            if (mat[i][j]>0){
                dij = dij + pow((pow(mat[i][j],2) - (pow(m[i][0] - m[j][0], 2) + pow(m[i][1] - m[j][1], 2) + pow(m[i][2] - m[j][2], 2))),2)/mat[i][j];
                count++;
            }
        }
    }
    return dij/count;

}

int qtd = 0;
int BranchAndPrune(double **mat, int len, vertex *v, int i){
    if(i < len){
        double thetai = theta(mat,i-1);
        double cti = cos(thetai);
        double sti = sin(thetai);
        double cwi = cosOmega(mat, i);
        //double cwi2 = cosO(mat, i-3);
        //printf("%f %f \n", cwi, cwi2 );
        //printf("%f %f \n", acos(cwi)*57.295779513, acos(cwi2)*57.295779513 );
        if (cwi*cwi > 1){
            printf("\n %f Error cwi", cwi);
            getchar();
        }
        double swi = sqrt(1-pow(cwi, 2));
        double di = dis(mat,i);
        double bi1[] = {0-cti,0-sti,0,0-di*cti,
                        sti*cwi,0-cti*cwi,0-swi,di*sti*cwi,
                        sti*swi,0-cti*swi,cwi,di*sti*swi,
                        0,0,0,1};
        int j;
        //for (j = 0; j < 16; j++)
        //{
       //     printf("%f ", bi1[j]);
        //}
       // printf("\n");

        gsl_matrix_view Bi1 = gsl_matrix_view_array(bi1, 4, 4);

        swi = 0-swi;
        double bi2[] = {0-cti,0-sti,0,0-di*cti,
                        sti*cwi,0-cti*cwi,0-swi,di*sti*cwi,
                        sti*swi,0-cti*swi,cwi,di*sti*swi,
                        0,0,0,1};

        //for (j = 0; j < 16; j++)
        //{
        //    printf("%f ", bi2[j]);
        //}
        //printf("\n");

        gsl_matrix_view Bi2 = gsl_matrix_view_array(bi2, 4, 4);

        vertex *rv = (vertex*)calloc(1, sizeof(vertex));
        rv->p = v;
        rv->C = (gsl_matrix_view*)calloc(1, sizeof(gsl_matrix_view));
        *(rv->C) = gsl_matrix_view_array((double*)calloc(16,sizeof(double)), 4, 4);

        gsl_blas_dgemm(CblasNoTrans, CblasNoTrans,
                   1.0, &(v->C->matrix), &Bi1.matrix,
                   0.0, &(rv->C->matrix));

        vertex *lv = (vertex*)calloc(1, sizeof(vertex));
        lv->p = v;
        lv->C = (gsl_matrix_view*)calloc(1, sizeof(gsl_matrix_view));
        *(lv->C) = gsl_matrix_view_array((double*)calloc(16,sizeof(double)), 4, 4);

        gsl_blas_dgemm(CblasNoTrans, CblasNoTrans,
                   1.0, &(v->C->matrix), &Bi2.matrix,
                   0.0, &(lv->C->matrix));

        gsl_matrix_view *Xi = (gsl_matrix_view*)calloc(1, sizeof(gsl_matrix_view));
        *Xi = gsl_matrix_view_array((double*)calloc(4,sizeof(double)), 4, 1);

        double *y = (double*)calloc(4,sizeof(double));
        y[3] = 1;
        gsl_matrix_view *Y = (gsl_matrix_view*)calloc(1, sizeof(gsl_matrix_view));
        *Y = gsl_matrix_view_array(y, 4, 1);

        gsl_blas_dgemm(CblasNoTrans, CblasNoTrans,
                    1.0, &(rv->C->matrix), &(Y->matrix),
                    0.0, &(Xi->matrix));
        rv->pos.x = Xi->matrix.data[0];
        rv->pos.y = Xi->matrix.data[1];
        rv->pos.z = Xi->matrix.data[2];

        gsl_blas_dgemm(CblasNoTrans, CblasNoTrans,
                    1.0, &(lv->C->matrix), &(Y->matrix),
                    0.0, &(Xi->matrix));
        lv->pos.x = Xi->matrix.data[0];
        lv->pos.y = Xi->matrix.data[1];
        lv->pos.z = Xi->matrix.data[2];
        free(y);
        free(Y);
        free(Xi->matrix.data);
        free(Xi);

        //printf("[ %f, %f", rv->pos.x, rv->pos.y);
        //printf("  %f]\n", rv->pos.z);
        //printf("[ %f, %f", lv->pos.x, lv->pos.y);
        //printf("  %f ]\n", lv->pos.z);

        //getchar();
            int s;
            //printf("\n");
        if (isFeasible(mat, len, rv, i)){
            v->r = rv;
            //for (s = 0; s < i; s++)
            //{
            //    printf(">");
            //}

            //printf("1\n");
            fflush(stdout);
            BranchAndPrune(mat, len, rv, i+1);
        }else {
            //for (s = 0; s < i; s++)
            //{
            //    printf("<");
            //}
            //
            //printf("2\n");
            free(rv->C->matrix.data);
            free(rv->C);
            free(rv);
            v->r = NULL;
        }

        if (isFeasible(mat, len, lv, i)){
            v->l = lv;

            //for (s = 0; s < i; s++)
            //{
            //    printf(">");
            //}
            //
            //printf("3\n");
            fflush(stdout);
            BranchAndPrune(mat, len, lv, i+1);
        }else {

            //for (s = 0; s < i; s++)
            //{
            //    printf("<");
            //}
            //
            //printf("4\n");
            free(lv->C->matrix.data);
            free(lv->C);
            free(lv);
            v->l = NULL;
        }
    }else{
        //printf(" ---------------- ue\n");
        //fflush(stdout);
        qtd++;
        double **mat2 = calloc(len, sizeof(double*));
        int i;
        for (i = 0; i < len; i++)
        {
            mat2[i] = calloc(3, sizeof(double));
        }
        i = len-1;
        printf("\n[");
        while (v)
        {
            mat2[i][0] = v->pos.x;
            mat2[i][1] = v->pos.y;
            mat2[i--][2] = v->pos.z;
            //printf("%f %f %f; ", v->pos.x, v->pos.y, v->pos.z);
            v = v->p;
        }

        double lde = LDE(mat, mat2, len);

        printf("\n \t LDE = %.20f\n", lde);

        for (i = 0; i < len; i++)
        {
            printf("%f %f %f; ", mat[i][0], mat[i][1], mat[i][2]);
        }

        printf("]\n");
        return 0;
    }
}

int printGrafo(int len, vertex *v, int i){
    int j = -1;
    if (v->l){
        printf("(%d, %d),", i, i+1);
        j = printGrafo(len, v->l, i+1);
    }
    if (v->r){
        if (j != -1){
            printf("(%d, %d),", i, j);
            j = printGrafo(len, v->r, j);
        }else
        {
            printf("(%d, %d),", i, i+1);
            return printGrafo(len, v->r, i+1);
        }
    }
    if (j == -1) return i+1;
    return j;
}

vertex *resul[10];

int solve(char *fileName, int len) {
    double **mat = matrixConstruct(fileName, len);
    if(mat == NULL){
        printf("Error when construct matrix\n");
        fflush(stdout);
        return 0;
    }

    double b1[] = {1,0,0,0,
                    0,1,0,0,
                    0,0,1,0,
                    0,0,0,1};
    gsl_matrix_view B1 = gsl_matrix_view_array(b1, 4, 4);

    double b2[] = {-1,0,0,0-dis(mat,1),
                    0,1,0,0,
                    0,0,-1,0,
                    0,0,0,1};
    gsl_matrix_view B2 = gsl_matrix_view_array(b2, 4, 4);

    double theta2 = theta(mat,1);
    //printf("[ cos %f\n", theta2);

    double ct2 = cos(theta2);
    double st2 = sin(theta2);
    double b3[] = {0-ct2,0-st2,0,0-dis(mat,2)*ct2,
                    st2,0-ct2,0,dis(mat,2)*st2,
                    0,0,1,0,
                    0,0,0,1};
    gsl_matrix_view B3 = gsl_matrix_view_array(b3, 4, 4);

    vertex *v = (vertex*)calloc(1, sizeof(vertex));
    v->C = &B1;
    //printf("[ %f, %f %f\n", v->pos.x, v->pos.y, v->pos.z);

    vertex *v2 = (vertex*)calloc(1, sizeof(vertex));
    v->r = v2;
    v2->C = &B2;
    v2->p = v;
    v2->pos.x = 0-dis(mat,1);

     //   printf("[ %f, %f %f\n", v2->pos.x, v2->pos.y, v2->pos.z);
    vertex *v3 = (vertex*)calloc(1, sizeof(vertex));
    v2->r = v3;
    v3->C = (gsl_matrix_view*)calloc(1, sizeof(gsl_matrix_view));
    *(v3->C) = gsl_matrix_view_array((double*)calloc(16,sizeof(double)), 4, 4);
    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans,
                   1.0, &B2.matrix, &(B3.matrix),
                   0.0, &(v3->C->matrix));
    v3->p = v2;
    v3->pos.x = dis(mat,2)*ct2 -dis(mat,1);
    v3->pos.y = dis(mat,2)*st2;

    //printf("[ %f, %f", v3->pos.x, v3->pos.y);
    //    printf("  %f]\n", v3->pos.z);

    BranchAndPrune(mat, len, v3, 3);
    printf("%d", qtd);
    //printf("[");
    //printGrafo(len, v, 0);
    //printf("]");
    return 1;
}

int countCols(char *fileName) {
    FILE *file = NULL;
    if ((file = fopen(fileName, "r")) == NULL) {
        /* error openning the file */
        perror("fopen: ");
        return 1;
    }

    int n, j = 0, k = 0;
    char buff[6];
    memset(buff, 0, sizeof(buff));
    if (fread(buff, sizeof(char), 5, file) < 5 || strcmp(buff, "E = [")) {
        fclose(file);
        perror("file out of params: ");
        return 1;
    }

    while ((n = fread(buff, sizeof(char), 1, file)) == 1) {
        if (buff[0] == ' ') {
            if (k > 0) {
                k = 0;
                j++;
            }
            continue;
        }
        else if (buff[0] == ';') {
            if (k) j++;
            break;
        }
        else {
            k++;
        }
    }
    fclose(file);
    return j;
}

int main(int argc, char **argv) {
    double time_spent = 0.0;

	clock_t begin = clock();

    if (argc < 3) {
        usage(argv[0]);
    }
    else {
        if (!strcmp(argv[1], "-s")) {
            int len;
            if (argc < 4) {
                len = countCols(argv[2]);
            }else {
                if ((len = atoi(argv[3])) == 0){
                    /* error openning the file */
                    perror("count_vertex param error: ");
                    return 1;
                }
            }

            solve(argv[2], len);
        } else if (!strcmp(argv[1], "-c")) {
            printf("%d\n",countCols(argv[2]));
        } else if (!strcmp(argv[1], "-show")) {
            printMat(matrixConstruct(argv[2], countCols(argv[2])), countCols(argv[2]));
        } else if (!strcmp(argv[1], "-d")) {
            ddd(argv[2],10);
        } else {
            usage(argv[0]);
        }
    }

    //getchar();

    clock_t end = clock();

	// calculate elapsed time by finding difference (end - begin) and
	// divide by CLOCKS_PER_SEC to convert to seconds
	time_spent += (double)(end - begin) / CLOCKS_PER_SEC;

	printf("\nTime elpased is %.20f seconds", time_spent);
    fflush(stdout);
}


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

/**
 * C BLAS function wrappers.
 */

inline void copy_vector_cblas(
  const double* const x, double* const y,
  const unsigned int rows )
{
  cblas_dcopy( rows, x, 1, y, 1 );
}

inline double inner_product_cblas(
  const double* const x, const double* const y,
  const unsigned int rows )
{
  return cblas_ddot( rows, x, 1, y, 1 );
}

inline void outer_product_cblas(
  const double* const x, const double* const y,
  const double alpha, const unsigned int rows_x, const unsigned int rows_y,
  double* m )
{
  cblas_dger( CblasRowMajor, rows_x, rows_y, alpha, x, 1, y, 1, m, rows_y);
}

inline void vector_vector_sum_cblas(
  const double* const x, double* const y,
  const double alpha, const unsigned int rows )
{
  cblas_daxpy( rows, alpha, x, 1, y, 1);
}

/**
 * Helper functions.
 */

inline void copy_matrix(
  const double* const m0, double* const m1,
  const unsigned int rows, const unsigned int cols )
{
  for ( unsigned int i = 0; i < rows; ++i )
  {
    for ( unsigned int j = 0; j < cols; ++j )
    {
      m1[ i * cols + j ] = m0[ i * cols + j ];
    }
  }
}

inline void matrix_matrix_sum(
  const double* const m0, const double* const m1,
  const double alpha, const unsigned int rows, const unsigned int cols,
  double* m2 )
{
  for ( unsigned int i = 0; i < rows; ++i )
  {
    for ( unsigned int j = 0; j < cols; ++j )
    {
      m2[ i * cols + j ] = m0[ i * cols + j ] + alpha * m1[ i * cols + j ];
    }
  }
}

inline void zero_vector(
  const unsigned int rows,
  double* x )
{
  memset(x, 0, rows * sizeof( double ) );
}

inline void zero_matrix(
  const unsigned int rows, const unsigned int cols,
  double* m )
{
  memset(m, 0, rows * cols * sizeof( double ) );
}

inline void zero_diagonal(
  const unsigned int rows, const unsigned int cols,
  double* m )
{
  for ( unsigned int i = 0; i < rows; ++i )
  {
    m[ i * cols + i ] = 0;
  }
}

inline double rand_double( void )
{
  return ( double ) rand() / ( double )( RAND_MAX );
}

inline unsigned int rand_int( const unsigned int max )
{
  return rand() % max;
}

inline unsigned int logistic( const double h )
{
  return 1. / ( 1. + exp( -h ) ) > rand_double();
}

/**
 * Train a fully visible Boltzmann machine with CD-1.
 */
void ctrain(
  double* W, double* b, unsigned int rows, unsigned int cols,
  double* data, unsigned int episodes,
  double epsilon_w, double epsilon_b,
  unsigned int batchsize, unsigned int seed,
  const double momentum_constant, const double decay_constant )
{
  double dW[ rows * cols ];
  double db[ cols ];
  double dW_minus[ rows * cols ];
  double db_minus[ cols ];
  double u;

  const double effective_epsilon_w = 1 / ( double )( batchsize ) * epsilon_w;
  const double effective_epsilon_b = 1 / ( double )( batchsize ) * epsilon_b;

  srand( seed );

  // initialize momentum terms to zero at start of learning
  zero_matrix( rows, cols, dW_minus );
  zero_vector( rows, db_minus );

  for ( unsigned int i = 0; i < episodes / batchsize; ++i )
  {
    // initialize weights and bias change to zero at start of batch
    zero_matrix( rows, cols, dW );
    zero_vector( rows, db );

    for ( unsigned int j = 0; j < batchsize; ++j )
    {
      // compute \Delta W += <ss>_0
      outer_product_cblas( &data[ i * batchsize * cols + j * cols], &data[ i * batchsize * cols + j * cols], 1., rows, cols, dW );

      // compute \Delta b += <s>_0
      vector_vector_sum_cblas( &data[ i * batchsize * cols + j * cols], db, 1, rows );

      // perform a single Gibbs step (update all units once) directly
      // on data vector to avoid copying
      for ( unsigned int k = 0; k < cols; ++k )
      {
        u = inner_product_cblas( &W[ k * cols ], &data[ i * batchsize * cols + j * cols], cols );
        data[ i * batchsize * cols + j * cols + k ] = logistic( u + b[ k ] );
      }

      // compute \Delta W -= <ss>_1
      outer_product_cblas( &data[ i * batchsize * cols + j * cols ], &data[ i * batchsize * cols + j * cols ], -1., rows, cols, dW );

      // compute \Delta b -= <s>_1
      vector_vector_sum_cblas( &data[ i * batchsize * cols + j * cols ], db, -1, rows );
    }

    // add momentum
    if ( momentum_constant > 0. )
    {
      matrix_matrix_sum( dW, dW_minus, momentum_constant, rows, cols, dW );
      vector_vector_sum_cblas( db_minus, db, momentum_constant, rows );
      copy_matrix( dW, dW_minus, rows, cols );
      copy_vector_cblas( db, db_minus, rows );
    }

    // decay weights
    if ( decay_constant > 0. )
    {
      // need to devide by learning rate, since dW is multiplied by it
      // below
      matrix_matrix_sum( dW, W, -decay_constant / effective_epsilon_w, rows, cols, dW );
    }

    // updates weights and biases
    matrix_matrix_sum( W, dW, effective_epsilon_w, rows, cols, W );
    vector_vector_sum_cblas( db, b, effective_epsilon_b, rows );

    // set diagonal of weight matrix to zero
    zero_diagonal( rows, cols, W );
  }
}

/**
 * Sample from a fully visible Boltzmann machine.
 */
void csample(
  const double* W, const double* b, const unsigned int rows, const unsigned int cols,
  const unsigned int episodes, const unsigned int seed,
  double* s, double* samples )
{
  double u;
  srand( seed );

  unsigned int i = 0;
  while(1)
  {
    // perform a single Gibbs step (update all units once on average)
    for ( unsigned int k = 0; k < cols; ++k )
    {
      const unsigned int j = rand_int( cols );
      u = inner_product_cblas( &W[ j * cols ], s, cols );
      s[ j ] = logistic( u + b[ j ] );
    }
    copy_vector_cblas( s, &samples[ i * rows], rows );
    ++i;
    if( i == episodes)
    {
      return;
    }
  }
}


#define _GNU_SOURCE
#include <stdio.h>
#include <math.h>
#include <gsl/gsl_rng.h>
#include <glib.h>
#if HAVE_MPI
#include <mpi.h>
#endif
#include "genetic/genetic.h"

#define N_SIMULATIONS 10000
#define SEED 707l
int ntasks = 1;
unsigned int nthreads = 1;
GMutex mutex[1];
GeneticVariable v[2];

double
evaluate (Entity * entity)
{
  double x, y, e1, e2;
  x = genetic_get_variable (entity, v);
  y = genetic_get_variable (entity, v + 1);
  e1 = x + y - 3.;
  e2 = x - y - 1.;
  e1 = e1 * e1 + e2 * e2;
  return e1;
}

int
main (int argn, char **argc)
{
  FILE *file;
  double *best_variables;
  char *best_genome;
  double xgenerations, mutation_ratio, reproduction_ratio, adaptation_ratio,
    evolution_ratio, best_objective;
  int rank;
  unsigned int ngenerations;
#if HAVE_MPI
  MPI_Init (&argn, &argc);
  MPI_Comm_size (MPI_COMM_WORLD, &ntasks);
  MPI_Comm_rank (MPI_COMM_WORLD, &rank);
#else
  rank = 0;
#endif
  nthreads = 4;
  v[0].maximum = 10.;
  v[0].minimum = -10.;
  v[0].nbits = 30;
  v[1].maximum = 10.;
  v[1].minimum = -10.;
  v[1].nbits = 30;
  file = fopen (argc[1], "r");
  if (fscanf (file, "%*s%lf%*s%lf%*s%lf%*s%lf", &xgenerations,
              &mutation_ratio, &reproduction_ratio, &adaptation_ratio) != 4)
    return 1;
  fclose (file);
  ngenerations = xgenerations;
  evolution_ratio = mutation_ratio + reproduction_ratio + adaptation_ratio;
  genetic_algorithm_default (2, v,
                             N_SIMULATIONS
                             / (1 + (ngenerations - 1) * evolution_ratio),
                             ngenerations, mutation_ratio, reproduction_ratio,
                             adaptation_ratio, SEED, 0., &evaluate,
                             &best_genome, &best_variables, &best_objective);
  if (rank == 0)
    {
      file = fopen (argc[2], "w");
      fprintf (file, "%.14le", best_objective);
      printf ("objective=%.14le\n", best_objective);
      fclose (file);
      g_free (best_genome);
      g_free (best_variables);
    }
#if HAVE_MPI
  MPI_Finalize ();
#endif
  return 0;
}


/* integration/fixed.c
 *
 * Copyright (C) 2017 Patrick Alken
 *
 * 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.
 */

/* the code in this module performs fixed-point quadrature calculations for
 * integrands and is based on IQPACK */

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

static int fixed_compute(const double a, const double b, const double alpha, const double beta,
                         gsl_integration_fixed_workspace * w);
static int imtqlx ( const int n, double d[], double e[], double z[] );

gsl_integration_fixed_workspace *
gsl_integration_fixed_alloc(const gsl_integration_fixed_type * type, const size_t n,
                            const double a, const double b, const double alpha, const double beta)
{
  int status;
  gsl_integration_fixed_workspace *w;

  /* check inputs */
  if (n < 1)
    {
      GSL_ERROR_VAL ("workspace size n must be at least 1", GSL_EDOM, 0);
    }

  w = calloc(1, sizeof(gsl_integration_fixed_workspace));
  if (w == NULL)
    {
      GSL_ERROR_VAL ("unable to allocate workspace", GSL_ENOMEM, 0);
    }

  w->weights = malloc(n * sizeof(double));
  if (w->weights == NULL)
    {
      gsl_integration_fixed_free(w);
      GSL_ERROR_VAL ("unable to allocate weights", GSL_ENOMEM, 0);
    }

  w->x = malloc(n * sizeof(double));
  if (w->x == NULL)
    {
      gsl_integration_fixed_free(w);
      GSL_ERROR_VAL ("unable to allocate x", GSL_ENOMEM, 0);
    }

  w->diag = malloc(n * sizeof(double));
  if (w->diag == NULL)
    {
      gsl_integration_fixed_free(w);
      GSL_ERROR_VAL ("unable to allocate diag", GSL_ENOMEM, 0);
    }

  w->subdiag = malloc(n * sizeof(double));
  if (w->subdiag == NULL)
    {
      gsl_integration_fixed_free(w);
      GSL_ERROR_VAL ("unable to allocate subdiag", GSL_ENOMEM, 0);
    }

  w->n = n;
  w->type = type;

  /* compute quadrature weights and nodes */
  status = fixed_compute(a, b, alpha, beta, w);
  if (status)
    {
      gsl_integration_fixed_free(w);
      GSL_ERROR_VAL ("error in integration parameters", GSL_EDOM, 0);
    }

  return w;
}

void
gsl_integration_fixed_free(gsl_integration_fixed_workspace * w)
{
  if (w->weights)
    free(w->weights);

  if (w->x)
    free(w->x);

  if (w->diag)
    free(w->diag);

  if (w->subdiag)
    free(w->subdiag);

  free(w);
}

size_t
gsl_integration_fixed_n(const gsl_integration_fixed_workspace * w)
{
  return w->n;
}

double *
gsl_integration_fixed_nodes(const gsl_integration_fixed_workspace * w)
{
  return w->x;
}

double *
gsl_integration_fixed_weights(const gsl_integration_fixed_workspace * w)
{
  return w->weights;
}

int
gsl_integration_fixed(const gsl_function * func, double * result,
                      const gsl_integration_fixed_workspace * w)
{
  const size_t n = w->n;
  size_t i;
  double sum = 0.0;

  for (i = 0; i < n; ++i)
    {
      double fi = GSL_FN_EVAL(func, w->x[i]);
      sum += w->weights[i] * fi;
    }

  *result = sum;

  return GSL_SUCCESS;
}

/*
fixed_compute()
  Compute quadrature weights and nodes
*/

static int
fixed_compute(const double a, const double b, const double alpha, const double beta,
              gsl_integration_fixed_workspace * w)
{
  int s;
  const size_t n = w->n;
  gsl_integration_fixed_params params;
  size_t i;

  params.a = a;
  params.b = b;
  params.alpha = alpha;
  params.beta = beta;

  /* check input parameters */
  s = (w->type->check)(n, &params);
  if (s)
    return s;

  /* initialize Jacobi matrix */
  s = (w->type->init)(n, w->diag, w->subdiag, &params);
  if (s)
    return s;

  if (params.zemu <= 0.0)
    {
      GSL_ERROR("zeroth moment must be positive", GSL_EINVAL);
    }

  for ( i = 0; i < n; i++ )
    {
      w->x[i] = w->diag[i];
    }

  w->weights[0] = sqrt (params.zemu);

  for ( i = 1; i < n; i++ )
    {
      w->weights[i] = 0.0;
    }

  /* diagonalize the Jacobi matrix */
  s = imtqlx (n, w->x, w->subdiag, w->weights);
  if (s)
    return s;

  for (i = 0; i < n; i++)
    {
      w->weights[i] = w->weights[i] * w->weights[i];
    }

  /*
   * The current weights and nodes are valid for a = 0, b = 1.
   * Now scale them for arbitrary a,b
   */
  {
    double p = pow ( params.slp, params.al + params.be + 1.0 );
    size_t k;

    for ( k = 0; k < n; k++ )
      {
        w->x[k] = params.shft + params.slp * w->x[k];
        w->weights[k] = w->weights[k] * p;
      }
  }

  return GSL_SUCCESS;
}

/******************************************************************************/
/*
  Purpose:

    IMTQLX diagonalizes a symmetric tridiagonal matrix.

  Discussion:

    This routine is a slightly modified version of the EISPACK routine to
    perform the implicit QL algorithm on a symmetric tridiagonal matrix.

    The authors thank the authors of EISPACK for permission to use this
    routine.

    It has been modified to produce the product Q' * Z, where Z is an input
    vector and Q is the orthogonal matrix diagonalizing the input matrix.
    The changes consist (essentially) of applying the orthogonal transformations
    directly to Z as they are generated.

  Licensing:

    This code is distributed under the GNU LGPL license.

  Modified:

    11 January 2010

  Author:

    Original FORTRAN77 version by Sylvan Elhay, Jaroslav Kautsky.
    C version by John Burkardt.

  Reference:

    Sylvan Elhay, Jaroslav Kautsky,
    Algorithm 655: IQPACK, FORTRAN Subroutines for the Weights of
    Interpolatory Quadrature,
    ACM Transactions on Mathematical Software,
    Volume 13, Number 4, December 1987, pages 399-415.

    Roger Martin, James Wilkinson,
    The Implicit QL Algorithm,
    Numerische Mathematik,
    Volume 12, Number 5, December 1968, pages 377-383.

  Parameters:

    Input, int N, the order of the matrix.

    Input/output, double D(N), the diagonal entries of the matrix.
    On output, the information in D has been overwritten.

    Input/output, double E(N), the subdiagonal entries of the
    matrix, in entries E(1) through E(N-1).  On output, the information in
    E has been overwritten.

    Input/output, double Z(N).  On input, a vector.  On output,
    the value of Q' * Z, where Q is the matrix that diagonalizes the
    input symmetric tridiagonal matrix.
*/

static int
imtqlx ( const int n, double d[], double e[], double z[] )
{
  double b;
  double c;
  double f;
  double g;
  int i;
  int ii;
  int itn = 30;
  int j;
  int k;
  int l;
  int m;
  int mml;
  double p;
  double r;
  double s;

  if ( n == 1 )
  {
    return GSL_SUCCESS;
  }

  e[n-1] = 0.0;

  for ( l = 1; l <= n; l++ )
  {
    j = 0;
    for ( ; ; )
    {
      for ( m = l; m <= n; m++ )
      {
        if ( m == n )
        {
          break;
        }

        if ( fabs ( e[m-1] ) <= GSL_DBL_EPSILON * ( fabs ( d[m-1] ) + fabs ( d[m] ) ) )
        {
          break;
        }
      }
      p = d[l-1];
      if ( m == l )
      {
        break;
      }
      if ( itn <= j )
      {
        return GSL_EMAXITER;
      }
      j = j + 1;
      g = ( d[l] - p ) / ( 2.0 * e[l-1] );
      r =  sqrt ( g * g + 1.0 );
      g = d[m-1] - p + e[l-1] / ( g + fabs ( r ) * GSL_SIGN ( g ) );
      s = 1.0;
      c = 1.0;
      p = 0.0;
      mml = m - l;

      for ( ii = 1; ii <= mml; ii++ )
      {
        i = m - ii;
        f = s * e[i-1];
        b = c * e[i-1];

        if ( fabs ( g ) <= fabs ( f ) )
        {
          c = g / f;
          r =  sqrt ( c * c + 1.0 );
          e[i] = f * r;
          s = 1.0 / r;
          c = c * s;
        }
        else
        {
          s = f / g;
          r =  sqrt ( s * s + 1.0 );
          e[i] = g * r;
          c = 1.0 / r;
          s = s * c;
        }
        g = d[i] - p;
        r = ( d[i-1] - g ) * s + 2.0 * c * b;
        p = s * r;
        d[i] = g + p;
        g = c * r - b;
        f = z[i];
        z[i] = s * z[i-1] + c * f;
        z[i-1] = c * z[i-1] - s * f;
      }
      d[l-1] = d[l-1] - p;
      e[l-1] = g;
      e[m-1] = 0.0;
    }
  }
/*
  Sorting.
*/
  for ( ii = 2; ii <= m; ii++ )
  {
    i = ii - 1;
    k = i;
    p = d[i-1];

    for ( j = ii; j <= n; j++ )
    {
      if ( d[j-1] < p )
      {
         k = j;
         p = d[j-1];
      }
    }

    if ( k != i )
    {
      d[k-1] = d[i-1];
      d[i-1] = p;
      p = z[i-1];
      z[i-1] = z[k-1];
      z[k-1] = p;
    }
  }

  return GSL_SUCCESS;
}