#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <assert.h>
#include <math.h>
#include <sys/time.h>
#include <mm_malloc.h>
#include <cblas.h>
#include <lapacke.h>

#include "random_matrix.h"
#include "driver.h"
#include "../../runtime/pcp.h"

static double compare_lower_triangular_matrices(int n, double *L1, int ldL1, double *L2, int ldL2)
{
#define L1(i,j) L1[(i) + (j) * ldL1]
#define L2(i,j) L2[(i) + (j) * ldL2]

    // Clear out the upper triangular part.
    for (int j = 0; j < n; ++j) {
        for (int i = 0; i < j; i++) {
            L1(i,j) = 0.0;
            L2(i,j) = 0.0;
        }
    }

    // Compute L1 := L1 - L2.
    for (int j = 0; j < n; j++) {
        for (int i = 0; i < n; i++) {
            L1(i,j) = L1(i,j) - L2(i,j);
        }
    }

    // Compute || L1 ||_F.
    double error = LAPACKE_dlange(LAPACK_COL_MAJOR, 'F', n, n, L1, ldL1);
    error /= n;
    return error;

#undef L1
#undef L2
}

static void usage(char *prog)
{
    printf("Usage: %s n b p q\n", prog);
    printf("\n");
    printf("n: The size of the matrix\n");
    printf("b: The tile size\n");
    printf("p: The number of threads (or 0 for one per core)\n");
    printf("q: The size of the reserved set (or 0 to use the regular mode)\n");
    exit(EXIT_FAILURE);
}

int main(int argc, char** argv)
{
    // Verify the number of command line options.
    if (argc != 5 && argc != 6) {
        usage(argv[0]);
    }

    // Parse command line.
    int n = atoi(argv[1]);
    int blksz = atoi(argv[2]);
    int num_threads = atoi(argv[3]);
    int reserved_set_size = atoi(argv[4]);
    int verify = 1;
    if (argc == 6) {
        verify = 0;
    }

    // Verify options.
    if (n < 1 ||
        blksz < 1 ||
        num_threads < 0 ||
        reserved_set_size < 0 ||
        (reserved_set_size >= num_threads && num_threads > 0)) {
        usage(argv[0]);
    }

    // Initialise random number generator.
    srand(time(NULL));

    // Generate random SPD matrix A.
    printf("Generating SPD matrix A...\n");
    int ldA = n;
    double *A = (double *) malloc(n * ldA * sizeof(double));
    generate_dense_spd_matrix(n, A, ldA);

    // Copy A to Ain.
    printf("Copying A to Ain...\n");
    double *Ain = (double *) malloc(n * ldA * sizeof(double));
    memcpy(Ain, A, n * ldA * sizeof(double));

    // Start the runtime system.
    pcp_start(num_threads);

    // Set the execution mode.
    if (reserved_set_size == 0) {
        pcp_set_mode(PCP_REGULAR);
    } else {
        pcp_set_mode(PCP_FIXED);
        pcp_set_reserved_set_size(reserved_set_size);
    }

    // Call the driver.
    printf("Calling the driver routine...\n");
    parallel_block_chol(n, blksz, A, ldA);

    if (verify) {
        // Verify the solution.
        printf("Verifying the solution... ");
        fflush(stdout);
        double tm = pcp_get_time();
        LAPACKE_dpotrf(LAPACK_COL_MAJOR, 'L', n, Ain, ldA);
        tm = pcp_get_time() - tm;
        double error = compare_lower_triangular_matrices(n, Ain, ldA, A, ldA);
        if (error < 1e-14) {
            printf("PASSED\n");
        } else {
            printf("FAILED\n");
        }
        printf("LAPACK_dpotrf took %.6lf seconds\n", tm);
    } else {
        printf("Verification disabled\n");
    }

    // Save the trace.
    printf("Saving the trace...\n");
    pcp_view_trace_tikz();

    // View statistics.
    printf("Printing statistics...\n");
    pcp_view_statistics_stdout();

    // Stop the runtime system.
    pcp_stop();

    // Clean up.
    free(A);
    free(Ain);

    return EXIT_SUCCESS;
}


/* linalg/test_tri.c
 *
 * Copyright (C) 2019 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.
 */

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_ieee_utils.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_rng.h>

static int
test_symmtd_decomp_eps(const gsl_matrix * m, const double eps, const char * desc)
{
  int s = 0;
  const size_t N = m->size1;
  size_t i, j;

  gsl_matrix * Q = gsl_matrix_alloc(N, N);
  gsl_matrix * T = gsl_matrix_calloc(N, N);
  gsl_matrix * A  = gsl_matrix_alloc(N, N);
  gsl_matrix * B  = gsl_matrix_alloc(N, N);
  gsl_vector * tau = gsl_vector_alloc(N - 1);
  gsl_vector_view diag = gsl_matrix_diagonal(T);
  gsl_vector_view subdiag = gsl_matrix_subdiagonal(T, 1);
  gsl_vector_view superdiag = gsl_matrix_superdiagonal(T, 1);

  gsl_matrix_memcpy(A, m);
  s += gsl_linalg_symmtd_decomp(A, tau);

  s += gsl_linalg_symmtd_unpack(A, tau, Q, &diag.vector, &subdiag.vector);
  gsl_vector_memcpy(&superdiag.vector, &subdiag.vector);

  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Q, T, 0.0, A); /* A := Q T */
  gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, A, Q, 0.0, B);   /* B := Q T Q^T */

  for (i = 0; i < N; i++)
    {
      for (j = 0; j <= i; j++)
        {
          double bij = gsl_matrix_get(B, i, j);
          double mij = gsl_matrix_get(m, i, j);

          gsl_test_rel(bij, mij, eps, "%s (%3lu)[%lu,%lu]: %22.18g   %22.18g\n",
                       desc, N, i, j, bij, mij);
        }
    }

  gsl_matrix_free(T);
  gsl_matrix_free(A);
  gsl_matrix_free(Q);
  gsl_matrix_free(B);
  gsl_vector_free(tau);

  return s;
}

static int
test_symmtd_decomp(gsl_rng * r)
{
  int s = 0;
  size_t N;

  for (N = 2; N <= 50; ++N)
    {
      gsl_matrix * A = gsl_matrix_alloc(N, N);

      create_symm_matrix(A, r);
      s += test_symmtd_decomp_eps(A, 1.0e5 * N * GSL_DBL_EPSILON, "symmtd_decomp random");

      gsl_matrix_free(A);
    }

  return s;
}

static int
test_hermtd_decomp_eps(const gsl_matrix_complex * m, const double eps, const char * desc)
{
  int s = 0;
  const size_t N = m->size1;
  size_t i, j;

  gsl_matrix_complex * Q = gsl_matrix_complex_alloc(N, N);
  gsl_matrix_complex * T = gsl_matrix_complex_calloc(N, N);
  gsl_matrix_complex * A  = gsl_matrix_complex_alloc(N, N);
  gsl_matrix_complex * B  = gsl_matrix_complex_alloc(N, N);
  gsl_vector_complex * tau = gsl_vector_complex_alloc(N - 1);
  gsl_vector_view diag, subdiag, superdiag;
  gsl_vector_complex_view v;

  v = gsl_matrix_complex_diagonal(T);
  diag = gsl_vector_complex_real(&v.vector);

  v = gsl_matrix_complex_subdiagonal(T, 1);
  subdiag = gsl_vector_complex_real(&v.vector);

  v = gsl_matrix_complex_superdiagonal(T, 1);
  superdiag = gsl_vector_complex_real(&v.vector);

  gsl_matrix_complex_memcpy(A, m);
  s += gsl_linalg_hermtd_decomp(A, tau);

  s += gsl_linalg_hermtd_unpack(A, tau, Q, &diag.vector, &subdiag.vector);
  gsl_vector_memcpy(&superdiag.vector, &subdiag.vector);

  gsl_blas_zgemm(CblasNoTrans, CblasNoTrans, GSL_COMPLEX_ONE, Q, T, GSL_COMPLEX_ZERO, A);    /* A := Q T */
  gsl_blas_zgemm(CblasNoTrans, CblasConjTrans, GSL_COMPLEX_ONE, A, Q, GSL_COMPLEX_ZERO, B);  /* B := Q T Q^T */

  for (i = 0; i < N; i++)
    {
      for (j = 0; j <= i; j++)
        {
          gsl_complex bij = gsl_matrix_complex_get(B, i, j);
          gsl_complex mij = gsl_matrix_complex_get(m, i, j);

          gsl_test_rel(GSL_REAL(bij), GSL_REAL(mij), eps, "%s real (%3lu)[%lu,%lu]: %22.18g   %22.18g\n",
                       desc, N, i, j, GSL_REAL(bij), GSL_REAL(mij));
          gsl_test_rel(GSL_IMAG(bij), GSL_IMAG(mij), eps, "%s imag (%3lu)[%lu,%lu]: %22.18g   %22.18g\n",
                       desc, N, i, j, GSL_IMAG(bij), GSL_IMAG(mij));
        }
    }

  gsl_matrix_complex_free(T);
  gsl_matrix_complex_free(A);
  gsl_matrix_complex_free(Q);
  gsl_matrix_complex_free(B);
  gsl_vector_complex_free(tau);

  return s;
}

static int
test_hermtd_decomp(gsl_rng * r)
{
  int s = 0;
  size_t N;

  for (N = 2; N <= 50; ++N)
    {
      gsl_matrix_complex * A = gsl_matrix_complex_alloc(N, N);

      create_herm_matrix(A, r);
      s += test_hermtd_decomp_eps(A, 1.0e5 * N * GSL_DBL_EPSILON, "hermtd_decomp random");

      gsl_matrix_complex_free(A);
    }

  return s;
}


#include <gsl/gsl_matrix.h>
#include "BandEnergy.h"

int main(int argc, char *argv[]) {
    HTightBinding *Hrs = ExtractHTightBinding("test_data/Fe_soc/Fe_soc_hr.dat");

    gsl_matrix *R = gsl_matrix_calloc(3, 3); // R -> all zeros
    // Overall scale for R doesn't matter.
    gsl_matrix_set(R, 0, 1, 1.0);
    gsl_matrix_set(R, 0, 2, 1.0);
    gsl_matrix_set(R, 1, 0, 1.0);
    gsl_matrix_set(R, 1, 2, 1.0);
    gsl_matrix_set(R, 2, 0, 1.0);
    gsl_matrix_set(R, 2, 1, 1.0);
    double num_electrons = 8.0;
    int na = 8;
    int nb = 8;
    int nc = 8;
    bool use_cache = true;
    double E_Fermi = 0.0;

    double energy = BandEnergy(&E_Fermi, Hrs, R, num_electrons, na, nb, nc, use_cache);

    printf("energy = %f\n", energy);
    printf("E_Fermi = %f\n", E_Fermi);

    return 0;
}


#ifndef TOY_VN_GENERATOR_H
#define TOY_VN_GENERATOR_H

#include <iostream>
#include <fstream>
#include <string>
#include <cmath>
#include <complex>
#include <vector>
#include <iomanip>
#include <stdio.h>
#include <algorithm>
#include <chrono>
#include <random>
#include <omp.h>
#include "sampled_distribution.h"
#include <gsl/gsl_math.h>
#include <gsl/gsl_eigen.h>

using namespace std;

namespace flow_generator
{
	// ------------------------------------
	// Initialize default parameters here.
	// ------------------------------------

	int n_pT                               = 6;
	double max_pT                          = 3000000.0;
	double min_pT				           = 0.0;
	int order                              = 2;
	//double eta_low                         = 2.0;
	//double eta_high                        = 10.0;

	int N_events_to_generate               = 1000;
	int N_particles_per_event              = 100;
	long N_total_events                    = 0;
	long N_running_total_events            = 0;

	bool use_seed                          = true;
	int seed_index                         = -1;

	bool print_randomly_generated_data     = false;

	bool include_psi2_PTslope_fluctuations = false;
	double v2_magnitude                    = 0.1;

	string resultsDirectory                = "./results/";
	string output_name                     = resultsDirectory + "/data.dat";

	// ------------------------------------
	// Define structs and vectors.
	// ------------------------------------

	// struct for event information
	typedef struct
	{
		int eventID;
		int Nparticles;

		vector<vector<double> > eta_vec;
		vector<vector<double> > pphi_vec;
		vector<vector<double> > pT_vec;
	} event_data;

	// declare needed vectors
	vector<event_data>                         all_events;

