/**
 *
 * @file core_zhetrf2_nopiv.c
 *
 *  PLASMA core_blas kernel
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.4.5
 * @author Dulceneia Becker
 * @date 2011-1-18
 **/
/*
 * @precisions normal z -> c d s
 */
#include <lapacke.h>
#include "dplasma_cores.h"
#include "dplasma_zcores.h"

#if defined(PARSEC_HAVE_STRING_H)
#include <string.h>
#endif  /* defined(PARSEC_HAVE_STRING_H) */
#if defined(PARSEC_HAVE_STDARG_H)
#include <stdarg.h>
#endif  /* defined(PARSEC_HAVE_STDARG_H) */
#include <stdio.h>
#ifdef PARSEC_HAVE_LIMITS_H
#include <limits.h>
#endif

#include <cblas.h>
#include <core_blas.h>

#define max(a, b) ((a) > (b) ? (a) : (b))
#define min(a, b) ((a) < (b) ? (a) : (b))

extern void CORE_zhetrf_nopiv(int uplo, int N, int ib,
                         PLASMA_Complex64_t *A, int LDA,
                         PLASMA_Complex64_t *WORK, int LDWORK,
                         int *INFO);

void CORE_zhetrf2_nopiv(PLASMA_enum uplo, int N, int ib,
        PLASMA_Complex64_t *A, int LDA,
        PLASMA_Complex64_t *WORK, int LWORK, int *INFO);

/***************************************************************************//**
 *
 * @ingroup CORE_PLASMA_Complex64_t
 *
 * CORE_zhetrf2_nopiv: ZHETRF_NOPIV followed by L*D
 *
 *******************************************************************************
 *
 * @return
 *          \retval PLASMA_SUCCESS successful exit
 *          \retval <0 if -i, the i-th argument had an illegal value
 *
 ******************************************************************************/
#if defined(PLASMA_PARSEC_HAVE_WEAK)
#pragma weak CORE_zhetrf2_nopiv = PCORE_zhetrf2_nopiv
#define CORE_zhetrf2_nopiv PCORE_zhetrf2_nopiv
#endif
void CORE_zhetrf2_nopiv(PLASMA_enum uplo, int N, int ib,
        PLASMA_Complex64_t *A, int LDA,
        PLASMA_Complex64_t *WORK, int LWORK, int *INFO)
{

    int j;
    PLASMA_Complex64_t alpha;

    /* Factorize A as L*D*L' using the lower/upper triangle of A */
    CORE_zhetrf_nopiv(uplo, N, ib, A, LDA, WORK, LWORK, INFO);

    if (uplo==PlasmaLower) {

        // Multiply L by D
        for (j=0; j<(N-1); j++) {
            alpha = A[LDA*j+j];
            cblas_zscal(N-j-1, CBLAS_SADDR(alpha), &A[LDA*j+j+1], 1);
        }

    } else if (uplo==PlasmaUpper) {

        // Multiply U by D
        for (j=1; j<N; j++) {
            alpha = A[LDA*j+j];
            cblas_zscal(j, CBLAS_SADDR(alpha), &A[LDA*j], 1);
        }

    }
}


#pragma once

#include <cstring>
#include <string>
#include <tuple>
#include <utility>

#include <gsl/assert>

namespace xmol::utils {

constexpr static int powers_of_10[] = {1, 10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000, 1000000000};

template <int LEN> inline std::pair<bool, int> parse_uint_fixed_length(const std::string& line, int pos) noexcept {
  bool success = GSL_LIKELY(line.size() >= pos + LEN);
  if (GSL_UNLIKELY(!success)) {
    return {false, 0};
  }
  int number = 0;
#pragma unroll
  for (int i = 0; i < LEN; i++) {
    unsigned digit = static_cast<unsigned>(line[pos + i]) - '0';
    success &= digit <= 9;
    number += digit * powers_of_10[LEN - i - 1];
  }
  return std::make_pair(success, number);
};

inline std::pair<bool, int> parse_uint(const std::string& line, int pos, int LEN) noexcept {
  switch (LEN) {
  case (1):
    return parse_uint_fixed_length<1>(line, pos);
  case (2):
    return parse_uint_fixed_length<2>(line, pos);
  case (3):
    return parse_uint_fixed_length<3>(line, pos);
  case (4):
    return parse_uint_fixed_length<4>(line, pos);
  case (5):
    return parse_uint_fixed_length<5>(line, pos);
  case (6):
    return parse_uint_fixed_length<6>(line, pos);
  case (7):
    return parse_uint_fixed_length<7>(line, pos);
  case (8):
    return parse_uint_fixed_length<8>(line, pos);
  case (9):
    return parse_uint_fixed_length<9>(line, pos);
  default:
    return {false, 0};
  }
};

enum class SpaceStrip { NONE, LEFT, RIGHT, LEFT_AND_RIGHT };

template <SpaceStrip stripping = SpaceStrip::NONE>
std::pair<bool, int> parse_int(const std::string& line, int pos, int LEN) noexcept {
  // short-circuit for invalid
  if (GSL_UNLIKELY(line.size() < pos + LEN)) {
    return {false, 0};
  }

  if (stripping == SpaceStrip::RIGHT || stripping == SpaceStrip::LEFT_AND_RIGHT) {
    while (line[pos + LEN - 1] == ' ' && LEN > 0) {
      --LEN;
    }
  }

  if (stripping == SpaceStrip::LEFT || stripping == SpaceStrip::LEFT_AND_RIGHT) {
    while (line[pos] == ' ' && LEN > 0) {
      ++pos;
      --LEN;
    }
  }

  if (line[pos] == '-') {
    bool success;
    int value;
    std::tie(success, value) = parse_uint(line, pos + 1, LEN - 1);
    return {success, -value};
  } else {
    return parse_uint(line, pos, LEN);
  }
};

template <int WIDTH, int PRECISION, SpaceStrip STRIP> struct parse_fixed_precision_fn {
  static_assert(PRECISION >= 0);
  static_assert(WIDTH > 0);
  static_assert(PRECISION == 0 || WIDTH >= PRECISION + 2);

  inline std::pair<bool, double> operator()(const std::string& line, int pos) const noexcept {

    if (GSL_UNLIKELY(line.size() < pos + WIDTH || (PRECISION > 0 && line[pos + WIDTH - PRECISION - 1] != '.'))) {
      return {false, 0};
    }

    int whole = WIDTH - PRECISION;
    if (PRECISION > 0) {
      --whole;
    }
    int precision = PRECISION;

    if (STRIP == SpaceStrip::LEFT_AND_RIGHT || STRIP == SpaceStrip::LEFT) {
      while (line[pos] == ' ' && whole > 0) {
        --whole;
        pos++;
      }
    }

    if (STRIP == SpaceStrip::LEFT_AND_RIGHT || STRIP == SpaceStrip::RIGHT) {
      while (line[pos + whole + precision] == ' ' && precision > 0) {
        --precision;
      }
    }

    int sign = 1;

    if (line[pos] == '-') {
      sign = -1;
      ++pos;
      --whole;
    }
    bool success;
    int whole_part;
    std::tie(success, whole_part) = parse_uint(line, pos, whole);

    if (GSL_UNLIKELY(!success)) {
      return {false, 0};
    }
    if (PRECISION == 0) {
      return {true, sign * whole_part};
    }
    bool success2;
    int fraction_part;
    std::tie(success2, fraction_part) = parse_uint(line, pos + whole + 1, precision);
    if (GSL_UNLIKELY(!success2)) {
      return {false, 0};
    }

    return {true, sign * (whole_part + double(fraction_part) / powers_of_10[precision])};
  };
};

inline namespace functional_objects {
struct parse_fixed_precision_fn___ {
  template <int WIDTH, int PRECISION, SpaceStrip STRIP = SpaceStrip::LEFT_AND_RIGHT>
  inline std::pair<bool, double> parse(const std::string& line, int pos) const {
    return parse_fixed_precision_fn<WIDTH,PRECISION,STRIP>{}(line,pos);
  }
};
constexpr parse_fixed_precision_fn___ parse_fixed_precision{};
}

std::pair<bool, double> parse_fixed_precision_rt(const std::string& line, int pos, int width) noexcept;
}


#ifndef _COMMON_
#define _COMMON_

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_roots.h>
#include "params.h"
//#include "dam_utils.h"
#include "cosmo_mad.h"

#define DTOR 0.01745329251

typedef struct {
  gsl_interp_accel *intacc;
  gsl_spline *spline;
  double x0,xf;
  double y0,yf;
} SplPar;

typedef struct {
  double om,ol,ob;
  double w0,wa,h0;
  double ns,s8;
  char **fname_window;
  char fname_bias[256];
  char fname_sbias[256];
  char fname_pk[256];
  char prefix_out[256];
  int lmax;
  Csm_params *cpar;
  double chi_horizon;
  double chi_LSS;
  double prefac_lensing;
  double dchi;
  int do_nc;
  int do_shear;
  int do_cmblens;
  int do_isw;
  int has_bg;
  int has_dens;
  int has_rsd;
  int has_lensing;
  SplPar *aofchi;
  SplPar *zofchi;
  SplPar *hofchi;
  SplPar *gfofchi;
  SplPar *fgofchi;
  SplPar **wind_0;
  SplPar **wind_M;
  SplPar **wind_L;
  SplPar *bias;
  SplPar *sbias;
  double *cl_dd;
  double *cl_d1l2;
  double *cl_d2l1;
  double *cl_dc;
  double *cl_di;
  double *cl_ll;
  double *cl_lc;
  double *cl_li;
  double *cl_cc;
  double *cl_ci;
  double *cl_ii;
  int do_w_theta;
  int do_w_theta_logbin;
  double th_min;
  double th_max;
  int n_th;
  int n_th_logint;
  double *wt_dd;
  double *wt_d1l2;
  double *wt_d2l1;
  double *wt_dc;
  double *wt_di;
  double *wt_ll_pp;
  double *wt_ll_mm;
  double *wt_lc;
  double *wt_li;
  double *wt_cc;
  double *wt_ci;
  double *wt_ii;
} RunParams;

//Defined in common.c
void dam_report_error(int level,char *fmt,...);
void *dam_malloc(size_t size);
void *dam_calloc(size_t nmemb,size_t size);
FILE *dam_fopen(const char *path,const char *mode);
int dam_linecount(FILE *f);
SplPar *spline_init(int n,double *x,double *y,double y0,double yf);
double spline_eval(double x,SplPar *spl);
void spline_free(SplPar *spl);
RunParams *param_new(void);
void param_free(RunParams *par);

//Defined in cosmo.c
RunParams *init_params(char *fname_ini);

//Defined in transfers.c
double transfer_wrap(int l,double k,RunParams *par,char *trtype,int ibin);

//Defined in spectra.c
void compute_spectra(RunParams *par);
void compute_w_theta(RunParams *par);

//Defined in io.c
int read_parameter_file(char *fname,RunParams *par);
void write_output(RunParams *par);

#endif //_COMMON_


#include "../globals.h"
#include <gsl/gsl_sort.h>
#include <gsl/gsl_sort_vector.h>
#include <gsl/gsl_vector.h>

void setup_Sedov_Blast()
{
    Problem.Boxsize[0] = 3;
    Problem.Boxsize[1] = 3;
    Problem.Boxsize[2] = 3;

    sprintf ( Problem.Name, "IC_SedovBlast" );

    const double rho = 1.24E7;

    Problem.Rho_Max = rho;

    Density_Func_Ptr = &Sedov_Blast_Density;
    U_Func_Ptr = &Sedov_Blast_U;
    PostProcessing_Func_Ptr = &Sedov_Blast_PostProcessing;
}

float Sedov_Blast_Density ( const int ipart , const double bias )
{
    return 1.24E7;
}

// This function calculates the distance from each particle to the center of the Box. It returns the distance to the NNpart times furthest neighbour of zero.

float Sedov_Blast_abs ()
{

    int NNpart = 296;

    gsl_vector *abs_of_zero = gsl_vector_alloc ( Param.Npart );

    for ( int i = 0; i < Param.Npart; i++ ) {

        double r = sqrt ( ( P[i].Pos[0] - 0.5 * Problem.Boxsize[0] ) * ( P[i].Pos[0] - 0.5 * Problem.Boxsize[0] ) + ( P[i].Pos[1] * 0.5 * Problem.Boxsize[1] ) * ( P[i].Pos[1] - 0.5 * Problem.Boxsize[1] )
                          + ( P[i].Pos[2] - 0.5 * Problem.Boxsize[2] ) * ( P[i].Pos[2] - 0.5 * Problem.Boxsize[2] ) );
        gsl_vector_set ( abs_of_zero, i, r );
    }

    gsl_sort_vector ( abs_of_zero );

    double dist = gsl_vector_get ( abs_of_zero, NNpart - 1 );

    gsl_vector_free ( abs_of_zero );

    return dist;

}

