/* ===============================================================================*/
/*   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.
// 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 parameters necessary for the calculation
static int nparams = 4;           // The number of free parameters (we can use any of beta, fsigma8, r_g, sigma_g, sigma_u)
static int Data[4] = {0,1,3,4};   // A vector of flags for the parameters we are interested in (0=beta, 1=fsigma8, 2=r_g, 3=sigma_g, 4=sigma_u). 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.1;         // 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.393;      // 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 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.745};   // 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.05;     // 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 = 0;         // 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

// 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/example_nbar_vel.dat",
                         "./example_files/example_nbar_red.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 * 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;

// 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();

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

    FILE * fout;
    int i, j;

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

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

    // Calculate the Fisher matrices for all bins.
    gsl_matrix * Fisher_Tot = gsl_matrix_alloc(nparams, nparams);
    for (i=0; i<nparams; i++) {
        for (j=0; j<nparams; j++) gsl_matrix_set(Fisher_Tot, i, j, 0.0);
    }

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

    if (verbosity == 0) printf("#     zmin         zmax         zeff      fsigma8(z_eff)   percentage error(z_eff)\n");

    int ziter;
    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)
        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);

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

        // 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
        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[6] = {numk, k, Data[i], Data[j], zmin_iter, zmax_iter};

                    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));
                gsl_matrix_set(Fisher, i, j, k_sum/(4.0*M_PI));
                gsl_matrix_set(Fisher, j, i, k_sum/(4.0*M_PI));

            }
        }

        for (i=0; i<nparams; i++) {
            for (j=0; j<nparams; j++) {
                double val = gsl_matrix_get(Fisher_Tot, i, j) + gsl_matrix_get(Fisher, i, j);
                gsl_matrix_set(Fisher_Tot, i, j, val);
            }
        }

        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;
        gsl_matrix * Covariance = gsl_matrix_alloc(nparams, nparams);
        p = gsl_permutation_alloc(nparams);
        gsl_linalg_LU_decomp(Fisher, p, &s);
        gsl_linalg_LU_invert(Fisher, p, Covariance);
        gsl_permutation_free(p);

        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) {
            for (i=0; i<nparams; i++) {
                if (Data[i] == 1) 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) {
            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 (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 (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 (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 (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 (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");
            }
        }

        gsl_matrix_free(Fisher);
        gsl_matrix_free(Covariance);
    }

    // Now the full Fisher matrix over all redshifts if we had more than 1 redshift bin
    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
        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, zmax};

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

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

        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_Tot, i, j));
                printf("],\n");
            }
        }

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

        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) {
            printf("# Full redshift range:\n");
            for (i=0; i<nparams; i++) {
                if (Data[i] == 1) printf("%12.6lf  %12.6lf  %12.6lf  %12.6lf  %12.6lf\n", zmin, zmax, z_eff, f*sigma8, 100.0*sqrt(gsl_matrix_get(Covariance, i, i))/(f*sigma8));
            }
        }

        if (verbosity > 0) {
            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 (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 (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 (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 (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 (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");
            }
        }
    }

    gsl_matrix_free(Fisher_Tot);
    gsl_spline_free(growth_spline);
    gsl_interp_accel_free(growth_acc);

    return 0;
}

// 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 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 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);

        vv_prefac  = 1.0e2*f*mu*veldamp/k;
        dd_prefac = (1.0/(beta*beta) + 2.0*r_g*mu*mu/beta + mu*mu*mu*mu)*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 4x4 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 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 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 P_gg=0.0, P_ug=0.0, P_uu=0.0;

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

        vv_prefac  = 1.0e2*f*mu*veldamp/k;
        dd_prefac = (1.0/(beta*beta) + 2.0*r_g*mu*mu/beta + mu*mu*mu*mu)*f*f*dendamp*dendamp;
        dv_prefac = (r_g/beta + 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/beta + r_g*mu*mu)*f*f*dendamp*dendamp*Pmm/(beta*beta);
                gsl_matrix_set(dPdt1, 0, 0, value);
                value = -(vv_prefac*f*r_g*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/(beta*beta) + 2.0*f*r_g*mu*mu/beta + f*mu*mu*mu*mu)*dendamp*dendamp*Pmm/sigma8;
                gsl_matrix_set(dPdt1, 0, 0, value);
                value = 2.0*vv_prefac*(r_g/beta + 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*(1.0/beta)*mu*mu*f*f*dendamp*dendamp*Pmm;
                gsl_matrix_set(dPdt1, 0, 0, value);
                value = vv_prefac*(1.0/beta)*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;
            default:
                break;
        }
        switch((int)p[3]) {
            // Differential w.r.t betaA
            case 0:
                value = -2.0*(1.0/beta + r_g*mu*mu)*f*f*dendamp*dendamp*Pmm/(beta*beta);
                gsl_matrix_set(dPdt2, 0, 0, value);
                value = -(vv_prefac*f*r_g*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/(beta*beta) + 2.0*f*r_g*mu*mu/beta + f*mu*mu*mu*mu)*dendamp*dendamp*Pmm/sigma8;
                gsl_matrix_set(dPdt2, 0, 0, value);
                value = 2.0*vv_prefac*(r_g/beta + 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*(1.0/beta)*mu*mu*f*f*dendamp*dendamp*Pmm;
                gsl_matrix_set(dPdt2, 0, 0, value);
                value = vv_prefac*(1.0/beta)*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;
            default:
                break;
        }

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

                //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\n", surv, k, mu, r, r_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));
    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]);
        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]);
    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 id less than k_max\n");
        exit(0);
    }

    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 <cblas.h>
#include <stdio.h>
#include <stdlib.h>
#include <papi.h>

#ifndef MATLEN
#define MATLEN 128
#endif

#ifndef QUIET
#define PRINT( exp ) exp
#else
#define PRINT( exp )
#endif

int main( ) {
  int EventSet = PAPI_NULL;
  long long values[ 4 ], s, e;
  int retval;
  double *a, *b, *c;

  /* INICIALIZAÇÃO */

  PRINT( printf( "Inicializando Matriz: %dx%d\n", MATLEN, MATLEN ) );

  a = ( double* ) malloc( MATLEN * MATLEN * sizeof( double ) );
  b = ( double* ) malloc( MATLEN * MATLEN * sizeof( double ) );
  c = ( double* ) malloc( MATLEN * MATLEN * sizeof( double ) );

  /*
   * CONFIGURAÇÃO DO PAPI
   * Init PAPI library
   */
  retval = PAPI_library_init( PAPI_VER_CURRENT );
  if( retval != PAPI_VER_CURRENT ) {
    printf( "Erro em PAPI_library_init : retval = %d\n", retval );
    exit( 1 );
  }
  if( ( retval = PAPI_create_eventset( &EventSet ) ) != PAPI_OK ) {
    printf( "Erro em PAPI_create_eventset : retval = %d\n", retval );
    exit( 1 );
  }
  if( PAPI_add_event( EventSet, PAPI_L2_DCM ) != PAPI_OK ) {
    printf( "Erro em PAPI_L2_DCM\n" );
    exit( 1 );
  }
  if( PAPI_add_event( EventSet, PAPI_DP_OPS ) != PAPI_OK ) {
    printf( "Erro em PAPI_DP_OPS\n" );
    exit( 1 );
  }
  if( PAPI_add_event( EventSet, PAPI_TOT_CYC ) != PAPI_OK ) {
    printf( "Erro em PAPI_TOT_CYC\n" );
    exit( 1 );
  }
  if( PAPI_add_event( EventSet, PAPI_TOT_INS ) != PAPI_OK ) {
    printf( "Erro em PAPI_TOT_INS\n" );
    exit( 1 );
  }
  if( ( retval = PAPI_start( EventSet ) ) != PAPI_OK ) {
    printf( "Erro em PAPI_start" );
    exit( 1 );
  }
  s = PAPI_get_real_usec( );
  /* FUNÇÃO A SER AVALIADA */

  cblas_dgemm( CblasRowMajor, CblasNoTrans, CblasNoTrans,
               MATLEN, MATLEN, MATLEN, 1.0, a, MATLEN,
               b, MATLEN, 0.0, c, MATLEN );

  /* FIM DA FUNÇÃO A SER AVALIADA */
  e = PAPI_get_real_usec( );
  if( ( retval = PAPI_read( EventSet, &values[ 0 ] ) ) != PAPI_OK ) {
    printf( "Erro em PAPI_read" );
    exit( 1 );
  }
  if( ( retval = PAPI_stop( EventSet, NULL ) ) != PAPI_OK ) {
    printf( "Erro em PAPI_stop" );
    exit( 1 );
  }
  double cpi = ( double ) values[ 2 ] / ( double ) values[ 3 ];
  double icp = ( double ) values[ 3 ] / ( double ) values[ 2 ];
  double mflops = ( double ) values[ 1 ];
  // double mflops = ( double ) 2 * MATLEN * MATLEN * MATLEN;
  mflops = ( mflops / ( ( double ) ( e - s ) ) );
  /* EXIBINDO INFORMAÇÕES */
  PRINT(
    printf( "PAPI_L2_DCM = %lld\n", values[ 0 ] );
    printf( "PAPI_DP_OPS = %lld\n", values[ 1 ] );

    /* CPI */
    printf( "PAPI_TOT_CYC = %lld\n", values[ 2 ] );
    printf( "PAPI_TOT_INS = %lld\n", values[ 3 ] );
    printf( "CPI: %.2f\n", cpi );
    printf( "ICP: %.2f\n", icp );

    printf( "Wallclock time: %lld ms\n", e - s );
    printf( "MFLOPS: %g\n", mflops );
    printf( "Fim\n" );
    );
  /*       MAT BLk Time  DCM   MFLOPS CPI */
  printf( "%d, %lld, %lld, %.2f, %.2f\n", MATLEN, e - s, values[ 0 ], mflops, cpi );
  free( a );
  free( b );
  free( c );
  return( 0 );
}


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