	vector<vector<double> >                    pT_vec;
	vector<vector<double> >                    eta_vec;
	vector<vector<double> >                    phi_vec;
	vector<vector<int> >                       event_vec;

	//---------------------------------------------------------
	// Generate data randomly to mimic (non-factorizable) flow.
	void generate_random_data(int dataset=-1)
	{
		double delta_pT = (max_pT-min_pT)/(n_pT);

		pT_vec.clear();
		eta_vec.clear();
		phi_vec.clear();
		event_vec.clear();
		pT_vec = vector<vector<double> >(n_pT, vector<double> (0));
		eta_vec = vector<vector<double> >(n_pT, vector<double> (0));
		phi_vec = vector<vector<double> >(n_pT, vector<double> (0));
		event_vec = vector<vector<int> >(n_pT, vector<int> (0));

		// Set up distributions
		unsigned seed = chrono::system_clock::now().time_since_epoch().count();
		default_random_engine generator;
		if ( use_seed )
			generator = default_random_engine(seed);
		else
			generator = default_random_engine(seed_index);

		uniform_real_distribution<double> mean_pT_distribution(0.90, 1.10);
		//const double mean_pT = mean_pT_distribution(generator);
		const double mean_pT = 1.0;
		exponential_distribution<double> pT_distribution( mean_pT );
		normal_distribution<double> eta_distribution(0.0, 2.0);
		uniform_real_distribution<double> v2_distribution(0.05, 0.45);
		uniform_real_distribution<double> psi2_distribution(0.0, 2.0*M_PI);
		uniform_real_distribution<double> pphi_distribution(0.0, 2.0*M_PI);
		normal_distribution<double> psi2_pTslope_distribution(0.0,0.1);

		auto phiFunc = [](double x, double v2) { return x + v2*sin(2.0*x); };
	    std::mt19937 gen;
		if ( use_seed )
			gen = std::mt19937(seed);
		else
			gen = std::mt19937(seed_index);

		double fluctuation_switch_factor
				= double(include_psi2_PTslope_fluctuations);

		for (int iEvent = 0; iEvent < N_events_to_generate; iEvent++)
		{

			//double v2 = v2_distribution(generator);
			double v2 = v2_magnitude;
			double psi2 = 0.0*psi2_distribution(generator);
			double psi2_pTslope = fluctuation_switch_factor
									* psi2_pTslope_distribution(generator);

			for (int iParticle = 0; iParticle < N_particles_per_event; iParticle++)
			{
				// ---------------------------------------------
				// Try to mimic different types of flow effects
				// ---------------------------------------------
				const double pT_value = pT_distribution(generator);
			    //Sampled_distribution<> dist(phiFunc, 0.0, 2.0*M_PI, v2*pT_value / (mean_pT+pT_value));
			    Sampled_distribution<> dist(phiFunc, 0.0, 2.0*M_PI, v2);
				const double pphi_value = dist(gen) + psi2 + psi2_pTslope*pT_value;
				//const double pphi_value = dist(gen) + psi2 + 0.5*M_PI*pT_value / (mean_pT+pT_value);

				// Sort into bins.
				double pT = pT_value;
				double eta = 0.0*eta_distribution(generator);
				double phi = pphi_value;
				int event = iEvent;

				if(pT > max_pT)
				{
					//cout << "Warning: pT = " << pT << " larger than max_pT = " << max_pT << " in event = " << event << endl;
					continue;
				}
				if(pT < min_pT)
				{
					//cout << "Warning: pT = " << pT << " smaller than min_pT = " << min_pT << " in event = " << event << endl;
					continue;
				}
				else
				{
					int bin = int((pT-min_pT)/delta_pT);
					eta_vec[bin].push_back(eta);
					phi_vec[bin].push_back(phi);
					pT_vec[bin].push_back(pT);
					event_vec[bin].push_back(event);
					//cout << "Check: " << pT << "   " << eta << "   " << phi << "   " << event << " " << bin << endl;
				}
			}
		}

		//----------------------------------------------
		all_events.clear();
		all_events = vector<event_data>(N_events_to_generate);
		for ( auto & this_event : all_events )
		{
			this_event.eta_vec = vector<vector<double> >( n_pT, vector<double>(0) );
			this_event.pphi_vec = vector<vector<double> >( n_pT, vector<double>(0) );
			this_event.pT_vec = vector<vector<double> >( n_pT, vector<double>(0) );
		}
		for (int bin = 0; bin < n_pT; bin++)
		{
			const int n_particles_in_this_bin = event_vec[bin].size();
			for (int iParticle = 0; iParticle < n_particles_in_this_bin; iParticle++)
			{
				int event_ID_of_this_particle = event_vec[bin][iParticle];

				all_events[event_ID_of_this_particle].eta_vec[bin].push_back( eta_vec[bin][iParticle] );
				all_events[event_ID_of_this_particle].pphi_vec[bin].push_back( phi_vec[bin][iParticle] );
				all_events[event_ID_of_this_particle].pT_vec[bin].push_back( pT_vec[bin][iParticle] );
				all_events[event_ID_of_this_particle].eventID = event_ID_of_this_particle;
			}
		}
		//----------------------------------------------

		//----------------------------------------------
		// Tally number of completed events
		N_running_total_events += N_events_to_generate;
		cout << "Generated " << N_particles_per_event * N_events_to_generate
				<< " particles belonging to "
				<< N_events_to_generate << " events."
				<< endl;
		cout << "Currently analyzed " << N_running_total_events
				<< " / " << N_total_events << " events." << endl;

		//----------------------------------------------
		// Dump simulated data to file if desired
		if ( print_randomly_generated_data )
		{
			int n_zero = static_cast<int>( log10( N_particles_per_event*N_total_events ) )-1;
			string dataset_stem = ( dataset >= 0 ) ?
									std::string(n_zero - std::to_string(dataset).length(), '0')
									+ std::to_string( dataset ) : "";
			string RNG_filename = resultsDirectory + "/event_" + dataset_stem + ".dat";
			ofstream RNG_output( RNG_filename.c_str() );
			for ( auto & this_event : all_events )
			for (int bin = 0; bin < n_pT; bin++)
			{
				const int n_particles_in_this_bin = this_event.eta_vec[bin].size();
				const auto & this_pT_bin = this_event.pT_vec[bin];
				const auto & this_pphi_bin = this_event.pphi_vec[bin];
				const auto & this_eta_bin = this_event.eta_vec[bin];
				for (int iParticle = 0; iParticle < n_particles_in_this_bin; iParticle++)
					RNG_output << setprecision(15) << setw(18)
							   /*<< this_pT_bin[iParticle] << "   "
                               << this_eta_bin[iParticle] << "   "*/
                               << this_pphi_bin[iParticle] /*<< "   "
                               << this_event.eventID*/ << endl;
			}
			RNG_output.close();
		}

		return;
	}

}

#endif


/*
 * SPDX-License-Identifier:  BSD-3-Clause
 *
 * example_02-StructOfArrays-Naive-Omp.c :
 *      Example of SPH Density Calculation using a
 *      naive implementation of the main density loop,
 *      no neighbours earch, and Struct of Arrays (SoA)
 *      data layout and OpenMP parallelization.
 *
 * (C) Copyright 2021 José Hugo Elsas
 * Author: José Hugo Elsas <jhelsas@gmail.com>
 *
 * Command Line Options:
 *   -runs  <int>   : Set the number of repetitions (runs) for
 *                      calculating the density. The value of
 *                      the density is based on the last
 *                      iteration.
 *                    Default value: 1
 *   -run_seed <int>: Flag to set an alternative seed use for
 *                      for the PRNG. Instead of feeding seed
 *                      to the PRNG directly, it feeds
 *                      seed + iteration, as to generate different
 *                      configurations for each iteration.
 *                    Default value: 0 - (possible 0/1)
 *   -seed     <int>: Set the seed to use for the SPH particles
 *                      uniform position generation in the box
 *                    Default value: 123123123
 *
 *   -N        <int>: Set the number of SPH particles to be used
 *                    Default value: 1e5 = 100,000
 *   -h      <float>: Set the value of the smoothing kernel
 *                      parameter h, which corresponds to half
 *                      of the support of the kernel.
 *                    Default value: 0.05
 *
 *   -Nx       <int>: Set the number of Cells in the X direction
 *                    Default value: 10
 *   -Ny       <int>: Set the number of Cells in the Y direction
 *                    Default value: 10
 *   -Nz       <int>: Set the number of Cells in the Z direction
 *                    Default value: 10
 *
 *   -Xmin   <float>: Set the lower bound in the X direction for
 *                      the Cell Linked List box
 *                    Default value: 0.0
 *   -Ymin   <float>: Set the lower bound in the Y direction for
 *                    the Cell Linked List box
 *                      Default value: 0.0
 *   -Ymin   <float>: Set the lower bound in the Z direction for
 *                      the Cell Linked List box
 *                    Default value: 0.0
 *
 *   -Xmax   <float>: Set the lower bound in the X direction for
 *                      the Cell Linked List box
 *                    Default value: 1.0
 *   -Ymax   <float>: Set the lower bound in the Y direction for
 *                      the Cell Linked List box
 *                    Default value: 1.0
 *   -Zmax   <float>: Set the lower bound in the Z direction for
 *                      the Cell Linked List box
 *                    Default value: 1.0
 */

#include <math.h>
#include <ctype.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <limits.h>
#include <unistd.h>
#include <stdbool.h>
#include <sys/time.h>
#include <inttypes.h>

#include <omp.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_heapsort.h>

#include "sph_data_types.h"
#include "sph_linked_list.h"
#include "sph_utils.h"

#ifndef M_PI
#define M_PI (3.14159265358979323846)
#endif

#define COMPUTE_BLOCKS 1

int main_loop(int run, bool run_seed, int64_t N, double h, long int seed,
              void *swap_arr, linkedListBox *box, SPHparticle *lsph, double *times);

int compute_density_3d_naive_omp(int N,double h,
                                 double* restrict x, double* restrict y,
                                 double* restrict z, double* restrict nu,
                                 double* restrict rho);

double w_bspline_3d(double r,double h);

int main(int argc, char **argv){
  bool run_seed = false;       // By default the behavior is is to use the same seed
  int runs = 1,err;            // it only runs once
  long int seed = 123123123;   // The default seed is 123123123
  int64_t N = 100000;          // The default number of particles is N = 1e5 = 100,000
  double h=0.05;               // The default kernel smoothing length is h = 0.05
  linkedListBox *box;          // Uninitialized Box containing the cells for the cell linked list method
  SPHparticle *lsph;           // Uninitialized array of SPH particles

  box = (linkedListBox*)malloc(1*sizeof(linkedListBox)); // Create a box

  // allow for command line customization of the run
  arg_parse(argc,argv,&N,&h,&seed,&runs,&run_seed,box);  // Parse the command
                                                         // line arguments and override default values

  err = SPHparticle_SoA_malloc(N,&lsph);                 // Create an array of N particles
  if(err)
    fprintf(stderr,"error in SPHparticle_SoA_malloc\n");

  void *swap_arr = malloc(N*sizeof(double));
  double times[runs*COMPUTE_BLOCKS];

  for(int run=0;run<runs;run+=1)
    main_loop(run,run_seed,N,h,seed,swap_arr,box,lsph,times);

  bool is_cll = false;
  const char *prefix = "ex02,naive,SoA,omp";
  print_time_stats(prefix,is_cll,N,h,seed,runs,lsph,box,times);
  print_sph_particles_density(prefix,is_cll,N,h,seed,runs,lsph,box);

  SPHparticleSOA_safe_free(N,&lsph);
  safe_free_box(box);
  free(swap_arr);

  return 0;
}

/*
 *  Function main_loop:
 *    Runs the main loop of the program, including the particle array generation,
 *    density calculation and the timings annotations.
 *
 *    Arguments:
 *       run <int>            : index (or value) or the present iteration
 *       run_seed <bool>      : boolean defining whether to use run index for seed or not
 *       N <int>              : Number of SPH particles to be used in the run
 *       h <double>           : Smoothing Length for the Smoothing Kernel w_bspline
 *       seed <long int>      : seed for GSL PRNG generator to generate particle positions
 *       box  <linkedListBox> : Box of linked list cells, encapsulating the 3d domain
 *       lsph <SPHparticle>   : Array (pointer) of SPH particles to be updated
 *       times <double>       : Array to store the computation timings to be updated
 *    Returns:
 *       0                    : error code returned
 *       lsph <SPHparticle>   : SPH particle array is updated in the rho field by reference
 *       times <double>       : Times is updated by reference
 */