#ifdef KINETIC_SEDOV
float Sedov_Blast_kinetic ()
{

    int NNpart = 32;
    Radius =
        double partpos[3][32];

    float maxDistance = Sedov_Blast_abs ();

    if ( Radius <= maxDistance ) {

        for ( int i =; i < 3; i++ ) {

            for ( int j = 0; j < NNpart, j++ ) {

                double pos = P[i].Pos[j];
                partpos[i][j] = pos;

            }

        }

    }

}
#endif // KINETIC_SEDOV

float Sedov_Blast_U ( const int ipart )
{
    return 0.0;
}

//! @todo improvement: use gsl_sort_vector_index
void Sedov_Blast_PostProcessing ()
{
    /*const double u_sn = 4.18971E5;
    int sn_count = pow ( Param.Npart / 3200., 3.0 );
    sn_count = min ( sn_count, 1 );
    //! @todo assign u_sn to sn_count innermost particles*/

    float maxDistance = Sedov_Blast_abs ();

    for ( int ipart = 0; ipart < Param.Npart; ++ipart ) {

        const double x = P[ipart].Pos[0] - Problem.Boxsize[0] * 0.5;
        const double y = P[ipart].Pos[1] - Problem.Boxsize[1] * 0.5;
        const double z = P[ipart].Pos[2] - Problem.Boxsize[2] * 0.5;
        double Radius = sqrt ( x * x + y * y + z * z );
        // int NNpart = 32;
        const double  Supernova_Energy = 0.00502765; // This is the value of one supernova energy per unit mass in Gadget units 1e51 erg
        // const double hsml = SphP[i].Hsml;

        if ( Radius <=  maxDistance ) {
            SphP[ipart].U = Supernova_Energy;
        }
    }

}


// Copyright (c) 2017, Lawrence Livermore National Security, LLC. Produced at
// the Lawrence Livermore National Laboratory. LLNL-CODE-734707. All Rights
// reserved. See files LICENSE and NOTICE for details.
//
// This file is part of CEED, a collection of benchmarks, miniapps, software
// libraries and APIs for efficient high-order finite element and spectral
// element discretizations for exascale applications. For more information and
// source code availability see http://github.com/ceed.
//
// The CEED research is supported by the Exascale Computing Project 17-SC-20-SC,
// a collaborative effort of two U.S. Department of Energy organizations (Office
// of Science and the National Nuclear Security Administration) responsible for
// the planning and preparation of a capable exascale ecosystem, including
// software, applications, hardware, advanced system engineering and early
// testbed platforms, in support of the nation's exascale computing imperative.

//                        libCEED + PETSc Example: CEED BPs
//
// This example demonstrates a simple usage of libCEED with PETSc to solve the
// CEED BP benchmark problems, see http://ceed.exascaleproject.org/bps,
// on a closed surface, such as the one of a discrete sphere.
//
// The code uses higher level communication protocols in DMPlex.
//
// Build with:
//
//     make bpssphere [PETSC_DIR=</path/to/petsc>] [CEED_DIR=</path/to/libceed>]
//
// Sample runs:
//
//     bpssphere -problem bp1 -degree 3
//     bpssphere -problem bp2 -degree 3
//     bpssphere -problem bp3 -degree 3
//     bpssphere -problem bp4 -degree 3
//     bpssphere -problem bp5 -degree 3 -ceed /cpu/self
//     bpssphere -problem bp6 -degree 3 -ceed /gpu/cuda
//
//TESTARGS -ceed {ceed_resource} -test -problem bp3 -degree 3 -dm_refine 2

/// @file
/// CEED BPs example using PETSc with DMPlex
/// See bps.c for a "raw" implementation using a structured grid.
/// and bpsdmplex.c for an implementation using an unstructured grid.
static const char help[] = "Solve CEED BPs on a sphere using DMPlex in PETSc\n";

#include <stdbool.h>
#include <string.h>
#include <ceed.h>
#include <petsc.h>
#include <petscdmplex.h>
#include <petscksp.h>

#include "bpssphere.h"
#include "include/sphereproblemdata.h"
#include "include/petscutils.h"
#include "include/petscversion.h"
#include "include/matops.h"
#include "include/libceedsetup.h"

#if PETSC_VERSION_LT(3,12,0)
#ifdef PETSC_HAVE_CUDA
#include <petsccuda.h>
// Note: With PETSc prior to version 3.12.0, providing the source path to
//       include 'cublas_v2.h' will be needed to use 'petsccuda.h'.
#endif
#endif