/* Author:  G. Jungman */
/* Miscellaneous support functions for Bessel function evaluations.
 */
#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include "gsl_sf_airy.h"
#include "gsl_sf_elementary.h"
#include "gsl_sf_exp.h"
#include "gsl_sf_gamma.h"
#include "gsl_sf_trig.h"

#include "error.h"

#include "bessel_amp_phase.h"
#include "bessel_temme.h"
#include "bessel.h"

#define CubeRoot2_  1.25992104989487316476721060728

/* Debye functions [Abramowitz+Stegun, 9.3.9-10] */

inline static double
debye_u1(const double * tpow)
{
  return (3.0*tpow[1] - 5.0*tpow[3])/24.0;
}

inline static double
debye_u2(const double * tpow)
{
  return (81.0*tpow[2] - 462.0*tpow[4] + 385.0*tpow[6])/1152.0;
}

inline
static double debye_u3(const double * tpow)
{
  return (30375.0*tpow[3] - 369603.0*tpow[5] + 765765.0*tpow[7] - 425425.0*tpow[9])/414720.0;
}

inline
static double debye_u4(const double * tpow)
{
  return (4465125.0*tpow[4] - 94121676.0*tpow[6] + 349922430.0*tpow[8] -
          446185740.0*tpow[10] + 185910725.0*tpow[12])/39813120.0;
}

inline
static double debye_u5(const double * tpow)
{
  return (1519035525.0*tpow[5]     - 49286948607.0*tpow[7] +
          284499769554.0*tpow[9]   - 614135872350.0*tpow[11] +
          566098157625.0*tpow[13]  - 188699385875.0*tpow[15])/6688604160.0;
}

#if 0
inline
static double debye_u6(const double * tpow)
{
  return (2757049477875.0*tpow[6] - 127577298354750.0*tpow[8] +
          1050760774457901.0*tpow[10] - 3369032068261860.0*tpow[12] +
          5104696716244125.0*tpow[14] - 3685299006138750.0*tpow[16] +
          1023694168371875.0*tpow[18])/4815794995200.0;
}
#endif

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

int
gsl_sf_bessel_IJ_taylor_e(const double nu, const double x,
                             const int sign,
                             const int kmax,
                             const double threshold,
                             gsl_sf_result * result
                             )
{
  /* CHECK_POINTER(result) */

  if(nu < 0.0 || x < 0.0) {
    DOMAIN_ERROR(result);
  }
  else if(x == 0.0) {
    if(nu == 0.0) {
      result->val = 1.0;
      result->err = 0.0;
    }
    else {
      result->val = 0.0;
      result->err = 0.0;
    }
    return GSL_SUCCESS;
  }
  else {
    gsl_sf_result prefactor;   /* (x/2)^nu / Gamma(nu+1) */
    gsl_sf_result sum;

    int stat_pre;
    int stat_sum;
    int stat_mul;

    if(nu == 0.0) {
      prefactor.val = 1.0;
      prefactor.err = 0.0;
      stat_pre = GSL_SUCCESS;
    }
    else if(nu < INT_MAX-1) {
      /* Separate the integer part and use
       * y^nu / Gamma(nu+1) = y^N /N! y^f / (N+1)_f,
       * to control the error.
       */
      const int    N = (int)floor(nu + 0.5);
      const double f = nu - N;
      gsl_sf_result poch_factor;
      gsl_sf_result tc_factor;
      const int stat_poch = gsl_sf_poch_e(N+1.0, f, &poch_factor);
      const int stat_tc   = gsl_sf_taylorcoeff_e(N, 0.5*x, &tc_factor);
      const double p = pow(0.5*x,f);
      prefactor.val  = tc_factor.val * p / poch_factor.val;
      prefactor.err  = tc_factor.err * p / poch_factor.val;
      prefactor.err += fabs(prefactor.val) / poch_factor.val * poch_factor.err;
      prefactor.err += 2.0 * GSL_DBL_EPSILON * fabs(prefactor.val);
      stat_pre = GSL_ERROR_SELECT_2(stat_tc, stat_poch);
    }
    else {
      gsl_sf_result lg;
      const int stat_lg = gsl_sf_lngamma_e(nu+1.0, &lg);
      const double term1  = nu*log(0.5*x);
      const double term2  = lg.val;
      const double ln_pre = term1 - term2;
      const double ln_pre_err = GSL_DBL_EPSILON * (fabs(term1)+fabs(term2)) + lg.err;
      const int stat_ex = gsl_sf_exp_err_e(ln_pre, ln_pre_err, &prefactor);
      stat_pre = GSL_ERROR_SELECT_2(stat_ex, stat_lg);
    }

    /* Evaluate the sum.
     * [Abramowitz+Stegun, 9.1.10]
     * [Abramowitz+Stegun, 9.6.7]
     */
    {
      const double y = sign * 0.25 * x*x;
      double sumk = 1.0;
      double term = 1.0;
      int k;

      for(k=1; k<=kmax; k++) {
        term *= y/((nu+k)*k);
        sumk += term;
        if(fabs(term/sumk) < threshold) break;
      }

      sum.val = sumk;
      sum.err = threshold * fabs(sumk);

      stat_sum = ( k >= kmax ? GSL_EMAXITER : GSL_SUCCESS );
    }

    stat_mul = gsl_sf_multiply_err_e(prefactor.val, prefactor.err,
                                        sum.val, sum.err,
                                        result);

    return GSL_ERROR_SELECT_3(stat_mul, stat_pre, stat_sum);
  }
}

/* x >> nu*nu+1
 * error ~ O( ((nu*nu+1)/x)^3 )
 *
 * empirical error analysis:
 *   choose  GSL_ROOT3_MACH_EPS * x > (nu*nu + 1)
 *
 * This is not especially useful. When the argument gets
 * large enough for this to apply, the cos() and sin()
 * start loosing digits. However, this seems inevitable
 * for this particular method.
 */