int main_loop(int run, bool run_seed, int64_t N, double h, long int seed,
              void *swap_arr, linkedListBox *box, SPHparticle *lsph, double *times)
{
  int err;

  // Initialize the particles' positions and other values
  if(run_seed)
    err = gen_unif_rdn_pos_box(N,seed+run,box,lsph);
  else
    err = gen_unif_rdn_pos_box(N,seed,box,lsph);

  if(err)
    fprintf(stderr,"error in gen_unif_rdn_pos\n");

  // ------------------------------------------------------ //

  double t0,t1;

  t0 = omp_get_wtime();

  compute_density_3d_naive_omp(N,h,lsph->x,lsph->y,lsph->z,
                               lsph->nu,lsph->rho);         // Compute the density for all particles

  t1 = omp_get_wtime();

  // ------------------------------------------------------ //

  times[COMPUTE_BLOCKS*run+0] = t1-t0;                      // Only one component to measure time

  return 0;
}

/*
 *  Function compute_density_3d_naive_omp:
 *    Computes the SPH density from the particles naively (i.e. direct loop).
 *    The outer-most loop is parallelized using openMP.
 *
 *    Arguments:
 *       N <int>              : Number of SPH particles to be used in the run
 *       h <double>           : Smoothing Length for the Smoothing Kernel w_bspline
 *       x       <double*>    : Array of particles' X positions
 *       y       <double*>    : Array of particles' Y positions
 *       z       <double*>    : Array of particles' Z positions
 *       nu      <double*>    : Array of particles' density weights (i.e. masses)
 *    Returns:
 *       0                    : error code returned
 *       rho       <double*>  : Array of particles' densities
 */
int compute_density_3d_naive_omp(int N,double h,
                                 double* restrict x, double* restrict y,
                                 double* restrict z,double* restrict nu,
                                 double* restrict rho){
  memset(rho,(int)0,N*sizeof(double));        // Pre-initialize the density to zero

  #pragma omp parallel for                    // Iterate in parallel
  for(int64_t ii=0;ii<N;ii+=1){               // For every particle
    for(int64_t jj=0;jj<N;jj+=1){             // Run over every other particle
      double dist = 0.;                       // the contributions from each direction by

      dist += (x[ii]-x[jj])*(x[ii]-x[jj]);    // adding the contribution in the X direction
      dist += (y[ii]-y[jj])*(y[ii]-y[jj]);    // adding the contribution in the Y direction
      dist += (z[ii]-z[jj])*(z[ii]-z[jj]);    // adding the contribution in the Z direction

      dist = sqrt(dist);                      // take the sqrt to have the distance

      rho[ii] += nu[jj]*w_bspline_3d(dist,h); // and add the contribution to the density
    }
  }

  return 0;
}

/*
 *  Function w_bspline_3d:
 *    Returns the normalized value of the cubic b-spline SPH smoothing kernel
 *
 *    Arguments:
 *       q <double>           : Distance between particles
 *       h <double>           : Smoothing Length for the Smoothing Kernel w_bspline
 *    Returns:
 *       wq <double>          : Normalized value of the kernel
 */
double w_bspline_3d(double r,double h){
  const double A_d = 3./(2.*M_PI*h*h*h);      // The 3d normalization constant
  double q=0.;                                // normalized distance, initialized to zero

  if(r<0||h<=0.)                              // If either distance or smoothing length
    exit(10);                                 // are negative, declare an emergency

  q = r/h;                                    // Compute the normalized distance
  if(q<=1)                                    // If the distance is small
    return A_d*(2./3.-q*q + q*q*q/2.0);       // Compute this first polynomal
  else if((1.<=q)&&(q<2.))                    // If the distance is a bit larger
    return A_d*(1./6.)*(2.-q)*(2.-q)*(2.-q);  // Compute this other polynomial
  else                                        // Otherwise, if the distance is large
    return 0.;                                // The value of the kernel is 0
}


/*
 *  hubble.c
 *
 *  written by christian wagner
 *
 */

#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_odeiv.h>
#include <gsl/gsl_errno.h>

#define VC 299792.458
#define TWOPI 6.283185307179586
#define PI    3.141592653589793
#define Tcmb  2.725
#define pow2(x) ((x)*(x))
#define pow3(x) ((x)*(x)*(x))
#define pow4(x) ((x)*(x)*(x)*(x))

#include "hubble.h"

double hubble(struct cosmo mycosmo, double a){
  double H0=mycosmo.hub*100.;
  double Omega_m = mycosmo.wm/pow2(mycosmo.hub);
  double Omega_r  = 4.15e-5/pow2(mycosmo.hub);
  double Omega_X  = 1.-Omega_m;
  double Omega_k = 0.;

  return  H0*sqrt(Omega_r/pow4(a)+Omega_m/pow3(a)+Omega_k/pow2(a)+Omega_X*pow(a,-3*(1+mycosmo.w0+mycosmo.wa))*exp(-3.*mycosmo.wa*(1.-a)));

}

double co_distance_int(double a,void * params){
    double t;
    struct cosmo mycosmo = *(struct cosmo *) params;

    return  1./(a*a*hubble(mycosmo,a));

}

double co_distance(struct cosmo mycosmo, double a){

  gsl_function  F;
   double result;
   double epsabs=0.001;
   double epsrel=0.001;
   double abserr;
   size_t neval;

   F.function = &co_distance_int;
   F.params = &mycosmo;

   gsl_integration_qng (&F, a, 1., epsabs, epsrel, &result, &abserr, &neval);

   return result*VC*mycosmo.hub;

}

double ang_dist(struct cosmo mycosmo, double a){
  return co_distance(mycosmo,a); // assuming flatness
}

int
ode_growth (double a, const double y[], double f[], void *params)
{

  struct cosmo mycosmo = * (struct cosmo *) params;

  double w_a=mycosmo.w0+(1-a)*mycosmo.wa;
  double Omega_m=mycosmo.wm/pow2(mycosmo.hub);
  double Omega_X=1-Omega_m;
  double Xvar=(1-Omega_m)*pow(a,-3.*(1+mycosmo.w0+mycosmo.wa))*exp(-3.*mycosmo.wa*(1.-a))/(Omega_m/(a*a*a)+(1-Omega_m)*pow(a,-3.*(1+mycosmo.w0+mycosmo.wa))*exp(-3.*mycosmo.wa*(1.-a)));

    f[0] = y[1];
    f[1] = -1.5*(1.-w_a)*Xvar*y[0]/(a*a) - (3.5-1.5*w_a*Xvar)*y[1]/a ;

    return GSL_SUCCESS;
}

int
jac_growth (double a, const double y[], double *dfdy,
     double dfdt[], void *params)
{
    return GSL_SUCCESS;
}

double growth(struct cosmo mycosmo, double a){

    const gsl_odeiv_step_type * T
	= gsl_odeiv_step_rkf45;

    gsl_odeiv_step * s
	= gsl_odeiv_step_alloc (T, 2);
    gsl_odeiv_control * c
	= gsl_odeiv_control_y_new (1e-12, 0.0);
    gsl_odeiv_evolve * e
	= gsl_odeiv_evolve_alloc (2);

    double mu = 10;
    gsl_odeiv_system sys = {ode_growth, jac_growth, 2, &mycosmo};

    double  t1=a;
    double  t = 1.E-12;

    double d1;

    double h = 1e-12;
    double y[2] = { 1.0, 0.0 };

    int status;

    while (t < t1){
      status = gsl_odeiv_evolve_apply (e, c, s,
					   &sys,
					   &t, t1,
					   &h, y);
      if  (h>0.001)
	h=0.001;

      if (status != GSL_SUCCESS)
	break;

    }

    d1=y[0]*(t);

    gsl_odeiv_evolve_free (e);
    gsl_odeiv_control_free (c);
    gsl_odeiv_step_free (s);

    return d1;
}

double z_lastscattering(struct cosmo mycosmo){
  /*
  z_lastscattering(cosmo):
  Returns z_LS from Hu & White, DampingTail paper.
  */
  double wm = mycosmo.wm;
  double wb = mycosmo.wb;
  double b1 = 0.0783*pow(wb,(-0.238))/(1+39.5*pow(wb,0.763));
  double b2 = 0.560/(1+21.1*pow(wb,1.81));
  double zls= 1048.*(1+0.00124*pow(wb,(-0.738)))*(1+b1*pow(wm,b2));
  return zls;
}

double soundhorizon(struct cosmo mycosmo){
  /*
  soundhorizon(cosmo):
  A fit to the sound horizon, in Mpc, from Hu & Sugiyama (1995), Eq. B6
  */
  double wm = mycosmo.wm;
  double wb = mycosmo.wb;
  double wg = 2.4888e-5*pow4(Tcmb/2.73);
  double r0 = 0.75*wb/wg;
  double zeq= 5464.0*(wm/0.135)/pow4(Tcmb/2.725)/(1+0.6851)-1.0;
  double req= r0/(1.+zeq);
  double zLS= z_lastscattering(mycosmo);
  double rls= r0/(1+zLS);
  double tmp= (sqrt(1.+rls)+sqrt(rls+req))/(1.+sqrt(req));
  tmp= 3997.0*sqrt(wg/wm/wb)*log(tmp);
  return(tmp);
}

double distls(struct cosmo mycosmo){
  /*
  distls(cosmo):
  Returns the distance to LS, in Mpc.
  */
  double zLS = z_lastscattering(mycosmo);
  double dLS = ang_dist(mycosmo,1./(1.+zLS))/mycosmo.hub;
  return(dLS);
}

double solvehh(double dLS,struct cosmo mycosmo){
  /*
  solvehh(dLS,cosmo):
  Solves for h given the other cosmological parameters and the distance
  to last scattering (in Mpc).
  */
  struct cosmo hmin = mycosmo;
  hmin.hub = 0.3;
  // printf("min_cosmo: %g %g %g %g %g\n",hmin.hub,hmin.w0,hmin.wa,hmin.wm,hmin.wb);
  double ymin = distls(hmin)-dLS;
  //  printf("ymin: %g\n",ymin);
  struct cosmo hmax = mycosmo;
  hmax.hub = 1.0;
  double ymax = distls(hmax)-dLS;
  //  printf("ymax: %g\n",ymax);
  double ymid;
  struct cosmo hmid = mycosmo;
  while (fabs(hmax.hub-hmin.hub)>1e-3){
    hmid.hub = (hmax.hub+hmin.hub)/2.0;
    ymid = distls(hmid)-dLS;
    if (ymin*ymid<0){
	hmax = hmid;
	ymax = ymid;
      } else {
      hmin = hmid;
      ymin = ymid;
    }
  }
  //  printf("hubble from CMB: %g\n",hmid.hub);
  return hmid.hub;
}

//void getH0fromCMB(double *xstar, double myh, double *rs, double *z_lss, double *d_lss, double *h_CMB, int *writeout, char *fname){
void getH0fromCMB(double *xstar, double *stuff){

  FILE *fp;

  struct cosmo mycosmo={ 0.72, xstar[3], 0, xstar[1], xstar[0]};

  double rs=soundhorizon(mycosmo);
  stuff[0] = rs;

  double z_lss=z_lastscattering(mycosmo);
  stuff[1] = z_lss;

  double d_lss=302.4*rs/PI;
  stuff[2] = d_lss;

  double hubble_cmb= solvehh(d_lss,mycosmo);
  stuff[3] = hubble_cmb;

}

// Linker function for use with Fortran

void geth0fromcmb_(double *xstar, double *stuff){
  void getH0fromCMB();
  getH0fromCMB(xstar,stuff);
}


/*******************************************************************
FUNCTION NAME:   interpolate - performs the interpolation to calculate
                 the pixel value e.g. during resampling

INPUT: interpolation  - interpolation method (nearest neighbor, bilinear, sinc)
       inbuf          - input image buffer
       nLines         - number of lines (image buffer)
       nSamples       - number of samples per line (image buffer)
       xLine          - line of interest
       xSample        - sample position within line
       weighting      - weighting function to be applied (Kaiser, Hamming)
       nKernel        - kernel size for sinc function
*******************************************************************/
#include <assert.h>
#include <math.h>

#include <glib.h>
#include <gsl/gsl_sf_bessel.h>
#include "asf.h"
#include "asf_raster.h"
#include "float_image.h"

#define SQR(X) ((X)*(X))

// Some parameters and weights for bicubic interpolation
#define CUBE(X) ((X)*(X)*(X))
#define P(X) ( X > 0  ? X : 0 )