int main(int argc, char **argv) {
  PetscInt ierr;
  MPI_Comm comm;
  char ceed_resource[PETSC_MAX_PATH_LEN] = "/cpu/self",
      filename[PETSC_MAX_PATH_LEN];
  double my_rt_start, my_rt, rt_min, rt_max;
  PetscInt degree = 3, q_extra, l_size, g_size, topo_dim = 2, num_comp_x = 3,
           num_comp_u = 1, xl_size;
  PetscScalar *r;
  PetscBool test_mode, benchmark_mode, read_mesh, write_solution, simplex;
  PetscLogStage solve_stage;
  Vec X, X_loc, rhs, rhs_loc;
  Mat mat_O;
  KSP ksp;
  DM  dm;
  UserO user_O;
  Ceed ceed;
  CeedData ceed_data;
  CeedQFunction qf_error;
  CeedOperator op_error;
  CeedVector rhs_ceed, target;
  BPType bp_choice;
  VecType vec_type;
  PetscMemType mem_type;

  ierr = PetscInitialize(&argc, &argv, NULL, help);
  if (ierr) return ierr;
  comm = PETSC_COMM_WORLD;

  // Read command line options
  ierr = PetscOptionsBegin(comm, NULL, "CEED BPs in PETSc", NULL); CHKERRQ(ierr);
  bp_choice = CEED_BP1;
  ierr = PetscOptionsEnum("-problem",
                          "CEED benchmark problem to solve", NULL,
                          bp_types, (PetscEnum)bp_choice, (PetscEnum *)&bp_choice,
                          NULL); CHKERRQ(ierr);
  num_comp_u = bp_options[bp_choice].num_comp_u;
  test_mode = PETSC_FALSE;
  ierr = PetscOptionsBool("-test",
                          "Testing mode (do not print unless error is large)",
                          NULL, test_mode, &test_mode, NULL); CHKERRQ(ierr);
  benchmark_mode = PETSC_FALSE;
  ierr = PetscOptionsBool("-benchmark",
                          "Benchmarking mode (prints benchmark statistics)",
                          NULL, benchmark_mode, &benchmark_mode, NULL);
  CHKERRQ(ierr);
  write_solution = PETSC_FALSE;
  ierr = PetscOptionsBool("-write_solution",
                          "Write solution for visualization",
                          NULL, write_solution, &write_solution, NULL);
  CHKERRQ(ierr);
  degree = test_mode ? 3 : 2;
  ierr = PetscOptionsInt("-degree", "Polynomial degree of tensor product basis",
                         NULL, degree, &degree, NULL); CHKERRQ(ierr);
  q_extra = bp_options[bp_choice].q_extra;
  ierr = PetscOptionsInt("-q_extra", "Number of extra quadrature points",
                         NULL, q_extra, &q_extra, NULL); CHKERRQ(ierr);
  ierr = PetscOptionsString("-ceed", "CEED resource specifier",
                            NULL, ceed_resource, ceed_resource,
                            sizeof(ceed_resource), NULL); CHKERRQ(ierr);
  read_mesh = PETSC_FALSE;
  ierr = PetscOptionsString("-mesh", "Read mesh from file", NULL,
                            filename, filename, sizeof(filename), &read_mesh);
  CHKERRQ(ierr);
  simplex = PETSC_FALSE;
  ierr = PetscOptionsBool("-simplex", "Use simplices, or tensor product cells",
                          NULL, simplex, &simplex, NULL); CHKERRQ(ierr);
  ierr = PetscOptionsEnd(); CHKERRQ(ierr);

  // Setup DM
  if (read_mesh) {
    ierr = DMPlexCreateFromFile(PETSC_COMM_WORLD, filename, NULL, PETSC_TRUE,
                                &dm);
    CHKERRQ(ierr);
  } else {
    // Create the mesh as a 0-refined sphere. This will create a cubic surface, not a box
    ierr = DMPlexCreateSphereMesh(PETSC_COMM_WORLD, topo_dim, simplex, 1., &dm);
    CHKERRQ(ierr);
    // Set the object name
    ierr = PetscObjectSetName((PetscObject)dm, "Sphere"); CHKERRQ(ierr);
    // Distribute mesh over processes
    {
      DM dm_dist = NULL;
      PetscPartitioner part;

      ierr = DMPlexGetPartitioner(dm, &part); CHKERRQ(ierr);
      ierr = PetscPartitionerSetFromOptions(part); CHKERRQ(ierr);
      ierr = DMPlexDistribute(dm, 0, NULL, &dm_dist); CHKERRQ(ierr);
      if (dm_dist) {
        ierr = DMDestroy(&dm); CHKERRQ(ierr);
        dm  = dm_dist;
      }
    }
    // Refine DMPlex with uniform refinement using runtime option -dm_refine
    ierr = DMPlexSetRefinementUniform(dm, PETSC_TRUE); CHKERRQ(ierr);
    ierr = DMSetFromOptions(dm); CHKERRQ(ierr);
    ierr = ProjectToUnitSphere(dm); CHKERRQ(ierr);
    // View DMPlex via runtime option
    ierr = DMViewFromOptions(dm, NULL, "-dm_view"); CHKERRQ(ierr);
  }

  // Create DM
  ierr = SetupDMByDegree(dm, degree, num_comp_u, topo_dim, false,
                         (BCFunction)NULL);
  CHKERRQ(ierr);

  // Create vectors
  ierr = DMCreateGlobalVector(dm, &X); CHKERRQ(ierr);
  ierr = VecGetLocalSize(X, &l_size); CHKERRQ(ierr);
  ierr = VecGetSize(X, &g_size); CHKERRQ(ierr);
  ierr = DMCreateLocalVector(dm, &X_loc); CHKERRQ(ierr);
  ierr = VecGetSize(X_loc, &xl_size); CHKERRQ(ierr);
  ierr = VecDuplicate(X, &rhs); CHKERRQ(ierr);

  // Operator
  ierr = PetscMalloc1(1, &user_O); CHKERRQ(ierr);
  ierr = MatCreateShell(comm, l_size, l_size, g_size, g_size,
                        user_O, &mat_O); CHKERRQ(ierr);
  ierr = MatShellSetOperation(mat_O, MATOP_MULT,
                              (void(*)(void))MatMult_Ceed); CHKERRQ(ierr);

  // Set up libCEED
  CeedInit(ceed_resource, &ceed);
  CeedMemType mem_type_backend;
  CeedGetPreferredMemType(ceed, &mem_type_backend);

  ierr = DMGetVecType(dm, &vec_type); CHKERRQ(ierr);
  if (!vec_type) { // Not yet set by user -dm_vec_type
    switch (mem_type_backend) {
    case CEED_MEM_HOST: vec_type = VECSTANDARD; break;
    case CEED_MEM_DEVICE: {
      const char *resolved;
      CeedGetResource(ceed, &resolved);
      if (strstr(resolved, "/gpu/cuda")) vec_type = VECCUDA;
      else if (strstr(resolved, "/gpu/hip/occa"))
        vec_type = VECSTANDARD; // https://github.com/CEED/libCEED/issues/678
      else if (strstr(resolved, "/gpu/hip")) vec_type = VECHIP;
      else vec_type = VECSTANDARD;
    }
    }
    ierr = DMSetVecType(dm, vec_type); CHKERRQ(ierr);
  }

  // Print summary
  if (!test_mode) {
    PetscInt P = degree + 1, Q = P + q_extra;
    const char *used_resource;
    CeedGetResource(ceed, &used_resource);
    ierr = PetscPrintf(comm,
                       "\n-- CEED Benchmark Problem %d on the Sphere -- libCEED + PETSc --\n"
                       "  libCEED:\n"
                       "    libCEED Backend                    : %s\n"
                       "    libCEED Backend MemType            : %s\n"
                       "  Mesh:\n"
                       "    Number of 1D Basis Nodes (p)       : %d\n"
                       "    Number of 1D Quadrature Points (q) : %d\n"
                       "    Global nodes                       : %D\n",
                       bp_choice+1, ceed_resource, CeedMemTypes[mem_type_backend], P, Q,
                       g_size/num_comp_u); CHKERRQ(ierr);
  }

  // Create RHS vector
  ierr = VecDuplicate(X_loc, &rhs_loc); CHKERRQ(ierr);
  ierr = VecZeroEntries(rhs_loc); CHKERRQ(ierr);
  ierr = VecGetArrayAndMemType(rhs_loc, &r, &mem_type); CHKERRQ(ierr);
  CeedVectorCreate(ceed, xl_size, &rhs_ceed);
  CeedVectorSetArray(rhs_ceed, MemTypeP2C(mem_type), CEED_USE_POINTER, r);

  // Setup libCEED's objects
  ierr = PetscMalloc1(1, &ceed_data); CHKERRQ(ierr);
  ierr = SetupLibceedByDegree(dm, ceed, degree, topo_dim, q_extra, num_comp_x,
                              num_comp_u, g_size, xl_size, bp_options[bp_choice],
                              ceed_data, true, rhs_ceed, &target); CHKERRQ(ierr);

  // Gather RHS
  CeedVectorTakeArray(rhs_ceed, MemTypeP2C(mem_type), NULL);
  ierr = VecRestoreArrayAndMemType(rhs_loc, &r); CHKERRQ(ierr);
  ierr = VecZeroEntries(rhs); CHKERRQ(ierr);
  ierr = DMLocalToGlobal(dm, rhs_loc, ADD_VALUES, rhs); CHKERRQ(ierr);
  CeedVectorDestroy(&rhs_ceed);

  // Create the error Q-function
  CeedQFunctionCreateInterior(ceed, 1, bp_options[bp_choice].error,
                              bp_options[bp_choice].error_loc, &qf_error);
  CeedQFunctionAddInput(qf_error, "u", num_comp_u, CEED_EVAL_INTERP);
  CeedQFunctionAddInput(qf_error, "true_soln", num_comp_u, CEED_EVAL_NONE);
  CeedQFunctionAddOutput(qf_error, "error", num_comp_u, CEED_EVAL_NONE);

  // Create the error operator
  CeedOperatorCreate(ceed, qf_error, NULL, NULL, &op_error);
  CeedOperatorSetField(op_error, "u", ceed_data->elem_restr_u,
                       ceed_data->basis_u, CEED_VECTOR_ACTIVE);
  CeedOperatorSetField(op_error, "true_soln", ceed_data->elem_restr_u_i,
                       CEED_BASIS_COLLOCATED, target);
  CeedOperatorSetField(op_error, "error", ceed_data->elem_restr_u_i,
                       CEED_BASIS_COLLOCATED, CEED_VECTOR_ACTIVE);

  // Set up Mat
  user_O->comm = comm;
  user_O->dm = dm;
  user_O->X_loc = X_loc;
  ierr = VecDuplicate(X_loc, &user_O->Y_loc); CHKERRQ(ierr);
  user_O->x_ceed = ceed_data->x_ceed;
  user_O->y_ceed = ceed_data->y_ceed;
  user_O->op = ceed_data->op_apply;
  user_O->ceed = ceed;

  // Setup solver
  ierr = KSPCreate(comm, &ksp); CHKERRQ(ierr);
  {
    PC pc;
    ierr = KSPGetPC(ksp, &pc); CHKERRQ(ierr);
    if (bp_choice == CEED_BP1 || bp_choice == CEED_BP2) {
      ierr = PCSetType(pc, PCJACOBI); CHKERRQ(ierr);
      ierr = PCJacobiSetType(pc, PC_JACOBI_ROWSUM); CHKERRQ(ierr);
    } else {
      ierr = PCSetType(pc, PCNONE); CHKERRQ(ierr);
      MatNullSpace nullspace;

      ierr = MatNullSpaceCreate(PETSC_COMM_WORLD, PETSC_TRUE, 0, 0, &nullspace);
      CHKERRQ(ierr);
      ierr = MatSetNullSpace(mat_O, nullspace); CHKERRQ(ierr);
      ierr = MatNullSpaceDestroy(&nullspace); CHKERRQ(ierr);
    }
    ierr = KSPSetType(ksp, KSPCG); CHKERRQ(ierr);
    ierr = KSPSetNormType(ksp, KSP_NORM_NATURAL); CHKERRQ(ierr);
    ierr = KSPSetTolerances(ksp, 1e-10, PETSC_DEFAULT, PETSC_DEFAULT,
                            PETSC_DEFAULT); CHKERRQ(ierr);
  }
  ierr = KSPSetFromOptions(ksp); CHKERRQ(ierr);
  ierr = KSPSetOperators(ksp, mat_O, mat_O); CHKERRQ(ierr);

  // First run, if benchmarking
  if (benchmark_mode) {
    ierr = KSPSetTolerances(ksp, 1e-10, PETSC_DEFAULT, PETSC_DEFAULT, 1);
    CHKERRQ(ierr);
    my_rt_start = MPI_Wtime();
    ierr = KSPSolve(ksp, rhs, X); CHKERRQ(ierr);
    my_rt = MPI_Wtime() - my_rt_start;
    ierr = MPI_Allreduce(MPI_IN_PLACE, &my_rt, 1, MPI_DOUBLE, MPI_MIN, comm);
    CHKERRQ(ierr);
    // Set maxits based on first iteration timing
    if (my_rt > 0.02) {
      ierr = KSPSetTolerances(ksp, 1e-10, PETSC_DEFAULT, PETSC_DEFAULT, 5);
      CHKERRQ(ierr);
    } else {
      ierr = KSPSetTolerances(ksp, 1e-10, PETSC_DEFAULT, PETSC_DEFAULT, 20);
      CHKERRQ(ierr);
    }
  }

  // Timed solve
  ierr = VecZeroEntries(X); CHKERRQ(ierr);
  ierr = PetscBarrier((PetscObject)ksp); CHKERRQ(ierr);

  // -- Performance logging
  ierr = PetscLogStageRegister("Solve Stage", &solve_stage); CHKERRQ(ierr);
  ierr = PetscLogStagePush(solve_stage); CHKERRQ(ierr);

  // -- Solve
  my_rt_start = MPI_Wtime();
  ierr = KSPSolve(ksp, rhs, X); CHKERRQ(ierr);
  my_rt = MPI_Wtime() - my_rt_start;

  // -- Performance logging
  ierr = PetscLogStagePop();

  // Output results
  {
    KSPType ksp_type;
    KSPConvergedReason reason;
    PetscReal rnorm;
    PetscInt its;
    ierr = KSPGetType(ksp, &ksp_type); CHKERRQ(ierr);
    ierr = KSPGetConvergedReason(ksp, &reason); CHKERRQ(ierr);
    ierr = KSPGetIterationNumber(ksp, &its); CHKERRQ(ierr);
    ierr = KSPGetResidualNorm(ksp, &rnorm); CHKERRQ(ierr);
    if (!test_mode || reason < 0 || rnorm > 1e-8) {
      ierr = PetscPrintf(comm,
                         "  KSP:\n"
                         "    KSP Type                           : %s\n"
                         "    KSP Convergence                    : %s\n"
                         "    Total KSP Iterations               : %D\n"
                         "    Final rnorm                        : %e\n",
                         ksp_type, KSPConvergedReasons[reason], its,
                         (double)rnorm); CHKERRQ(ierr);
    }
    if (!test_mode) {
      ierr = PetscPrintf(comm,"  Performance:\n"); CHKERRQ(ierr);
    }
    {
      PetscReal max_error;
      ierr = ComputeErrorMax(user_O, op_error, X, target, &max_error);
      CHKERRQ(ierr);
      PetscReal tol = 5e-4;
      if (!test_mode || max_error > tol) {
        ierr = MPI_Allreduce(&my_rt, &rt_min, 1, MPI_DOUBLE, MPI_MIN, comm);
        CHKERRQ(ierr);
        ierr = MPI_Allreduce(&my_rt, &rt_max, 1, MPI_DOUBLE, MPI_MAX, comm);
        CHKERRQ(ierr);
        ierr = PetscPrintf(comm,
                           "    Pointwise Error (max)              : %e\n"
                           "    CG Solve Time                      : %g (%g) sec\n",
                           (double)max_error, rt_max, rt_min); CHKERRQ(ierr);
      }
    }
    if (benchmark_mode && (!test_mode)) {
      ierr = PetscPrintf(comm,
                         "    DoFs/Sec in CG                     : %g (%g) million\n",
                         1e-6*g_size*its/rt_max, 1e-6*g_size*its/rt_min); CHKERRQ(ierr);
    }
  }

  // Output solution
  if (write_solution) {
    PetscViewer vtk_viewer_soln;

    ierr = PetscViewerCreate(comm, &vtk_viewer_soln); CHKERRQ(ierr);
    ierr = PetscViewerSetType(vtk_viewer_soln, PETSCVIEWERVTK); CHKERRQ(ierr);
    ierr = PetscViewerFileSetName(vtk_viewer_soln, "solution.vtu"); CHKERRQ(ierr);
    ierr = VecView(X, vtk_viewer_soln); CHKERRQ(ierr);
    ierr = PetscViewerDestroy(&vtk_viewer_soln); CHKERRQ(ierr);
  }

  // Cleanup
  ierr = VecDestroy(&X); CHKERRQ(ierr);
  ierr = VecDestroy(&X_loc); CHKERRQ(ierr);
  ierr = VecDestroy(&user_O->Y_loc); CHKERRQ(ierr);
  ierr = MatDestroy(&mat_O); CHKERRQ(ierr);
  ierr = PetscFree(user_O); CHKERRQ(ierr);
  ierr = CeedDataDestroy(0, ceed_data); CHKERRQ(ierr);
  ierr = DMDestroy(&dm); CHKERRQ(ierr);

  ierr = VecDestroy(&rhs); CHKERRQ(ierr);
  ierr = VecDestroy(&rhs_loc); CHKERRQ(ierr);
  ierr = KSPDestroy(&ksp); CHKERRQ(ierr);
  CeedVectorDestroy(&target);
  CeedQFunctionDestroy(&qf_error);
  CeedOperatorDestroy(&op_error);
  CeedDestroy(&ceed);
  return PetscFinalize();
}


/* Copyright (c) 2014, Giuseppe Argentieri <giuseppe.argentieri@ts.infn.it>

 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 */
/*
 *
 *
 *       Filename:  initial.h
 *
 *    Description:  Initial values
 *
 *        Version:  1.0
 *        Created:  16/05/2014 00:44:34
 *       Revision:  none
 *        License:  BSD
 *
 *         Author:  Giuseppe Argentieri (ga), giuseppe.argentieri@ts.infn.it
 *   Organization:  Università degli Studi di Trieste
 *
 *
 */

#include <gsl/gsl_const_mksa.h>

#define	BOLTZ	GSL_CONST_MKSA_BOLTZMANN	/* Boltzmann constant  */
#define HBAR	GSL_CONST_MKSA_PLANCKS_CONSTANT_HBAR /* hbar */

const double omega_c = 1000 ;			/* critical ohmic frequency */
const double alpha = 5e-3 ;			/* coupling strength */
const double Delta = 8.0e+9 ;			/* pumping amplitude (GHz) */

double T = .1 ;				/*
						 *  The real temperature is given by
						 *  temp = T*HBAR*Delta/BOLTZ
						 *
						 *  HBAR*Delta/BOLTZ = 0.061
						 *
						 */

const double D = 1 ;                            /* normalized delta */
double OMEGA = 2 ;				/* normalized pumping frequency */

const double gamma0 = 0.05 ;			/* energy hopping between sites */

const double t_end = 200 ;			/* time end */
const double STEP = .01 ;			/* time step */

const double R[] = { 1, 0, 0.5, -0.4 } ;   /* initial state: |z,-> */

/* const double R[] = { 1, 0, 0.5, -0.4 } ; */ /* initial state with neg. e.p. */
/* const double r[] = { 1, 0, 1, 0 } ;  	 initial state with pos. t.d. */


// @(#)root/matrix:$Id$
// Authors: Fons Rademakers, Eddy Offermann   Feb 2004