int
gsl_sf_bessel_Jnu_asympx_e(const double nu, const double x, gsl_sf_result * result)
{
  double mu   = 4.0*nu*nu;
  double mum1 = mu-1.0;
  double mum9 = mu-9.0;
  double chi = x - (0.5*nu + 0.25)*M_PI;
  double P   = 1.0 - mum1*mum9/(128.0*x*x);
  double Q   = mum1/(8.0*x);
  double pre = sqrt(2.0/(M_PI*x));
  double c   = cos(chi);
  double s   = sin(chi);
  double r   = mu/x;
  result->val  = pre * (c*P - s*Q);
  result->err  = pre * GSL_DBL_EPSILON * (fabs(c*P) + fabs(s*Q));
  result->err += pre * fabs(0.1*r*r*r);
  return GSL_SUCCESS;
}

/* x >> nu*nu+1
 */
int
gsl_sf_bessel_Ynu_asympx_e(const double nu, const double x, gsl_sf_result * result)
{
  double ampl;
  double theta;
  double alpha = x;
  double beta  = -0.5*nu*M_PI;
  int stat_a = gsl_sf_bessel_asymp_Mnu_e(nu, x, &ampl);
  int stat_t = gsl_sf_bessel_asymp_thetanu_corr_e(nu, x, &theta);
  double sin_alpha = sin(alpha);
  double cos_alpha = cos(alpha);
  double sin_chi   = sin(beta + theta);
  double cos_chi   = cos(beta + theta);
  double sin_term     = sin_alpha * cos_chi + sin_chi * cos_alpha;
  double sin_term_mag = fabs(sin_alpha * cos_chi) + fabs(sin_chi * cos_alpha);
  result->val  = ampl * sin_term;
  result->err  = fabs(ampl) * GSL_DBL_EPSILON * sin_term_mag;
  result->err += fabs(result->val) * 2.0 * GSL_DBL_EPSILON;

  if(fabs(alpha) > 1.0/GSL_DBL_EPSILON) {
    result->err *= 0.5 * fabs(alpha);
  }
  else if(fabs(alpha) > 1.0/GSL_SQRT_DBL_EPSILON) {
    result->err *= 256.0 * fabs(alpha) * GSL_SQRT_DBL_EPSILON;
  }

  return GSL_ERROR_SELECT_2(stat_t, stat_a);
}

/* x >> nu*nu+1
 */
int
gsl_sf_bessel_Inu_scaled_asympx_e(const double nu, const double x, gsl_sf_result * result)
{
  double mu   = 4.0*nu*nu;
  double mum1 = mu-1.0;
  double mum9 = mu-9.0;
  double pre  = 1.0/sqrt(2.0*M_PI*x);
  double r    = mu/x;
  result->val = pre * (1.0 - mum1/(8.0*x) + mum1*mum9/(128.0*x*x));
  result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val) + pre * fabs(0.1*r*r*r);
  return GSL_SUCCESS;
}

/* x >> nu*nu+1
 */
int
gsl_sf_bessel_Knu_scaled_asympx_e(const double nu, const double x, gsl_sf_result * result)
{
  double mu   = 4.0*nu*nu;
  double mum1 = mu-1.0;
  double mum9 = mu-9.0;
  double pre  = sqrt(M_PI/(2.0*x));
  double r    = nu/x;
  result->val = pre * (1.0 + mum1/(8.0*x) + mum1*mum9/(128.0*x*x));
  result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val) + pre * fabs(0.1*r*r*r);
  return GSL_SUCCESS;
}

/* nu -> Inf; uniform in x > 0  [Abramowitz+Stegun, 9.7.7]
 *
 * error:
 *   The error has the form u_N(t)/nu^N  where  0 <= t <= 1.
 *   It is not hard to show that |u_N(t)| is small for such t.
 *   We have N=6 here, and |u_6(t)| < 0.025, so the error is clearly
 *   bounded by 0.025/nu^6. This gives the asymptotic bound on nu
 *   seen below as nu ~ 100. For general MACH_EPS it will be
 *                     nu > 0.5 / MACH_EPS^(1/6)
 *   When t is small, the bound is even better because |u_N(t)| vanishes
 *   as t->0. In fact u_N(t) ~ C t^N as t->0, with C ~= 0.1.
 *   We write
 *                     err_N <= min(0.025, C(1/(1+(x/nu)^2))^3) / nu^6
 *   therefore
 *                     min(0.29/nu^2, 0.5/(nu^2+x^2)) < MACH_EPS^{1/3}
 *   and this is the general form.
 *
 * empirical error analysis, assuming 14 digit requirement:
 *   choose   x > 50.000 nu   ==>  nu >   3
 *   choose   x > 10.000 nu   ==>  nu >  15
 *   choose   x >  2.000 nu   ==>  nu >  50
 *   choose   x >  1.000 nu   ==>  nu >  75
 *   choose   x >  0.500 nu   ==>  nu >  80
 *   choose   x >  0.100 nu   ==>  nu >  83
 *
 * This makes sense. For x << nu, the error will be of the form u_N(1)/nu^N,
 * since the polynomial term will be evaluated near t=1, so the bound
 * on nu will become constant for small x. Furthermore, increasing x with
 * nu fixed will decrease the error.
 */
int
gsl_sf_bessel_Inu_scaled_asymp_unif_e(const double nu, const double x, gsl_sf_result * result)
{
  int i;
  double z = x/nu;
  double root_term = sqrt(1.0 + z*z);
  double pre = 1.0/sqrt(2.0*M_PI*nu * root_term);
  double eta = root_term + log(z/(1.0+root_term));
  double ex_arg = ( z < 1.0/GSL_ROOT3_DBL_EPSILON ? nu*(-z + eta) : -0.5*nu/z*(1.0 - 1.0/(12.0*z*z)) );
  gsl_sf_result ex_result;
  int stat_ex = gsl_sf_exp_e(ex_arg, &ex_result);
  if(stat_ex == GSL_SUCCESS) {
    double t = 1.0/root_term;
    double sum;
    double tpow[16];
    tpow[0] = 1.0;
    for(i=1; i<16; i++) tpow[i] = t * tpow[i-1];
    sum = 1.0 + debye_u1(tpow)/nu + debye_u2(tpow)/(nu*nu) + debye_u3(tpow)/(nu*nu*nu)
          + debye_u4(tpow)/(nu*nu*nu*nu) + debye_u5(tpow)/(nu*nu*nu*nu*nu);
    result->val  = pre * ex_result.val * sum;
    result->err  = pre * ex_result.val / (nu*nu*nu*nu*nu*nu);
    result->err += pre * ex_result.err * fabs(sum);
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    result->val = 0.0;
    result->err = 0.0;
    return stat_ex;
  }
}

/* nu -> Inf; uniform in x > 0  [Abramowitz+Stegun, 9.7.8]
 *
 * error:
 *   identical to that above for Inu_scaled
 */
int
gsl_sf_bessel_Knu_scaled_asymp_unif_e(const double nu, const double x, gsl_sf_result * result)
{
  int i;
  double z = x/nu;
  double root_term = sqrt(1.0 + z*z);
  double pre = sqrt(M_PI/(2.0*nu*root_term));
  double eta = root_term + log(z/(1.0+root_term));
  double ex_arg = ( z < 1.0/GSL_ROOT3_DBL_EPSILON ? nu*(z - eta) : 0.5*nu/z*(1.0 + 1.0/(12.0*z*z)) );
  gsl_sf_result ex_result;
  int stat_ex = gsl_sf_exp_e(ex_arg, &ex_result);
  if(stat_ex == GSL_SUCCESS) {
    double t = 1.0/root_term;
    double sum;
    double tpow[16];
    tpow[0] = 1.0;
    for(i=1; i<16; i++) tpow[i] = t * tpow[i-1];
    sum = 1.0 - debye_u1(tpow)/nu + debye_u2(tpow)/(nu*nu) - debye_u3(tpow)/(nu*nu*nu)
          + debye_u4(tpow)/(nu*nu*nu*nu) - debye_u5(tpow)/(nu*nu*nu*nu*nu);
    result->val  = pre * ex_result.val * sum;
    result->err  = pre * ex_result.err * fabs(sum);
    result->err += pre * ex_result.val / (nu*nu*nu*nu*nu*nu);
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    result->val = 0.0;
    result->err = 0.0;
    return stat_ex;
  }
}