// For sinc interpolation, the goal is to weight source values using a
// sinc function.  We want to position the peak of the sinc at the
// point of interest.  The sinc filter itself will interpolate a small
// number of point, perhaps 8 or 16, for example.  In order to achieve
// the desired location of the peak, we want to introduce a shift in
// the sinc function computed.  However, we don't want to recompute
// the sinc function for every point to be interpolated!  The solution
// is to precompute an array of slightly shifted sinc functions, and
// then choose the appropriate one for the point we are interpolating.
// For example, if we have known values for 1 and 2, and we want to
// interpolate a value for 1.2, we choose the (1.2 - 1.0) * (2 - 1) *
// NUM_SINCS sinc function to evaluate the result.
#define NUM_SINCS 512 // Number of sinc functions to compute.

// For convenience using buffered image functions
#define GET_PIXEL(x, y) float_image_get_pixel (inbuf, x, y)
#define SET_PIXEL(x, y, value) float_image_set_pixel (inbuf, x, y, value)

// Fetch pixel ii, jj from inbuf, where inbuf is taken to be an image
// nSamples wide and nLines high.  If ii, jj is outside the image, a
// "reflected" value is returned.  For example, these two calls will
// return the same pixel:
//
//     get_pixel_with_reflection (inbuf, sample_count, line_count, -1, -2)
//     get_pixel_with_reflection (inbuf, sample_count, line_count, 1, 2);
//
// If the reflected indicies are still outside the image, an exception
// is triggered.

//static float
//get_pixel_with_reflection (float *inbuf, int nSamples, int nLines, int sample,
//			   int line)
//{
//  int ii = sample;		// Convenience alias.
//  int jj = line;		// Convenience alias.
//  // Handle reflection at image edges.
//  if ( G_UNLIKELY (ii < 0) ) {
//    ii = -ii;
//  }
//  else if ( G_UNLIKELY (ii >= nSamples) ) {
//    ii = ii - (ii - nSamples + 1) - 1;
//  }
//  if ( G_UNLIKELY (jj < 0) ) {
//    jj = -jj;
//  }
//  else if ( G_UNLIKELY (jj >= nLines) ) {
//    jj = jj - (jj - nLines + 1) - 1;
//  }
//  // Now we better be in the image.
//  assert (ii >= 0 && ii < nSamples);
//  assert (jj >= 0 && jj < nLines);
//
//  return inbuf[nSamples * jj + ii];
//}

void samples2coefficients(FloatImage *inbuf, char dimension)
{
  int ii, kk;
  float z = sqrt(3.0) - 2.0;
  float lambda = (1.0 - z) * (1.0 - 1.0/z);
  float value;

  // Apply overall gain
  for (ii=0; ii<inbuf->size_x; ii++)
    for (kk=0; kk<inbuf->size_y; kk++) {
      value = GET_PIXEL(ii,kk) * lambda;
      SET_PIXEL(ii,kk,value);
    }

  for (kk=0; kk<inbuf->size_y; kk++) {
    // Causal initialization (first pixel of line)

  }

  // Causal recursion

  // Anticausal initialization

  // Anticausal recursion

}

float interpolate(interpolate_type_t interpolation, FloatImage *inbuf, float yLine,
		  float xSample, weighting_type_t weighting, int sinc_points)
{
  int ix, iy, ii, kk, ll, xI[4], yI[4];
  float a00, a10, a01, a11;
  float value, dx, dy, wx[4], wy[4], w;

  /* Get closed pixel position to start from */

  switch ( interpolation )
    {
    case NEAREST:
      ix = (int) (xSample + 0.5);
      iy = (int) (yLine + 0.5);
      value = GET_PIXEL(ix,iy);
      break;

    case BILINEAR:
      assert (xSample >= 0.0);
      assert (yLine >= 0.0);
      assert (xSample <= inbuf->size_x - 1);
      assert (yLine <= inbuf->size_y - 1);
      ix = floor(xSample);
      iy = floor(yLine);

      a00 = GET_PIXEL(ix,iy);
      a10 = GET_PIXEL(ix+1,iy) - GET_PIXEL(ix,iy);
      a01 = GET_PIXEL(ix,iy+1) - GET_PIXEL(ix,iy);
      a11 = GET_PIXEL(ix,iy) - GET_PIXEL(ix+1,iy) - GET_PIXEL(ix,iy+1)
	+ GET_PIXEL(ix+1,iy+1);
      value = (a00 + a10 * (xSample - ix) + a01 * (yLine - iy)
	       + a11 * (xSample - ix) * (yLine - iy));
      break;

    case BICUBIC:
      assert (xSample >= 0.0);
      assert (yLine >= 0.0);
      assert (xSample <= inbuf->size_x - 1);
      assert (yLine <= inbuf->size_y - 1);
      ix = floor(xSample);
      iy = floor(yLine);
      dx = xSample - ix;
      dy = yLine - iy;

      // Calculating weights
      for (ii=-1; ii<=2; ii++) {
	wx[ii+1] = 1.0/6.0 * (CUBE(P(ii-dx+2)) - 4*CUBE(P(ii-dx+1))
			      + 6*CUBE(P(ii-dx)) - 4*CUBE(P(ii-dx-1)));
	wy[ii+1] = 1.0/6.0 * (CUBE(P(dy-ii+2)) - 4*CUBE(P(dy-ii+1))
			      + 6*CUBE(P(dy-ii)) - 4*CUBE(P(dy-ii-1)));
      }

      // Get the interpolated pixel value
      value = 0.0;
      for (ii=-1; ii<=2; ii++)
	for (kk=-1; kk<=2; kk++)
	  value += GET_PIXEL(ix+ii,iy+kk) * wx[ii+1] * wy[kk+1];

      break;

    case SPLINES:
      // Determine B-Spline coefficients.
      // This is a essentially a pre-filtering applied to the entire image
      // that converts the image from a sample representation into a
      // representation based on B-spline coefficients.
      // Without this step the result would look blurry at the end.

      // Convert samples to coefficients in x direction
      samples2coefficients(inbuf, 'x');

      // Convert samples to coefficients in y direction
      samples2coefficients(inbuf, 'y');

      // Calculate interpolation indices
      ii = (int)floor(xSample) - 1;
      kk = (int)floor(yLine) - 1;
      for (ll=0; ll<=3; ll++) {
	xI[ll] = ii++;
	yI[ll] = kk++;
      }

      // Calculate interpolation weights
      w = xSample - (float)xI[1];
      wx[3] = (1.0/6.0)*CUBE(xSample);
      wx[0] = (1.0/6.0) + (1.0/2.0)*w*(w-1.0) - wx[3];
      wx[2] = w + wx[0] - 2.0*wx[3];
      wx[1] = 1.0 - wx[0] - wx[2] - wx[3];
      w = yLine - (float)yI[1];
      wy[3] = (1.0/6.0)*CUBE(xSample);
      wy[0] = (1.0/6.0) + (1.0/2.0)*w*(w-1.0) - wy[3];
      wy[2] = w + wy[0] - 2.0*wy[3];
      wy[1] = 1.0 - wy[0] - wy[2] - wy[3];

      // Get interpolation value
      value = 0.0;
      for (kk=0; kk<=3; kk++) {
	w = 0.0;
	for (ii=0; ii<=3; ii++)
	  w += wx[ii] * 1;
	value += wy[kk] * w;
      }
      break;
    case SINC:
      break;
    default:
      assert (FALSE);
      break;
  }

  return value;
}


////////////////////////////////////////////////////////////
//
// Copyright (c) 2018 Jan Filipowicz, Filip Turobos
//
// 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.
//
////////////////////////////////////////////////////////////

#ifndef SALESMAN_EXAMPLE_PATH_EVALUATOR_H
#define SALESMAN_EXAMPLE_PATH_EVALUATOR_H

#include <functional>
#include <numeric>
#include <gsl/gsl_assert>
#include "permutation.h"

template<class Matrix>
class path_evaluator {
public:
	using matrix_type = Matrix;
	using row_type = typename matrix_type::value_type;
	using value_type = typename row_type::value_type;
	explicit path_evaluator(const matrix_type& matrix);
	value_type operator()(const permutation& perm) const;
private:
	matrix_type matrix;
};

template<class Matrix>
inline path_evaluator<Matrix>::path_evaluator(const matrix_type& matrix)
	: matrix(matrix) {}

template<class Matrix>
inline auto path_evaluator<Matrix>::operator()(const permutation& perm) const -> value_type {
	Expects(perm.size() == matrix.size());
	Expects(perm.size() > 0);
	const auto distance = [this](unsigned dest, unsigned src) {
		return gsl::at(gsl::at(matrix, src), dest);
	};
	return std::inner_product(std::next(perm.begin()), perm.end(), perm.begin(), distance(perm.front(), perm.back()), std::plus<>(), distance);
}

#endif


/* This module contains the routine rotCurveSpline, which takes a
   tabulated rotaiton curve and calculates a B-spline fit to it and
   its derivatives. */

#include <stdio.h>
#include <math.h>
#include <gsl/gsl_bspline.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_multifit.h>
#include "rotCurveSpline.h"

struct dpsidr_params {
  gsl_bspline_workspace *wksp;
#ifdef GSL1
  gsl_bspline_deriv_workspace *dwksp;
#endif
  gsl_vector *B, *coef;
  gsl_matrix *dB, *cov;
};

double dpsidr(double r, void *params) {
  double vphi, err;
  struct dpsidr_params *p = (struct dpsidr_params *) params;

#ifdef GSL1
  gsl_bspline_deriv_eval(log(r), 2, p->dB, p->wksp, p->dwksp);
#else
  gsl_bspline_deriv_eval(log(r), 2, p->dB, p->wksp);
#endif
  gsl_matrix_get_col(p->B, p->dB, 0);
  gsl_multifit_linear_est(p->B, p->coef, p->cov, &vphi, &err);
  return(-vphi*vphi/r);
}