/*************************************************************************
 * Copyright (C) 1995-2000, Rene Brun and Fons Rademakers.               *
 * All rights reserved.                                                  *
 *                                                                       *
 * For the licensing terms see $ROOTSYS/LICENSE.                         *
 * For the list of contributors see $ROOTSYS/README/CREDITS.             *
 *************************************************************************/

#ifndef ROOT_TMatrixTSparse
#define ROOT_TMatrixTSparse

#include "TMatrixTBase.h"
#include "TMatrixTUtils.h"

#ifdef CBLAS
#include <vecLib/vBLAS.h>
//#include <cblas.h>
#endif

//////////////////////////////////////////////////////////////////////////
//                                                                      //
// TMatrixTSparse                                                       //
//                                                                      //
// Template class of a general sparse matrix in the Harwell-Boeing      //
// format                                                               //
//                                                                      //
//////////////////////////////////////////////////////////////////////////

template<class Element> class TMatrixT;

template<class Element> class TMatrixTSparse : public TMatrixTBase<Element> {

protected:

   Int_t   *fRowIndex;  //[fNrowIndex] row index
   Int_t   *fColIndex;  //[fNelems]    column index
   Element *fElements;  //[fNelems]

   void Allocate(Int_t nrows,Int_t ncols,Int_t row_lwb = 0,Int_t col_lwb = 0,
                 Int_t init = 0,Int_t nr_nonzeros = 0);

  // Elementary constructors
   void AMultB (const TMatrixTSparse<Element> &a,const TMatrixTSparse<Element> &b,Int_t constr=0) {
                const TMatrixTSparse<Element> bt(TMatrixTSparse::kTransposed,b); AMultBt(a,bt,constr); }
   void AMultB (const TMatrixTSparse<Element> &a,const TMatrixT<Element>       &b,Int_t constr=0) {
                const TMatrixTSparse<Element> bsp = b;
                const TMatrixTSparse<Element> bt(TMatrixTSparse::kTransposed,bsp); AMultBt(a,bt,constr); }
   void AMultB (const TMatrixT<Element>       &a,const TMatrixTSparse<Element> &b,Int_t constr=0) {
                const TMatrixTSparse<Element> bt(TMatrixTSparse::kTransposed,b); AMultBt(a,bt,constr); }

   void AMultBt(const TMatrixTSparse<Element> &a,const TMatrixTSparse<Element> &b,Int_t constr=0);
   void AMultBt(const TMatrixTSparse<Element> &a,const TMatrixT<Element>       &b,Int_t constr=0);
   void AMultBt(const TMatrixT<Element>       &a,const TMatrixTSparse<Element> &b,Int_t constr=0);

   void APlusB (const TMatrixTSparse<Element> &a,const TMatrixTSparse<Element> &b,Int_t constr=0);
   void APlusB (const TMatrixTSparse<Element> &a,const TMatrixT<Element>       &b,Int_t constr=0);
   void APlusB (const TMatrixT<Element>       &a,const TMatrixTSparse<Element> &b,Int_t constr=0) { APlusB(b,a,constr); }

   void AMinusB(const TMatrixTSparse<Element> &a,const TMatrixTSparse<Element> &b,Int_t constr=0);
   void AMinusB(const TMatrixTSparse<Element> &a,const TMatrixT<Element>       &b,Int_t constr=0);
   void AMinusB(const TMatrixT<Element>       &a,const TMatrixTSparse<Element> &b,Int_t constr=0);

public:

   enum EMatrixCreatorsOp1 { kZero,kUnit,kTransposed,kAtA };
   enum EMatrixCreatorsOp2 { kMult,kMultTranspose,kPlus,kMinus };

   TMatrixTSparse() { fElements = 0; fRowIndex = 0; fColIndex = 0; }
   TMatrixTSparse(Int_t nrows,Int_t ncols);
   TMatrixTSparse(Int_t row_lwb,Int_t row_upb,Int_t col_lwb,Int_t col_upb);
   TMatrixTSparse(Int_t row_lwb,Int_t row_upb,Int_t col_lwb,Int_t col_upb,Int_t nr_nonzeros,
                  Int_t *row, Int_t *col,Element *data);
   TMatrixTSparse(const TMatrixTSparse<Element> &another);
   TMatrixTSparse(const TMatrixT<Element>       &another);

   TMatrixTSparse(EMatrixCreatorsOp1 op,const TMatrixTSparse<Element> &prototype);
   TMatrixTSparse(const TMatrixTSparse<Element> &a,EMatrixCreatorsOp2 op,const TMatrixTSparse<Element> &b);
   TMatrixTSparse(const TMatrixTSparse<Element> &a,EMatrixCreatorsOp2 op,const TMatrixT      <Element> &b);
   TMatrixTSparse(const TMatrixT      <Element> &a,EMatrixCreatorsOp2 op,const TMatrixTSparse<Element> &b);

   virtual ~TMatrixTSparse() { Clear(); }

   virtual const Element *GetMatrixArray  () const;
   virtual       Element *GetMatrixArray  ();
   virtual const Int_t    *GetRowIndexArray() const;
   virtual       Int_t    *GetRowIndexArray();
   virtual const Int_t    *GetColIndexArray() const;
   virtual       Int_t    *GetColIndexArray();

   virtual TMatrixTBase<Element>   &SetRowIndexArray(Int_t *data) { memmove(fRowIndex,data,(this->fNrows+1)*sizeof(Int_t)); return *this; }
   virtual TMatrixTBase<Element>   &SetColIndexArray(Int_t *data) { memmove(fColIndex,data,this->fNelems*sizeof(Int_t)); return *this; }

           TMatrixTSparse<Element> &SetSparseIndex  (Int_t nelem_new);
           TMatrixTSparse<Element> &SetSparseIndex  (const TMatrixTBase<Element> &another);
           TMatrixTSparse<Element> &SetSparseIndexAB(const TMatrixTSparse<Element> &a,const TMatrixTSparse<Element> &b);
           TMatrixTSparse<Element> &SetSparseIndexAB(const TMatrixT      <Element> &a,const TMatrixTSparse<Element> &b);
           TMatrixTSparse<Element> &SetSparseIndexAB(const TMatrixTSparse<Element> &a,const TMatrixT      <Element> &b)
                                              { return SetSparseIndexAB(b,a); }

   virtual void                     GetMatrix2Array (Element *data,Option_t * /*option*/ ="") const;
   virtual TMatrixTBase<Element>   &SetMatrixArray  (const Element *data,Option_t * /*option*/="")
                                                    { memcpy(fElements,data,this->fNelems*sizeof(Element)); return *this; }
   virtual TMatrixTBase<Element>   &SetMatrixArray  (Int_t nr_nonzeros,Int_t *irow,Int_t *icol,Element *data);
   virtual TMatrixTBase<Element>   &InsertRow       (Int_t row,Int_t col,const Element *v,Int_t n=-1);
   virtual void                     ExtractRow      (Int_t row,Int_t col,      Element *v,Int_t n=-1) const;

   virtual TMatrixTBase<Element>   &ResizeTo(Int_t nrows,Int_t ncols,Int_t nr_nonzeros=-1);
   virtual TMatrixTBase<Element>   &ResizeTo(Int_t row_lwb,Int_t row_upb,Int_t col_lwb,Int_t col_upb,Int_t nr_nonzeros=-1);
   inline  TMatrixTBase<Element>   &ResizeTo(const TMatrixTSparse<Element> &m) {return ResizeTo(m.GetRowLwb(),m.GetRowUpb(),m.GetColLwb(),
                                                                                m.GetColUpb(),m.GetNoElements()); }

   virtual void Clear(Option_t * /*option*/ ="") { if (this->fIsOwner) {
                                                      if (fElements) { delete [] fElements; fElements = 0; }
                                                      if (fRowIndex) { delete [] fRowIndex; fRowIndex = 0; }
                                                      if (fColIndex) { delete [] fColIndex; fColIndex = 0; }
                                                   }
                                                   this->fNelems    = 0;
                                                   this->fNrowIndex = 0;
                                                 }

           TMatrixTSparse<Element> &Use   (Int_t row_lwb,Int_t row_upb,Int_t col_lwb,Int_t col_upb,Int_t nr_nonzeros,
                                           Int_t *pRowIndex,Int_t *pColIndex,Element *pData);
   const   TMatrixTSparse<Element> &Use   (Int_t row_lwb,Int_t row_upb,Int_t col_lwb,Int_t col_upb,Int_t nr_nonzeros,
                                           const Int_t *pRowIndex,const Int_t *pColIndex,const Element *pData) const
                                            { return (const TMatrixTSparse<Element>&)
                                                     ((const_cast<TMatrixTSparse<Element> *>(this))->Use(row_lwb,row_upb,col_lwb,col_upb,nr_nonzeros,
                                                                                const_cast<Int_t *>(pRowIndex),
                                                                                const_cast<Int_t *>(pColIndex),
                                                                                const_cast<Element *>(pData))); }
           TMatrixTSparse<Element> &Use   (Int_t nrows,Int_t ncols,Int_t nr_nonzeros,
                                           Int_t *pRowIndex,Int_t *pColIndex,Element *pData);
   const   TMatrixTSparse<Element> &Use   (Int_t nrows,Int_t ncols,Int_t nr_nonzeros,
                                           const Int_t *pRowIndex,const Int_t *pColIndex,const Element *pData) const;
           TMatrixTSparse<Element> &Use   (TMatrixTSparse<Element> &a);
   const   TMatrixTSparse<Element> &Use   (const TMatrixTSparse<Element> &a) const;

   virtual TMatrixTBase<Element>   &GetSub(Int_t row_lwb,Int_t row_upb,Int_t col_lwb,Int_t col_upb,
                                            TMatrixTBase<Element> &target,Option_t *option="S") const;
           TMatrixTSparse<Element>  GetSub(Int_t row_lwb,Int_t row_upb,Int_t col_lwb,Int_t col_upb,Option_t *option="S") const;
   virtual TMatrixTBase<Element>   &SetSub(Int_t row_lwb,Int_t col_lwb,const TMatrixTBase<Element> &source);

   virtual Bool_t IsSymmetric() const { return (*this == TMatrixTSparse<Element>(kTransposed,*this)); }
   TMatrixTSparse<Element> &Transpose (const TMatrixTSparse<Element> &source);
   inline TMatrixTSparse<Element> &T () { return this->Transpose(*this); }

   inline void Mult(const TMatrixTSparse<Element> &a,const TMatrixTSparse<Element> &b) { AMultB(a,b,0); }

   virtual TMatrixTBase<Element> &Zero       ();
   virtual TMatrixTBase<Element> &UnitMatrix ();

   virtual Element RowNorm () const;
   virtual Element ColNorm () const;
   virtual Int_t   NonZeros() const { return this->fNelems; }

   virtual TMatrixTBase<Element> &NormByDiag(const TVectorT<Element> &/*v*/,Option_t * /*option*/)
                                              { MayNotUse("NormByDiag"); return *this; }

   // Either access a_ij as a(i,j)
   Element  operator()(Int_t rown,Int_t coln) const;
   Element &operator()(Int_t rown,Int_t coln);

   // or as a[i][j]
   inline const TMatrixTSparseRow_const<Element> operator[](Int_t rown) const { return TMatrixTSparseRow_const<Element>(*this,rown); }
   inline       TMatrixTSparseRow      <Element> operator[](Int_t rown)       { return TMatrixTSparseRow      <Element>(*this,rown); }

   TMatrixTSparse<Element> &operator=(const TMatrixT<Element>       &source);
   TMatrixTSparse<Element> &operator=(const TMatrixTSparse<Element> &source);

   TMatrixTSparse<Element> &operator= (Element val);
   TMatrixTSparse<Element> &operator-=(Element val);
   TMatrixTSparse<Element> &operator+=(Element val);
   TMatrixTSparse<Element> &operator*=(Element val);

   TMatrixTSparse<Element> &operator+=(const TMatrixTSparse<Element> &source) { TMatrixTSparse<Element> tmp(*this); Clear();
                                                                                if (this == &source) APlusB (tmp,tmp,1);
                                                                                else                 APlusB (tmp,source,1);
                                                                                return *this; }
   TMatrixTSparse<Element> &operator+=(const TMatrixT<Element>       &source) { TMatrixTSparse<Element> tmp(*this); Clear();
                                                                                APlusB(tmp,source,1); return *this; }
   TMatrixTSparse<Element> &operator-=(const TMatrixTSparse<Element> &source) { TMatrixTSparse<Element> tmp(*this); Clear();
                                                                                if (this == &source) AMinusB (tmp,tmp,1);
                                                                                else                 AMinusB(tmp,source,1);
                                                                                return *this; }
   TMatrixTSparse<Element> &operator-=(const TMatrixT<Element>       &source) { TMatrixTSparse<Element> tmp(*this); Clear();
                                                                                AMinusB(tmp,source,1); return *this; }
   TMatrixTSparse<Element> &operator*=(const TMatrixTSparse<Element> &source) { TMatrixTSparse<Element> tmp(*this); Clear();
                                                                                if (this == &source) AMultB (tmp,tmp,1);
                                                                                else                 AMultB (tmp,source,1);
                                                                                return *this; }
   TMatrixTSparse<Element> &operator*=(const TMatrixT<Element>       &source) { TMatrixTSparse<Element> tmp(*this); Clear();
                                                                                AMultB(tmp,source,1);
                                                                                return *this; }

   virtual TMatrixTBase  <Element> &Randomize  (Element alpha,Element beta,Double_t &seed);
   virtual TMatrixTSparse<Element> &RandomizePD(Element alpha,Element beta,Double_t &seed);

   ClassDef(TMatrixTSparse,3) // Template of Sparse Matrix class
};