/* Evaluate J_mu(x),J_{mu+1}(x) and Y_mu(x),Y_{mu+1}(x)  for |mu| < 1/2
 */
int
gsl_sf_bessel_JY_mu_restricted(const double mu, const double x,
                               gsl_sf_result * Jmu, gsl_sf_result * Jmup1,
                               gsl_sf_result * Ymu, gsl_sf_result * Ymup1)
{
  /* CHECK_POINTER(Jmu) */
  /* CHECK_POINTER(Jmup1) */
  /* CHECK_POINTER(Ymu) */
  /* CHECK_POINTER(Ymup1) */

  if(x < 0.0 || fabs(mu) > 0.5) {
    Jmu->val   = 0.0;
    Jmu->err   = 0.0;
    Jmup1->val = 0.0;
    Jmup1->err = 0.0;
    Ymu->val   = 0.0;
    Ymu->err   = 0.0;
    Ymup1->val = 0.0;
    Ymup1->err = 0.0;
    GSL_ERROR ("error", GSL_EDOM);
  }
  else if(x == 0.0) {
    if(mu == 0.0) {
      Jmu->val   = 1.0;
      Jmu->err   = 0.0;
    }
    else {
      Jmu->val   = 0.0;
      Jmu->err   = 0.0;
    }
    Jmup1->val = 0.0;
    Jmup1->err = 0.0;
    Ymu->val   = 0.0;
    Ymu->err   = 0.0;
    Ymup1->val = 0.0;
    Ymup1->err = 0.0;
    GSL_ERROR ("error", GSL_EDOM);
  }
  else {
    int stat_Y;
    int stat_J;

    if(x < 2.0) {
      /* Use Taylor series for J and the Temme series for Y.
       * The Taylor series for J requires nu > 0, so we shift
       * up one and use the recursion relation to get Jmu, in
       * case mu < 0.
       */
      gsl_sf_result Jmup2;
      int stat_J1 = gsl_sf_bessel_IJ_taylor_e(mu+1.0, x, -1, 100, GSL_DBL_EPSILON,  Jmup1);
      int stat_J2 = gsl_sf_bessel_IJ_taylor_e(mu+2.0, x, -1, 100, GSL_DBL_EPSILON, &Jmup2);
      double c = 2.0*(mu+1.0)/x;
      Jmu->val  = c * Jmup1->val - Jmup2.val;
      Jmu->err  = c * Jmup1->err + Jmup2.err;
      Jmu->err += 2.0 * GSL_DBL_EPSILON * fabs(Jmu->val);
      stat_J = GSL_ERROR_SELECT_2(stat_J1, stat_J2);
      stat_Y = gsl_sf_bessel_Y_temme(mu, x, Ymu, Ymup1);
      return GSL_ERROR_SELECT_2(stat_J, stat_Y);
    }
    else if(x < 1000.0) {
      double P, Q;
      double J_ratio;
      double J_sgn;
      const int stat_CF1 = gsl_sf_bessel_J_CF1(mu, x, &J_ratio, &J_sgn);
      const int stat_CF2 = gsl_sf_bessel_JY_steed_CF2(mu, x, &P, &Q);
      double Jprime_J_ratio = mu/x - J_ratio;
      double gamma = (P - Jprime_J_ratio)/Q;
      Jmu->val = J_sgn * sqrt(2.0/(M_PI*x) / (Q + gamma*(P-Jprime_J_ratio)));
      Jmu->err = 4.0 * GSL_DBL_EPSILON * fabs(Jmu->val);
      Jmup1->val = J_ratio * Jmu->val;
      Jmup1->err = fabs(J_ratio) * Jmu->err;
      Ymu->val = gamma * Jmu->val;
      Ymu->err = fabs(gamma) * Jmu->err;
      Ymup1->val = Ymu->val * (mu/x - P - Q/gamma);
      Ymup1->err = Ymu->err * fabs(mu/x - P - Q/gamma) + 4.0*GSL_DBL_EPSILON*fabs(Ymup1->val);
      return GSL_ERROR_SELECT_2(stat_CF1, stat_CF2);
    }
    else {
      /* Use asymptotics for large argument.
       */
      const int stat_J0 = gsl_sf_bessel_Jnu_asympx_e(mu,     x, Jmu);
      const int stat_J1 = gsl_sf_bessel_Jnu_asympx_e(mu+1.0, x, Jmup1);
      const int stat_Y0 = gsl_sf_bessel_Ynu_asympx_e(mu,     x, Ymu);
      const int stat_Y1 = gsl_sf_bessel_Ynu_asympx_e(mu+1.0, x, Ymup1);
      stat_J = GSL_ERROR_SELECT_2(stat_J0, stat_J1);
      stat_Y = GSL_ERROR_SELECT_2(stat_Y0, stat_Y1);
      return GSL_ERROR_SELECT_2(stat_J, stat_Y);
    }
  }
}

int
gsl_sf_bessel_J_CF1(const double nu, const double x,
                    double * ratio, double * sgn)
{
  const double RECUR_BIG = GSL_SQRT_DBL_MAX;
  const int maxiter = 10000;
  int n = 1;
  double Anm2 = 1.0;
  double Bnm2 = 0.0;
  double Anm1 = 0.0;
  double Bnm1 = 1.0;
  double a1 = x/(2.0*(nu+1.0));
  double An = Anm1 + a1*Anm2;
  double Bn = Bnm1 + a1*Bnm2;
  double an;
  double fn = An/Bn;
  double dn = a1;
  double s  = 1.0;

  while(n < maxiter) {
    double old_fn;
    double del;
    n++;
    Anm2 = Anm1;
    Bnm2 = Bnm1;
    Anm1 = An;
    Bnm1 = Bn;
    an = -x*x/(4.0*(nu+n-1.0)*(nu+n));
    An = Anm1 + an*Anm2;
    Bn = Bnm1 + an*Bnm2;

    if(fabs(An) > RECUR_BIG || fabs(Bn) > RECUR_BIG) {
      An /= RECUR_BIG;
      Bn /= RECUR_BIG;
      Anm1 /= RECUR_BIG;
      Bnm1 /= RECUR_BIG;
      Anm2 /= RECUR_BIG;
      Bnm2 /= RECUR_BIG;
    }

    old_fn = fn;
    fn = An/Bn;
    del = old_fn/fn;

    dn = 1.0 / (2.0*(nu+n)/x - dn);
    if(dn < 0.0) s = -s;

    if(fabs(del - 1.0) < 2.0*GSL_DBL_EPSILON) break;
  }

  *ratio = fn;
  *sgn   = s;

  if(n >= maxiter)
    GSL_ERROR ("error", GSL_EMAXITER);
  else
    return GSL_SUCCESS;
}

/* Evaluate the continued fraction CF1 for J_{nu+1}/J_nu
 * using Gautschi (Euler) equivalent series.
 * This exhibits an annoying problem because the
 * a_k are not positive definite (in fact they are all negative).
 * There are cases when rho_k blows up. Example: nu=1,x=4.
 */