void
rotCurveSpline(const double *rTab, const double *vTab,
	       const unsigned long nTab,
	       const unsigned long bspline_degree,
	       const unsigned long bspline_breakpoints,
	       const double *r, const unsigned long nr,
	       double *vphi, double *psi,
	       double *beta) {

  gsl_vector *xInput, *vInput, *coef, *B, *bkpt_vec;
  gsl_matrix *dB, *cov, *pred;
  gsl_bspline_workspace *wksp;
#ifdef GSL1
  gsl_bspline_deriv_workspace *dwksp;
#endif
  gsl_multifit_linear_workspace *mw;
  gsl_integration_workspace *intwksp;
  gsl_function dpsidr_func;
  struct dpsidr_params params;
  double x0, x1, v0, bkpt_sum_target, sum, x, Bij, chisq;
  double dvphidlogr, err, psiInt;
  unsigned long bspline_ncoef, i, j, ptr;

  /* Safety check: make sure we have enough data points for the choice
     of degree and breakpoints */
  bspline_ncoef = bspline_breakpoints + bspline_degree - 2;
  if (bspline_ncoef > nTab) {
    fprintf(stderr, "Error: %lu data points for %lu coefficients; ndata must be >= ncoef = nbreakpoints + degree - 2\n", nTab, bspline_ncoef);
    exit(1);
  }

  /* Initialize the GSL vectors to hold the data table */
  xInput = gsl_vector_alloc(nTab);
  vInput = gsl_vector_alloc(nTab);
  for (i=0; i<nTab; i++) {
    gsl_vector_set(xInput, i, log(rTab[i]));
    gsl_vector_set(vInput, i, vTab[i]);
  }

  /* Allocate workspace for and associated stuff for B-spline solver */
  wksp = gsl_bspline_alloc(bspline_degree, bspline_breakpoints);
#ifdef GSL1
  dwksp = gsl_bspline_deriv_alloc(bspline_degree);
#endif
  B = gsl_vector_alloc(bspline_ncoef);
  dB = gsl_matrix_alloc(bspline_ncoef, 3);

  /* Compute breakpoint locations using the method of Gans & Gill,
     Applied Spectroscopy, 38, 297, 1984. The formula for
     uniformly-spaced data is that two successive breakpoints t_i and t_i+1
     should be separated by an amount such that
     sum_{x=t_i}^{x=t_i+1} f(x)^0.5 dx = S_T
     where
     S_T = (1/(m+1)) sum f(x_i)^0.5 dx.
     Here the first sum runs over all data points between t_i and
     t_i+1, and the second runs all data points; m is the number of
     knots, and f(x) is the function to be approximated. This formula
     assumes uniform spacing dx, but here it is modified for
     non-uniform data spacing such that dx is taken to be the mean
     size of the data interval, averaging over the left and right
     sides.
  */
  v0 = gsl_vector_get(vInput, 0);
  x0 = gsl_vector_get(xInput, 0);
  x1 = gsl_vector_get(xInput, 1);
  bkpt_sum_target = sqrt(v0) * (x1-x0);
  for (i=1; i<nTab-1; i++) {
    v0 = gsl_vector_get(vInput, i);
    x0 = gsl_vector_get(xInput, i-1);
    x1 = gsl_vector_get(xInput, i+1);
    bkpt_sum_target += sqrt(v0) * 0.5 * (x1-x0);
  }
  v0 = gsl_vector_get(vInput, nTab-1);
  x0 = gsl_vector_get(xInput, nTab-2);
  x1 = gsl_vector_get(xInput, nTab-1);
  bkpt_sum_target += sqrt(v0) * (x1-x0);
  bkpt_sum_target /= (bspline_breakpoints + 1);
  bkpt_vec = gsl_vector_alloc(bspline_breakpoints);
  gsl_vector_set(bkpt_vec, 0, gsl_vector_get(xInput, 0));
  gsl_vector_set(bkpt_vec, bspline_breakpoints-1,
		 gsl_vector_get(xInput, nTab-1));
  ptr=0;
  for (i=1; i<bspline_breakpoints-2; i++) {
    sum = 0;
    while (sum < bkpt_sum_target) {
      if (ptr == 0) {
	v0 = gsl_vector_get(vInput, 0);
	x0 = gsl_vector_get(xInput, 0);
	x1 = gsl_vector_get(xInput, 1);
	sum += sqrt(v0) * (x1-x0);
      } else if (ptr == nTab-1) {
	v0 = gsl_vector_get(vInput, nTab-1);
	x0 = gsl_vector_get(xInput, nTab-2);
	x1 = gsl_vector_get(xInput, nTab-1);
	sum += sqrt(v0) * (x1-x0);
      } else {
	v0 = gsl_vector_get(vInput, ptr);
	x0 = gsl_vector_get(xInput, ptr-1);
	x1 = gsl_vector_get(xInput, ptr+1);
	sum += sqrt(v0) * 0.5 * (x1-x0);
      }
      ptr++;
      if (ptr == nTab) break;
    }
    if (ptr == nTab) break;
    gsl_vector_set(bkpt_vec, i, gsl_vector_get(xInput, ptr-1));
  }
  gsl_vector_set(bkpt_vec, i, gsl_vector_get(xInput, nTab-1));

  /* Construct knots vector */
  gsl_bspline_knots(bkpt_vec, wksp);

  /* Allocate workspace for the least squares fit to find the B spline
     coefficients; pred is the predictor variable matrix, cov is the
     covariance matrix, coef is the vector of best-fitting
     coefficients, and mw is the workspace used by the gsl fitter */
  pred = gsl_matrix_alloc(nTab, bspline_ncoef);
  mw = gsl_multifit_linear_alloc(nTab, bspline_ncoef);
  cov = gsl_matrix_alloc(bspline_ncoef, bspline_ncoef);
  coef = gsl_vector_alloc(bspline_ncoef);

  /* Load the predictor matrix with the B spline functions evaluated
     at the input positions */
  for (i=0; i<nTab; i++) {
    x = gsl_vector_get(xInput, i); /* Position of data point */
    gsl_bspline_eval(x, B, wksp); /* Evaluate B splines at this
				     position */
    /* Load predictor matrix */
    for (j=0; j<bspline_ncoef; j++) {
      Bij = gsl_vector_get(B, j);
      gsl_matrix_set(pred, i, j, Bij);
    }
  }

  /* Perform least squares fit to find coefficients */
  gsl_multifit_linear(pred, vInput, coef, cov, &chisq, mw);

  /* Compute B-spline fits at input positions */
  for (i=0; i<nr; i++) {
#ifdef GSL1
    gsl_bspline_deriv_eval(log(r[i]), 2, dB, wksp, dwksp);
#else
    gsl_bspline_deriv_eval(log(r[i]), 2, dB, wksp);
#endif
    gsl_matrix_get_col(B, dB, 0);
    gsl_multifit_linear_est(B, coef, cov, vphi+i, &err);
    gsl_matrix_get_col(B, dB, 1);
    gsl_multifit_linear_est(B, coef, cov, &dvphidlogr, &err);
    beta[i] = dvphidlogr / vphi[i];
  }

  /* Compute potential at cell centers and edges by numerically
     integrating from grid edge to center */
  intwksp = gsl_integration_workspace_alloc(4096);
  dpsidr_func.function = &dpsidr;
  dpsidr_func.params = &params;
  params.wksp = wksp;
#ifdef GSL1
  params.dwksp = dwksp;
#endif
  params.B = B;
  params.coef = coef;
  params.dB = dB;
  params.cov = cov;
  psi[nr-1] = 0.0;
  for (i=nr-1; i>0; i--) {
    gsl_integration_qag(&dpsidr_func, r[i], r[i-1],
			1e-6*vphi[i]*vphi[i]*
			(r[i+1]-r[i]),
			1e-6, 4096, 6, intwksp, &psiInt, &err);
    psi[i-1] = psi[i] - psiInt;
  }

  /* Free memory */
  gsl_matrix_free(pred);
  gsl_vector_free(bkpt_vec);
  gsl_vector_free(xInput);
  gsl_vector_free(vInput);
  gsl_multifit_linear_free(mw);
  gsl_vector_free(coef);
  gsl_vector_free(B);
  gsl_matrix_free(dB);
  gsl_matrix_free(cov);
  gsl_bspline_free(wksp);
#ifdef GSL1
  gsl_bspline_deriv_free(dwksp);
#endif
  gsl_integration_workspace_free(intwksp);
}


#include <stdio.h>
#include <math.h>
#include <gsl/gsl_integration.h>

void integral_recur ( int nmin, int nmax, double vals[]);

void integral_recur (int nmin, int nmax, double vals[])
{
    double IN = 0;
    int i = nmax - 1;
    vals [nmax] = IN;
    while ( i >= nmin)
    {
        vals[i] = (IN + exp (-1.))/((double) i);
        i = i - 1;
    }
}


/*****************************************************************\
           __
          / /
		 / /                     __  __
		/ /______    _______    / / / / ________   __       __
	   / ______  \  /_____  \  / / / / / _____  | / /      / /
	  / /      | / _______| / / / / / / /____/ / / /      / /
	 / /      / / / _____  / / / / / / _______/ / /      / /
	/ /      / / / /____/ / / / / / / |______  / |______/ /
   /_/      /_/ |________/ / / / /  \_______/  \_______  /
                          /_/ /_/                     / /
			                                         / /
		       High Level Game Framework            /_/

  ---------------------------------------------------------------

  Copyright (c) 2007-2011 - Rodrigo Braz Monteiro.
  This file is subject to the terms of halley_license.txt.

\*****************************************************************/

#pragma once

#include <cmath>
#include "angle.h"
#include "vector2.h"
#include "vector3.h"
#include <halley/utils/utils.h>
#include <gsl/gsl_assert>

namespace Halley {
	template <typename T=float, int Alignment=4>
	class alignas(Alignment * sizeof(T)) Vector4D {
	private:
		constexpr inline T mod(T a, T b) const { return a % b; }

	public:
		using ScalarType = T;
		T x, y, z, w;

		struct Uninitialized {};

		// Constructors
		constexpr inline Vector4D ()
			: x(0)
			, y(0)
			, z(0)
			, w(0)
		{}

		constexpr inline Vector4D(Uninitialized u) {}

		constexpr inline Vector4D (T x, T y, T z, T w)
			: x(x)
			, y(y)
			, z(z)
			, w(w)
		{}

		constexpr inline Vector4D(const Vector4D& other) = default;
		constexpr inline Vector4D(Vector4D&& other) noexcept = default;

		template <typename V, int A>
		constexpr inline explicit Vector4D (const Vector4D<V, A>& vec)
			: x(T(vec.x))
			, y(T(vec.y))
			, z(T(vec.z))
			, w(T(vec.w))
		{}

		template <typename V, int A>
		constexpr inline explicit Vector4D (Vector4D<V, A>&& vec)
			: x(T(vec.x))
			, y(T(vec.y))
			, z(T(vec.z))
			, w(T(vec.w))
		{}

		constexpr inline explicit Vector4D(const Vector2D<T>& v, T z = 0, T w = 0)
			: x(v.x)
			, y(v.y)
			, z(z)
			, w(w)
		{}

		constexpr inline explicit Vector4D(const Vector3D<T>& v, T w = 0)
			: x(v.x)
			, y(v.y)
			, z(v.z)
			, w(w)
		{}

		~Vector4D() = default;

		// Getter
		constexpr inline T& operator[](size_t n)
		{
			Expects(n <= 3);
			return (&x)[n];
		}

		constexpr inline const T& operator[](size_t n) const
		{
			Expects(n <= 3);
			return (&x)[n];
		}

		T* data()
		{
			return &x;
		}

		const T* data() const
		{
			return &x;
		}

		// Assignment and comparison
		constexpr inline Vector4D& operator=(const Vector4D& param) = default;
		constexpr inline Vector4D& operator=(Vector4D&& param) = default;
		constexpr inline Vector4D& operator=(T param) { x = param; y = param; z = param; w = param; return *this; }
		constexpr inline bool operator==(Vector4D param) const { return x == param.x && y == param.y && z == param.z && w == param.w; }
		constexpr inline bool operator!=(Vector4D param) const { return x != param.x || y != param.y || z != param.z || w != param.w; }

		// Basic algebra
		constexpr inline Vector4D operator+(Vector4D param) const { return Vector4D(x + param.x, y + param.y, z + param.z, w + param.w); }
		constexpr inline Vector4D operator-(Vector4D param) const { return Vector4D(x - param.x, y - param.y, z - param.z, w - param.w); }
		constexpr inline Vector4D operator*(Vector4D param) const { return Vector4D(x * param.x, y * param.y, z * param.z, w * param.w); }
		constexpr inline Vector4D operator/(Vector4D param) const { return Vector4D(x / param.x, y / param.y, z / param.z, w / param.w); }
		constexpr inline Vector4D operator%(Vector4D param) const { return Vector4D(mod(x, param.x), mod(y, param.y), mod(z, param.z), mod(w, param.w)); }

		constexpr inline Vector4D modulo(Vector4D param) const { return Vector4D(Halley::modulo<T>(x, param.x), Halley::modulo<T>(y, param.y), Halley::modulo<T>(z, param.z), Halley::modulo<T>(w, param.w)); }
		constexpr inline Vector4D floorDiv(Vector4D param) const { return Vector4D(Halley::floorDiv(x, param.x), Halley::floorDiv(y, param.y), Halley::floorDiv(z, param.z), Halley::floorDiv(w, param.w)); }

		constexpr inline Vector4D operator-() const { return Vector4D(-x, -y, -z, -w); }

		template <typename V>
		constexpr inline Vector4D operator * (V param) const { return Vector4D(T(x * param), T(y * param), T(z * param), T(w * param)); }

		template <typename V>
		constexpr inline Vector4D operator / (V param) const { return Vector4D(T(x / param), T(y / param), T(z * param), T(w * param)); }

		// In-place operations
		constexpr inline Vector4D& operator += (Vector4D param) { x += param.x; y += param.y; z += param.z; w += param.w; return *this; }
		constexpr inline Vector4D& operator -= (Vector4D param) { x -= param.x; y -= param.y; z -= param.z; w -= param.w; return *this; }
		constexpr inline Vector4D& operator *= (Vector4D param) { x *= param.x; y *= param.y; z *= param.z; w *= param.w; return *this; }
		constexpr inline Vector4D& operator /= (Vector4D param) { x /= param.x; y /= param.y; z /= param.z; w /= param.w; return *this; }
		constexpr inline Vector4D& operator *= (const T param) { x *= param; y *= param; z *= param; w *= param; return *this; }
		constexpr inline Vector4D& operator /= (const T param) { x /= param; y /= param; z /= param; w /= param; return *this; }

		// Get the normalized vector (unit vector)
		inline Vector4D unit () const
		{
			float len = length();
			if (len != 0) {
				return (*this) / len;
			} else {
				return Vector4D();
			}
		}

		inline Vector4D normalized() const
		{
			return unit();
		}

		inline void normalize()
		{
			*this = unit();
		}

		// Dot product
		constexpr inline T dot (Vector4D param) const
		{

			return (x * param.x) + (y * param.y) + (z * param.z) + (w * param.w);
		}