#ifndef __CINT__
// When building with -fmodules, it instantiates all pending instantiations,
// instead of delaying them until the end of the translation unit.
// We 'got away with' probably because the use and the definition of the
// explicit specialization do not occur in the same TU.
//
// In case we are building with -fmodules, we need to forward declare the
// specialization in order to compile the dictionary G__Matrix.cxx.
template <> TClass *TMatrixTSparse<double>::Class();
#endif // __CINT__

template <class Element> inline const Element *TMatrixTSparse<Element>::GetMatrixArray  () const { return fElements; }
template <class Element> inline       Element *TMatrixTSparse<Element>::GetMatrixArray  ()       { return fElements; }
template <class Element> inline const Int_t   *TMatrixTSparse<Element>::GetRowIndexArray() const { return fRowIndex; }
template <class Element> inline       Int_t   *TMatrixTSparse<Element>::GetRowIndexArray()       { return fRowIndex; }
template <class Element> inline const Int_t   *TMatrixTSparse<Element>::GetColIndexArray() const { return fColIndex; }
template <class Element> inline       Int_t   *TMatrixTSparse<Element>::GetColIndexArray()       { return fColIndex; }

template <class Element>
inline       TMatrixTSparse<Element> &TMatrixTSparse<Element>::Use   (Int_t nrows,Int_t ncols,Int_t nr_nonzeros,
                                                                      Int_t *pRowIndex,Int_t *pColIndex,Element *pData)
                                                                        { return Use(0,nrows-1,0,ncols-1,nr_nonzeros,pRowIndex,pColIndex,pData); }
template <class Element>
inline const TMatrixTSparse<Element> &TMatrixTSparse<Element>::Use   (Int_t nrows,Int_t ncols,Int_t nr_nonzeros,
                                                                      const Int_t *pRowIndex,const Int_t *pColIndex,const Element *pData) const
                                                                        { return Use(0,nrows-1,0,ncols-1,nr_nonzeros,pRowIndex,pColIndex,pData); }
template <class Element>
inline       TMatrixTSparse<Element> &TMatrixTSparse<Element>::Use   (TMatrixTSparse<Element> &a)
                                                                        { R__ASSERT(a.IsValid());
                                                                           return Use(a.GetRowLwb(),a.GetRowUpb(),a.GetColLwb(),a.GetColUpb(),
                                                                                a.GetNoElements(),a.GetRowIndexArray(),
                                                                                a.GetColIndexArray(),a.GetMatrixArray()); }
template <class Element>
inline const TMatrixTSparse<Element> &TMatrixTSparse<Element>::Use   (const TMatrixTSparse<Element> &a) const
                                                                        { R__ASSERT(a.IsValid());
                                                                           return Use(a.GetRowLwb(),a.GetRowUpb(),a.GetColLwb(),a.GetColUpb(),
                                                                                a.GetNoElements(),a.GetRowIndexArray(),
                                                                                a.GetColIndexArray(),a.GetMatrixArray()); }

template <class Element>
inline       TMatrixTSparse<Element>  TMatrixTSparse<Element>::GetSub(Int_t row_lwb,Int_t row_upb,Int_t col_lwb,Int_t col_upb,
                                                                      Option_t *option) const
                                                                        {
                                                                          TMatrixTSparse<Element> tmp;
                                                                          this->GetSub(row_lwb,row_upb,col_lwb,col_upb,tmp,option);
                                                                          return tmp;
                                                                        }

template <class Element> TMatrixTSparse<Element>  operator+ (const TMatrixTSparse<Element> &source1,const TMatrixTSparse<Element> &source2);
template <class Element> TMatrixTSparse<Element>  operator+ (const TMatrixTSparse<Element> &source1,const TMatrixT<Element>       &source2);
template <class Element> TMatrixTSparse<Element>  operator+ (const TMatrixT<Element>       &source1,const TMatrixTSparse<Element> &source2);
template <class Element> TMatrixTSparse<Element>  operator+ (const TMatrixTSparse<Element> &source ,      Element                  val    );
template <class Element> TMatrixTSparse<Element>  operator+ (      Element                  val    ,const TMatrixTSparse<Element> &source );
template <class Element> TMatrixTSparse<Element>  operator- (const TMatrixTSparse<Element> &source1,const TMatrixTSparse<Element> &source2);
template <class Element> TMatrixTSparse<Element>  operator- (const TMatrixTSparse<Element> &source1,const TMatrixT<Element>       &source2);
template <class Element> TMatrixTSparse<Element>  operator- (const TMatrixT<Element>       &source1,const TMatrixTSparse<Element> &source2);
template <class Element> TMatrixTSparse<Element>  operator- (const TMatrixTSparse<Element> &source ,      Element                  val    );
template <class Element> TMatrixTSparse<Element>  operator- (      Element                  val    ,const TMatrixTSparse<Element> &source );
template <class Element> TMatrixTSparse<Element>  operator* (const TMatrixTSparse<Element> &source1,const TMatrixTSparse<Element> &source2);
template <class Element> TMatrixTSparse<Element>  operator* (const TMatrixTSparse<Element> &source1,const TMatrixT<Element>       &source2);
template <class Element> TMatrixTSparse<Element>  operator* (const TMatrixT<Element>       &source1,const TMatrixTSparse<Element> &source2);
template <class Element> TMatrixTSparse<Element>  operator* (      Element                  val    ,const TMatrixTSparse<Element> &source );
template <class Element> TMatrixTSparse<Element>  operator* (const TMatrixTSparse<Element> &source,       Element                  val    );

template <class Element> TMatrixTSparse<Element> &Add        (TMatrixTSparse<Element> &target,      Element                   scalar,
                                                              const TMatrixTSparse<Element> &source);
template <class Element> TMatrixTSparse<Element> &ElementMult(TMatrixTSparse<Element> &target,const TMatrixTSparse<Element>  &source);
template <class Element> TMatrixTSparse<Element> &ElementDiv (TMatrixTSparse<Element> &target,const TMatrixTSparse<Element>  &source);

template <class Element> Bool_t AreCompatible(const TMatrixTSparse<Element> &m1,const TMatrixTSparse<Element> &m2,Int_t verbose=0);

#endif


/* specfunc/sinint.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 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.
 */

/* Author:  G. Jungman */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_trig.h>
#include <gsl/gsl_sf_expint.h>

#include "error.h"

#include "chebyshev.h"
#include "cheb_eval.c"

/*-*-*-*-*-*-*-*-*-*-*-* Private Section *-*-*-*-*-*-*-*-*-*-*-*/

/* based on SLATEC r9sifg.f, W. Fullerton */

/*
 series for f1   on the interval  2.00000e-02 to  6.25000e-02
                                        with weighted error   2.82e-17
                                         log weighted error  16.55
                               significant figures required  15.36
                                    decimal places required  17.20
*/
static double f1_data[20] = {
   -0.1191081969051363610,
   -0.0247823144996236248,
    0.0011910281453357821,
   -0.0000927027714388562,
    0.0000093373141568271,
   -0.0000011058287820557,
    0.0000001464772071460,
   -0.0000000210694496288,
    0.0000000032293492367,
   -0.0000000005206529618,
    0.0000000000874878885,
   -0.0000000000152176187,
    0.0000000000027257192,
   -0.0000000000005007053,
    0.0000000000000940241,
   -0.0000000000000180014,
    0.0000000000000035063,
   -0.0000000000000006935,
    0.0000000000000001391,
   -0.0000000000000000282
};
static cheb_series f1_cs = {
  f1_data,
  19,
  -1, 1,
  10
};

/*

 series for f2   on the interval  0.00000e+00 to  2.00000e-02
                                        with weighted error   4.32e-17
                                         log weighted error  16.36
                               significant figures required  14.75
                                    decimal places required  17.10
*/
static double f2_data[29] = {
   -0.0348409253897013234,
   -0.0166842205677959686,
    0.0006752901241237738,
   -0.0000535066622544701,
    0.0000062693421779007,
   -0.0000009526638801991,
    0.0000001745629224251,
   -0.0000000368795403065,
    0.0000000087202677705,
   -0.0000000022601970392,
    0.0000000006324624977,
   -0.0000000001888911889,
    0.0000000000596774674,
   -0.0000000000198044313,
    0.0000000000068641396,
   -0.0000000000024731020,
    0.0000000000009226360,
   -0.0000000000003552364,
    0.0000000000001407606,
   -0.0000000000000572623,
    0.0000000000000238654,
   -0.0000000000000101714,
    0.0000000000000044259,
   -0.0000000000000019634,
    0.0000000000000008868,
   -0.0000000000000004074,
    0.0000000000000001901,
   -0.0000000000000000900,
    0.0000000000000000432
};
static cheb_series f2_cs = {
  f2_data,
  28,
  -1, 1,
  14
};

/*

 series for g1   on the interval  2.00000e-02 to  6.25000e-02
                                        with weighted error   5.48e-17
                                         log weighted error  16.26
                               significant figures required  15.47
                                    decimal places required  16.92
*/
static double g1_data[21] = {
   -0.3040578798253495954,
   -0.0566890984597120588,
    0.0039046158173275644,
   -0.0003746075959202261,
    0.0000435431556559844,
   -0.0000057417294453025,
    0.0000008282552104503,
   -0.0000001278245892595,
    0.0000000207978352949,
   -0.0000000035313205922,
    0.0000000006210824236,
   -0.0000000001125215474,
    0.0000000000209088918,
   -0.0000000000039715832,
    0.0000000000007690431,
   -0.0000000000001514697,
    0.0000000000000302892,
   -0.0000000000000061400,
    0.0000000000000012601,
   -0.0000000000000002615,
    0.0000000000000000548
};
static cheb_series g1_cs = {
  g1_data,
  20,
  -1, 1,
  13
};

/*

 series for g2   on the interval  0.00000e+00 to  2.00000e-02
                                        with weighted error   5.01e-17
                                         log weighted error  16.30
                               significant figures required  15.12
                                    decimal places required  17.07
*/
static double g2_data[34] = {
   -0.0967329367532432218,
   -0.0452077907957459871,
    0.0028190005352706523,
   -0.0002899167740759160,
    0.0000407444664601121,
   -0.0000071056382192354,
    0.0000014534723163019,
   -0.0000003364116512503,
    0.0000000859774367886,
   -0.0000000238437656302,
    0.0000000070831906340,
   -0.0000000022318068154,
    0.0000000007401087359,
   -0.0000000002567171162,
    0.0000000000926707021,
   -0.0000000000346693311,
    0.0000000000133950573,
   -0.0000000000053290754,
    0.0000000000021775312,
   -0.0000000000009118621,
    0.0000000000003905864,
   -0.0000000000001708459,
    0.0000000000000762015,
   -0.0000000000000346151,
    0.0000000000000159996,
   -0.0000000000000075213,
    0.0000000000000035970,
   -0.0000000000000017530,
    0.0000000000000008738,
   -0.0000000000000004487,
    0.0000000000000002397,
   -0.0000000000000001347,
    0.0000000000000000801,
   -0.0000000000000000501
};
static cheb_series g2_cs = {
  g2_data,
  33,
  -1, 1,
  20
};