#if 0
int
gsl_sf_bessel_J_CF1_ser(const double nu, const double x,
                        double * ratio, double * sgn)
{
  const int maxk = 20000;
  double tk   = 1.0;
  double sum  = 1.0;
  double rhok = 0.0;
  double dk = 0.0;
  double s  = 1.0;
  int k;

  for(k=1; k<maxk; k++) {
    double ak = -0.25 * (x/(nu+k)) * x/(nu+k+1.0);
    rhok = -ak*(1.0 + rhok)/(1.0 + ak*(1.0 + rhok));
    tk  *= rhok;
    sum += tk;

    dk = 1.0 / (2.0/x - (nu+k-1.0)/(nu+k) * dk);
    if(dk < 0.0) s = -s;

    if(fabs(tk/sum) < GSL_DBL_EPSILON) break;
  }

  *ratio = x/(2.0*(nu+1.0)) * sum;
  *sgn   = s;

  if(k == maxk)
    GSL_ERROR ("error", GSL_EMAXITER);
  else
    return GSL_SUCCESS;
}
#endif

/* Evaluate the continued fraction CF1 for I_{nu+1}/I_nu
 * using Gautschi (Euler) equivalent series.
 */
int
gsl_sf_bessel_I_CF1_ser(const double nu, const double x, double * ratio)
{
  const int maxk = 20000;
  double tk   = 1.0;
  double sum  = 1.0;
  double rhok = 0.0;
  int k;

  for(k=1; k<maxk; k++) {
    double ak = 0.25 * (x/(nu+k)) * x/(nu+k+1.0);
    rhok = -ak*(1.0 + rhok)/(1.0 + ak*(1.0 + rhok));
    tk  *= rhok;
    sum += tk;
    if(fabs(tk/sum) < GSL_DBL_EPSILON) break;
  }

  *ratio = x/(2.0*(nu+1.0)) * sum;

  if(k == maxk)
    GSL_ERROR ("error", GSL_EMAXITER);
  else
    return GSL_SUCCESS;
}

int
gsl_sf_bessel_JY_steed_CF2(const double nu, const double x,
                           double * P, double * Q)
{
  const int max_iter = 10000;
  const double SMALL = 1.0e-100;

  int i = 1;

  double x_inv = 1.0/x;
  double a = 0.25 - nu*nu;
  double p = -0.5*x_inv;
  double q = 1.0;
  double br = 2.0*x;
  double bi = 2.0;
  double fact = a*x_inv/(p*p + q*q);
  double cr = br + q*fact;
  double ci = bi + p*fact;
  double den = br*br + bi*bi;
  double dr = br/den;
  double di = -bi/den;
  double dlr = cr*dr - ci*di;
  double dli = cr*di + ci*dr;
  double temp = p*dlr - q*dli;
  q = p*dli + q*dlr;
  p = temp;
  for (i=2; i<=max_iter; i++) {
    a  += 2*(i-1);
    bi += 2.0;
    dr = a*dr + br;
    di = a*di + bi;
    if(fabs(dr)+fabs(di) < SMALL) dr = SMALL;
    fact = a/(cr*cr+ci*ci);
    cr = br + cr*fact;
    ci = bi - ci*fact;
    if(fabs(cr)+fabs(ci) < SMALL) cr = SMALL;
    den = dr*dr + di*di;
    dr /= den;
    di /= -den;
    dlr = cr*dr - ci*di;
    dli = cr*di + ci*dr;
    temp = p*dlr - q*dli;
    q = p*dli + q*dlr;
    p = temp;
    if(fabs(dlr-1.0)+fabs(dli) < GSL_DBL_EPSILON) break;
  }

  *P = p;
  *Q = q;

  if(i == max_iter)
    GSL_ERROR ("error", GSL_EMAXITER);
  else
    return GSL_SUCCESS;
}

/* Evaluate continued fraction CF2, using Thompson-Barnett-Temme method,
 * to obtain values of exp(x)*K_nu and exp(x)*K_{nu+1}.
 *
 * This is unstable for small x; x > 2 is a good cutoff.
 * Also requires |nu| < 1/2.
 */
int
gsl_sf_bessel_K_scaled_steed_temme_CF2(const double nu, const double x,
                                       double * K_nu, double * K_nup1,
                                       double * Kp_nu)
{
  const int maxiter = 10000;

  int i = 1;
  double bi = 2.0*(1.0 + x);
  double di = 1.0/bi;
  double delhi = di;
  double hi    = di;

  double qi   = 0.0;
  double qip1 = 1.0;

  double ai = -(0.25 - nu*nu);
  double a1 = ai;
  double ci = -ai;
  double Qi = -ai;

  double s = 1.0 + Qi*delhi;

  for(i=2; i<=maxiter; i++) {
    double dels;
    double tmp;
    ai -= 2.0*(i-1);
    ci  = -ai*ci/i;
    tmp  = (qi - bi*qip1)/ai;
    qi   = qip1;
    qip1 = tmp;
    Qi += ci*qip1;
    bi += 2.0;
    di  = 1.0/(bi + ai*di);
    delhi = (bi*di - 1.0) * delhi;
    hi += delhi;
    dels = Qi*delhi;
    s += dels;
    if(fabs(dels/s) < GSL_DBL_EPSILON) break;
  }

  hi *= -a1;

  *K_nu   = sqrt(M_PI/(2.0*x)) / s;
  *K_nup1 = *K_nu * (nu + x + 0.5 - hi)/x;
  *Kp_nu  = - *K_nup1 + nu/x * *K_nu;
  if(i == maxiter)
    GSL_ERROR ("error", GSL_EMAXITER);
  else
    return GSL_SUCCESS;
}

int gsl_sf_bessel_cos_pi4_e(double y, double eps, gsl_sf_result * result)
{
  const double sy = sin(y);
  const double cy = cos(y);
  const double s = sy + cy;
  const double d = sy - cy;
  const double abs_sum = fabs(cy) + fabs(sy);
  double seps;
  double ceps;
  if(fabs(eps) < GSL_ROOT5_DBL_EPSILON) {
    const double e2 = eps*eps;
    seps = eps * (1.0 - e2/6.0 * (1.0 - e2/20.0));
    ceps = 1.0 - e2/2.0 * (1.0 - e2/12.0);
  }
  else {
    seps = sin(eps);
    ceps = cos(eps);
  }
  result->val = (ceps * s - seps * d)/ M_SQRT2;
  result->err = 2.0 * GSL_DBL_EPSILON * (fabs(ceps) + fabs(seps)) * abs_sum / M_SQRT2;

  /* Try to account for error in evaluation of sin(y), cos(y).
   * This is a little sticky because we don't really know
   * how the library routines are doing their argument reduction.
   * However, we will make a reasonable guess.
   * FIXME ?
   */
  if(y > 1.0/GSL_DBL_EPSILON) {
    result->err *= 0.5 * y;
  }
  else if(y > 1.0/GSL_SQRT_DBL_EPSILON) {
    result->err *= 256.0 * y * GSL_SQRT_DBL_EPSILON;
  }

  return GSL_SUCCESS;
}

int gsl_sf_bessel_sin_pi4_e(double y, double eps, gsl_sf_result * result)
{
  const double sy = sin(y);
  const double cy = cos(y);
  const double s = sy + cy;
  const double d = sy - cy;
  const double abs_sum = fabs(cy) + fabs(sy);
  double seps;
  double ceps;
  if(fabs(eps) < GSL_ROOT5_DBL_EPSILON) {
    const double e2 = eps*eps;
    seps = eps * (1.0 - e2/6.0 * (1.0 - e2/20.0));
    ceps = 1.0 - e2/2.0 * (1.0 - e2/12.0);
  }
  else {
    seps = sin(eps);
    ceps = cos(eps);
  }
  result->val = (ceps * d + seps * s)/ M_SQRT2;
  result->err = 2.0 * GSL_DBL_EPSILON * (fabs(ceps) + fabs(seps)) * abs_sum / M_SQRT2;

  /* Try to account for error in evaluation of sin(y), cos(y).
   * See above.
   * FIXME ?
   */
  if(y > 1.0/GSL_DBL_EPSILON) {
    result->err *= 0.5 * y;
  }
  else if(y > 1.0/GSL_SQRT_DBL_EPSILON) {
    result->err *= 256.0 * y * GSL_SQRT_DBL_EPSILON;
  }

  return GSL_SUCCESS;
}