		// Length
		constexpr inline T length () const { static_cast<T>(std::sqrt(squaredLength())); }
		constexpr inline T len () const { return length(); }

		// Squared length, often useful and much faster
		constexpr inline T squaredLength () const { return x*x + y*y + z*z + w*w; }

		// Floor
		inline Vector4D floor() const { return Vector4D(floor(x), floor(y), floor(z), floor(w)); }
		inline Vector4D ceil() const { return Vector4D(ceil(x), ceil(y), ceil(z), ceil(w)); }

		constexpr Vector2D<T> toVector2() const
		{
			return Vector2D<T>(x / w, y / w);
		}

		constexpr Vector3D<T> toVector3() const
		{
			return Vector3D<T>(x / w, y / w, z / w);
		}

		// Some swizzles
		constexpr Vector2D<T> xy() const { return Vector2D<T>(x, y); }
		constexpr Vector2D<T> yz() const { return Vector2D<T>(y, z); }
		constexpr Vector2D<T> zw() const { return Vector2D<T>(z, w); }
		constexpr Vector3D<T> xyz() const { return Vector3D<T>(x, y, z); }
	};

	////////////////////
	// Global operators
	template <typename T, typename V>
	constexpr inline Vector4D<T> operator * (V f, const Vector4D<T> &v)
	{
		return Vector4D<T>(T(v.x * f), T(v.y * f), T(v.z * f), T(v.w * f));
	}

	////////////
	// Typedefs
	using Vector4d = Vector4D<double, 4>;
	using Vector4f = Vector4D<float, 4>;
	using Vector4i = Vector4D<int, 4>;
	using Vector4s = Vector4D<short, 4>;
	using Vector4c = Vector4D<char, 4>;
}


#include <gsl/gsl_complex.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_blas.h>

/* Level 1 */
int mgsl_blas_caxpy(gsl_complex_float *alpha, gsl_vector_complex_float *x, gsl_vector_complex_float *y)
{
  return gsl_blas_caxpy(*alpha, x, y);
}

int mgsl_blas_zaxpy(gsl_complex *alpha, gsl_vector_complex *x, gsl_vector_complex *y)
{
  return gsl_blas_zaxpy(*alpha, x, y);
}

void mgsl_blas_cscal(gsl_complex_float *alpha, gsl_vector_complex_float *x)
{
  gsl_blas_cscal(*alpha, x);
}

void mgsl_blas_zscal(gsl_complex *alpha, gsl_vector_complex *x)
{
  gsl_blas_zscal(*alpha, x);
}

/* Level 2 */
int mgsl_blas_cgemv(CBLAS_TRANSPOSE_t TransA, gsl_complex_float *alpha, gsl_matrix_complex_float *A, gsl_vector_complex_float *x, gsl_complex_float *beta, gsl_vector_complex_float *y)
{
  return gsl_blas_cgemv(TransA, *alpha, A, x, *beta, y);
}

int mgsl_blas_zgemv(CBLAS_TRANSPOSE_t TransA, gsl_complex *alpha, gsl_matrix_complex *A, gsl_vector_complex *x, gsl_complex *beta, gsl_vector_complex *y)
{
  return gsl_blas_zgemv(TransA, *alpha, A, x, *beta, y);
}

int mgsl_blas_chemv(CBLAS_UPLO_t Uplo, gsl_complex_float *alpha, gsl_matrix_complex_float *A, gsl_vector_complex_float *x, gsl_complex_float *beta, gsl_vector_complex_float *y)
{
  return gsl_blas_chemv(Uplo, *alpha, A, x, *beta, y);
}

int mgsl_blas_zhemv(CBLAS_UPLO_t Uplo, gsl_complex *alpha, gsl_matrix_complex *A, gsl_vector_complex *x, gsl_complex *beta, gsl_vector_complex *y)
{
  return gsl_blas_zhemv(Uplo, *alpha, A, x, *beta, y);
}

int mgsl_blas_cgeru(gsl_complex_float *alpha, gsl_vector_complex_float *x, gsl_vector_complex_float *y, gsl_matrix_complex_float *A)
{
  return gsl_blas_cgeru(*alpha, x, y, A);
}

int mgsl_blas_zgeru(gsl_complex *alpha, gsl_vector_complex *x, gsl_vector_complex *y, gsl_matrix_complex *A)
{
  return gsl_blas_zgeru(*alpha, x, y, A);
}

int mgsl_blas_cgerc(gsl_complex_float *alpha, gsl_vector_complex_float *x, gsl_vector_complex_float *y, gsl_matrix_complex_float *A)
{
  return gsl_blas_cgerc(*alpha, x, y, A);
}

int mgsl_blas_zgerc(gsl_complex *alpha, gsl_vector_complex *x, gsl_vector_complex *y, gsl_matrix_complex *A)
{
  return gsl_blas_zgerc(*alpha, x, y, A);
}

int mgsl_blas_cher2(CBLAS_UPLO_t Uplo, gsl_complex_float *alpha, gsl_vector_complex_float *x, gsl_vector_complex_float *y, gsl_matrix_complex_float *A)
{
  return gsl_blas_cher2(Uplo, *alpha, x, y, A);
}

int mgsl_blas_zher2(CBLAS_UPLO_t Uplo, gsl_complex *alpha, gsl_vector_complex *x, gsl_vector_complex *y, gsl_matrix_complex *A)
{
  return gsl_blas_zher2(Uplo, *alpha, x, y, A);
}

int mgsl_blas_cgemm(CBLAS_TRANSPOSE_t TransA, CBLAS_TRANSPOSE_t TransB, gsl_complex_float *alpha, gsl_matrix_complex_float *A, gsl_matrix_complex_float *B, gsl_complex_float *beta, gsl_matrix_complex_float *C)
{
  return gsl_blas_cgemm(TransA, TransB, *alpha, A, B, *beta, C);
}

int mgsl_blas_zgemm(CBLAS_TRANSPOSE_t TransA, CBLAS_TRANSPOSE_t TransB, gsl_complex *alpha, gsl_matrix_complex *A, gsl_matrix_complex *B, gsl_complex *beta, gsl_matrix_complex *C)
{
  return gsl_blas_zgemm(TransA, TransB, *alpha, A, B, *beta, C);
}

int mgsl_blas_csymm(CBLAS_SIDE_t Side, CBLAS_UPLO_t Uplo, gsl_complex_float *alpha, gsl_matrix_complex_float *A, gsl_matrix_complex_float *B, gsl_complex_float *beta, gsl_matrix_complex_float *C)
{
  return gsl_blas_csymm(Side, Uplo, *alpha, A, B, *beta, C);
}

int mgsl_blas_zsymm(CBLAS_SIDE_t Side, CBLAS_UPLO_t Uplo, gsl_complex *alpha, gsl_matrix_complex *A, gsl_matrix_complex *B, gsl_complex *beta, gsl_matrix_complex *C)
{
  return gsl_blas_zsymm(Side, Uplo, *alpha, A, B, *beta, C);
}

int mgsl_blas_chemm(CBLAS_SIDE_t Side, CBLAS_UPLO_t Uplo, gsl_complex_float *alpha, gsl_matrix_complex_float *A, gsl_matrix_complex_float *B, gsl_complex_float *beta, gsl_matrix_complex_float *C)
{
  return gsl_blas_chemm(Side, Uplo, *alpha, A, B, *beta, C);
}

int mgsl_blas_zhemm(CBLAS_SIDE_t Side, CBLAS_UPLO_t Uplo, gsl_complex *alpha, gsl_matrix_complex *A, gsl_matrix_complex *B, gsl_complex *beta, gsl_matrix_complex *C)
{
  return gsl_blas_zhemm(Side, Uplo, *alpha, A, B, *beta, C);
}

int mgsl_blas_ctrmm(CBLAS_SIDE_t Side, CBLAS_UPLO_t Uplo, CBLAS_TRANSPOSE_t TransA, CBLAS_DIAG_t Diag, gsl_complex_float *alpha, gsl_matrix_complex_float *A, gsl_matrix_complex_float *B)
{
  return gsl_blas_ctrmm(Side, Uplo, TransA, Diag, *alpha, A, B);
}

int mgsl_blas_ztrmm(CBLAS_SIDE_t Side, CBLAS_UPLO_t Uplo, CBLAS_TRANSPOSE_t TransA, CBLAS_DIAG_t Diag, gsl_complex *alpha, gsl_matrix_complex *A, gsl_matrix_complex *B)
{
  return gsl_blas_ztrmm(Side, Uplo, TransA, Diag, *alpha, A, B);
}

int mgsl_blas_ctrsm(CBLAS_SIDE_t Side, CBLAS_UPLO_t Uplo, CBLAS_TRANSPOSE_t TransA, CBLAS_DIAG_t Diag, gsl_complex_float *alpha, gsl_matrix_complex_float *A, gsl_matrix_complex_float *B)
{
  return gsl_blas_ctrsm(Side, Uplo, TransA, Diag, *alpha, A, B);
}

int mgsl_blas_ztrsm(CBLAS_SIDE_t Side, CBLAS_UPLO_t Uplo, CBLAS_TRANSPOSE_t TransA, CBLAS_DIAG_t Diag, gsl_complex *alpha, gsl_matrix_complex *A, gsl_matrix_complex *B)
{
  return gsl_blas_ztrsm(Side, Uplo, TransA, Diag, *alpha, A, B);
}

int mgsl_blas_csyrk(CBLAS_UPLO_t Uplo, CBLAS_TRANSPOSE_t TransA, gsl_complex_float *alpha, gsl_matrix_complex_float *A, gsl_complex_float *beta, gsl_matrix_complex_float *C)
{
  return gsl_blas_csyrk(Uplo, TransA, *alpha, A, *beta, C);
}

int mgsl_blas_zsyrk(CBLAS_UPLO_t Uplo, CBLAS_TRANSPOSE_t TransA, gsl_complex *alpha, gsl_matrix_complex *A, gsl_complex *beta, gsl_matrix_complex *C)
{
  return gsl_blas_zsyrk(Uplo, TransA, *alpha, A, *beta, C);
}

int mgsl_blas_csyr2k(CBLAS_UPLO_t Uplo, CBLAS_TRANSPOSE_t TransA, gsl_complex_float *alpha, gsl_matrix_complex_float *A, gsl_matrix_complex_float *B, gsl_complex_float *beta, gsl_matrix_complex_float *C)
{
  return gsl_blas_csyr2k(Uplo, TransA, *alpha, A, B, *beta, C);
}

int mgsl_blas_zsyr2k(CBLAS_UPLO_t Uplo, CBLAS_TRANSPOSE_t TransA, gsl_complex *alpha, gsl_matrix_complex *A, gsl_matrix_complex *B, gsl_complex *beta, gsl_matrix_complex *C)
{
  return gsl_blas_zsyr2k(Uplo, TransA, *alpha, A, B, *beta, C);
}

int mgsl_blas_cher2k(CBLAS_UPLO_t Uplo, CBLAS_TRANSPOSE_t TransA, gsl_complex_float *alpha, gsl_matrix_complex_float *A, gsl_matrix_complex_float *B, float beta, gsl_matrix_complex_float *C)
{
  return gsl_blas_cher2k(Uplo, TransA, *alpha, A, B, beta, C);
}

int mgsl_blas_zher2k(CBLAS_UPLO_t Uplo, CBLAS_TRANSPOSE_t TransA, gsl_complex *alpha, gsl_matrix_complex *A, gsl_matrix_complex *B, double beta, gsl_matrix_complex *C)
{
  return gsl_blas_zher2k(Uplo, TransA, *alpha, A, B, beta, C);
}


/////////////////
//example25.5.c
/////////////////
#include <stdio.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_odeiv.h>
#include <math.h>

typedef struct{
  double *a; //y=a[0]x[0]+a[1]x[1]+...a[ka]y[ka]+b[0]u[0]+b[1]u[1]+...+b[kb]u[kb]
  //  double *a; //y[t+1]=a[0]y[t]+a[1]y[t-1]+...a[ka]y[t-ka]+b[0]u[t]+b[1]u[t-1]+...+b[kb]u[t-kb]
  double *b;
  double *x;
  double *u;
  int k,kx,ku;
  double xmax;
  double umax;
  double y;
  int _dim;
  double *_y;
  double *_y_err;
  //  double *_dydt_in;
  //  double *_dydt_out;
  double h;
  double _t;
} LINEAR;