/* x >= 4.0 */
static void fg_asymp(const double x, gsl_sf_result * f, gsl_sf_result * g)
{
  /*
      xbig = sqrt (1.0/r1mach(3))
      xmaxf = exp (amin1(-alog(r1mach(1)), alog(r1mach(2))) - 0.01)
      xmaxg = 1.0/sqrt(r1mach(1))
      xbnd = sqrt(50.0)
  */
  const double xbig  = 1.0/GSL_SQRT_DBL_EPSILON;
  const double xmaxf = 1.0/GSL_DBL_MIN;
  const double xmaxg = 1.0/GSL_SQRT_DBL_MIN;
  const double xbnd  = 7.07106781187;

  const double x2 = x*x;

  if(x <= xbnd) {
    gsl_sf_result result_c1;
    gsl_sf_result result_c2;
    cheb_eval_e(&f1_cs, (1.0/x2-0.04125)/0.02125, &result_c1);
    cheb_eval_e(&g1_cs, (1.0/x2-0.04125)/0.02125, &result_c2);
    f->val = (1.0 + result_c1.val)/x;
    g->val = (1.0 + result_c2.val)/x2;
    f->err = result_c1.err/x  + 2.0 * GSL_DBL_EPSILON * fabs(f->val);
    g->err = result_c2.err/x2 + 2.0 * GSL_DBL_EPSILON * fabs(g->val);
  }
  else if(x <= xbig) {
    gsl_sf_result result_c1;
    gsl_sf_result result_c2;
    cheb_eval_e(&f2_cs, 100.0/x2-1.0, &result_c1);
    cheb_eval_e(&g2_cs, 100.0/x2-1.0, &result_c2);
    f->val = (1.0 + result_c1.val)/x;
    g->val = (1.0 + result_c2.val)/x2;
    f->err = result_c1.err/x  + 2.0 * GSL_DBL_EPSILON * fabs(f->val);
    g->err = result_c2.err/x2 + 2.0 * GSL_DBL_EPSILON * fabs(g->val);
  }
  else {
    f->val = (x < xmaxf ? 1.0/x  : 0.0);
    g->val = (x < xmaxg ? 1.0/x2 : 0.0);
    f->err = 2.0 * GSL_DBL_EPSILON * fabs(f->val);
    g->err = 2.0 * GSL_DBL_EPSILON * fabs(g->val);
  }

  return;
}

/* based on SLATEC si.f, W. Fullerton

 series for si   on the interval  0.00000e+00 to  1.60000e+01
                                        with weighted error   1.22e-17
                                         log weighted error  16.91
                               significant figures required  16.37
                                    decimal places required  17.45
*/

static double si_data[12] = {
  -0.1315646598184841929,
  -0.2776578526973601892,
   0.0354414054866659180,
  -0.0025631631447933978,
   0.0001162365390497009,
  -0.0000035904327241606,
   0.0000000802342123706,
  -0.0000000013562997693,
   0.0000000000179440722,
  -0.0000000000001908387,
   0.0000000000000016670,
  -0.0000000000000000122
};

static cheb_series si_cs = {
  si_data,
  11,
  -1, 1,
  9
};

/*
 series for ci   on the interval  0.00000e+00 to  1.60000e+01
                                        with weighted error   1.94e-18
                                         log weighted error  17.71
                               significant figures required  17.74
                                    decimal places required  18.27
*/
static double ci_data[13] = {
   -0.34004281856055363156,
   -1.03302166401177456807,
    0.19388222659917082877,
   -0.01918260436019865894,
    0.00110789252584784967,
   -0.00004157234558247209,
    0.00000109278524300229,
   -0.00000002123285954183,
    0.00000000031733482164,
   -0.00000000000376141548,
    0.00000000000003622653,
   -0.00000000000000028912,
    0.00000000000000000194
};
static cheb_series ci_cs = {
  ci_data,
  12,
  -1, 1,
  9
};

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