/************************************************************************
 *                                                                      *
  Asymptotic approximations 8.11.5, 8.12.5, and 8.42.7 from
  G.N.Watson, A Treatise on the Theory of Bessel Functions,
  2nd Edition (Cambridge University Press, 1944).
  Higher terms in expansion for x near l given by
  Airey in Phil. Mag. 31, 520 (1916).

  This approximation is accurate to near 0.1% at the boundaries
  between the asymptotic regions; well away from the boundaries
  the accuracy is better than 10^{-5}.
 *                                                                      *
 ************************************************************************/
#if 0
double besselJ_meissel(double nu, double x)
{
  double beta = pow(nu, 0.325);
  double result;

  /* Fitted matching points.   */
  double llimit = 1.1 * beta;
  double ulimit = 1.3 * beta;

  double nu2 = nu * nu;

  if (nu < 5. && x < 1.)
    {
      /* Small argument and order. Use a Taylor expansion. */
      int k;
      double xo2 = 0.5 * x;
      double gamfactor = pow(nu,nu) * exp(-nu) * sqrt(nu * 2. * M_PI)
	* (1. + 1./(12.*nu) + 1./(288.*nu*nu));
      double prefactor = pow(xo2, nu) / gamfactor;
      double C[5];

      C[0] = 1.;
      C[1] = -C[0] / (nu+1.);
      C[2] = -C[1] / (2.*(nu+2.));
      C[3] = -C[2] / (3.*(nu+3.));
      C[4] = -C[3] / (4.*(nu+4.));

      result = 0.;
      for(k=0; k<5; k++)
	result += C[k] * pow(xo2, 2.*k);

      result *= prefactor;
    }
  else if(x < nu - llimit)
    {
      /* Small x region: x << l.    */
      double z = x / nu;
      double z2 = z*z;
      double rtomz2 = sqrt(1.-z2);
      double omz2_2 = (1.-z2)*(1.-z2);

      /* Calculate Meissel exponent. */
      double term1 = 1./(24.*nu) * ((2.+3.*z2)/((1.-z2)*rtomz2) -2.);
      double term2 = - z2*(4. + z2)/(16.*nu2*(1.-z2)*omz2_2);
      double V_nu = term1 + term2;

      /* Calculate the harmless prefactor. */
      double sterlingsum = 1. + 1./(12.*nu) + 1./(288*nu2);
      double harmless = 1. / (sqrt(rtomz2*2.*M_PI*nu) * sterlingsum);

      /* Calculate the logarithm of the nu dependent prefactor. */
      double ln_nupre = rtomz2 + log(z) - log(1. + rtomz2);

      result = harmless * exp(nu*ln_nupre - V_nu);
    }
  else if(x < nu + ulimit)
    {
      /* Intermediate region 1: x near nu. */
      double eps = 1.-nu/x;
      double eps_x = eps * x;
      double eps_x_2 = eps_x * eps_x;
      double xo6 = x/6.;
      double B[6];
      static double gam[6] = {2.67894, 1.35412, 1., 0.89298, 0.902745, 1.};
      static double sf[6] = {0.866025, 0.866025, 0., -0.866025, -0.866025, 0.};

      /* Some terms are identically zero, because sf[] can be zero.
       * Some terms do not appear in the result.
       */
      B[0] = 1.;
      B[1] = eps_x;
      /* B[2] = 0.5 * eps_x_2 - 1./20.; */
      B[3] = eps_x * (eps_x_2/6. - 1./15.);
      B[4] = eps_x_2 * (eps_x_2 - 1.)/24. + 1./280.;
      /* B[5] = eps_x * (eps_x_2*(0.5*eps_x_2 - 1.)/60. + 43./8400.); */

      result  = B[0] * gam[0] * sf[0] / pow(xo6, 1./3.);
      result += B[1] * gam[1] * sf[1] / pow(xo6, 2./3.);
      result += B[3] * gam[3] * sf[3] / pow(xo6, 4./3.);
      result += B[4] * gam[4] * sf[4] / pow(xo6, 5./3.);

      result /= (3.*M_PI);
    }
  else
    {
      /* Region of very large argument. Use expansion
       * for x>>l, and we need not be very exacting.
       */
      double secb = x/nu;
      double sec2b= secb*secb;

      double cotb = 1./sqrt(sec2b-1.);      /* cotb=cot(beta) */

      double beta = acos(nu/x);
      double trigarg = nu/cotb - nu*beta - 0.25 * M_PI;

      double cot3b = cotb * cotb * cotb;
      double cot6b = cot3b * cot3b;

      double sum1, sum2, expterm, prefactor, trigcos;

      sum1  = 2.0 + 3.0 * sec2b;
      trigarg -= sum1 * cot3b / (24.0 * nu);

      trigcos = cos(trigarg);

      sum2 = 4.0 + sec2b;
      expterm = sum2 * sec2b * cot6b / (16.0 * nu2);

      expterm = exp(-expterm);
      prefactor = sqrt(2. * cotb / (nu * M_PI));

      result = prefactor * expterm * trigcos;
    }

  return  result;
}
#endif


#ifndef __MATH_GSL_MATRIX__
#define __MATH_GSL_MATRIX__

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

class GslMatrixItem {
 public:
  GslMatrixItem(gsl_matrix* ptr, size_t index1, size_t index2) :
    ptr_(ptr),
    index1_(index1),
    index2_(index2) { }

  operator const double() {
    return gsl_matrix_get(ptr_, index1_, index2_);
  }

  double operator =(const double v) {
    gsl_matrix_set(ptr_, index1_, index2_, v);
    return v;
  }

  double operator +=(const double v) {
    double old_v = gsl_matrix_get(ptr_, index1_, index2_);
    gsl_matrix_set(ptr_, index1_, index2_, v + old_v);
    return v + old_v;
  }
 private:
  gsl_matrix* ptr_;
  size_t index1_;
  size_t index2_;
};

class GslMatrixBase {
 public:
  GslMatrixBase& operator=(const double v) {
    if (v == 0.0) {
      SetZero();
    } else {
      SetAll(v);
    }
    return *this;
  }

  GslMatrixItem operator()(const size_t index1, const size_t index2) const {
    assert(ptr_ != NULL);
    return GslMatrixItem(ptr_, index1, index2);
  }

  void SetZero() {
    assert(ptr_ != NULL);
    gsl_matrix_set_zero(ptr_);
  }

  void SetAll(const double v) {
    assert(ptr_ != NULL);
    gsl_matrix_set_all(ptr_, v);
  }

  void Reset(gsl_matrix* val) {
    if(ptr_ != NULL) {
      gsl_matrix_free(ptr_);
    }
    ptr_ = val;
  }

  int Fprintf(FILE* stream, const char* format) const {
    assert(ptr_ != NULL);
    return gsl_matrix_fprintf(stream, ptr_, format);
  }

  int Fscanf(FILE* stream) {
    assert(ptr_ != NULL);
    return gsl_matrix_fscanf(stream, ptr_);
  }

  void Set(const int i, const int j, double val) {
    gsl_matrix_set(ptr_, i, j, val);
  }

  /*
    double operator()(const int nCol, const int nRow) {
    return gsl_matrix_get(ptr_, nCol, nRow);
    }
  */

  int size1() const {
    return ptr_->size1;
  }

  int size2() const {
    return ptr_->size2;
  }

  double Trace() const {
    double val = 0;
    assert(ptr_ != NULL);
    assert(ptr_->size1 == ptr_->size2);
    for (size_t ii = 0; ii < ptr_->size1; ++ii) {
      val += gsl_matrix_get(ptr_, ii, ii);
    }
    return val;
  }

  double Sum() const {
    double val = 0;
    assert(ptr_ != NULL);
    for (size_t ii = 0; ii < ptr_->size1; ++ii) {
      for (size_t jj = 0; jj < ptr_->size2; ++jj) {
	val += gsl_matrix_get(ptr_, ii, jj);
      }
    }
    return val;
  }