//#define dim_linear 4
//int linearfunc (double t, const double y[], double f[], void *params)
//{
//  LINEAR *c = (LINEAR *)params;
//  f[0] = y[1];
//  f[1] = (-(2*c->dr+c->C)*y[1] -c->ddX*cos(y[0]) -c->g*sin(y[0]))/c->r;
//  f[2] = y[3];
//  f[3] = (c->F+c->T*sin(y[0]))/c->M;
//  return GSL_SUCCESS;
//}
////////////////
#define square(x) ((x)*(x))
LINEAR linear;
int PLANT_NO=1;
int initialize()
{
  if(PLANT_NO==1){
    linear.kx=1;
    linear.ku=1;
    linear.k=linear.kx+linear.ku;
    linear.a=(double*)malloc(sizeof(double)*linear.k);
    linear.b=&linear.a[linear.kx];
    linear.x=(double*)malloc(sizeof(double)*linear.k+1);
    linear.u=&linear.x[linear.kx];

    linear.a[0]=0.9;
    linear.b[0]=1;
  }
  else if(PLANT_NO==2){
    linear.kx=3;
    linear.ku=1;
    linear.k=linear.kx+linear.ku;
    linear.a=(double*)malloc(sizeof(double)*linear.k);
    linear.b=&linear.a[linear.kx];
    linear.x=(double*)malloc(sizeof(double)*linear.k+1);
    linear.u=&linear.x[linear.kx];

    linear.a[0]=1;
    linear.a[1]=1;
    linear.a[2]=1;
    linear.b[0]=1;
  }
  linear.umax=6.0;//check//  linear.Fmax=30;//check
  int i;for(i=0;i<linear.kx;i++) linear.x[i]=0;
#ifdef LINEARSUB
  //  linear.Fmax=_linear_Fmax;//check//  linear.Fmax=30;//check
  //  linear.xmax=20.0;//check//  linear.Fmax=30;//check
  if(_AP_umax>0) AP_u_max=linear.umax=_AP_umax;
  else AP_u_max=linear.umax;

  AP_u_min=-AP_u_max;
  rr=AP_r=_AP_r;//10
  rr_kyoyou=_rr_kyoyou;
  C_MODE=11;
#else
  linear.h=0.01;
#endif
  linear._t=0;
  //  int i;for(i=0;i<linear._dim;i++) linear._dydt_in[i]=0;

  return(0);
}
#ifndef LINEARSUB
char *fn="linear2io.dat";
FILE *fp;
#endif//#ifndef LINEARSUB

double plant(double uu)
{
  //store previous x and u
  int i;
  for(i=0;i<linear.kx-1;i++) linear.x[i+1]=linear.x[i];
  for(i=0;i<linear.ku-1;i++) linear.u[i+1]=linear.u[i];
  //store former output
  linear.x[0]=linear.y;

  //current input
  if(uu>linear.umax) linear.u[0]=linear.umax;
  else if(uu<-linear.umax) linear.u[0]=-linear.umax;
  else linear.u[0]=uu;
  // current output
  linear.y=0;
  for(i=0;i<linear.k;i++) linear.y+=linear.a[i]*linear.x[i];
  linear._t++;

  //  fprintf(stdout,"y%g=(%g)*(%g)+(%g)*(%g) %g %g %g\n",linear.y,linear.a[0],linear.x[0],linear.a[1],linear.x[1],uu,linear.u[0],linear.x[1]);

#ifndef LINEARSUB
  fprintf(fp,"%g %g %g %g", linear._t,linear.y,linear.u[0],linear.x[0]);//linear.a,linear.da
  //  fprintf(fp,"%.7e %.7e %.7e %.7e", linear._t,linear.y,linear.x[0],linear.u[0]);//linear.a,linear.da
  fprintf(fp,"\n");
#endif
  return(linear.y);
  //  return(linear.X);
}

#ifndef LINEARSUB
int main()
{
  ////////////////////////////////////////////////////
  /// method 1                                     ///
  /// input ddX                                    ///
  /// output a,x,y,F                               ///
  ////////////////////////////////////////////////////
  //  double h = 0.001;//,hh=0.1; h = 0.0001;//,hh=0.1;
  double t0=5;     //stationary
  double t1=5.0+t0;//accelerate(speed up)
  double t2=2.0+t1;//free run
  double t3=5.0+t2;//decelerate(slow down)
  double t4=20+t3;   //free run

  initialize();
  linear.h=1;
  int M=(t4/linear.h)+1;
  double *uu=(double*)malloc(sizeof(double)*M);
  double uumax=20;//

  int n;
  double t;
  ////////////////////////////////////////////////////
  ///   set ddX                                    ///
  ////////////////////////////////////////////////////
  int n0 =t0/linear.h+0.5, n4=t4/linear.h+0.5;
  for(t=0;t<t4;t+=linear.h){
    n=t/linear.h;
    if(t<=t0) uu[n]=0; //zero
    else if(t<=t1) uu[n]=uumax/(t1-t0); //accelerate
    else if(t<=t2) uu[n]=0;            //free move
    else if(t<=t3) uu[n]=-uumax/(t3-t2);//decelerate
    else uu[n]=0;                      //free move
    //    fprintf(stdout,"uu[%d]=%e\n",n,uu[n]);
  }
  ////////////////////////////////////////////////////
  ///   Solve by the Runge Kutta Method of GSL     ///
  ////////////////////////////////////////////////////
  fp=fopen(fn,"w");
  fprintf(fp,"#%.7e %d %d %.7e %.7e %.7e %.7e #h,n0,n4,a[0]-a[3]\n",linear.h,n0,n4,linear.a[0],linear.a[1],linear.a[2],linear.a[3]);
  fprintf(fp,"#t,y,x,u\n");
  for(t=0;t<t4;t+=linear.h){
    n=t/linear.h;
    plant(uu[n]);
  }
  fclose(fp);
  fprintf(stdout,"Results are stored in '%s'.\n",fn);

  return 0;
}

#endif //#ifndef SUB


#ifndef FUNCTIONS_H
#define FUNCTIONS_H

#include <vector>
#include <string>
#include <fstream>

#include <gsl/gsl_matrix.h>

#include "structures.h"

double rad2deg(double rad);
double deg2rad(double deg);
double vect_len(const vect &v);
void norm(vect &vr);

double double_dot_prod(const gsl_matrix* m1, const gsl_matrix* m2);
double calc_evM(const gsl_matrix* def);
double calc_internal_energy(const gsl_matrix* shear, const slip_system_matrix *smatrix, grain *g);
double calc_energy(const gsl_matrix* slpsysmat, const gsl_matrix* def_l, const slip_system_matrix *smatrix, grain *g, bool falg=false);
void cleanup_energy();

double update_rot_matrix(const std::vector<int>& comb, const gsl_matrix* shaermin, const std::vector<slip_system*>& systems, const gsl_matrix* uk1, const gsl_matrix* uk2, gsl_matrix* rmat, int def_step);

gsl_matrix* get_rot_matrix(const grain *g, bool free_m=false);
grain* grain_from_rot_matrix(gsl_matrix *rot_mat);

void print_matrix(const gsl_matrix *m);

void load_settings(double *max_def, double *def_step, gsl_matrix *deformation, gsl_matrix *k1, gsl_matrix *k2, const std::string &settings_file_name);
void load_grains(std::vector<grain*> &grains, const std::string &grains_file_name);
void load_slip_systems(std::vector<slip_system*> &slip_systems, const std::string &slip_system_file, int max_def);

std::vector<grain*>* prepare_rot_matrices(const std::string &grains_file_name);
void fill_grain_crss(std::vector<grain*> *grains, const std::vector<slip_system*> &slip_systems);
void prepare_slip_systems_matrices(std::vector<slip_system_matrix*> &matrices, std::vector<slip_system*> &slip_systems);

void convert2U(const gsl_matrix* mat, gsl_matrix* umat);
void calc_handler(handler &h, const gsl_matrix *def, const gsl_matrix *rm);
void get_uhandler(U_handler &uh, const handler &h, int s=2);

void write_grains(const std::vector<grain*> &grinas, std::ofstream &file, const std::string &delimiter);
void export_grains_to_file(const std::vector<grain*> &grains, const std::string &export_file, const std::string &delimiter);
void export_taylor_factor_to_file(const std::vector<double> &total_tf, const std::string& filename);
void write_slip_system_activity(const std::vector<slip_system*> &slip_systems, std::ofstream &export_file, int grains_num, double eVM);
void export_slip_system_activity_to_file(const std::vector<slip_system*> &slip_systems, const std::string& filename, int grains_num, double eVM);

void free_grains(std::vector<grain*> &grains);
void free_slip_systems(std::vector<slip_system*> &slip_systems);
void free_rotation_matrices(std::vector<gsl_matrix*> rot_matrices);
void free_slip_systems_matrices(std::vector<slip_system_matrix*> &matrices);

#endif	//FUNCTIONS_H


#pragma once

#include <string>

#include <gsl/gsl_complex.h>

#include "Expression.h"

struct NumericalExpression: public Expression {

    NumericalExpression(Scanner::Type kind);

    expression derivative(const std::string& var) override;
    expression integrate(const std::string& var) override;
};

class NumExpression: public NumericalExpression {
    double real, imag;

    NumExpression(double real, double imag);
    NumExpression(const std::string&);

    public:
        static expression construct(double real, double imag = 0);
        static expression construct(const gsl_complex& z);
        static expression construct(const std::string&);

        bool isNumber() const override { return true; }

        gsl_complex complex(const Variables& vars = emptyVars) const override;

        EXPRESSION_OVERRIDES

};

class HexExpression: public NumericalExpression {
    unsigned long long num;

    HexExpression(unsigned long long num);
    HexExpression(const std::string&);

    public:
        static expression construct(unsigned long long num);
        static expression construct(const std::string&);

        bool isNumber() const override { return true; }

        EXPRESSION_OVERRIDES

};

class BinExpression: public NumericalExpression {
    unsigned long long num;

    BinExpression(unsigned long long num);
    BinExpression(const std::string&);

    public:
        static expression construct(unsigned long long num);
        static expression construct(const std::string&);

        bool isNumber() const override { return true; }

        EXPRESSION_OVERRIDES

};


/* ode-initval/bsimp.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2004 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/* Bulirsch-Stoer Implicit */

/* Author:  G. Jungman
 */
/* Bader-Deuflhard implicit extrapolative stepper.
 * [Numer. Math., 41, 373 (1983)]
 */
#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_odeiv.h>

#include "odeiv_util.h"

#define SEQUENCE_COUNT 8
#define SEQUENCE_MAX   7

/* Bader-Deuflhard extrapolation sequence */
static const int bd_sequence[SEQUENCE_COUNT] =
  { 2, 6, 10, 14, 22, 34, 50, 70 };

typedef struct
{
  gsl_matrix *d;                /* workspace for extrapolation         */
  gsl_matrix *a_mat;            /* workspace for linear system matrix  */
  gsl_permutation *p_vec;       /* workspace for LU permutation        */

  double x[SEQUENCE_MAX];       /* workspace for extrapolation */

  /* state info */
  size_t k_current;
  size_t k_choice;
  double h_next;
  double eps;

  /* workspace for extrapolation step */
  double *yp;
  double *y_save;
  double *yerr_save;
  double *y_extrap_save;
  double *y_extrap_sequence;
  double *extrap_work;
  double *dfdt;
  double *y_temp;
  double *delta_temp;
  double *weight;
  gsl_matrix *dfdy;

  /* workspace for the basic stepper */
  double *rhs_temp;
  double *delta;

  /* order of last step */
  size_t order;
}
bsimp_state_t;

/* Compute weighting factor */

static void
compute_weights (const double y[], double w[], size_t dim)
{
  size_t i;
  for (i = 0; i < dim; i++)
    {
      double u = fabs(y[i]);
      w[i] = (u > 0.0) ? u : 1.0;
    }
}

/* Calculate a choice for the "order" of the method, using the
 * Deuflhard criteria.
 */