int gsl_sf_Si_e(const double x, gsl_sf_result * result)
{
  double ax = fabs(x);

  /* CHECK_POINTER(result) */

  if(ax < GSL_SQRT_DBL_EPSILON) {
    result->val = x;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else if(ax <= 4.0) {
    gsl_sf_result result_c;
    cheb_eval_e(&si_cs, (x*x-8.0)*0.125, &result_c);
    result->val  =  x * (0.75 + result_c.val);
    result->err  = ax * result_c.err;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    /* Note there is no loss of precision
     * here bcause of the leading constant.
     */
    gsl_sf_result f;
    gsl_sf_result g;
    fg_asymp(ax, &f, &g);
    result->val  = 0.5 * M_PI - f.val*cos(ax) - g.val*sin(ax);
    result->err  = f.err + g.err;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    if(x < 0.0) result->val = -result->val;
    return GSL_SUCCESS;
  }
}

int gsl_sf_Ci_e(const double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(x <= 0.0) {
    DOMAIN_ERROR(result);
  }
  else if(x <= 4.0) {
    const double lx = log(x);
    const double y  = (x*x-8.0)*0.125;
    gsl_sf_result result_c;
    cheb_eval_e(&ci_cs, y, &result_c);
    result->val  = lx - 0.5 + result_c.val;
    result->err  = 2.0 * GSL_DBL_EPSILON * (fabs(lx) + 0.5) + result_c.err;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    gsl_sf_result sin_result;
    gsl_sf_result cos_result;
    int stat_sin = gsl_sf_sin_e(x, &sin_result);
    int stat_cos = gsl_sf_cos_e(x, &cos_result);
    gsl_sf_result f;
    gsl_sf_result g;
    fg_asymp(x, &f, &g);
    result->val  = f.val*sin_result.val - g.val*cos_result.val;
    result->err  = fabs(f.err*sin_result.val);
    result->err += fabs(g.err*cos_result.val);
    result->err += fabs(f.val*sin_result.err);
    result->err += fabs(g.val*cos_result.err);
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_ERROR_SELECT_2(stat_sin, stat_cos);
  }
}

/*-*-*-*-*-*-*-*-*-* Functions w/ Natural Prototypes *-*-*-*-*-*-*-*-*-*-*/

#include "eval.h"

double gsl_sf_Si(const double x)
{
  EVAL_RESULT(gsl_sf_Si_e(x, &result));
}

double gsl_sf_Ci(const double x)
{
  EVAL_RESULT(gsl_sf_Ci_e(x, &result));
}


/*
 *  gsl_binomial_randomdev.h
 *
 *  This file is part of NEST.
 *
 *  Copyright (C) 2004 The NEST Initiative
 *
 *  NEST 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.
 *
 *  NEST 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 NEST.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

// Generated includes:
#include "config.h"

#ifndef GSL_BINOMIAL_RANDOMDEV_H
#define GSL_BINOMIAL_RANDOMDEV_H

// Includes from libnestutil:
#include "lockptr.h"

// Includes from librandom:
#include "gslrandomgen.h"
#include "randomdev.h"
#include "randomgen.h"

// Includes from sli:
#include "dictdatum.h"

#ifdef HAVE_GSL

// External includes:
#include <gsl/gsl_randist.h>
#include <gsl/gsl_rng.h>

/*BeginDocumentation
Name: rdevdict::gsl_binomial - GSL binomial random deviate generator
Description:
This function returns a random integer from the binomial distribution,
the number of successes in n independent trials with probability
p. The probability distribution for binomial variates is,

   p(k) = (n! / k!(n-k)!) p^k (1-p)^(n-k)  , 0<=k<=n, n>0

Please note that the RNG used to initialize gsl_binomial has to be
from the GSL (prefixed gsl_ in rngdict)

Parameters:
   p - probability of success in a single trial (double)
   n - number of trials (positive integer)

SeeAlso: CreateRDV, RandomArray, rdevdict
Author: Jochen Martin Eppler
*/

namespace librandom
{

/**
 Class GSL_BinomialRandomDev

 Generates an RNG which returns Binomial(k;p;n)
 distributed random numbers out of an RNG which returns
 binomially distributed random numbers:

    p(k) = (n! / k!(n-k)!) p^k (1-p)^(n-k)  , 0<=k<=n, n<0

 Arguments:
  - pointer to an RNG
  - parameter p (optional, default = 0.5)
  - parameter n (optional, default = 1)

 @see
 http://www.gnu.org/software/gsl/manual/html_node/The-Binomial-Distribution.html
 @ingroup RandomDeviateGenerators
*/

class GSL_BinomialRandomDev : public RandomDev
{
public:
  // accept only lockPTRs for initialization,
  // otherwise creation of a lock ptr would
  // occur as side effect---might be unhealthy
  GSL_BinomialRandomDev( RngPtr, double p_s = 0.5, unsigned int n_s = 1 );
  GSL_BinomialRandomDev( double p_s = 0.5, unsigned int n_s = 1 );

  /**
   * set parameters for p and n
   * @parameters
   * p - success probability for single trial
   * n - number of trials
   */
  void set_p_n( double, unsigned int );
  void set_p( double );       //!<set p
  void set_n( unsigned int ); //!<set n

  /**
   * Import sets of overloaded virtual functions.
   * We need to explicitly include sets of overloaded
   * virtual functions into the current scope.
   * According to the SUN C++ FAQ, this is the correct
   * way of doing things, although all other compilers
   * happily live without.
   */
  using RandomDev::operator();
  using RandomDev::ldev;

  long ldev();               //!< draw integer
  long ldev( RngPtr ) const; //!< draw integer, threaded
  bool
  has_ldev() const
  {
    return true;
  }

  double operator()( RngPtr ) const; //!< return as double, threaded

  //! set distribution parameters from SLI dict
  void set_status( const DictionaryDatum& );

  //! get distribution parameters from SLI dict
  void get_status( DictionaryDatum& ) const;

private:
  double p_;       //!<probability p of binomial distribution
  unsigned int n_; //!<parameter n in binomial distribution

  gsl_rng* rng_;
};

inline double GSL_BinomialRandomDev::operator()( RngPtr rthrd ) const
{
  return static_cast< double >( ldev( rthrd ) );
}
}

#endif

#endif


/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
 *                      gerkey@usc.edu    kaspers@robotics.usc.edu
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License as published by the Free Software Foundation; either
 *  version 2.1 of the License, or (at your option) any later version.
 *
 *  This library 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
 *  Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */
/**************************************************************************
 * Desc: Useful pdf functions
 * Author: Andrew Howard
 * Date: 10 Dec 2002
 * CVS: $Id: pf_pdf.h 6345 2008-04-17 01:36:39Z gerkey $
 *************************************************************************/

#ifndef PF_PDF_H
#define PF_PDF_H

#include "nav2d_localizer/pf_vector.h"

//#include <gsl/gsl_rng.h>
//#include <gsl/gsl_randist.h>

#ifdef __cplusplus
extern "C" {
#endif

/**************************************************************************
 * Gaussian
 *************************************************************************/

// Gaussian PDF info
typedef struct
{
  // Mean, covariance and inverse covariance
  pf_vector_t x;
  pf_matrix_t cx;
  //pf_matrix_t cxi;
  double cxdet;

  // Decomposed covariance matrix (rotation * diagonal)
  pf_matrix_t cr;
  pf_vector_t cd;

  // A random number generator
  //gsl_rng *rng;

} pf_pdf_gaussian_t;

// Create a gaussian pdf
pf_pdf_gaussian_t *pf_pdf_gaussian_alloc(pf_vector_t x, pf_matrix_t cx);

// Destroy the pdf
void pf_pdf_gaussian_free(pf_pdf_gaussian_t *pdf);

// Compute the value of the pdf at some point [z].
//double pf_pdf_gaussian_value(pf_pdf_gaussian_t *pdf, pf_vector_t z);

// Draw randomly from a zero-mean Gaussian distribution, with standard
// deviation sigma.
// We use the polar form of the Box-Muller transformation, explained here:
//   http://www.taygeta.com/random/gaussian.html
double pf_ran_gaussian(double sigma);

// Generate a sample from the the pdf.
pf_vector_t pf_pdf_gaussian_sample(pf_pdf_gaussian_t *pdf);

#if 0

/**************************************************************************
 * Discrete
 *************************************************************************/

// Discrete PDF info
typedef struct
{
  // The list of discrete probs
  int prob_count;
  double *probs;

  // A random number generator
  gsl_rng *rng;

  // The discrete prob generator
  gsl_ran_discrete_t *ran;

} pf_pdf_discrete_t;

// Create a discrete pdf
pf_pdf_discrete_t *pf_pdf_discrete_alloc(int count, double *probs);

// Destroy the pdf
void pf_pdf_discrete_free(pf_pdf_discrete_t *pdf);

// Compute the value of the probability of some element [i]
double pf_pdf_discrete_value(pf_pdf_discrete_t *pdf, int i);

// Generate a sample from the the pdf.
int pf_pdf_discrete_sample(pf_pdf_discrete_t *pdf);
#endif

#ifdef __cplusplus
}
#endif

#endif


#ifndef _PURSUIT_SHIP_H_
#define _PURSUIT_SHIP_H_

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

#include <boost/shared_ptr.hpp>

#include <Ravelin/Pose3d.h>
#include <Ravelin/SpatialRBInertiad.h>

#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_multifit_nlin.h>

#include <ompl/control/SpaceInformation.h>
#include <ompl/base/goals/GoalState.h>
#include <ompl/base/spaces/SE2StateSpace.h>
#include <ompl/control/spaces/RealVectorControlSpace.h>
#include <ompl/control/planners/rrt/RRT.h>
#include <ompl/control/SimpleSetup.h>
#include <ompl/config.h>
#include <ompl/control/SimpleDirectedControlSampler.h>
#include <ompl/control/ODESolver.h>

#include "experiment.h"
#include "aabb.h"
#include "control_space.h"

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

class space_c;
typedef boost::shared_ptr<space_c> space_p;
class ship_c;
typedef boost::shared_ptr<ship_c> ship_p;

//-----------------------------------------------------------------------------
class ship_c {
public:

  //---------------------------------------------------------------------------
  // Members
  //---------------------------------------------------------------------------

  Ravelin::Pose3d _pose;
  Ravelin::SVelocityd _velocity;

  // the inertial mass matrix of the ship
  Ravelin::SpatialRBInertiad _inertial;

  space_p _space;
  //----------

  // the set of possible player types
  enum role_e {
    NONE = 0,
    PREY,
    PREDATOR
  };

  role_e role;

  ship_p adversary;

  double time;
  double dtime;

  double integration_step;
  double controller_step;

  // the ship's bounding box in the ship's frame of reference
  aabb_c ship_frame_bb;

   // whether or not a capture event has occurred
  bool capture;
  bool stopped;

  //---------------------------------------------------------------------------
  // Constructors
  //---------------------------------------------------------------------------
  ship_c( role_e role );
  ship_c( ompl::base::StateSpace *_statespace );

  //---------------------------------------------------------------------------
  // Destructor
  //---------------------------------------------------------------------------
  virtual ~ship_c( void );

  //---------------------------------------------------------------------------
  // Base Class Definitions
  //---------------------------------------------------------------------------
  virtual bool open( void ) { return false; }
  virtual bool read( std::vector<double>& q ) { return false; }
  virtual bool write( const std::vector<double>& u ) { return false; }
  virtual void reset( void ) { }

  virtual bool read_ke( double& ke ) { return false; }

public:
  static unsigned long compute_prey_command_seed( double time, double dtime );
  static double compute_predator_time_randval( double time, double dtime );

  // computes commands (forces) if the ship is prey
  static void compute_prey_command( const std::vector<double>& pred_state, const std::vector<double>& prey_state, std::vector<double>& prey_u, const double& time, const double& dtime, space_c* space );

  // normalizes the quaternion components of the state
  static void renormalize_state_quat(std::vector<double>& q);

  // compute the bounding box for the ship given a state
  aabb_c aabb( const std::vector<double>& q );

  // query whether a bounding box intersects another bounding box
  //bool intersects_any_obstacle( const aabb_c& mybb, aabb_c& obstacle );

  // query whether the ship intersects the world bounding box
  bool intersects_world_bounds( const aabb_c& mybb );

  // compute any force(field) that the boundary contributes to repel collision
  static Ravelin::Vector3d boundary_force( space_c* space, const Ravelin::Vector3d& pos, const Ravelin::Vector3d& vel );

  static Ravelin::SForced drag_force( Ravelin::SVelocityd& v, boost::shared_ptr<Ravelin::Pose3d> P_CoM );

  // computes a repulsive force for a given distance
  static double repulsive_force( double repulsion_factor, double dist );
  static double flee_force( double repulsion_factor, double dist );

  // - Feedback Control -
  // compute the desired state based on the current command and current state
  void compute_desired_state( const std::vector<double>& u, const std::vector<double>& x_current, std::vector<double>& x_desired );
 // computes the feedback command based on error in the current state
  void compute_feedback( const std::vector<double>& x_current, std::vector<double>& x_desired, const std::vector<double>& u_current, std::vector<double>& u_feedback );

  // - Dynamics -
  // compute the inverse dynamics for the ship
  static void inv_dyn( space_c* space,  const std::vector<double>& q, const std::vector<double>& qdot_des, std::vector<double>& u, const Ravelin::SpatialRBInertiad& inertial );

  // ordinary differential equations for the ship
  static void ode( space_c* space, const std::vector<double>& q, const std::vector<double>& u, std::vector<double>& dq, const Ravelin::SpatialRBInertiad& inertial );

  // - OMPL --
  // plans using the ompl rrt planner
  //bool plan_rrt( const std::vector<double>& pred_q, const std::vector<double>& prey_q, std::vector<double>& u );

  bool compute_predator_plan( const std::vector<double>& pred_q, const std::vector<double>& prey_q, std::vector< std::vector<double> >& us, std::vector<double>& durations, unsigned& control_count );

 // the reference to the statespace the planner has generated
  ompl::base::StateSpace *_statespace;

  // query whether or not the state generated by the planner is valid
  bool is_state_valid(const ompl::control::SpaceInformation *si, const ompl::base::State *state);

  // allocates the predator/prey control sampler for the planner
  ompl::control::DirectedControlSamplerPtr allocate_pp_control_sampler( const ompl::control::SpaceInformation* si );

  // ode.  called by the eular_integrator_c in integrator.h
  void operator()( const ompl::base::State* state, const ompl::control::Control* control, std::vector<double>& dstate, const Ravelin::SpatialRBInertiad& inertial ) const;

  // post integration update method
  void update( ompl::base::State* state, const std::vector<double>& dstate ) const;

  static double compute_distance( const std::vector<double>& pred_state, const std::vector<double>& prey_state );

  private:
    void get_predator_control(const std::vector<double>& last_pred_q, const std::vector<double>& new_prey_q, const Ravelin::SpatialRBInertiad& predJ, space_c* space, double DT, std::vector<double>& u);
    void integrate_prey(const std::vector<double>& prey_q, const std::vector<double>& pred_q, const Ravelin::SpatialRBInertiad& Jprey, double time, space_c* space, double DT, std::vector<double>& qnew);
    void integrate_predator(const std::vector<double>& q, const std::vector<double>& u, const Ravelin::SpatialRBInertiad& J, space_c* space, double DT, std::vector<double>& qnew);
};

#endif // _PURSUIT_SHIP_H_


/*! \file interp.h
    \brief Functions for interpolation to initialize the root solver
 */

// used by the gsl interpolation functions
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_sort_double.h>

#ifndef REAL_TYPEDEF
#define REAL_TYPEDEF
#ifndef single			// compiler option determines variable size
typedef double REAL;
#else
typedef float REAL;
#endif
#endif

#define N_INTERP_POINTS 3
#define NEAREST_TOL 1.0e-3

#define MIN_IDX(indices,values) ((values[0] < values[1]) ? indices[0]:indices[1])
#define DISTANCE(x1,y1,x2,y2) hypot(x1-x2,y1-y2)
#define MAXIMUM(x,y) ((x) > (y) ? (x) : (y))

// interpolation functions
REAL interpBetw2ptsDist(REAL distx_x0,REAL f0,REAL f1,REAL distx1_x0);
int cartesianToCylindrical(int npts,REAL R[],REAL Z[],REAL zeta[],REAL x[],REAL y[],REAL z[]);
int interp2d(int nx,int nxi,REAL x[],REAL y[],REAL z[],REAL w[],REAL xi[],REAL yi[],REAL zi[],REAL wi[]);
int find_nearestn(int nclosest,int nx,int nxi,size_t idx_closest[nx][nclosest],REAL x[],REAL y[],REAL xi[],REAL yi[]);
int calc_slopes(double mx[],double my[],size_t idxs[],double x[],double y[],double z[]);

// legacy functions used by previous versions of the root finder
int interp(REAL y[],REAL xi[],REAL si[],int ny,int nxi);
int interpxy(REAL x[],REAL y[],REAL xi[],REAL yi[],int nx,int nxi);
int interpgsl(REAL x[],REAL y[],REAL xi[],REAL yi[],int nx,int nxi);
int find_nearest(int nx,int xirows,int xicols,int xi_idx_below[],int yi_idx_below[],REAL x[],REAL y[],REAL xi[][xicols],REAL yi[][xicols]);
int interpReg2d(int nx,int xirows,int xicols,REAL x[],REAL y[],REAL z[],REAL w[],REAL xi[][xicols],REAL yi[][xicols],REAL zi[][xicols],REAL wi[][xicols]);

// ---  local function definitions

//! Linear interpolation using two points to a third point
/*! Uses the distance between the two known points ``distx1_x0'' = (x1-x0),
    the distance between the unknown point to a known point ``distx_x0'' = (x-x0),
    and the values of the function at x1 and x0 (f1 = f(x1),f2 = f(x2)),
    to find the value at x: f= f(x).
 */
REAL interpBetw2ptsDist(REAL distx_x0,REAL f0,REAL f1,REAL distx1_x0){
  REAL f = (f1-f0)/distx1_x0*distx_x0 + f0;
  return f;
}

//! Converts cartesian coordinates (x,y,z) to cylindrical coordinates (R,Z,zeta)
int cartesianToCylindrical(int npts,REAL R[],REAL Z[],REAL zeta[],REAL x[],REAL y[],REAL z[]){
  int i;
  for(i=0;i<npts;i++){
    R[i]=hypot(x[i],z[i]);
    Z[i]=y[i];
    zeta[i]=-atan2(z[i],x[i]);	// negative because in forming right-hand coord system
  }                               //  x: radially out, y: vertically up, z: toroidal
                                  //  esi routines give theta going clockwise,
                                  //  a: out in minor radius, gives z and theta antiparallel

  return 0;
}

//! Finds the nearest n points on a grid of (xi,yi) for each point (x,y)
int find_nearestn(int nclosest,int nx,int nxi,size_t idx_closest[nx][nclosest],REAL x[],REAL y[],REAL xi[],REAL yi[]){
  double dist[nxi],d01,d02,d12,hyp;
  int point,i,n_temp;
  size_t *closest_pts;
  size_t *idx_closest_pt;

#if DEBUG
  fprintf(stderr,"point,   d01,    d02,   d12,    hyp,    collinear\n");
#endif

  for(point=0;point<nx;point++){		// loop for each x,y

    for(i=0;i<nxi;i++){	// find the distance between (x,y) and all the (xi,yi)
      //      dist[i]=sqrt((x[point]-xi[i])*(x[point]-xi[i])+(y[point]-yi[i])*(y[point]-yi[i]));
      dist[i]=DISTANCE(x[point],y[point],xi[i],yi[i]);
    }
    idx_closest_pt=idx_closest[point];
    gsl_sort_smallest_index(idx_closest_pt,nclosest,dist,1,nxi);// find the nearest xi

    // make sure they aren't collinear
    d01=DISTANCE(xi[idx_closest_pt[0]],yi[idx_closest_pt[0]],xi[idx_closest_pt[1]],yi[idx_closest_pt[1]]);
    d02=DISTANCE(xi[idx_closest_pt[0]],yi[idx_closest_pt[0]],xi[idx_closest_pt[2]],yi[idx_closest_pt[2]]);
    d12=DISTANCE(xi[idx_closest_pt[1]],yi[idx_closest_pt[1]],xi[idx_closest_pt[2]],yi[idx_closest_pt[2]]);
    //    hyp = fmax(fmax(d01,d02),d12);
    hyp = MAXIMUM(MAXIMUM(d01,d02),d12);
    n_temp = nclosest;
    while((d01+d02+d12-2.0*hyp < NEAREST_TOL) && (nclosest < nxi)){ // loop until the points aren't collinear
      n_temp++;
      closest_pts = (size_t *)malloc(sizeof(size_t)*n_temp);
      gsl_sort_smallest_index(closest_pts,n_temp,dist,1,nxi);// find the nearest xi
      idx_closest_pt[2]=closest_pts[n_temp-1];
      d02=DISTANCE(xi[idx_closest_pt[0]],yi[idx_closest_pt[0]],xi[idx_closest_pt[2]],yi[idx_closest_pt[2]]);
      d12=DISTANCE(xi[idx_closest_pt[1]],yi[idx_closest_pt[1]],xi[idx_closest_pt[2]],yi[idx_closest_pt[2]]);
      //hyp = fmax(fmax(d01,d02),d12);
      hyp = MAXIMUM(MAXIMUM(d01,d02),d12);

      free(closest_pts);
    }
#if DEBUG
    fprintf(stderr,"%d, %g, %g, %g, %g, %g\n",point,d01,d02,d12,hyp,2.0*hyp-d01-d02-d12);
#endif

  }
    //   for(i=0;i<xirows*xicols;i++){
    //     fprintf(stderr,"xi       yi\n");
    //   fprintf(stderr,"%g, %g\n",xi[0][i],yi[0][i]);
    //   }
    //   for(i=0;i<nx;i++){
    //     fprintf(stderr,"x,      xi_below,      y,    ,yi_below\n");
    //     fprintf(stderr,"%g, %g, %g, %g\n",x[i],xi[xi_idx_below[i]][0],y[i],yi[0][yi_idx_below[i]]);
    //   }

    return 0;
 }

  //! Calculates the slopes (mx,my) for three points (x,y) which each have function value z
  /*! these three points are have the indices idxs */
  int calc_slopes(double mx[],double my[],size_t idxs[],double x[],double y[],double z[]){
    size_t i,ip1,ip2,j,jp1,jp2;
    double dx[N_INTERP_POINTS],dy[N_INTERP_POINTS],dz[N_INTERP_POINTS];

    for(j=0;j<N_INTERP_POINTS;j++){
      ip1 = idxs[(size_t)fmod(j+1,N_INTERP_POINTS)];
      ip2 = idxs[(size_t)fmod(j+2,N_INTERP_POINTS)];
      dx[j] = x[ip1]-x[ip2];
      dy[j] = y[ip1]-y[ip2];
      dz[j] = z[ip1]-z[ip2];
    }
    for(j=0;j<N_INTERP_POINTS;j++){
      jp1 = (size_t)fmod(j+1,N_INTERP_POINTS);
      jp2 = (size_t)fmod(j+2,N_INTERP_POINTS);
      i = idxs[j];
      ip1 = idxs[jp1];
      ip2 = idxs[jp2];
      mx[j] = -1.0*(y[i]*dz[j]+y[ip1]*dz[jp1]+y[ip2]*dz[jp2]);
      mx[j]/=(x[i]*dy[j]+x[ip1]*dy[jp1]+x[ip2]*dy[jp2]);
      my[j] = -1.0*(x[i]*dz[j]+x[ip1]*dz[jp1]+x[ip2]*dz[jp2]);
      my[j]/=(y[i]*dx[j]+y[ip1]*dx[jp1]+y[ip2]*dx[jp2]);
    }

    return 0;
  }

//! High-level 2d interpolation function
/*! Given values zi = f(xi,yi) and wi = g(xi,yi) on an arbitrary, irregular grid (xi,yi)
    finds the values at (x,y).
    The method is to find the nearest 3 grid points for each (x,y), then linearly interpolate (triangulation).
 */
  int interp2d(int nx,int nxi,REAL x[],REAL y[],REAL z[],REAL w[],REAL xi[],REAL yi[],REAL zi[],REAL wi[]){
    int point;
    size_t *idxs;
    double mxi[N_INTERP_POINTS],myi[N_INTERP_POINTS],a,b,c;

    // find the indices of the 3 (xi,yi) closest to x,y
    size_t idx_closest[nx][N_INTERP_POINTS];
    find_nearestn(N_INTERP_POINTS,nx,nxi,idx_closest,x,y,xi,yi);

    // loop around points
    for(point=0;point<nx;point++){
      idxs = idx_closest[point];
      // first get z
      // calculate the slopes (mx,my) for the nearest 3 points
      calc_slopes(mxi,myi,idxs,xi,yi,zi);
      // linear interp at each nearest point
      a = zi[idxs[0]] + mxi[0]*(x[point]-xi[idxs[0]]) + myi[0]*(y[point]-yi[idxs[0]]);
      b = zi[idxs[1]] + mxi[1]*(x[point]-xi[idxs[1]]) + myi[1]*(y[point]-yi[idxs[1]]);
      c = zi[idxs[2]] + mxi[2]*(x[point]-xi[idxs[2]]) + myi[2]*(y[point]-yi[idxs[2]]);
      // average to get the best guess
      z[point] = (a+b+c)/3.0;

      // next get w
      // calculate the slopes (mx,my) for the nearest 3 points
      calc_slopes(mxi,myi,idxs,xi,yi,wi);
      // linear interp at each nearest point
      a = wi[idxs[0]] + mxi[0]*(x[point]-xi[idxs[0]]) + myi[0]*(y[point]-yi[idxs[0]]);
      b = wi[idxs[1]] + mxi[1]*(x[point]-xi[idxs[1]]) + myi[1]*(y[point]-yi[idxs[1]]);
      c = wi[idxs[2]] + mxi[2]*(x[point]-xi[idxs[2]]) + myi[2]*(y[point]-yi[idxs[2]]);
      // average to get the best guess
      w[point] = (a+b+c)/3.0;

    }

    return 0;
  }

// --- legacy functions

int interp(REAL y[],REAL xi[],REAL si[],int ny,int nxi){
  int i,j;
  //  printf("interp:\n%10s,%10s,%10s,%10s,%10s\n","q_orig","q_final","q_i","q_i+1","s_i");
  for(i=0;i<ny;i++){
    for(j=0;j<nxi-1;j++) if(xi[j+1] > y[i]) break;
    //printf("%10g,",y[i]);
    y[i] = xi[j] + si[j]*(y[i]-xi[j]);
    //printf("%10g,%10g,%10g,%10g\n",y[i],xi[j],xi[j+1],si[j]);
  }
  return 0;
}

int interpxy(REAL x[],REAL y[],REAL xi[],REAL yi[],int nx,int nxi){
  int i,j;
  REAL si[nxi-1];
  // use linear interpolation
  //printf("\nstheta:");
  for(i=0;i<nxi-1;i++){
    si[i] = (yi[i+1]-yi[i])/(xi[i+1]-xi[i]);
    //printf("%g,",si[i]);
  }
  si[nxi-2] = si[0];
  //printf("\n");

  //printf("interp:\n%10s,%10s,%10s,%10s,%10s\n","q_orig","q_final","q_i","q_i+1","s_i");
  for(i=0;i<nx;i++){
    for(j=0;j<nxi-1;j++) if(xi[j+1] > x[i]) break;
    //printf("%10g,",x[i]);
    y[i] = yi[j] + si[j]*(x[i]-xi[j]);
    //printf("%10g,%10g,%10g,%10g\n",y[i],xi[j],xi[j+1],si[j]);
  }

  return 0;
}

// interpolate the array x[] of size nx
// on the grid xi[],y[i] of size nxi
int interpgsl(REAL x[],REAL y[],REAL xi[],REAL yi[],int nx,int nxi){
  // make sure periodic
  //  yi[nxi-1]=yi[0];
  // xi[nxi-1]=-xi[0];
  gsl_interp_accel *acc = gsl_interp_accel_alloc ();
  const gsl_interp_type *t = gsl_interp_cspline;
  gsl_spline *spline = gsl_spline_alloc (t, nxi);

  int i;
  gsl_spline_init (spline, xi, yi, nxi);
  //printf("\ninterpgsl:\n");
  for (i = 0; i < nx; i++)
    {
      y[i] = gsl_spline_eval (spline, x[i], acc);
      //printf ("%g, %g\n", x[i], y[i]);
    }

  gsl_spline_free (spline);
  gsl_interp_accel_free (acc);

  return 0;
}

int find_nearest1d(int nx,int nxi,int nnearest,int stride,size_t xi_idx[nx][nnearest],REAL x[],REAL xi[]){
  int i,j;
  REAL xerr[nxi];
  for(i=0;i<nx;i++){
    for(j=0;j<nxi;j++){
      xerr[j]=fabs(x[i]-xi[j*stride]);
    }
    gsl_sort_smallest_index(xi_idx[i],nnearest,xerr,stride,nxi);// find the n nearest xi
  }

  return 0;
}

// xi varies slowly (each row), yi varies quickly (each col)
int find_nearest(int nx,int xirows,int xicols,int xi_idx_below[],int yi_idx_below[],REAL x[],REAL y[],REAL xi[][xicols],REAL yi[][xicols]){
  size_t nearest2_idxs[2];
  double nearest2_vals[2],xerr[xirows],yerr[xirows];
  int i,j;

  for(i=0;i<nx;i++){		// loop for each x,y

    for(j=0;j<xirows;j++){	// find the error between x and all the xi
      xerr[j]=fabs(x[i]-xi[j][0]);
    }
    gsl_sort_smallest_index(nearest2_idxs,2,xerr,1,xirows);// find the two nearest xi
    nearest2_vals[0] = xi[nearest2_idxs[0]][0];
    nearest2_vals[1] = xi[nearest2_idxs[1]][0];
    xi_idx_below[i] = MIN_IDX(nearest2_idxs,nearest2_vals); // this is the nearest xi less than x

    for(j=0;j<xicols;j++){	// find the error between y and all the yi
      yerr[j]=fabs(y[i]-yi[0][j]);
    }
    gsl_sort_smallest_index(nearest2_idxs,2,yerr,1,xicols);// find the two nearest yi
    nearest2_vals[0] = yi[0][nearest2_idxs[0]];
    nearest2_vals[1] = yi[0][nearest2_idxs[1]];
    yi_idx_below[i] = MIN_IDX(nearest2_idxs,nearest2_vals); // this is the nearest yi less than y
  }

  return 0;
}

// limitation: assumes regularly-spaced grids xi and yi
int interpReg2d(int nx,int xirows,int xicols,REAL x[],REAL y[],REAL z[],REAL w[],REAL xi[][xicols],REAL yi[][xicols],REAL zi[][xicols],REAL wi[][xicols]){
  int i,j;

  // form the matrix of of slopes for y interpolation
  double slopeziyi[xirows][xicols-1],slopewiyi[xirows][xicols-1];
  double invdi;
  for(i=0;i<xirows;i++){
    for(j=0;j<xicols-1;j++){
      invdi = 1.0/(yi[i][j+1]-yi[i][j]);
      slopeziyi[i][j] = (zi[i][j+1]-zi[i][j])*invdi;
      slopewiyi[i][j] = (wi[i][j+1]-wi[i][j])*invdi;
    }
  }

  // find the indices of the nearest points (xi[i],yi[j]); note always exists (xi[i+1],yi[j+1])
  int xi_idx_below[nx],yi_idx_below[nx];
  find_nearest(nx,xirows,xicols,xi_idx_below,yi_idx_below,x,y,xi,yi);

  // for fixed x, interpolate in y
  double a,b,dist_y_yi,x_frac;
  for(i=0;i<nx;i++){
    dist_y_yi=y[i]-yi[0][yi_idx_below[i]];
    x_frac = (x[i]-xi[xi_idx_below[i]][0])/(xi[xi_idx_below[i]+1][0]-xi[xi_idx_below[i]][0]);

    // compute z[i]
    // for fixed xi = i, interpolate in y between yi[j] and yi[j+1]
    a = slopeziyi[xi_idx_below[i]][yi_idx_below[i]]*dist_y_yi + zi[xi_idx_below[i]][yi_idx_below[i]];
    // for fixed xi = i+1, interpolate in y between yi[j] and yi[j+1]
    b = slopeziyi[xi_idx_below[i]+1][yi_idx_below[i]]*dist_y_yi + zi[xi_idx_below[i]+1][yi_idx_below[i]];
    // now interpolate in x between xi[i] and xi[i+1]
    z[i] = (b-a)*x_frac + a;

    // compute w[i]
    // for fixed xi = i, interpolate in y between yi[j] and yi[j+1]
    a = slopewiyi[xi_idx_below[i]][yi_idx_below[i]]*dist_y_yi + wi[xi_idx_below[i]][yi_idx_below[i]];
    // for fixed xi = i+1, interpolate in y between yi[j] and yi[j+1]
    b = slopewiyi[xi_idx_below[i]+1][yi_idx_below[i]]*dist_y_yi + wi[xi_idx_below[i]+1][yi_idx_below[i]];
    // now interpolate in x between xi[i] and xi[i+1]
    w[i] = (b-a)*x_frac + a;

  }

  return 0;
}