  /*
   * Apply the transpose of this matrix to a vector x and store the result.

   int TransMul(const GslVector& x, GslVector& res, double scale = 0.0) {
   return gsl_blas_dgemv(CblasTrans, 1.0, ptr_, x.ptr(), scale, res.ptr());
   }

   int Mul(const GslVector& x, GslVector& res, double scale = 0.0) {
   return gsl_blas_dgemv(CblasNoTrans, 1.0, ptr_, x.ptr(), scale, res.ptr());
   }
  */

  const gsl_matrix* ptr() const { return ptr_; }
  gsl_matrix* mutable_ptr() { return ptr_; }

 protected:
  GslMatrixBase() : ptr_(NULL) {
  }
  gsl_matrix* ptr_;

 private:
  GslMatrixBase(const GslMatrixBase&) { }
};

class GslMatrix : public GslMatrixBase {
 public:
  GslMatrix(const size_t size1, const size_t size2) : GslMatrixBase() {
    Allocate(size1, size2);
  }

  void Allocate(const size_t size1, const size_t size2) {
    assert(ptr_ == NULL);
    ptr_ = gsl_matrix_alloc(size1, size2);
  }

  GslMatrix() : GslMatrixBase() {
  }

  GslMatrix(gsl_matrix* val) : GslMatrixBase() {
    ptr_ = val;
  }

  ~GslMatrix() {
    if(ptr_ != NULL) {
      gsl_matrix_free(ptr_);
    }
  }

  GslMatrixBase& operator=(const double v) {
    GslMatrixBase::operator=(v);
    return *this;
  }
 private:
  GslMatrix(const GslMatrix&) { }
};

class GslSubmatrix : public GslMatrixBase {
 public:
 GslSubmatrix(GslMatrixBase& matrix, size_t k1, size_t k2, size_t n1, size_t n2) :
  view_(gsl_matrix_submatrix(matrix.mutable_ptr(), k1, k2, n1, n2)) {
     ptr_ = &view_.matrix;
  }

 GslSubmatrix(gsl_matrix* matrix, size_t k1, size_t k2, size_t n1, size_t n2) :
  view_(gsl_matrix_submatrix(matrix, k1, k2, n1, n2)) {
     ptr_ = &view_.matrix;
  }

  GslMatrixBase& operator=(const double v) {
    GslMatrixBase::operator=(v);
    return *this;
  }
 private:
  gsl_matrix_view view_;
  GslSubmatrix(const GslSubmatrix&) { }
};

#endif  // __MATH_GSL_MATRIX__


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "compearth.h"
/*
#ifdef COMPEARTH_USE_MKL
#include <mkl_cblas.h>
#else
#include <cblas.h>
#endif
*/

/*!
 * @brief Converts a moment tensor, M, from input system defined by i1in
 *        to moment tensor, Mout, to output system defind by i2in.
 *
 * @param[in] nmt      Number of moment tensors.
 * @param[in] i1in     Coordinate system for M: \n
 *                       = CE_USE (1) -> up, south, east \n
 *                       = CE_NED (2) -> north, east, down \n
 *                       = CE_NWU (3) -> north west, up \n
 *                       = CE_ENU (4) -> east, north, up \n
 *                       = CE_SEU (5) -> south, east, up \n
 * @param[in] i2in     Coordinate system for Mout: \n
 *                       = CE_USE (1) -> up, south, east \n
 *                       = CE_NED (2) -> north, east, down \n
 *                       = CE_NWU (3) -> north west, up \n
 *                       = CE_ENU (4) -> east, north, up \n
 *                       = CE_SEU (5) -> south, east, up \n
 * @param[in] M        Input moment tensor in system i1in.  This is an
 *                     an array of dimension [6 x nmt].
 *                     The C indices {0,1,2,3,4,5} correspond to matrix
 *                     indices: {11, 22, 33, 12, 13, 23}.
 *
 * @param[out] Mout    Corresponding moment tensor now in system i2in.
 *                     This is an array of dimension [6 x nmt].
 *                     the C indices {0,1,2,3,4,5} correspond to matrix
 *                     indices: {11, 22, 33, 12, 13, 23}
 *
 * @result 0 indicates success.
 *
 * @date 2016 - Ben Baker converted Carl Tape's convert_MT.m to C
 *
 * @copyright MIT
 *
 */