static size_t
bsimp_deuf_kchoice (double eps, size_t dimension)
{
  const double safety_f = 0.25;
  const double small_eps = safety_f * eps;

  double a_work[SEQUENCE_COUNT];
  double alpha[SEQUENCE_MAX][SEQUENCE_MAX];

  int i, k;

  a_work[0] = bd_sequence[0] + 1.0;

  for (k = 0; k < SEQUENCE_MAX; k++)
    {
      a_work[k + 1] = a_work[k] + bd_sequence[k + 1];
    }

  for (i = 0; i < SEQUENCE_MAX; i++)
    {
      alpha[i][i] = 1.0;
      for (k = 0; k < i; k++)
        {
          const double tmp1 = a_work[k + 1] - a_work[i + 1];
          const double tmp2 = (a_work[i + 1] - a_work[0] + 1.0) * (2 * k + 1);
          alpha[k][i] = pow (small_eps, tmp1 / tmp2);
        }
    }

  a_work[0] += dimension;

  for (k = 0; k < SEQUENCE_MAX; k++)
    {
      a_work[k + 1] = a_work[k] + bd_sequence[k + 1];
    }

  for (k = 0; k < SEQUENCE_MAX - 1; k++)
    {
      if (a_work[k + 2] > a_work[k + 1] * alpha[k][k + 1])
        break;
    }

  return k;
}

static void
poly_extrap (gsl_matrix * d,
             const double x[],
             const unsigned int i_step,
             const double x_i,
             const double y_i[],
             double y_0[], double y_0_err[], double work[], const size_t dim)
{
  size_t j, k;

  DBL_MEMCPY (y_0_err, y_i, dim);
  DBL_MEMCPY (y_0, y_i, dim);

  if (i_step == 0)
    {
      for (j = 0; j < dim; j++)
        {
          gsl_matrix_set (d, 0, j, y_i[j]);
        }
    }
  else
    {
      DBL_MEMCPY (work, y_i, dim);

      for (k = 0; k < i_step; k++)
        {
          double delta = 1.0 / (x[i_step - k - 1] - x_i);
          const double f1 = delta * x_i;
          const double f2 = delta * x[i_step - k - 1];

          for (j = 0; j < dim; j++)
            {
              const double q_kj = gsl_matrix_get (d, k, j);
              gsl_matrix_set (d, k, j, y_0_err[j]);
              delta = work[j] - q_kj;
              y_0_err[j] = f1 * delta;
              work[j] = f2 * delta;
              y_0[j] += y_0_err[j];
            }
        }

      for (j = 0; j < dim; j++)
        {
          gsl_matrix_set (d, i_step, j, y_0_err[j]);
        }
    }
}

/* Basic implicit Bulirsch-Stoer step.  Divide the step h_total into
 * n_step smaller steps and do the Bader-Deuflhard semi-implicit
 * iteration.  */

static int
bsimp_step_local (void *vstate,
                  size_t dim,
                  const double t0,
                  const double h_total,
                  const unsigned int n_step,
                  const double y[],
                  const double yp[],
                  const double dfdt[],
                  const gsl_matrix * dfdy,
                  double y_out[],
                  const gsl_odeiv_system * sys)
{
  bsimp_state_t *state = (bsimp_state_t *) vstate;

  gsl_matrix *const a_mat = state->a_mat;
  gsl_permutation *const p_vec = state->p_vec;

  double *const delta = state->delta;
  double *const y_temp = state->y_temp;
  double *const delta_temp = state->delta_temp;
  double *const rhs_temp = state->rhs_temp;
  double *const w = state->weight;

  gsl_vector_view y_temp_vec = gsl_vector_view_array (y_temp, dim);
  gsl_vector_view delta_temp_vec = gsl_vector_view_array (delta_temp, dim);
  gsl_vector_view rhs_temp_vec = gsl_vector_view_array (rhs_temp, dim);

  const double h = h_total / n_step;
  double t = t0 + h;

  double sum;

  /* This is the factor sigma referred to in equation 3.4 of the
     paper.  A relative change in y exceeding sigma indicates a
     runaway behavior. According to the authors suitable values for
     sigma are >>1.  I have chosen a value of 100*dim. BJG */

  const double max_sum = 100.0 * dim;

  int signum, status;
  size_t i, j;
  size_t n_inter;

  /* Calculate the matrix for the linear system. */
  for (i = 0; i < dim; i++)
    {
      for (j = 0; j < dim; j++)
        {
          gsl_matrix_set (a_mat, i, j, -h * gsl_matrix_get (dfdy, i, j));
        }
      gsl_matrix_set (a_mat, i, i, gsl_matrix_get (a_mat, i, i) + 1.0);
    }

  /* LU decomposition for the linear system. */

  gsl_linalg_LU_decomp (a_mat, p_vec, &signum);

  /* Compute weighting factors */

  compute_weights (y, w, dim);

  /* Initial step. */

  for (i = 0; i < dim; i++)
    {
      y_temp[i] = h * (yp[i] + h * dfdt[i]);
    }

  gsl_linalg_LU_solve (a_mat, p_vec, &y_temp_vec.vector, &delta_temp_vec.vector);

  sum = 0.0;

  for (i = 0; i < dim; i++)
    {
      const double di = delta_temp[i];
      delta[i] = di;
      y_temp[i] = y[i] + di;
      sum += fabs(di) / w[i];
    }

  if (sum > max_sum)
    {
      return GSL_EFAILED ;
    }

  /* Intermediate steps. */

  status = GSL_ODEIV_FN_EVAL (sys, t, y_temp, y_out);

  if (status)
    {
      return status;
    }

  for (n_inter = 1; n_inter < n_step; n_inter++)
    {
      for (i = 0; i < dim; i++)
        {
          rhs_temp[i] = h * y_out[i] - delta[i];
        }

      gsl_linalg_LU_solve (a_mat, p_vec, &rhs_temp_vec.vector, &delta_temp_vec.vector);

      sum = 0.0;

      for (i = 0; i < dim; i++)
        {
          delta[i] += 2.0 * delta_temp[i];
          y_temp[i] += delta[i];
          sum += fabs(delta[i]) / w[i];
        }

      if (sum > max_sum)
        {
          return GSL_EFAILED ;
        }

      t += h;

      status = GSL_ODEIV_FN_EVAL (sys, t, y_temp, y_out);

      if (status)
        {
          return status;
        }
    }

  /* Final step. */

  for (i = 0; i < dim; i++)
    {
      rhs_temp[i] = h * y_out[i] - delta[i];
    }

  gsl_linalg_LU_solve (a_mat, p_vec, &rhs_temp_vec.vector, &delta_temp_vec.vector);

  sum = 0.0;

  for (i = 0; i < dim; i++)
    {
      y_out[i] = y_temp[i] + delta_temp[i];
      sum += fabs(delta_temp[i]) / w[i];
    }

  if (sum > max_sum)
    {
      return GSL_EFAILED ;
    }

  return GSL_SUCCESS;
}

static void *
bsimp_alloc (size_t dim)
{
  bsimp_state_t *state = (bsimp_state_t *) malloc (sizeof (bsimp_state_t));

  state->d = gsl_matrix_alloc (SEQUENCE_MAX, dim);
  state->a_mat = gsl_matrix_alloc (dim, dim);
  state->p_vec = gsl_permutation_alloc (dim);

  state->yp = (double *) malloc (dim * sizeof (double));
  state->y_save = (double *) malloc (dim * sizeof (double));
  state->yerr_save = (double *) malloc (dim * sizeof (double));
  state->y_extrap_save = (double *) malloc (dim * sizeof (double));
  state->y_extrap_sequence = (double *) malloc (dim * sizeof (double));
  state->extrap_work = (double *) malloc (dim * sizeof (double));
  state->dfdt = (double *) malloc (dim * sizeof (double));
  state->y_temp = (double *) malloc (dim * sizeof (double));
  state->delta_temp = (double *) malloc (dim * sizeof(double));
  state->weight = (double *) malloc (dim * sizeof(double));

  state->dfdy = gsl_matrix_alloc (dim, dim);

  state->rhs_temp = (double *) malloc (dim * sizeof(double));
  state->delta = (double *) malloc (dim * sizeof (double));

  {
    size_t k_choice = bsimp_deuf_kchoice (GSL_SQRT_DBL_EPSILON, dim); /*FIXME: choice of epsilon? */
    state->k_choice = k_choice;
    state->k_current = k_choice;
    state->order = 2 * k_choice;
  }

  state->h_next = -GSL_SQRT_DBL_MAX;

  return state;
}

/* Perform the basic semi-implicit extrapolation
 * step, of size h, at a Deuflhard determined order.
 */
static int
bsimp_apply (void *vstate,
             size_t dim,
             double t,
             double h,
             double y[],
             double yerr[],
             const double dydt_in[],
             double dydt_out[],
             const gsl_odeiv_system * sys)
{
  bsimp_state_t *state = (bsimp_state_t *) vstate;

  double *const x = state->x;
  double *const yp = state->yp;
  double *const y_save = state->y_save;
  double *const yerr_save = state->yerr_save;
  double *const y_extrap_sequence = state->y_extrap_sequence;
  double *const y_extrap_save = state->y_extrap_save;
  double *const extrap_work = state->extrap_work;
  double *const dfdt = state->dfdt;
  gsl_matrix *d = state->d;
  gsl_matrix *dfdy = state->dfdy;

  const double t_local = t;
  size_t i, k;

  if (h + t_local == t_local)
    {
      return GSL_EUNDRFLW;      /* FIXME: error condition */
    }

  DBL_MEMCPY (y_extrap_save, y, dim);

  /* Save inputs */
  DBL_MEMCPY (y_save, y, dim);
  DBL_MEMCPY (yerr_save, yerr, dim);

  /* Evaluate the derivative. */
  if (dydt_in != NULL)
    {
      DBL_MEMCPY (yp, dydt_in, dim);
    }
  else
    {
      int s = GSL_ODEIV_FN_EVAL (sys, t_local, y, yp);

      if (s != GSL_SUCCESS)
	{
          return s;
	}
    }

  /* Evaluate the Jacobian for the system. */
  {
    int s = GSL_ODEIV_JA_EVAL (sys, t_local, y, dfdy->data, dfdt);

    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  /* Make a series of refined extrapolations,
   * up to the specified maximum order, which
   * was calculated based on the Deuflhard
   * criterion upon state initialization.  */
  for (k = 0; k <= state->k_current; k++)
    {
      const unsigned int N = bd_sequence[k];
      const double r = (h / N);
      const double x_k = r * r;

      int status = bsimp_step_local (state,
                                     dim, t_local, h, N,
                                     y_extrap_save, yp,
                                     dfdt, dfdy,
                                     y_extrap_sequence,
                                     sys);

      if (status == GSL_EFAILED)
        {
          /* If the local step fails, set the error to infinity in
             order to force a reduction in the step size */

          for (i = 0; i < dim; i++)
            {
              yerr[i] = GSL_POSINF;
            }

          break;
        }

      else if (status != GSL_SUCCESS)
	{
	  return status;
	}

      x[k] = x_k;

      poly_extrap (d, x, k, x_k, y_extrap_sequence, y, yerr, extrap_work, dim);
    }

  /* Evaluate dydt_out[]. */

  if (dydt_out != NULL)
    {
      int s = GSL_ODEIV_FN_EVAL (sys, t + h, y, dydt_out);

      if (s != GSL_SUCCESS)
        {
          DBL_MEMCPY (y, y_save, dim);
          DBL_MEMCPY (yerr, yerr_save, dim);
          return s;
        }
    }

  return GSL_SUCCESS;
}

static unsigned int
bsimp_order (void *vstate)
{
  bsimp_state_t *state = (bsimp_state_t *) vstate;
  return state->order;
}

static int
bsimp_reset (void *vstate, size_t dim)
{
  bsimp_state_t *state = (bsimp_state_t *) vstate;

  state->h_next = 0;

  DBL_ZERO_MEMSET (state->yp, dim);

  return GSL_SUCCESS;
}

static void
bsimp_free (void * vstate)
{
  bsimp_state_t *state = (bsimp_state_t *) vstate;

  free (state->delta);
  free (state->rhs_temp);

  gsl_matrix_free (state->dfdy);

  free (state->weight);
  free (state->delta_temp);
  free (state->y_temp);
  free (state->dfdt);
  free (state->extrap_work);
  free (state->y_extrap_sequence);
  free (state->y_extrap_save);
  free (state->y_save);
  free (state->yerr_save);
  free (state->yp);

  gsl_permutation_free (state->p_vec);
  gsl_matrix_free (state->a_mat);
  gsl_matrix_free (state->d);
  free (state);
}

static const gsl_odeiv_step_type bsimp_type = {
  "bsimp",                      /* name */
  1,                            /* can use dydt_in */
  1,                            /* gives exact dydt_out */
  &bsimp_alloc,
  &bsimp_apply,
  &bsimp_reset,
  &bsimp_order,
  &bsimp_free
};

const gsl_odeiv_step_type *gsl_odeiv_step_bsimp = &bsimp_type;