int compearth_convertMT(const int nmt,
                        const enum compearthCoordSystem_enum i1in,
                        const enum compearthCoordSystem_enum i2in,
                        const double *__restrict__ M,
                        double *__restrict__ Mout)
{
    int i, i1, i2;
    // Check the inputs to avoid seg faults
    if (nmt < 1 || M == NULL || Mout == NULL)
    {
        if (nmt < 1){fprintf(stderr, "%s: No moment tensors\n", __func__);}
        if (M == NULL){fprintf(stderr, "%s: Error M is NULL\n", __func__);}
        if (Mout == NULL)
        {
            fprintf(stderr, "%s: Error Mout is NULL\n", __func__);
        }
        return -1;
    }
    for (i=0; i<6*nmt; i++){Mout[i] = 0.0;}
    // Quick checks
    i1 = (int) i1in;
    i2 = (int) i2in;
    if (i1 < 1 || i1 > 5)
    {
        fprintf(stderr, "%s: Error unknown input coordinate system %d\n",
                __func__, i1);
        return -1;
    }
    if (i2 < 1 || i2 > 5)
    {
        fprintf(stderr, "%s: Error unkonwn output coordinate system %d\n",
                __func__, i2);
        return -1;
    }
    // Base case
    if (i1 == i2)
    {
        memcpy(Mout, M, (size_t) nmt*6*sizeof(double));
        //cblas_dcopy(6*nmt, M, 1, Mout, 1);
        return 0;
    }
    // Convert
    if (i1 == 1)
    {
        // up-south-east (GCMT) to north-east-down (AkiRichards)
        // (AR, 1980, p. 118)
        if (i2 == 2)
        {
            for (i=0; i<nmt; i++)
            {
                Mout[6*i+0] = M[6*i+1]; //  tt -> xx
                Mout[6*i+1] = M[6*i+2]; //  pp -> yy
                Mout[6*i+2] = M[6*i+0]; //  rr -> zz
                Mout[6*i+3] =-M[6*i+5]; // -tp -> xy
                Mout[6*i+4] = M[6*i+3]; //  rt -> xz
                Mout[6*i+5] =-M[6*i+4]; // -rp -> yz
            }
       }
       // up-south-east (GCMT) to north-west-up
       else if (i2 == 3)
       {
            for (i=0; i<nmt; i++)
            {
                Mout[6*i+0] = M[6*i+1];
                Mout[6*i+1] = M[6*i+2];
                Mout[6*i+2] = M[6*i+0];
                Mout[6*i+3] = M[6*i+5];
                Mout[6*i+4] =-M[6*i+3];
                Mout[6*i+5] =-M[6*i+4];
            }
       }
       // up-south-east (GCMT) to east-north-up
       else if (i2 == 4)
       {
            for (i=0; i<nmt; i++)
            {
                Mout[6*i+0] = M[6*i+2];
                Mout[6*i+1] = M[6*i+1];
                Mout[6*i+2] = M[6*i+0];
                Mout[6*i+3] =-M[6*i+5];
                Mout[6*i+4] = M[6*i+4];
                Mout[6*i+5] =-M[6*i+3];
            }
       }
       // up-south-east (GCMT) to south-east-up
       else if (i2 == 5)
       {
            for (i=0; i<nmt; i++)
            {
                Mout[6*i+0] = M[6*i+1];
                Mout[6*i+1] = M[6*i+2];
                Mout[6*i+2] = M[6*i+0];
                Mout[6*i+3] = M[6*i+5];
                Mout[6*i+4] = M[6*i+3];
                Mout[6*i+5] = M[6*i+4];
            }
        }
    }
    else if (i1 == 2)
    {
        // north-east-down (AkiRichards) to up-south-east (GCMT)
        // (AR, 1980, p. 118)
        if (i2 == 1)
        {
            for (i=0; i<nmt; i++)
            {
                Mout[6*i+0] = M[6*i+2]; //  zz -> rr
                Mout[6*i+1] = M[6*i+0]; //  xx -> tt
                Mout[6*i+2] = M[6*i+1]; //  yy -> pp
                Mout[6*i+3] = M[6*i+4]; //  xz -> rt
                Mout[6*i+4] =-M[6*i+5]; // -yz -> rp
                Mout[6*i+5] =-M[6*i+3]; // -xy -> tp
            }
        }
        // north-east-down (AkiRichards) to north-west-up
        else if (i2 == 3)
        {
            for (i=0; i<nmt; i++)
            {
                Mout[6*i+0] = M[6*i+0];
                Mout[6*i+1] = M[6*i+1];
                Mout[6*i+2] = M[6*i+2];
                Mout[6*i+3] =-M[6*i+3];
                Mout[6*i+4] =-M[6*i+4];
                Mout[6*i+5] = M[6*i+5];
            }
        }
        // north-east-down (AkiRichards) to east-north-up
        else if (i2 == 4)
        {
            for (i=0; i<nmt; i++)
            {
                Mout[6*i+0] = M[6*i+1];
                Mout[6*i+1] = M[6*i+0];
                Mout[6*i+2] = M[6*i+2];
                Mout[6*i+3] = M[6*i+3];
                Mout[6*i+4] =-M[6*i+5];
                Mout[6*i+5] =-M[6*i+4];
            }
        }
        // north-east-down (AkiRichards) to south-east-up
        else if (i2 == 5)
        {
            for (i=0; i<nmt; i++)
            {
                Mout[6*i+0] = M[6*i+0];
                Mout[6*i+1] = M[6*i+1];
                Mout[6*i+2] = M[6*i+2];
                Mout[6*i+3] =-M[6*i+3];
                Mout[6*i+4] = M[6*i+4];
                Mout[6*i+5] =-M[6*i+5];
            }
        }
    }
    else if (i1 == 3)
    {
        // north-west-up to up-south-east (GCMT)
        if (i2 == 1)
        {
            for (i=0; i<nmt; i++)
            {
                Mout[6*i+0] = M[6*i+2];
                Mout[6*i+1] = M[6*i+0];
                Mout[6*i+2] = M[6*i+1];
                Mout[6*i+3] =-M[6*i+4];
                Mout[6*i+4] =-M[6*i+5];
                Mout[6*i+5] = M[6*i+3];
            }
        }
        // north-west-up to north-east-down (AkiRichards)
        else if (i2 == 2)
        {
            for (i=0; i<nmt; i++)
            {
                Mout[6*i+0] = M[6*i+0];
                Mout[6*i+1] = M[6*i+1];
                Mout[6*i+2] = M[6*i+2];
                Mout[6*i+3] =-M[6*i+3];
                Mout[6*i+4] =-M[6*i+4];
                Mout[6*i+5] = M[6*i+5];
            }
        }
        // north-west-up to east-north-up
        else if (i2 == 4)
        {
            for (i=0; i<nmt; i++)
            {
                Mout[6*i+0] = M[6*i+1];
                Mout[6*i+1] = M[6*i+0];
                Mout[6*i+2] = M[6*i+2];
                Mout[6*i+3] =-M[6*i+3];
                Mout[6*i+4] =-M[6*i+5];
                Mout[6*i+5] = M[6*i+4];
            }
        }
        // north-west-up to south-east-up
        else if (i2 == 5)
        {
            for (i=0; i<nmt; i++)
            {
                Mout[6*i+0] = M[6*i+0];
                Mout[6*i+1] = M[6*i+1];
                Mout[6*i+2] = M[6*i+2];
                Mout[6*i+3] = M[6*i+3];
                Mout[6*i+4] =-M[6*i+4];
                Mout[6*i+5] =-M[6*i+5];
            }
        }
    }
    else if (i1 == 4)
    {
        // east-north-up to up-south-east (GCMT)
        if (i2 == 1)
        {
            for (i=0; i<nmt; i++)
            {
                Mout[6*i+0] = M[6*i+2];
                Mout[6*i+1] = M[6*i+1];
                Mout[6*i+2] = M[6*i+0];
                Mout[6*i+3] =-M[6*i+5];
                Mout[6*i+4] = M[6*i+4];
                Mout[6*i+5] =-M[6*i+3];
            }
        }
        // east-north-up to north-east-down (AkiRichards)
        else if (i2 == 2)
        {
            for (i=0; i<nmt; i++)
            {
                Mout[6*i+0] = M[6*i+1];
                Mout[6*i+1] = M[6*i+0];
                Mout[6*i+2] = M[6*i+2];
                Mout[6*i+3] = M[6*i+3];
                Mout[6*i+4] =-M[6*i+5];
                Mout[6*i+5] =-M[6*i+4];
            }
        }
        // east-north-up to north-west-up
        else if (i2 == 3)
        {
            for (i=0; i<nmt; i++)
            {
                Mout[6*i+0] = M[6*i+1];
                Mout[6*i+1] = M[6*i+0];
                Mout[6*i+2] = M[6*i+2];
                Mout[6*i+3] =-M[6*i+3];
                Mout[6*i+4] = M[6*i+5];
                Mout[6*i+5] =-M[6*i+4];
            }
        }
        // east-north-up to south-east-up
        else if (i2 == 5)
        {
            for (i=0; i<nmt; i++)
            {
                Mout[6*i+0] = M[6*i+1];
                Mout[6*i+1] = M[6*i+0];
                Mout[6*i+2] = M[6*i+2];
                Mout[6*i+3] =-M[6*i+3];
                Mout[6*i+4] =-M[6*i+5];
                Mout[6*i+5] = M[6*i+4];
            }
        }
    }
    else if (i1 == 5)
    {
        // south-east-up to up-south-east (GCMT)
        if (i2 == 1)
        {
            for (i=0; i<nmt; i++)
            {
                Mout[6*i+0] = M[6*i+2];
                Mout[6*i+1] = M[6*i+0];
                Mout[6*i+2] = M[6*i+1];
                Mout[6*i+3] = M[6*i+4];
                Mout[6*i+4] = M[6*i+5];
                Mout[6*i+5] = M[6*i+3];
            }
        }
        // south-east-up to north-east-down (AkiRichards)
        else if (i2 == 2)
        {
            for (i=0; i<nmt; i++)
            {
                Mout[6*i+0] = M[6*i+0];
                Mout[6*i+1] = M[6*i+1];
                Mout[6*i+2] = M[6*i+2];
                Mout[6*i+3] =-M[6*i+3];
                Mout[6*i+4] = M[6*i+4];
                Mout[6*i+5] =-M[6*i+5];
            }
        }
        // south-east-up to north-west-up
        else if (i2 == 3)
        {
            for (i=0; i<nmt; i++)
            {
                Mout[6*i+0] = M[6*i+0];
                Mout[6*i+1] = M[6*i+1];
                Mout[6*i+2] = M[6*i+2];
                Mout[6*i+3] = M[6*i+3];
                Mout[6*i+4] =-M[6*i+4];
                Mout[6*i+5] =-M[6*i+5];
            }
        }
        // south-east-up to east-north-up
        else if (i2 == 4)
        {
            for (i=0; i<nmt; i++)
            {
                Mout[6*i+0] = M[6*i+1];
                Mout[6*i+1] = M[6*i+0];
                Mout[6*i+2] = M[6*i+2];
                Mout[6*i+3] =-M[6*i+3];
                Mout[6*i+4] = M[6*i+5];
                Mout[6*i+5] =-M[6*i+4];
            }
        }
    }
    return 0;
}