/* aHAFF.cpp
   g++ -I/usr/local/include/eigen3 -o aHAF aHAF.cpp -O3
*/

#include <iostream>
#include <cstdlib>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <Eigen/Eigenvalues>

using namespace std;
using namespace Eigen;

int *pl = NULL;
int *rl = NULL;
unsigned long int rsn;
double *phi = NULL;
double *phiOld = NULL;
double *phiOlder = NULL;
double *Hphi = NULL;
double *A;
double *B;
MatrixXd Lanczos;
SelfAdjointEigenSolver<MatrixXd> es;
int L,Sztot;
int maxlen;
double g, oldnorm, norm;
ofstream ofs,ofs2;

void initialize(int);
void exit();
int Sz(unsigned long int);
int spin(unsigned long int, int);
int INV(unsigned long int);
unsigned long int flip(unsigned long int,int);
bool flippable(unsigned long int,int);

void initialize(int opt) {
  A = new double[200];
  B = new double[200];
  pl = new int [L];           // watch zero start !
  for (int i=0;i<L;i++) {
    pl[i] = i+1;
  }
  pl[L-1] = 0;
  // allocate memory
  unsigned long int size=1;   // L!/(L/2)!(L/2)!
  int L2 = L/2;               // L must be even
  int j=1;
  for (int i=L2+1;i<=L;i++) {
    size = size*i/j;
    j += 1;
  }
  cout << "     " <<  endl;
  double d;
  cout << "double: " << sizeof(d) << endl;
  cout << "int: " << sizeof(j) << endl;
  cout << "ulint: " << sizeof(size) << endl;
  cout << "basis size for Sztot=0: " << size << endl;
  if (!(phi  = new double[size+1] )) {   // rsn starts from 1 !!
   cout << "out of memory" << endl;
   exit(1);
  }
  if (!(phiOld  = new double[size+1] )) {
   cout << "out of memory" << endl;
   exit(1);
  }
  if (!(phiOlder  = new double[size+1] )) {
   cout << "out of memory" << endl;
   exit(1);
  }
  if (!(Hphi  = new double[size+1] )) {
   cout << "out of memory" << endl;
   exit(1);
  }
  if (!(rl = new int[size+1] )) {
   cout << "out of memory" << endl;
   exit(1);
  }
  // set up relabeled state codes
  unsigned long int maxnum = 1 << L;   // a dirty trick to get 2^L (cpp does not support pow(int,int)!!
  cout << " 2^L = " << maxnum << endl;
  rsn = 1;
  for (unsigned long int sn=0;sn<maxnum;sn++) {
    if (Sz(sn) == Sztot) {
      rl[rsn] = sn;
      rsn++;
    }
  }
  maxlen = rsn - 1;
  cout << " basis size for Sztot= " << Sztot << " : " << maxlen << endl;
  // initial state
  if (opt == 1){                      // initial state = neel1 + neel2
    int neel1 = maxnum/3;
    int neel2 = 2*neel1;
    for (rsn=1;rsn<=maxlen;rsn++) {
       phi[rsn] = 0.0;
    }
    phi[INV(neel1)] = 1.0/sqrt(2.0);
    phi[INV(neel2)] = 1.0/sqrt(2.0);
    oldnorm = 1.0;
    if (L == 4) {
      cout << "neel1 " << neel1 << " inv " << INV(neel1) << endl;
      cout << "neel2 " << neel2 << " inv " << INV(neel2) << endl;
    }
  } else {                          // random start
    int seed;
    cout << " enter the random seed " << endl;
    cin >> seed;
    srand(seed);
    oldnorm = 0.0;
    for (rsn=1;rsn<=maxlen;rsn++) {
     phi[rsn] = 2.0*( (double) rand()/RAND_MAX - 0.5);
     oldnorm = oldnorm + phi[rsn]*phi[rsn];
    }
  }
  // file miscellany
  ofs2.open("aHAFab.dat");
  ofs.open("aHAFout.dat");
  ofs << "# aHAF.cpp" << endl;
  ofs << "# L = " << L <<  endl;
  ofs << "# g = " << g << endl;
  ofs << "# Sztot = " << Sztot << endl;
  if (opt == 1) {
   ofs << "# Neel start " << endl;
  } else {
   ofs << "# random start " << endl;
  }
}

void exit() {
  delete [] phi;
  delete [] phiOld;
  delete [] phiOlder;
  delete [] Hphi;
  delete [] rl;
  delete [] pl;
  delete [] A;
  delete [] B;
  ofs.close();
  ofs2.close();
}

int INV(unsigned long int sn) {
  // find the state index, rsn, for a state, sn=rl(rsn)
  unsigned long int mid;
  unsigned long int low = 1;
  unsigned long int high = maxlen;
  st:  mid = (low + high)/2;
  if (sn ==  rl[mid]) return mid;
  if (sn <  rl[mid]) {
    high = mid - 1;
  } else {
    low = mid + 1;
  }
  goto st;
}

int spin(unsigned long int sn, int bt) {
  // return the bt'th bit in sn converted to +-
  int S = -1;
  int k = sn & 1 << bt;
  if (k != 0) S=1;
  return S;
}

int Sz(unsigned long int sn) {
 // compute Sztot for a state sn
 int S=0;
 for (int b=0;b<L;b++) {
  S += spin(sn,b);
 }
 return S;
}

int T(unsigned long int rsn) {
  // compute H0 for a state rsn  (need to divide by 4)
  int S=0;
  unsigned long int sn = rl[rsn];
  for (int bt=0;bt<L;bt++) {
    S += spin(sn,bt)*spin(sn,pl[bt]);
  }
  return S;
}

unsigned long int flip(unsigned long int rsn, int i) {
 // flip bits i and i+1  using a nasty trick; return a relabeled statecode
 // will fail drastically if b(i) = b(i+1)
 unsigned long int sn = rl[rsn];
 sn ^= 1 << i;
 sn ^= 1 << pl[i];
 return INV(sn);
}

bool flippable(unsigned long int rsn, int bt) {
 // check if bit i != bit i+1
 bool f = false;
 int b0 = (rl[rsn] >> bt) & 1;
 int b1 = (rl[rsn] >> pl[bt]) & 1;
 if (b0 != b1) f = true;
 return f;
}

int main() {
  int opt;
  cout << " enter L, Sztot, g " << endl;
  cin >> L >> Sztot >> g;
  cout << " enter 1 for Neel start, 2 for random " << endl;
  cin >> opt;

  initialize(opt);

  // H{phi}
  for (rsn=1;rsn<=maxlen;rsn++) {
   Hphi[rsn] = T(rsn)*phi[rsn]/4.0;
  }
  for (rsn=1;rsn<=maxlen;rsn++) {
   for (int bt=0;bt<L;bt++) {
    if (flippable(rsn,bt)) {
     Hphi[flip(rsn,bt)] += g/2.0*phi[rsn];
    }
   }
  }

  double sum = 0.0;
  for (rsn=1;rsn<=maxlen;rsn++) {
    sum += phi[rsn]*Hphi[rsn];
    phiOld[rsn] = phi[rsn];
  }
  A[0] = sum/oldnorm;
  B[0] = 0.0;
  cout << " variational  e0 = " << A[0]/L << endl;
  ofs << "0 A: " << A[0] << endl;

  for (int loop=1;loop<=4*L;loop++) {    // Lanzcos loop  :::::::::::::::::::::::::::
   for (rsn=1;rsn<=maxlen;rsn++) {
     phi[rsn] = Hphi[rsn] - A[loop-1]*phiOld[rsn] - B[loop-1]*phiOlder[rsn];
   }
   // H{phi}
   for (rsn=1;rsn<=maxlen;rsn++) {
    Hphi[rsn] = T(rsn)*phi[rsn]/4.0;
   }
   for (rsn=1;rsn<=maxlen;rsn++) {
    for (int bt=0;bt<L;bt++) {
     if (flippable(rsn,bt)) {
      Hphi[flip(rsn,bt)] += g/2.0*phi[rsn];
     }
    }
   }
   double norm = 0.0;
   for (rsn=1;rsn<=maxlen;rsn++) {
     norm += phi[rsn]*phi[rsn];
   }
   double sum = 0.0;
   for (rsn=1;rsn<=maxlen;rsn++) {
     sum += phi[rsn]*Hphi[rsn];
   }
   A[loop] = sum/norm;
   B[loop] = norm/oldnorm;
   oldnorm = norm;
   for (rsn=1;rsn<=maxlen;rsn++) {
     phiOlder[rsn] = phiOld[rsn];
     phiOld[rsn] = phi[rsn] ;
   }

   ofs2 << loop << setprecision(16) << std::fixed << " A: " << A[loop] << " B: " << B[loop] << endl;
   // diagonalise
   Lanczos.resize(loop+1,loop+1);
   Lanczos.setZero();
   for (int i=0;i<=loop;i++) {
     Lanczos(i,i) = A[i];
   }
   for (int i=0;i<loop;i++) {
     Lanczos(i,i+1) = sqrt(B[i+1]);
     Lanczos(i+1,i) = Lanczos(i,i+1);
   }
   es.compute(Lanczos);
   double lam0 = es.eigenvalues()[0];
   double lam1 = es.eigenvalues()[1];
   ofs << loop << setprecision(12) << std::fixed << " " << lam0 << " " << lam1;
   if (loop > 3 ) {
    double lam2 = es.eigenvalues()[2];
    double lam3 = es.eigenvalues()[3];
    ofs << " " << lam2 << " " << lam3;
   }
   ofs << endl;
   cout << loop << setprecision(12) << std::fixed <<  "\t e0: " << lam0/L << "\t e1: " << lam1/L << "\t gap: " << lam1-lam0 << endl;
  }  // end of Lanczos iteration loop           ::::::::::::::::::::::::::::::

  exit();
  cout << endl << " data in aHAFout.dat" << endl;
  cout << " A,B in aHAFab.dat" << endl << endl;
  return 0;
}


/*  calculate CMB related observables, depends on Cosmology calculator(s).

    Shift parameter: R = (1+z*) * DA(z�6�5) * sqrt(Omegam) * H0 / c
    acoustic scale: lA = (1+z*) * Pi * DA(z*) / rs(z*)

where z* is the decoupling redshift
*/

#ifndef __CMB_DISTANCE_PRIOR__
#define __CMB_DISTANCE_PRIOR__

#include <vector>
#include <string>
#include <armadillo>
#include <imcmc/imcmc.hpp>
#include "ParamList.hpp"

using namespace imcmc;

struct Data_CMB_Dist {

    DataInfo data_info;
    bool use_Hu_fitting;
    int format;                     // 0--WMAP; 1--Planck
    arma::rowvec distance_prior;    // {lA,R,zdec}
    arma::mat covmat_inv;           // inverse of the covariance matrix.
    void Init( std::string& CMB_dist_prior_dataset );

    Data_CMB_Dist();
    ~Data_CMB_Dist();

//  add Planck CMB distance prior
    arma::rowvec distance_prior_plk;    // {R,lA,Omegabh2}
    arma::mat covmat_inv_plk;           // this (inv-)covariance matrix is different from those from WMAP
    double std_R, std_lA, std_Obh2;     // 1-sigma errors for {R,lA,Omegabh2}
};

typedef Data_CMB_Dist CMB_Dist;

//  likelihood function prototype:
double Prior_CMB_Dist(  imcmc_double&   param,
                        double&         lndet,
                        double&         chisq,
                        void*           model,
                        void*           data,
                        istate&         state );

#endif  //  __CMB_DISTANCE_PRIOR__


/*
 * Metric.hpp
 *
 *  Created on: May 18, 2012
 *      Author: david
 */

#ifndef DASP_METRIC_HPP_
#define DASP_METRIC_HPP_

#include "Point.hpp"
#include <Danvil/Tools/MoreMath.h>
#include <Danvil/Tools/FunctionCache.h>
#include <Eigen/Dense>

namespace dasp
{

	namespace metric
	{
		inline int PixelDistanceSquared(const Point& a, const Point& b) {
			const int dx = a.px - b.px;
			const int dy = a.py - b.py;
			return dx*dx + dy*dy;
		}

		inline float ImageDistanceRaw(const Point& x, const Point& y) {
			return static_cast<float>(PixelDistanceSquared(x,y)) / (y.cluster_radius_px * y.cluster_radius_px);
		}

		inline float SpatialDistanceRaw(const Eigen::Vector3f& x, const Eigen::Vector3f& y) {
			return (x - y).squaredNorm();
		}

		inline float SpatialDistanceRaw(const Point& x, const Point& y) {
			return SpatialDistanceRaw(x.position, y.position);
		}

		inline float ColorDistanceRaw(const Eigen::Vector3f& u, const Eigen::Vector3f& v) {
			return (u - v).squaredNorm();
		}

		inline float ColorDistanceRaw(const Point& u, const Point& v) {
			return ColorDistanceRaw(u.color, v.color);
		}

		inline float NormalDistanceRaw(const Eigen::Vector3f& u, const Eigen::Vector3f& v) {
			// this is an approximation to the angle between the normals
			return 1.0f - u.dot(v);
		}

		inline float NormalDistanceRaw(const Point& u, const Point& v) {
			return NormalDistanceRaw(u.normal, v.normal);
		}

		inline float NormalDistanceWithDepth(const Point& u, const Point& v) {
			const float q = NormalDistanceRaw(u.normal, v.normal);
			return 2.0f * q / (u.position.z() + v.position.z());
		}

	}

	/** Computes the density-adaptive distance from a point to a center point
	 * - uses pixel distance and color metric
	 * - Takes the density at the center point
	 */
	struct DensityAdaptiveMetric_UxRGB
	{
		DensityAdaptiveMetric_UxRGB(float w_u, float w_c) {
			weights_ = {
				w_u,
				w_c };
		}

		float operator()(const Point& p, const Point& q) const {
			return weights_.dot(
					Eigen::Vector2f(
							metric::ImageDistanceRaw(p, q),
							metric::ColorDistanceRaw(p, q)));
		}

	private:
		Eigen::Vector2f weights_;
	};

	/** Computes the density-adaptive distance from a point to a center point
	 * - uses pixel distance, color metric and depth difference
	 * - Takes the density at the center point
	 */
	struct DensityAdaptiveMetric_UxRGBxD
	{
		DensityAdaptiveMetric_UxRGBxD(float w_u, float w_c, float w_d) {
			weights_ = {
				w_u,
				w_c,
				w_d };
		}

		float operator()(const Point& p, const Point& q) const {
			return weights_.dot(
					Eigen::Vector3f(
							metric::ImageDistanceRaw(p, q),
							metric::ColorDistanceRaw(p, q),
							std::abs(p.depth() - q.depth())));
		}

	private:
		Eigen::Vector3f weights_;
	};

	/** Computes the depth-adaptive distance from a point to a center point
	 * - Takes the density at the center point
	 * - Uses 3D position instead of pixel position
	 * - Additionally uses normals.
	 */
	struct DepthAdaptiveMetric
	{
		DepthAdaptiveMetric(float w_r, float w_c, float w_n, float R) {
			weights_ = {
				w_r / (R * R),
				w_c,
				w_n };
		}

		float operator()(const Point& p, const Point& q) const {
			return weights_.dot(
					Eigen::Vector3f(
							metric::SpatialDistanceRaw(p, q),
							metric::ColorDistanceRaw(p, q),
							metric::NormalDistanceWithDepth(p, q)));
		}

	private:
		Eigen::Vector3f weights_;
	};

	template<bool SupressConvexEdges=true>
	struct ClassicSpectralAffinity
	{
		ClassicSpectralAffinity(unsigned int num_superpixels, float superpixel_radius, float w_spatial=1.0f, float w_color=1.0f, float w_normal=1.0f)
		: num_superpixels_(num_superpixels),
		  superpixel_radius_(superpixel_radius)
		{
			scl_spatial_ = w_spatial / (4.0f * superpixel_radius_ * superpixel_radius_);
			scl_color_ = w_color / (std::sqrt(static_cast<float>(num_superpixels_)) * cWeightRho);
			scl_normal_ = w_normal;
		}

		float operator()(const Point& x, const Point& y) const {
			const Eigen::Vector3f& x_pos = x.position;
			const Eigen::Vector3f& y_pos = y.position;
			const Eigen::Vector3f& x_col = x.color;
			const Eigen::Vector3f& y_col = y.color;
			const Eigen::Vector3f& x_norm = x.normal;
			const Eigen::Vector3f& y_norm = y.normal;
			// spatial distance
			float scl_d_spatial = metric::SpatialDistanceRaw(x_pos, y_pos) * scl_spatial_;
			scl_d_spatial = std::max(0.0f, scl_d_spatial - 1.2f); // distance of 1 indicates estimated distance
			// color distance
			float d_color = metric::ColorDistanceRaw(x_col, y_col);
			// normal distance
			float d_normal;
			if(SupressConvexEdges) {
				// only use concave edges
				Eigen::Vector3f d = y_pos - x_pos;
				d_normal = (x_norm - y_norm).dot(d) * Danvil::MoreMath::FastInverseSqrt(d.squaredNorm());
				d_normal = std::max(0.0f, d_normal);
			}
			else {
				d_normal = metric::NormalDistanceRaw(x_norm, y_norm);
			}
			// compute total edge connectivity
			float d_combined = scl_d_spatial + scl_color_*d_color + scl_normal_*d_normal;
			return exp_cache_(d_combined);
		}

	private:
		static constexpr float cWeightRho = 0.01f; // 640x480 clusters would yield 0.1 which is used in gPb
		unsigned int num_superpixels_;
		float superpixel_radius_;
		float scl_spatial_;
		float scl_color_;
		float scl_normal_;
		Danvil::ExpNegFunctionCache<float> exp_cache_; // used for std::exp(-x)
	};

	struct ImprovedSpectralAffinity
	{
		ImprovedSpectralAffinity(float superpixel_radius,
			float w_spatial=1.0f, float w_color=1.0f, float w_normal=1.0f)
		: superpixel_radius_(superpixel_radius),
		  ww_(w_spatial), wc_(w_color), wn_(w_normal)
		{
			float w_total = ww_ + wc_ + wn_;
			ww_ /= w_total;
			wc_ /= w_total;
			wn_ /= w_total;
		}

		float operator()(const Cluster& x, const Cluster& y) const {
			const Eigen::Vector3f& x_pos = x.center.position;
			const Eigen::Vector3f& y_pos = y.center.position;
			const Eigen::Vector3f& x_col = x.center.color;
			const Eigen::Vector3f& y_col = y.center.color;
			const Eigen::Vector3f& x_norm = x.center.normal;
			const Eigen::Vector3f& y_norm = y.center.normal;
			// spatial distance
			float dw = (x_pos - y_pos).squaredNorm() / (4.0f * superpixel_radius_ * superpixel_radius_);
			dw = std::max(0.0f, dw - 1.2f); // distance of 1 indicates estimated distance
			// color distance
			float dc = 3.2f * (x_col - y_col).squaredNorm();
			// normal distance (only use concave edges)
			Eigen::Vector3f u = y_pos - x_pos;
			float dn = (x_norm - y_norm).dot(u) / u.norm();
			dn = std::max(0.0f, dn);
			// compute total edge connectivity
			float d = ww_*dw + wc_*dc + wn_*dn;
			return exp_cache_(d);
		}

	private:
		float superpixel_radius_;
		float ww_, wc_, wn_;
		Danvil::ExpNegFunctionCache<float> exp_cache_; // used for std::exp(-x)
	};

	struct ClassicSpectralAffinitySLIC
	{
		ClassicSpectralAffinitySLIC(unsigned int num_superpixels, float w_color=1.0f)
		: num_superpixels_(num_superpixels),
		  w_color(w_color)
		{}

		float operator()(const Point& x, const Point& y) const {
			float w_maha_color = 4.0f * metric::ColorDistanceRaw(x, y) / (std::sqrt(static_cast<float>(num_superpixels_)) * cWeightRho);
			return std::exp(-w_color*w_maha_color);
		}

	private:
		static constexpr float cWeightRho = 0.01f; // 640x480 clusters would yield 0.1 which is used in gPb
		unsigned int num_superpixels_;
		float w_color;
	};

}

#endif


//
//  GridRaycast.hpp
//  PinkTopaz
//
//  Created by Andrew Fox on 11/18/17.
//
//

#ifndef GridRaycast_hpp
#define GridRaycast_hpp

#include "Ray.hpp"
#include <vector>
#include <glm/vec3.hpp>
#include <boost/coroutine2/all.hpp>

namespace GridRaycastRange {

// Iterate over cells which fall within the specified frustum.
inline void
raycast(boost::coroutines2::coroutine<glm::vec3>::push_type &sink,
        const Ray &ray,
        size_t maxDepth)
{
    /* Implementation is based on:
     * "A Fast Voxel Traversal Algorithm for Ray Tracing"
     * John Amanatides, Andrew Woo
     * http://www.cse.yorku.ca/~amana/research/grid.pdf
     *
     * See also: http://www.xnawiki.com/index.php?title=Voxel_traversal
     */

    // NOTES:
    // * This code assumes that the ray's position and direction are in 'cell coordinates', which means
    //   that one unit equals one cell in all directions.
    // * When the ray doesn't start within the voxel grid, calculate the first position at which the
    //   ray could enter the grid. If it never enters the grid, there is nothing more to do here.
    // * Also, it is important to test when the ray exits the voxel grid when the grid isn't infinite.
    // * The Point3D structure is a simple structure having three integer fields (X, Y and Z).

    // The cell in which the ray starts.
    int x = (int)ray.origin.x;
    int y = (int)ray.origin.y;
    int z = (int)ray.origin.z;

    // Determine which way we go.
    int stepX = (ray.direction.x<0) ? -1 : (ray.direction.x==0) ? 0 : +1;
    int stepY = (ray.direction.y<0) ? -1 : (ray.direction.y==0) ? 0 : +1;
    int stepZ = (ray.direction.z<0) ? -1 : (ray.direction.z==0) ? 0 : +1;

    // Calculate cell boundaries. When the step (i.e. direction sign) is positive,
    // the next boundary is AFTER our current position, meaning that we have to add 1.
    // Otherwise, it is BEFORE our current position, in which case we add nothing.
    glm::ivec3 cellBoundary(x + (stepX > 0 ? 1 : 0),
                            y + (stepY > 0 ? 1 : 0),
                            z + (stepZ > 0 ? 1 : 0));

    // NOTE: For the following calculations, the result will be Single.PositiveInfinity
    // when ray.Direction.X, Y or Z equals zero, which is OK. However, when the left-hand
    // value of the division also equals zero, the result is Single.NaN, which is not OK.

    // Determine how far we can travel along the ray before we hit a voxel boundary.
    glm::vec3 tMax((cellBoundary.x - ray.origin.x) / ray.direction.x,    // Boundary is a plane on the YZ axis.
                   (cellBoundary.y - ray.origin.y) / ray.direction.y,    // Boundary is a plane on the XZ axis.
                   (cellBoundary.z - ray.origin.z) / ray.direction.z);   // Boundary is a plane on the XY axis.
    if (isnan(tMax.x)) { tMax.x = +INFINITY; }
    if (isnan(tMax.y)) { tMax.y = +INFINITY; }
    if (isnan(tMax.z)) { tMax.z = +INFINITY; }

    // Determine how far we must travel along the ray before we have crossed a gridcell.
    glm::vec3 tDelta(stepX / ray.direction.x,                    // Crossing the width of a cell.
                     stepY / ray.direction.y,                    // Crossing the height of a cell.
                     stepZ / ray.direction.z);                   // Crossing the depth of a cell.
    if (isnan(tDelta.x)) { tDelta.x = +INFINITY; }
    if (isnan(tDelta.y)) { tDelta.y = +INFINITY; }
    if (isnan(tDelta.z)) { tDelta.z = +INFINITY; }

    // For each step, determine which distance to the next voxel boundary is lowest (i.e.
    // which voxel boundary is nearest) and walk that way.
    for (size_t i = 0; i < maxDepth; ++i) {
        sink(glm::vec3(x, y, z));

        // Do the next step.
        if (tMax.x < tMax.y && tMax.x < tMax.z) {
            // tMax.X is the lowest, an YZ cell boundary plane is nearest.
            x += stepX;
            tMax.x += tDelta.x;
        } else if (tMax.y < tMax.z) {
            // tMax.Y is the lowest, an XZ cell boundary plane is nearest.
            y += stepY;
            tMax.y += tDelta.y;
        } else {
            // tMax.Z is the lowest, an XY cell boundary plane is nearest.
            z += stepZ;
            tMax.z += tDelta.z;
        }
    }
}

} // namespace GridRaycastRange

// Return a range to iterate over grid cells which fall on the specified ray.
// The ray is specified in world space coordinates.
inline boost::coroutines2::coroutine<glm::vec3>::pull_type
slice(const GridIndexer &grid,
      const Ray &ray,
      size_t maxDepth)
{
    if constexpr (EnableVerboseBoundsChecking) {
        if (!grid.inbounds(ray.origin)) {
            throw OutOfBoundsException(fmt::format("OutOfBoundsException -- grid.boundingBox={} ; ray.origin={}",
                                                   grid.boundingBox(),
                                                   glm::to_string(ray.origin)));
        }
    }

#if 0
    // TODO: Fix the conversion from world-space ray to cell-space direction.
    // Convert the world-space ray direction to a cell-space direction.
    const AABB box = grid.boundingBox();
    const glm::vec3 mins = box.mins();
    const glm::vec3 p = (ray.direction - mins) / (box.extent*2.0f);
    const glm::ivec3 res = grid.gridResolution();
    const glm::vec3 ccDir(glm::normalize(glm::vec3(p.x * res.x, p.y * res.y, p.z * res.z)));

    // Convert the world-space ray origin to cell-space.
    const glm::ivec3 iccOrigin = grid.cellCoordsAtPoint(ray.origin);
    const glm::vec3 ccOrigin(iccOrigin.x, iccOrigin.y, iccOrigin.z);

    return boost::coroutines2::coroutine<glm::vec3>::pull_type([&](boost::coroutines2::coroutine<glm::vec3>::push_type &sink){
        GridRaycastRange::raycast(sink, Ray(ccOrigin, ccDir), maxDepth);
    });
#else
    return boost::coroutines2::coroutine<glm::vec3>::pull_type([&](boost::coroutines2::coroutine<glm::vec3>::push_type &sink){
        GridRaycastRange::raycast(sink, ray, maxDepth);
    });
#endif
}

#endif /* GridRaycast_hpp */


/*
 * File: pybind.cc
 * Created Date: 2019-09-11
 * Author: Lei Pan
 * Contact: <panlei7@gmail.com>
 *
 * Last Modified: Wednesday September 25th 2019 11:37:53 am
 *
 * MIT License
 *
 * Copyright (c) 2019 Lei Pan
 *
 * 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.
 * -----
 * HISTORY:
 * Date      	 By	Comments
 * ----------	---
 * ----------------------------------------------------------
 */

#include "fast_hankel_transform.hpp"

#include <Eigen/Dense>
#include <pybind11/complex.h>
#include <pybind11/eigen.h>
#include <pybind11/pybind11.h>

namespace py = pybind11;
using namespace Eigen;
using CRefCMat = const Ref<const MatrixXd>;

PYBIND11_MODULE(fhtcxx, m) {
  py::class_<FastHankelTransform>(m, "FastHankelTransform")
      .def(py::init<int, double, double>())
      .def("sampling", &FastHankelTransform::sampling)
      .def("set_feval", &FastHankelTransform::set_feval)
      .def("calculate", &FastHankelTransform::calculate);
}


// BLAS level 1 (matrix rows and columns)

//#define BOOST_NO_FUNCTION_TEMPLATE_ORDERING

#include <iostream>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/ublas/vector_proxy.hpp>
#include <boost/numeric/bindings/blas/level1.hpp>
#include "utils.h"

namespace blas = boost::numeric::bindings::blas;
namespace bindings = boost::numeric::bindings;
namespace ublas = boost::numeric::ublas;

using std::cout;
using std::endl;
using std::size_t;

typedef ublas::vector<double> vct_t;
typedef ublas::matrix<double> matr_t;
typedef ublas::matrix_row<matr_t> mr_t;
typedef ublas::matrix_column<matr_t> mc_t;
typedef ublas::matrix_range<matr_t> mrng_t;
typedef ublas::matrix_slice<matr_t> msl_t;
typedef ublas::matrix_slice<matr_t const> cmsl_t;

int main() {

  cout << endl;

  int r = 7;
  int c = 8;
  matr_t m (r, c);
  init_m (m, times_plus<double> (10, 1, 1));
  print_m (m, "m");
  cout << endl;

  vct_t v (10);
  blas::set (0., v);
  print_v (v, "v");
  cout << endl;

  // m[2,.] <- 0.1 m[2,.]
  mr_t mr2 (m, 2);
  blas::scal (0.1, mr2);
  print_m (m, "0.1 m[2,.]");
  cout << endl;

  // m[2,.] <-> m[4,.]
  mr_t mr4 (m, 4);
  blas::swap (mr2, mr4);
  print_m (m, "m[2,.] <-> m[4,.]");
  cout << endl;

  // m[4,.] <- m[2,.]
  blas::copy (mr2, mr4);
  print_m (m, "m[4,.] <- m[2,.]");
  cout << endl;

  // v[2..6] <- 10 m[5,.][1..5]
  mr_t mr5 (m, 5);
  ublas::vector_range<vct_t> vr (v, ublas::range (2, 6));
  ublas::vector_range<mr_t> mr5r (mr5, ublas::range (1, 5));
  blas::axpy (10.0, mr5r, vr);
  print_v (v, "v[2..6] <- 10 m[5,.][1..5]");
  cout << endl;

  // ||m[.,3]||_1, ||m[.,3]||_2
  mc_t mc3 (m, 3);
  cout << "||m[.,3]||_1 = " << blas::asum (mc3) << endl;
  cout << "||m[.,3]||_2 = " << blas::nrm2 (mc3) << endl;
  cout << endl;

  // m[.,5] <- 0.01 m[.,3] + m[.,5]
  mc_t mc5 (m, 5);
  blas::axpy (0.01, mc3, mc5);
  print_m (m, "m[.,5] <- 0.01 m[.,3] + m[.,5]");
  cout << endl;

  // 0.1 m[.,5][1:2:3]
  ublas::vector_slice<mc_t> mc5s (mc5, ublas::slice (1, 2, 3));
  blas::scal (0.1, mc5s);
  print_m (m, "0.1 m[.,5][1:2:3]");
  cout << endl;

  // 0.1 m[4,.][1:2:4][1..3]
  ublas::vector_slice<mr_t> mr4s (mr4, ublas::slice (1, 2, 4));
  ublas::vector_range<ublas::vector_slice<mr_t> >
    mr4sr (mr4s, ublas::range (1, 3));
  blas::scal (0.1, mr4sr);
  print_m (m, "0.1 m[4,.][1:2:4][1..3]");
  cout << endl;

  // new initialization
  init_m (m, times_plus<double> (10, 1, 1));
#ifndef F_USE_DETAIL
  for (int i = 0; i < m.size1(); ++i) {
    mr_t mri (m, i);
    blas::scal (0.1, mri);
  }
#else
  // cblas level 1 function applied to matrix
  blas::detail::scal (traits::matrix_storage_size (m),
                       0.1, traits::matrix_storage (m), 1);
#endif
  matr_t const cm (m);
  print_m (cm, "new m, cm == const m");
  cout << endl;

  // m[2..6][1..8]
  mrng_t mrng (m, ublas::range (2, 6), ublas::range (1, 8));
  print_m (mrng, "mrng = m[2..6][1..8]");
  cout << endl;

  // mrng[1,.] <-> mrng[2,.]
  ublas::matrix_row<mrng_t> mrngr1 (mrng, 1);
  ublas::matrix_row<mrng_t> mrngr2 (mrng, 2);
  blas::swap (mrngr1, mrngr2);
  print_m (m, "mrng[1,.] <-> mrng[2,.]");
  cout << endl;

  // mrng[2,.] <-> mrng[1,.]
  blas::swap (mrngr2, mrngr1);
  print_m (m, "mrng[2,.] <-> mrng[1,.]");
  cout << endl;

  // mrng[.,3] <- 0.01 mrng[.,5] + mrng[.,3]
  ublas::matrix_column<mrng_t> mrngc3 (mrng, 3);
  ublas::matrix_column<mrng_t> const mrngc5 (mrng, 5);
  blas::axpy (0.01, mrngc5, mrngc3);
  print_m (m, "mrng[.,3] <- 0.01 mrng[.,5] + mrng[.,3]");
  cout << endl;

  // cm[1:2:3][2:3:2]
  cmsl_t msl (cm, ublas::slice (1, 2, 3), ublas::slice (2, 3, 2));
  print_m (msl, "cmsl = cm[1:2:3][2:3:2]");
  cout << endl;

  // ||cmsl[.,0]||_1
  ublas::matrix_column<cmsl_t> mslc0 (msl, 0);
  cout << "||cmsl[.,0]||_1 = " << blas::asum (mslc0) << endl;
  cout << endl;

  // mrng[.,3][1..4] <= 0.0001 cmsl[.,0] + mrng[.,3][1..4]
  ublas::vector_range<ublas::matrix_column<mrng_t> >
    vrmrngc3 (mrngc3, ublas::range (1, 4));
  blas::axpy (0.0001, mslc0, vrmrngc3);
  print_m (m, "mrng[.,3][1..4] <= 0.0001 cmsl[.,0] + mrng[.,3][1..4]");
  cout << endl;

}


// ---------------------------------------------------------------------
//
// Copyright (c) 2017 - 2019 by the IBAMR developers
// All rights reserved.
//
// This file is part of IBAMR.
//
// IBAMR is free software and is distributed under the 3-clause BSD
// license. The full text of the license can be found in the file
// COPYRIGHT at the top level directory of IBAMR.
//
// ---------------------------------------------------------------------

// Config files
#include <IBAMR_config.h>
#include <IBTK_config.h>

#include <SAMRAI_config.h>

// Headers for basic PETSc functions
#include <petscsys.h>

// Headers for basic SAMRAI objects
#include <BergerRigoutsos.h>
#include <CartesianGridGeometry.h>
#include <LoadBalancer.h>
#include <StandardTagAndInitialize.h>

// Headers for basic libMesh objects
#include <libmesh/boundary_info.h>
#include <libmesh/boundary_mesh.h>
#include <libmesh/equation_systems.h>
#include <libmesh/exodusII_io.h>
#include <libmesh/mesh.h>
#include <libmesh/mesh_generation.h>
#include <libmesh/mesh_triangle_interface.h>

// Headers for application-specific algorithm/data structure objects
#include <ibamr/IBExplicitHierarchyIntegrator.h>
#include <ibamr/IBFEMethod.h>
#include <ibamr/IBFESurfaceMethod.h>
#include <ibamr/INSCollocatedHierarchyIntegrator.h>
#include <ibamr/INSStaggeredHierarchyIntegrator.h>

#include <ibtk/AppInitializer.h>
#include <ibtk/LEInteractor.h>
#include <ibtk/libmesh_utilities.h>
#include <ibtk/muParserCartGridFunction.h>
#include <ibtk/muParserRobinBcCoefs.h>

#include <boost/multi_array.hpp>

// Set up application namespace declarations
#include <ibamr/app_namespaces.h>

inline double
kernel(double x)
{
    x += 4.;
    const double x2 = x * x;
    const double x3 = x * x2;
    const double x4 = x * x3;
    const double x5 = x * x4;
    const double x6 = x * x5;
    const double x7 = x * x6;
    if (x <= 0.)
        return 0.;
    else if (x <= 1.)
        return .1984126984126984e-3 * x7;
    else if (x <= 2.)
        return .1111111111111111e-1 * x6 - .1388888888888889e-2 * x7 - .3333333333333333e-1 * x5 +
               .5555555555555556e-1 * x4 - .5555555555555556e-1 * x3 + .3333333333333333e-1 * x2 -
               .1111111111111111e-1 * x + .1587301587301587e-2;
    else if (x <= 3.)
        return .4333333333333333 * x5 - .6666666666666667e-1 * x6 + .4166666666666667e-2 * x7 - 1.500000000000000 * x4 +
               3.055555555555556 * x3 - 3.700000000000000 * x2 + 2.477777777777778 * x - .7095238095238095;
    else if (x <= 4.)
        return 9. * x4 - 1.666666666666667 * x5 + .1666666666666667 * x6 - .6944444444444444e-2 * x7 -
               28.44444444444444 * x3 + 53. * x2 - 54.22222222222222 * x + 23.59047619047619;
    else if (x <= 5.)
        return 96. * x3 - 22.11111111111111 * x4 + 3. * x5 - .2222222222222222 * x6 + .6944444444444444e-2 * x7 -
               245.6666666666667 * x2 + 344. * x - 203.9650793650794;
    else if (x <= 6.)
        return 483.5000000000000 * x2 - 147.0555555555556 * x3 + 26.50000000000000 * x4 - 2.833333333333333 * x5 +
               .1666666666666667 * x6 - .4166666666666667e-2 * x7 - 871.2777777777778 * x + 664.0904761904762;
    else if (x <= 7.)
        return 943.1222222222222 * x - 423.7000000000000 * x2 + 104.9444444444444 * x3 - 15.50000000000000 * x4 +
               1.366666666666667 * x5 - .6666666666666667e-1 * x6 + .1388888888888889e-2 * x7 - 891.1095238095238;
    else if (x <= 8.)
        return 416.1015873015873 - 364.0888888888889 * x + 136.5333333333333 * x2 - 28.44444444444444 * x3 +
               3.555555555555556 * x4 - .2666666666666667 * x5 + .1111111111111111e-1 * x6 - .1984126984126984e-3 * x7;
    else
        return 0.;
} // kernel

// Elasticity model data.
namespace ModelData
{
// The tether penalty functions each require some data that is set in the
// input file. This data is passed to each object through the void *ctx
// context data pointer. Here we collect all relevant tether data in a struct:
struct TetherData
{
    const double c1_s;
    const double kappa_s_body;
    const double eta_s_body;
    const double kappa_s_surface;
    const double eta_s_surface;

    TetherData(Pointer<Database> input_db)
        : c1_s(input_db->getDouble("C1_S")),
          kappa_s_body(input_db->getDouble("KAPPA_S_BODY")),
          eta_s_body(input_db->getDouble("ETA_S_BODY")),
          kappa_s_surface(input_db->getDouble("KAPPA_S_SURFACE")),
          eta_s_surface(input_db->getDouble("ETA_S_SURFACE"))
    {
    }
};

// Tether (penalty) stress function.
void
PK1_stress_function(TensorValue<double>& PP,
                    const TensorValue<double>& FF,
                    const libMesh::Point& /*x*/,
                    const libMesh::Point& /*X*/,
                    Elem* const /*elem*/,
                    const vector<const vector<double>*>& /*var_data*/,
                    const vector<const vector<VectorValue<double> >*>& /*grad_var_data*/,
                    double /*time*/,
                    void* ctx)
{
    const TetherData* const tether_data = reinterpret_cast<TetherData*>(ctx);

    PP = 2.0 * tether_data->c1_s * (FF - tensor_inverse_transpose(FF, NDIM));
    return;
} // PK1_stress_function

// Tether (penalty) force functions.
void
tether_force_function(VectorValue<double>& F,
                      const TensorValue<double>& /*FF*/,
                      const libMesh::Point& x,
                      const libMesh::Point& X,
                      Elem* const /*elem*/,
                      const vector<const vector<double>*>& var_data,
                      const vector<const vector<VectorValue<double> >*>& /*grad_var_data*/,
                      double /*time*/,
                      void* ctx)
{
    const TetherData* const tether_data = reinterpret_cast<TetherData*>(ctx);

    const std::vector<double>& U = *var_data[0];
    for (unsigned int d = 0; d < NDIM; ++d)
    {
        F(d) = tether_data->kappa_s_body * (X(d) - x(d)) - tether_data->eta_s_body * U[d];
    }
    return;
} // tether_force_function

void
tether_force_function(VectorValue<double>& F,
                      const VectorValue<double>& n,
                      const VectorValue<double>& /*N*/,
                      const TensorValue<double>& /*FF*/,
                      const libMesh::Point& x,
                      const libMesh::Point& X,
                      Elem* const /*elem*/,
                      const unsigned short /*side*/,
                      const vector<const vector<double>*>& var_data,
                      const vector<const vector<VectorValue<double> >*>& /*grad_var_data*/,
                      double /*time*/,
                      void* ctx)
{
    const TetherData* const tether_data = reinterpret_cast<TetherData*>(ctx);

    VectorValue<double> D = X - x;
    VectorValue<double> D_n = (D * n) * n;
    VectorValue<double> U;
    for (unsigned int d = 0; d < NDIM; ++d) U(d) = (*var_data[0])[d];
    VectorValue<double> U_t = U - (U * n) * n;
    F = tether_data->kappa_s_surface * D - tether_data->eta_s_surface * U;
    return;
} // tether_force_function
} // namespace ModelData
using namespace ModelData;

// Function prototypes
static ofstream drag_stream, lift_stream, U_L1_norm_stream, U_L2_norm_stream, U_max_norm_stream;
void postprocess_data(Pointer<Database> input_db,
                      Pointer<PatchHierarchy<NDIM> > patch_hierarchy,
                      Pointer<INSHierarchyIntegrator> navier_stokes_integrator,
                      Mesh& mesh,
                      EquationSystems* equation_systems,
                      const int iteration_num,
                      const double loop_time,
                      const string& data_dump_dirname);

/*******************************************************************************
 * For each run, the input filename and restart information (if needed) must   *
 * be given on the command line.  For non-restarted case, command line is:     *
 *                                                                             *
 *    executable <input file name>                                             *
 *                                                                             *
 * For restarted run, command line is:                                         *
 *                                                                             *
 *    executable <input file name> <restart directory> <restart number>        *
 *                                                                             *
 *******************************************************************************/

int
main(int argc, char* argv[])
{
    // Initialize libMesh, PETSc, MPI, and SAMRAI.
    LibMeshInit init(argc, argv);
    SAMRAI_MPI::setCommunicator(PETSC_COMM_WORLD);
    SAMRAI_MPI::setCallAbortInSerialInsteadOfExit();
    SAMRAIManager::startup();

    { // cleanup dynamically allocated objects prior to shutdown

        // Parse command line options, set some standard options from the input
        // file, initialize the restart database (if this is a restarted run),
        // and enable file logging.
        Pointer<AppInitializer> app_initializer = new AppInitializer(argc, argv, "IB.log");
        Pointer<Database> input_db = app_initializer->getInputDatabase();

        // Setup user-defined kernel function.
        LEInteractor::s_kernel_fcn = &kernel;
        LEInteractor::s_kernel_fcn_stencil_size = 8;

        // Get various standard options set in the input file.
        const bool dump_viz_data = app_initializer->dumpVizData();
        const int viz_dump_interval = app_initializer->getVizDumpInterval();
        const bool uses_visit = dump_viz_data && app_initializer->getVisItDataWriter();
#ifdef LIBMESH_HAVE_EXODUS_API
        const bool uses_exodus = dump_viz_data && !app_initializer->getExodusIIFilename().empty();
#else
        const bool uses_exodus = false;
        if (!app_initializer->getExodusIIFilename().empty())
        {
            plog << "WARNING: libMesh was compiled without Exodus support, so no "
                 << "Exodus output will be written in this program.\n";
        }
#endif
        const string exodus_filename = app_initializer->getExodusIIFilename();

        const bool dump_restart_data = app_initializer->dumpRestartData();
        const int restart_dump_interval = app_initializer->getRestartDumpInterval();
        const string restart_dump_dirname = app_initializer->getRestartDumpDirectory();
        const string restart_read_dirname = app_initializer->getRestartReadDirectory();
        const int restart_restore_num = app_initializer->getRestartRestoreNumber();

        const bool dump_postproc_data = app_initializer->dumpPostProcessingData();
        const int postproc_data_dump_interval = app_initializer->getPostProcessingDataDumpInterval();
        const string postproc_data_dump_dirname = app_initializer->getPostProcessingDataDumpDirectory();
        if (dump_postproc_data && (postproc_data_dump_interval > 0) && !postproc_data_dump_dirname.empty())
        {
            Utilities::recursiveMkdir(postproc_data_dump_dirname);
        }

        const bool dump_timer_data = app_initializer->dumpTimerData();
        const int timer_dump_interval = app_initializer->getTimerDumpInterval();

        // Create a simple FE mesh.
        Mesh solid_mesh(init.comm(), NDIM);
        const double dx = input_db->getDouble("DX");
        const double ds = input_db->getDouble("MFAC") * dx;
        string elem_type = input_db->getString("ELEM_TYPE");
        const double R = 0.5;
        if (NDIM == 2 && (elem_type == "TRI3" || elem_type == "TRI6"))
        {
#ifdef LIBMESH_HAVE_TRIANGLE
            const int num_circum_nodes = ceil(2.0 * M_PI * R / ds);
            for (int k = 0; k < num_circum_nodes; ++k)
            {
                const double theta = 2.0 * M_PI * static_cast<double>(k) / static_cast<double>(num_circum_nodes);
                solid_mesh.add_point(libMesh::Point(R * cos(theta), R * sin(theta)));
            }
            TriangleInterface triangle(solid_mesh);
            triangle.triangulation_type() = TriangleInterface::GENERATE_CONVEX_HULL;
            triangle.elem_type() = Utility::string_to_enum<ElemType>(elem_type);
            triangle.desired_area() = 1.5 * sqrt(3.0) / 4.0 * ds * ds;
            triangle.insert_extra_points() = true;
            triangle.smooth_after_generating() = true;
            triangle.triangulate();
#else
            TBOX_ERROR("ERROR: libMesh appears to have been configured without support for Triangle,\n"
                       << "       but Triangle is required for TRI3 or TRI6 elements.\n");
#endif
        }
        else
        {
            // NOTE: number of segments along boundary is 4*2^r.
            const double num_circum_segments = 2.0 * M_PI * R / ds;
            const int r = log2(0.25 * num_circum_segments);
            MeshTools::Generation::build_sphere(solid_mesh, R, r, Utility::string_to_enum<ElemType>(elem_type));
        }

        // Ensure nodes on the surface are on the analytic boundary.
        MeshBase::element_iterator el_end = solid_mesh.elements_end();
        for (MeshBase::element_iterator el = solid_mesh.elements_begin(); el != el_end; ++el)
        {
            Elem* const elem = *el;
            for (unsigned int side = 0; side < elem->n_sides(); ++side)
            {
                const bool at_mesh_bdry = !elem->neighbor_ptr(side);
                if (!at_mesh_bdry) continue;
                for (unsigned int k = 0; k < elem->n_nodes(); ++k)
                {
                    if (!elem->is_node_on_side(k, side)) continue;
                    Node& n = elem->node_ref(k);
                    n = R * n.unit();
                }
            }
        }
        solid_mesh.prepare_for_use();

        BoundaryMesh boundary_mesh(solid_mesh.comm(), solid_mesh.mesh_dimension() - 1);
        solid_mesh.boundary_info->sync(boundary_mesh);
        boundary_mesh.prepare_for_use();

        bool use_boundary_mesh = input_db->getBoolWithDefault("USE_BOUNDARY_MESH", false);
        Mesh& mesh = use_boundary_mesh ? boundary_mesh : solid_mesh;

        // Create major algorithm and data objects that comprise the
        // application. These objects are configured from the input database
        // and, if this is a restarted run, from the restart database.
        Pointer<INSHierarchyIntegrator> navier_stokes_integrator;
        const string solver_type = app_initializer->getComponentDatabase("Main")->getString("solver_type");
        if (solver_type == "STAGGERED")
        {
            navier_stokes_integrator = new INSStaggeredHierarchyIntegrator(
                "INSStaggeredHierarchyIntegrator",
                app_initializer->getComponentDatabase("INSStaggeredHierarchyIntegrator"));
        }
        else if (solver_type == "COLLOCATED")
        {
            navier_stokes_integrator = new INSCollocatedHierarchyIntegrator(
                "INSCollocatedHierarchyIntegrator",
                app_initializer->getComponentDatabase("INSCollocatedHierarchyIntegrator"));
        }
        else
        {
            TBOX_ERROR("Unsupported solver type: " << solver_type << "\n"
                                                   << "Valid options are: COLLOCATED, STAGGERED");
        }
        Pointer<IBStrategy> ib_ops;
        if (use_boundary_mesh)
        {
            ib_ops = new IBFESurfaceMethod(
                "IBFEMethod",
                app_initializer->getComponentDatabase("IBFEMethod"),
                &mesh,
                app_initializer->getComponentDatabase("GriddingAlgorithm")->getInteger("max_levels"),
                /*register_for_restart*/ true,
                restart_read_dirname,
                restart_restore_num);
        }
        else
        {
            ib_ops =
                new IBFEMethod("IBFEMethod",
                               app_initializer->getComponentDatabase("IBFEMethod"),
                               &mesh,
                               app_initializer->getComponentDatabase("GriddingAlgorithm")->getInteger("max_levels"),
                               /*register_for_restart*/ true,
                               restart_read_dirname,
                               restart_restore_num);
        }
        Pointer<IBHierarchyIntegrator> time_integrator =
            new IBExplicitHierarchyIntegrator("IBHierarchyIntegrator",
                                              app_initializer->getComponentDatabase("IBHierarchyIntegrator"),
                                              ib_ops,
                                              navier_stokes_integrator);
        Pointer<CartesianGridGeometry<NDIM> > grid_geometry = new CartesianGridGeometry<NDIM>(
            "CartesianGeometry", app_initializer->getComponentDatabase("CartesianGeometry"));
        Pointer<PatchHierarchy<NDIM> > patch_hierarchy = new PatchHierarchy<NDIM>("PatchHierarchy", grid_geometry);
        Pointer<StandardTagAndInitialize<NDIM> > error_detector =
            new StandardTagAndInitialize<NDIM>("StandardTagAndInitialize",
                                               time_integrator,
                                               app_initializer->getComponentDatabase("StandardTagAndInitialize"));
        Pointer<BergerRigoutsos<NDIM> > box_generator = new BergerRigoutsos<NDIM>();
        Pointer<LoadBalancer<NDIM> > load_balancer =
            new LoadBalancer<NDIM>("LoadBalancer", app_initializer->getComponentDatabase("LoadBalancer"));
        Pointer<GriddingAlgorithm<NDIM> > gridding_algorithm =
            new GriddingAlgorithm<NDIM>("GriddingAlgorithm",
                                        app_initializer->getComponentDatabase("GriddingAlgorithm"),
                                        error_detector,
                                        box_generator,
                                        load_balancer);

        // Configure the IBFE solver.
        TetherData tether_data(input_db);
        void* const tether_data_ptr = reinterpret_cast<void*>(&tether_data);
        EquationSystems* equation_systems;
        std::vector<int> vars(NDIM);
        for (unsigned int d = 0; d < NDIM; ++d) vars[d] = d;
        vector<SystemData> sys_data(1, SystemData(IBFEMethod::VELOCITY_SYSTEM_NAME, vars));
        if (use_boundary_mesh)
        {
            Pointer<IBFESurfaceMethod> ibfe_ops = ib_ops;
            ibfe_ops->initializeFEEquationSystems();
            equation_systems = ibfe_ops->getFEDataManager()->getEquationSystems();
            IBFESurfaceMethod::LagSurfaceForceFcnData surface_fcn_data(
                tether_force_function, sys_data, tether_data_ptr);
            ibfe_ops->registerLagSurfaceForceFunction(surface_fcn_data);
        }
        else
        {
            Pointer<IBFEMethod> ibfe_ops = ib_ops;
            ibfe_ops->initializeFEEquationSystems();
            equation_systems = ibfe_ops->getFEDataManager()->getEquationSystems();
            IBFEMethod::PK1StressFcnData PK1_stress_data(
                PK1_stress_function, std::vector<IBTK::SystemData>(), tether_data_ptr);
            PK1_stress_data.quad_order =
                Utility::string_to_enum<libMesh::Order>(input_db->getStringWithDefault("PK1_QUAD_ORDER", "THIRD"));
            ibfe_ops->registerPK1StressFunction(PK1_stress_data);

            IBFEMethod::LagBodyForceFcnData body_fcn_data(tether_force_function, sys_data, tether_data_ptr);
            ibfe_ops->registerLagBodyForceFunction(body_fcn_data);

            IBFEMethod::LagSurfaceForceFcnData surface_fcn_data(tether_force_function, sys_data, tether_data_ptr);
            ibfe_ops->registerLagSurfaceForceFunction(surface_fcn_data);

            if (input_db->getBoolWithDefault("ELIMINATE_PRESSURE_JUMPS", false))
            {
                ibfe_ops->registerStressNormalizationPart();
            }
        }

        // Create Eulerian initial condition specification objects.
        if (input_db->keyExists("VelocityInitialConditions"))
        {
            Pointer<CartGridFunction> u_init = new muParserCartGridFunction(
                "u_init", app_initializer->getComponentDatabase("VelocityInitialConditions"), grid_geometry);
            navier_stokes_integrator->registerVelocityInitialConditions(u_init);
        }

        if (input_db->keyExists("PressureInitialConditions"))
        {
            Pointer<CartGridFunction> p_init = new muParserCartGridFunction(
                "p_init", app_initializer->getComponentDatabase("PressureInitialConditions"), grid_geometry);
            navier_stokes_integrator->registerPressureInitialConditions(p_init);
        }

        // Create Eulerian boundary condition specification objects (when necessary).
        const IntVector<NDIM>& periodic_shift = grid_geometry->getPeriodicShift();
        vector<RobinBcCoefStrategy<NDIM>*> u_bc_coefs(NDIM);
        if (periodic_shift.min() > 0)
        {
            for (unsigned int d = 0; d < NDIM; ++d)
            {
                u_bc_coefs[d] = NULL;
            }
        }
        else
        {
            for (unsigned int d = 0; d < NDIM; ++d)
            {
                const std::string bc_coefs_name = "u_bc_coefs_" + std::to_string(d);

                const std::string bc_coefs_db_name = "VelocityBcCoefs_" + std::to_string(d);

                u_bc_coefs[d] = new muParserRobinBcCoefs(
                    bc_coefs_name, app_initializer->getComponentDatabase(bc_coefs_db_name), grid_geometry);
            }
            navier_stokes_integrator->registerPhysicalBoundaryConditions(u_bc_coefs);
        }

        // Create Eulerian body force function specification objects.
        if (input_db->keyExists("ForcingFunction"))
        {
            Pointer<CartGridFunction> f_fcn = new muParserCartGridFunction(
                "f_fcn", app_initializer->getComponentDatabase("ForcingFunction"), grid_geometry);
            time_integrator->registerBodyForceFunction(f_fcn);
        }

        // Set up visualization plot file writers.
        Pointer<VisItDataWriter<NDIM> > visit_data_writer = app_initializer->getVisItDataWriter();
        if (uses_visit)
        {
            time_integrator->registerVisItDataWriter(visit_data_writer);
        }
        std::unique_ptr<ExodusII_IO> exodus_io(uses_exodus ? new ExodusII_IO(mesh) : NULL);

        // Check to see if this is a restarted run to append current exodus files
        if (uses_exodus)
        {
            const bool from_restart = RestartManager::getManager()->isFromRestart();
            exodus_io->append(from_restart);
        }

        // Initialize hierarchy configuration and data on all patches.
        if (use_boundary_mesh)
        {
            Pointer<IBFESurfaceMethod> ibfe_ops = ib_ops;
            ibfe_ops->initializeFEData();
        }
        else
        {
            Pointer<IBFEMethod> ibfe_ops = ib_ops;
            ibfe_ops->initializeFEData();
        }
        time_integrator->initializePatchHierarchy(patch_hierarchy, gridding_algorithm);

        // Deallocate initialization objects.
        app_initializer.setNull();

        // Print the input database contents to the log file.
        plog << "Input database:\n";
        input_db->printClassData(plog);

        // Write out initial visualization data.
        int iteration_num = time_integrator->getIntegratorStep();
        double loop_time = time_integrator->getIntegratorTime();
        if (dump_viz_data)
        {
            pout << "\n\nWriting visualization files...\n\n";
            if (uses_visit)
            {
                time_integrator->setupPlotData();
                visit_data_writer->writePlotData(patch_hierarchy, iteration_num, loop_time);
            }
            if (uses_exodus)
            {
                exodus_io->write_timestep(
                    exodus_filename, *equation_systems, iteration_num / viz_dump_interval + 1, loop_time);
            }
        }

        // Open streams to save lift and drag coefficients and the norms of the
        // velocity.
        if (SAMRAI_MPI::getRank() == 0)
        {
            drag_stream.open("C_D.curve", ios_base::out | ios_base::trunc);
            lift_stream.open("C_L.curve", ios_base::out | ios_base::trunc);
            U_L1_norm_stream.open("U_L1.curve", ios_base::out | ios_base::trunc);
            U_L2_norm_stream.open("U_L2.curve", ios_base::out | ios_base::trunc);
            U_max_norm_stream.open("U_max.curve", ios_base::out | ios_base::trunc);

            drag_stream.precision(10);
            lift_stream.precision(10);
            U_L1_norm_stream.precision(10);
            U_L2_norm_stream.precision(10);
            U_max_norm_stream.precision(10);
        }

        // Main time step loop.
        double loop_time_end = time_integrator->getEndTime();
        double dt = 0.0;
        while (!MathUtilities<double>::equalEps(loop_time, loop_time_end) && time_integrator->stepsRemaining())
        {
            iteration_num = time_integrator->getIntegratorStep();
            loop_time = time_integrator->getIntegratorTime();

            pout << "\n";
            pout << "+++++++++++++++++++++++++++++++++++++++++++++++++++\n";
            pout << "At beginning of timestep # " << iteration_num << "\n";
            pout << "Simulation time is " << loop_time << "\n";

            dt = time_integrator->getMaximumTimeStepSize();
            time_integrator->advanceHierarchy(dt);
            loop_time += dt;

            pout << "\n";
            pout << "At end       of timestep # " << iteration_num << "\n";
            pout << "Simulation time is " << loop_time << "\n";
            pout << "+++++++++++++++++++++++++++++++++++++++++++++++++++\n";
            pout << "\n";

            // At specified intervals, write visualization and restart files,
            // print out timer data, and store hierarchy data for post
            // processing.
            iteration_num += 1;
            const bool last_step = !time_integrator->stepsRemaining();
            if (dump_viz_data && (iteration_num % viz_dump_interval == 0 || last_step))
            {
                pout << "\nWriting visualization files...\n\n";
                if (uses_visit)
                {
                    time_integrator->setupPlotData();
                    visit_data_writer->writePlotData(patch_hierarchy, iteration_num, loop_time);
                }
                if (uses_exodus)
                {
                    exodus_io->write_timestep(
                        exodus_filename, *equation_systems, iteration_num / viz_dump_interval + 1, loop_time);
                }
            }
            if (dump_restart_data && (iteration_num % restart_dump_interval == 0 || last_step))
            {
                pout << "\nWriting restart files...\n\n";
                RestartManager::getManager()->writeRestartFile(restart_dump_dirname, iteration_num);
                if (use_boundary_mesh)
                {
                    dynamic_cast<IBFESurfaceMethod&>(*ib_ops).writeFEDataToRestartFile(restart_dump_dirname,
                                                                                iteration_num);
                }
                else
                {
                    dynamic_cast<IBFEMethod&>(*ib_ops).writeFEDataToRestartFile(restart_dump_dirname, iteration_num);
                }
            }
            if (dump_timer_data && (iteration_num % timer_dump_interval == 0 || last_step))
            {
                pout << "\nWriting timer data...\n\n";
                TimerManager::getManager()->print(plog);
            }
            if (dump_postproc_data && (iteration_num % postproc_data_dump_interval == 0 || last_step))
            {
                postprocess_data(input_db,
                                 patch_hierarchy,
                                 navier_stokes_integrator,
                                 mesh,
                                 equation_systems,
                                 iteration_num,
                                 loop_time,
                                 postproc_data_dump_dirname);
            }
        }

        // Close the logging streams.
        if (SAMRAI_MPI::getRank() == 0)
        {
            drag_stream.close();
            lift_stream.close();
            U_L1_norm_stream.close();
            U_L2_norm_stream.close();
            U_max_norm_stream.close();
        }

        // Cleanup Eulerian boundary condition specification objects (when
        // necessary).
        for (unsigned int d = 0; d < NDIM; ++d) delete u_bc_coefs[d];

    } // cleanup dynamically allocated objects prior to shutdown

    SAMRAIManager::shutdown();
} // main

void
postprocess_data(Pointer<Database> input_db,
                 Pointer<PatchHierarchy<NDIM> > /*patch_hierarchy*/,
                 Pointer<INSHierarchyIntegrator> /*navier_stokes_integrator*/,
                 Mesh& mesh,
                 EquationSystems* equation_systems,
                 const int /*iteration_num*/,
                 const double loop_time,
                 const string& /*data_dump_dirname*/)
{
    TetherData tether_data(input_db);
    void* const tether_data_ptr = reinterpret_cast<void*>(&tether_data);

    const unsigned int dim = mesh.mesh_dimension();
    double F_integral[NDIM];
    for (unsigned int d = 0; d < NDIM; ++d) F_integral[d] = 0.0;

    System& x_system = equation_systems->get_system(IBFEMethod::COORDS_SYSTEM_NAME);
    System& U_system = equation_systems->get_system(IBFEMethod::VELOCITY_SYSTEM_NAME);
    NumericVector<double>* x_vec = x_system.solution.get();
    NumericVector<double>* x_ghost_vec = x_system.current_local_solution.get();
    x_vec->localize(*x_ghost_vec);
    NumericVector<double>* U_vec = U_system.solution.get();
    NumericVector<double>* U_ghost_vec = U_system.current_local_solution.get();
    U_vec->localize(*U_ghost_vec);
    const DofMap& dof_map = x_system.get_dof_map();
    std::vector<std::vector<unsigned int> > dof_indices(NDIM);

    std::unique_ptr<FEBase> fe(FEBase::build(dim, dof_map.variable_type(0)));
    std::unique_ptr<QBase> qrule = QBase::build(QGAUSS, dim, SEVENTH);
    fe->attach_quadrature_rule(qrule.get());
    const vector<double>& JxW = fe->get_JxW();
    const vector<libMesh::Point>& q_point = fe->get_xyz();
    const vector<vector<double> >& phi = fe->get_phi();
    const vector<vector<VectorValue<double> > >& dphi = fe->get_dphi();

    std::unique_ptr<FEBase> fe_face(FEBase::build(dim, dof_map.variable_type(0)));
    std::unique_ptr<QBase> qrule_face = QBase::build(QGAUSS, dim - 1, SEVENTH);
    fe_face->attach_quadrature_rule(qrule_face.get());
    const vector<double>& JxW_face = fe_face->get_JxW();
    const vector<libMesh::Point>& q_point_face = fe_face->get_xyz();
    const vector<libMesh::Point>& normal_face = fe_face->get_normals();
    const vector<vector<double> >& phi_face = fe_face->get_phi();
    const vector<vector<VectorValue<double> > >& dphi_face = fe_face->get_dphi();

    std::vector<double> U_qp_vec(NDIM);
    std::vector<const std::vector<double>*> var_data(1);
    var_data[0] = &U_qp_vec;
    std::vector<const std::vector<libMesh::VectorValue<double> >*> grad_var_data;

    TensorValue<double> FF, FF_inv_trans;
    boost::multi_array<double, 2> x_node, U_node;
    VectorValue<double> F, N, U, n, x;

    const MeshBase::const_element_iterator el_begin = mesh.active_local_elements_begin();
    const MeshBase::const_element_iterator el_end = mesh.active_local_elements_end();
    for (MeshBase::const_element_iterator el_it = el_begin; el_it != el_end; ++el_it)
    {
        Elem* const elem = *el_it;
        fe->reinit(elem);
        for (unsigned int d = 0; d < NDIM; ++d)
        {
            dof_map.dof_indices(elem, dof_indices[d], d);
        }
        get_values_for_interpolation(x_node, *x_ghost_vec, dof_indices);
        get_values_for_interpolation(U_node, *U_ghost_vec, dof_indices);

        const unsigned int n_qp = qrule->n_points();
        for (unsigned int qp = 0; qp < n_qp; ++qp)
        {
            interpolate(x, qp, x_node, phi);
            jacobian(FF, qp, x_node, dphi);
            interpolate(U, qp, U_node, phi);
            for (unsigned int d = 0; d < NDIM; ++d)
            {
                U_qp_vec[d] = U(d);
            }
            tether_force_function(F, FF, x, q_point[qp], elem, var_data, grad_var_data, loop_time, tether_data_ptr);
            for (int d = 0; d < NDIM; ++d)
            {
                F_integral[d] += F(d) * JxW[qp];
            }
        }
        for (unsigned short int side = 0; side < elem->n_sides(); ++side)
        {
            if (elem->neighbor_ptr(side)) continue;
            fe_face->reinit(elem, side);
            const unsigned int n_qp_face = qrule_face->n_points();
            for (unsigned int qp = 0; qp < n_qp_face; ++qp)
            {
                interpolate(x, qp, x_node, phi_face);
                jacobian(FF, qp, x_node, dphi_face);
                interpolate(U, qp, U_node, phi_face);
                for (unsigned int d = 0; d < NDIM; ++d)
                {
                    U_qp_vec[d] = U(d);
                }
                N = normal_face[qp];
                tensor_inverse_transpose(FF_inv_trans, FF, NDIM);
                n = (FF_inv_trans * N).unit();

                tether_force_function(
                    F, n, N, FF, x, q_point_face[qp], elem, side, var_data, grad_var_data, loop_time, tether_data_ptr);
                for (int d = 0; d < NDIM; ++d)
                {
                    F_integral[d] += F(d) * JxW_face[qp];
                }
            }
        }
    }
    SAMRAI_MPI::sumReduction(F_integral, NDIM);
    static const double rho = 1.0;
    static const double U_max = 1.0;
    static const double D = 1.0;
    if (SAMRAI_MPI::getRank() == 0)
    {
        drag_stream << loop_time << " " << -F_integral[0] / (0.5 * rho * U_max * U_max * D) << endl;
        lift_stream << loop_time << " " << -F_integral[1] / (0.5 * rho * U_max * U_max * D) << endl;
    }
    return;
} // postprocess_data


// #define EIGEN_TAUCS_SUPPORT
// #define EIGEN_CHOLMOD_SUPPORT
#include <iostream>
#include <Eigen/Sparse>

// g++ -DSIZE=10000 -DDENSITY=0.001  sparse_cholesky.cpp -I.. -DDENSEMATRI -O3 -g0 -DNDEBUG   -DNBTRIES=1 -I /home/gael/Coding/LinearAlgebra/taucs_full/src/ -I/home/gael/Coding/LinearAlgebra/taucs_full/build/linux/  -L/home/gael/Coding/LinearAlgebra/taucs_full/lib/linux/ -ltaucs /home/gael/Coding/LinearAlgebra/GotoBLAS/libgoto.a -lpthread -I /home/gael/Coding/LinearAlgebra/SuiteSparse/CHOLMOD/Include/ $CHOLLIB -I /home/gael/Coding/LinearAlgebra/SuiteSparse/UFconfig/ /home/gael/Coding/LinearAlgebra/SuiteSparse/CCOLAMD/Lib/libccolamd.a   /home/gael/Coding/LinearAlgebra/SuiteSparse/CHOLMOD/Lib/libcholmod.a -lmetis /home/gael/Coding/LinearAlgebra/SuiteSparse/AMD/Lib/libamd.a  /home/gael/Coding/LinearAlgebra/SuiteSparse/CAMD/Lib/libcamd.a   /home/gael/Coding/LinearAlgebra/SuiteSparse/CCOLAMD/Lib/libccolamd.a  /home/gael/Coding/LinearAlgebra/SuiteSparse/COLAMD/Lib/libcolamd.a -llapack && ./a.out

#define NOGMM
#define NOMTL

#ifndef SIZE
#define SIZE 10
#endif

#ifndef DENSITY
#define DENSITY 0.01
#endif

#ifndef REPEAT
#define REPEAT 1
#endif

#include "BenchSparseUtil.h"

#ifndef MINDENSITY
#define MINDENSITY 0.0004
#endif

#ifndef NBTRIES
#define NBTRIES 10
#endif

#define BENCH(X) \
  timer.reset(); \
  for (int _j=0; _j<NBTRIES; ++_j) { \
    timer.start(); \
    for (int _k=0; _k<REPEAT; ++_k) { \
        X  \
  } timer.stop(); }

// typedef SparseMatrix<Scalar,UpperTriangular> EigenSparseTriMatrix;
typedef SparseMatrix<Scalar,SelfAdjoint|LowerTriangular> EigenSparseSelfAdjointMatrix;

void fillSpdMatrix(float density, int rows, int cols,  EigenSparseSelfAdjointMatrix& dst)
{
  dst.startFill(rows*cols*density);
  for(int j = 0; j < cols; j++)
  {
    dst.fill(j,j) = internal::random<Scalar>(10,20);
    for(int i = j+1; i < rows; i++)
    {
      Scalar v = (internal::random<float>(0,1) < density) ? internal::random<Scalar>() : 0;
      if (v!=0)
        dst.fill(i,j) = v;
    }

  }
  dst.endFill();
}

#include <Eigen/Cholesky>

template<int Backend>
void doEigen(const char* name, const EigenSparseSelfAdjointMatrix& sm1, int flags = 0)
{
  std::cout << name << "..." << std::flush;
  BenchTimer timer;
  timer.start();
  SparseLLT<EigenSparseSelfAdjointMatrix,Backend> chol(sm1, flags);
  timer.stop();
  std::cout << ":\t" << timer.value() << endl;

  std::cout << "  nnz: " << sm1.nonZeros() << " => " << chol.matrixL().nonZeros() << "\n";
//   std::cout << "sparse\n" << chol.matrixL() << "%\n";
}

int main(int argc, char *argv[])
{
  int rows = SIZE;
  int cols = SIZE;
  float density = DENSITY;
  BenchTimer timer;

  VectorXf b = VectorXf::Random(cols);
  VectorXf x = VectorXf::Random(cols);

  bool densedone = false;

  //for (float density = DENSITY; density>=MINDENSITY; density*=0.5)
//   float density = 0.5;
  {
    EigenSparseSelfAdjointMatrix sm1(rows, cols);
    std::cout << "Generate sparse matrix (might take a while)...\n";
    fillSpdMatrix(density, rows, cols, sm1);
    std::cout << "DONE\n\n";

    // dense matrices
    #ifdef DENSEMATRIX
    if (!densedone)
    {
      densedone = true;
      std::cout << "Eigen Dense\t" << density*100 << "%\n";
      DenseMatrix m1(rows,cols);
      eiToDense(sm1, m1);
      m1 = (m1 + m1.transpose()).eval();
      m1.diagonal() *= 0.5;

//       BENCH(LLT<DenseMatrix> chol(m1);)
//       std::cout << "dense:\t" << timer.value() << endl;

      BenchTimer timer;
      timer.start();
      LLT<DenseMatrix> chol(m1);
      timer.stop();
      std::cout << "dense:\t" << timer.value() << endl;
      int count = 0;
      for (int j=0; j<cols; ++j)
        for (int i=j; i<rows; ++i)
          if (!internal::isMuchSmallerThan(internal::abs(chol.matrixL()(i,j)), 0.1))
            count++;
      std::cout << "dense: " << "nnz = " << count << "\n";
//       std::cout << "dense:\n" << m1 << "\n\n" << chol.matrixL() << endl;
    }
    #endif

    // eigen sparse matrices
    doEigen<Eigen::DefaultBackend>("Eigen/Sparse", sm1, Eigen::IncompleteFactorization);

    #ifdef EIGEN_CHOLMOD_SUPPORT
    doEigen<Eigen::Cholmod>("Eigen/Cholmod", sm1, Eigen::IncompleteFactorization);
    #endif

    #ifdef EIGEN_TAUCS_SUPPORT
    doEigen<Eigen::Taucs>("Eigen/Taucs", sm1, Eigen::IncompleteFactorization);
    #endif

    #if 0
    // TAUCS
    {
      taucs_ccs_matrix A = sm1.asTaucsMatrix();

      //BENCH(taucs_ccs_matrix* chol = taucs_ccs_factor_llt(&A, 0, 0);)
//       BENCH(taucs_supernodal_factor_to_ccs(taucs_ccs_factor_llt_ll(&A));)
//       std::cout << "taucs:\t" << timer.value() << endl;

      taucs_ccs_matrix* chol = taucs_ccs_factor_llt(&A, 0, 0);

      for (int j=0; j<cols; ++j)
      {
        for (int i=chol->colptr[j]; i<chol->colptr[j+1]; ++i)
          std::cout << chol->values.d[i] << " ";
      }
    }

    // CHOLMOD
    #ifdef EIGEN_CHOLMOD_SUPPORT
    {
      cholmod_common c;
      cholmod_start (&c);
      cholmod_sparse A;
      cholmod_factor *L;

      A = sm1.asCholmodMatrix();
      BenchTimer timer;
//       timer.reset();
      timer.start();
      std::vector<int> perm(cols);
//       std::vector<int> set(ncols);
      for (int i=0; i<cols; ++i)
        perm[i] = i;
//       c.nmethods = 1;
//       c.method[0] = 1;

      c.nmethods = 1;
      c.method [0].ordering = CHOLMOD_NATURAL;
      c.postorder = 0;
      c.final_ll = 1;

      L = cholmod_analyze_p(&A, &perm[0], &perm[0], cols, &c);
      timer.stop();
      std::cout << "cholmod/analyze:\t" << timer.value() << endl;
      timer.reset();
      timer.start();
      cholmod_factorize(&A, L, &c);
      timer.stop();
      std::cout << "cholmod/factorize:\t" << timer.value() << endl;

      cholmod_sparse* cholmat = cholmod_factor_to_sparse(L, &c);

      cholmod_print_factor(L, "Factors", &c);

      cholmod_print_sparse(cholmat, "Chol", &c);
      cholmod_write_sparse(stdout, cholmat, 0, 0, &c);
//
//       cholmod_print_sparse(&A, "A", &c);
//       cholmod_write_sparse(stdout, &A, 0, 0, &c);

//       for (int j=0; j<cols; ++j)
//       {
//           for (int i=chol->colptr[j]; i<chol->colptr[j+1]; ++i)
//             std::cout << chol->values.s[i] << " ";
//       }
    }
    #endif

    #endif

  }

  return 0;
}


#ifndef LMHPP
#define LMHPP
/* -------------------------------------------------------

   Levenberg Marquardt algorithm
   Coded by J. de la Cruz Rodriguez (ISP-SU, 2020)

   ------------------------------------------------------- */

#include <vector>
#include <iostream>
#include <cmath>
#include <algorithm>

#include <Eigen/Dense>

#include "Milne.hpp"

namespace lm{

  // ***************************************** //

  template<typename T>
  struct Par{
    bool isCyclic;
    bool limited;
    T scale;
    T limits[2];

    Par(): isCyclic(false), limited(false), scale(1.0), limits{0,0}{};
    Par(bool const cyclic, bool const ilimited, T const scal, T const mi, T const ma):
      isCyclic(cyclic), limited(ilimited), scale(scal), limits{mi,ma}{};

    Par(Par<T> const& in): isCyclic(in.isCyclic) ,limited(in.limited), scale(in.scale), limits{in.limits[0], in.limits[1]}{};

    Par<T> &operator=(Par<T> const& in)
    {
      isCyclic = in.isCyclic, limited = in.limited, scale=in.scale, limits[0]=in.limits[0], limits[1]=in.limits[1];
      return *this;
    }

    inline void Normalize(T &val)const{val /= scale;};

    inline void Scale(T &val)const{val *= scale;};

    inline void Check(T &val)const{
      if(!limited) return;
      if(isCyclic){
	if(val > limits[1]) val -= 3.1415926f;
	if(val < limits[0]) val += 3.1416026f;
      }
      val = std::max<T>(std::min<T>(val, limits[1]),limits[0]);
    }

    inline void CheckNormalized(T &val)const{
      if(!limited) return;
      Scale(val);
      Check(val);
      Normalize(val);
    }

  };

  // ***************************************** //

  template<typename T>
  struct container{
    int const nDat;
    T const mu;
    int Nreal;
    const ml::Milne<T>& Me;
    const T* __restrict__ d;
    const T* __restrict__ sig;
    const std::vector<Par<T>> &Pinfo;

    container(int const nd, T const imu, ml::Milne<T> const& iMe, const T* __restrict__ din, const T* __restrict__ sigin, const std::vector<Par<T>> &Pi): nDat(nd), mu(imu), Nreal(1), Me(iMe), d(din), sig(sigin), Pinfo(Pi)
    {
      // --- only account for non-dummy points in the data array --- //
      Nreal = 0;
      for(int ii = 0; ii<nDat; ++ii)
	if(sig[ii] < 1.e20) Nreal += 1;

    }

  };

  // ***************************************** //

  template<typename T> constexpr inline T SQ(T const v){return v*v;}

  // ***************************************** //

  template<typename T>
  T getChi2(int const nDat, const T* __restrict__ r)
  {
    double sum = 0.0;
    int const nDat4 = nDat/4;

    if(nDat4*4 == nDat){
      double sumI = 0.0;
      double sumQ = 0.0;
      double sumU = 0.0;
      double sumV = 0.0;

      for(int ii=0; ii<nDat4; ++ii){
	sumI += SQ(r[ii*4+0]);
	sumQ += SQ(r[ii*4+1]);
	sumU += SQ(r[ii*4+2]);
	sumV += SQ(r[ii*4+3]);
      }
      sum = sumI + (sumQ + sumU + sumV);
    }else{

      for(int ii=0; ii<nDat;++ii)
	sum += SQ(r[ii]);
    }

    return static_cast<T>(sum);
  }

  // ***************************************** //

  template<typename T>
  T fx(container<T> const& myData, int const nPar, const T* __restrict__ m_in, T* __restrict__ syn, T* __restrict__ r)
  {

    // --- Copy model --- //

    T* __restrict__ m = new T [nPar]();
    std::memcpy(m,m_in,nPar*sizeof(T));

    // --- ///

    int const nDat = myData.nDat;
    const T* __restrict__ dat = myData.d;
    const T* __restrict__ sig = myData.sig;

    // --- Scale up model --- //

    for(int ii=0; ii<nPar; ++ii){
      myData.Pinfo[ii].Scale(m[ii]);
    }

    // --- calculate spectrum --- //

    myData.Me.synthesize(m, syn, myData.mu);

    // --- calculate residue --- //

    T const scl = sqrt(T(myData.Nreal));
    for(int ii=0; ii<nDat; ++ii) r[ii] = (dat[ii] - syn[ii]) / (sig[ii] * scl);

    delete [] m;

    // --- get Chi2 --- //

    return getChi2<T>(nDat, r);
  }
    // ***************************************** //

  template<typename T>
  T fx_dx(container<T> const& myData, int const nPar, const T* __restrict__ m_in, T* __restrict__ syn, T* __restrict__ r, T* __restrict__ J)
  {

    // --- Copy model --- //

    T* __restrict__ m = new T [nPar]();
    std::memcpy(m,m_in,nPar*sizeof(T));

    int const nDat = myData.nDat;
    const T* __restrict__ dat = myData.d;
    const T* __restrict__ sig = myData.sig;

    // --- Scale up model --- //

    for(int ii=0; ii<nPar; ++ii){
      myData.Pinfo[ii].Scale(m[ii]);
    }

    // --- calculate spectrum --- //

    myData.Me.synthesize_rf(m, syn, J, myData.mu);

    // --- calculate residue --- //

    T const scl = sqrt(T(myData.Nreal));
    for(int ii=0; ii<nDat; ++ii) r[ii] = (dat[ii] - syn[ii]) / (sig[ii] * scl);

    // --- scale J --- //

    for(int ii = 0; ii<nPar; ++ii){

      T const iScl = myData.Pinfo[ii].scale / scl;

      for(int ww = 0; ww<nDat; ++ww)
	J[ii*nDat + ww] *=  iScl / sig[ww];
    }

    // --- clean up model array --- //
    delete [] m;

    // --- get Chi2 --- //

    return getChi2<T>(nDat, r);
  }

  // **************************************** //

  template<typename T>
  struct LevMar{
    int nPar;
    std::vector<T> diag;
    std::vector<Par<T>> Pinfo;

    void set(int const& iPar){
      nPar = iPar;
      diag = std::vector<T>(iPar,0.0);
      Pinfo = std::vector<Par<T>>(iPar, Par<T>());
    }

    LevMar(): nPar(0), diag(), Pinfo(){};
    LevMar(int const &nPar_i):LevMar(){set(nPar_i);}

    LevMar(LevMar<T> const& in): LevMar(){nPar = in.nPar, diag=in.diag, Pinfo = in.Pinfo;}

    LevMar<T> &operator=(LevMar<T> const& in){nPar = in.nPar, diag=in.diag(), Pinfo = in.Pinfo; return *this;}

    static inline T checkLambda(T val, T const &mi, T const& ma){return std::max<T>(std::min<T>(ma, val), mi);}

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

    T getCorrection(container<T> const& myData, T* __restrict__ m, const T* __restrict__ J, T* __restrict__ syn, T* __restrict__ r, T const iLam)const
    {

      // --- Simplify the notation --- //

      using Mat = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
      using Vec = Eigen::Matrix<T, Eigen::Dynamic, 1>;

      using cVecMap = Eigen::Map<const Eigen::Matrix<T, Eigen::Dynamic, 1>>;
      using cMatMap = Eigen::Map<const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>;

      // --- Define some quantities --- //

      int const nDat = myData.nDat;
      int const cPar = nPar;

      Mat A(cPar, cPar); A.setZero();
      Vec B(cPar); B.setZero();
      Vec dm(cPar); dm.setZero();

      // --- get Hessian matrix --- //

      for(int jj = 0; jj<cPar; ++jj){

	// --- Compute left-hand side of the system --- //

	for(int ii=0; ii<=jj; ++ii){
	  double sum = 0.0;

	  for(int ww=0; ww<nDat; ++ww) sum += J[jj*nDat + ww]*J[ii*nDat + ww];
	  A(jj,ii) = A(ii,jj) = static_cast<T>(sum);
	}//ii

	// --- Compute right-hand side of the system --- //

	double sum = 0.0;
	for(int ww = 0; ww<nDat; ww++) sum += J[jj*nDat+ww] * r[ww];
	B[jj] = static_cast<T>(sum);

	A(jj,jj) *= 1+iLam;
      } // jj

      // --- Solve linear system to get solution --- //

      //Eigen::BDCSVD<Mat> sy(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
      Eigen::ColPivHouseholderQR<Mat> sy(A); // Also rank revealing but much faster than SVD
      sy.setThreshold(1.e-14);

      dm = sy.solve(B);

      // --- add to model and check parameters --- //

      for(int ii =0; ii<cPar; ++ii){
	m[ii] += dm[ii];
	Pinfo[ii].CheckNormalized(m[ii]);
      }

      return fx<T>(myData, nPar, m, syn, r);
    }

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

    T getStep(container<T> const& myData, T* __restrict__ m, const T* __restrict__ J, T* __restrict__ syn, T* __restrict__ r, T &iLam, bool braket, T const maxLam, T const minLam)const{

      // if(!braket){
	return getCorrection(myData, m, J, syn, r, iLam);
	// }

    }

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

    T fitData(ml::Milne<T> const& Me,  int const nDat, const T* __restrict__ dat, T* __restrict__ syn, const T* __restrict__ sig, T* __restrict__ m, T const mu, int const max_iter = 20, T iLam = sqrt(10.0f), T const Chi2_thres = 1.0, T const fx_thres = 2.e-3, int const delay_braket = 2, bool verbose = true)const
    {
      static constexpr T const facLam = 3.1622776601683795;
      static constexpr T const maxLam = 100*facLam;
      static constexpr T const minLam = 1.e-4;
      static constexpr int const max_n_reject = 6;

      // --- Init container --- //

      container<T> myData(nDat, mu, Me, dat, sig, Pinfo);

      // --- Check initial Lambda value --- //

      iLam = checkLambda(iLam, minLam, maxLam);

      // --- Init temp arrays and values--- //

      int const cPar = nPar;
      T bestChi2     = 1.e32;
      T     Chi2     = 1.e32;

      T* __restrict__ bestModel  = new T [cPar]();
      T* __restrict__ bestSyn    = new T [nDat]();

      T* __restrict__     J      = new T [cPar*nDat]();
      T* __restrict__     r      = new T [nDat]();

      // --- Work with normalized quantities --- //

      for(int ii =0; ii<cPar; ++ii){
	Pinfo[ii].Check(m[ii]);
	Pinfo[ii].Normalize(m[ii]);
      }

      std::memcpy(bestModel,  m, cPar*sizeof(T));

      // --- get derivatives and init Chi2 --- //

      bestChi2 = fx_dx<T>(myData, nPar, bestModel, bestSyn, r, J);

      // --- Init iteration --- //

      if(verbose){
	fprintf(stderr, "\nLevDer::fitData: [Init] Chi2=%13.5f\n", bestChi2);
      }

      // --- Iterate --- //

      int iter = 0, n_rejected = 0;
      bool quit = false, tooSmall = false;
      T oLam = 0, dfx = 0;

      while(iter < max_iter){

	oLam = iLam;
	std::memcpy(m, bestModel, nPar*sizeof(T));

	// --- Get model correction --- //

	Chi2 = getStep(myData, m, J, syn, r, iLam, false, minLam, maxLam);

	// --- Did Chi2 improve? --- //

	if(Chi2 < bestChi2){

	  oLam = iLam;
	  dfx = (bestChi2 - Chi2) / bestChi2;

	  bestChi2 = Chi2;
	  std::memcpy(bestModel,   m, cPar*sizeof(T));
	  std::memcpy(bestSyn  , syn, nDat*sizeof(T));

	  if(iLam > 1.0001*minLam)
	    iLam = checkLambda(iLam/facLam, minLam, maxLam);
	  else
	    iLam = 10*minLam;

	  if(dfx < fx_thres){
	    if(tooSmall) quit = true;
	    else tooSmall = true;
	  }

	  n_rejected = 0;
	}else{

	  // --- Increase lambda and re-try --- //

	  iLam = checkLambda(iLam*SQ<T>(facLam), minLam, maxLam);
	  n_rejected += 1;
	  if(verbose)
	    fprintf(stderr,"LevMar::fitData: Chi2=%13.5f > %13.5f -> Increasing lambda %f -> %f\n", Chi2, bestChi2, oLam, iLam);

	  if(n_rejected<max_n_reject) continue;

	}

	// --- Check what has happened with Chi2 --- //

	if(n_rejected >= max_n_reject){
	  if(verbose)
	    fprintf(stderr, "LevMar::fitData: maximum number of rejected iterations reached, finishing inversion");
	  break;
	}

	if(verbose)
	  fprintf(stderr, "LevMar::fitData [%3d] Chi2=%13.5f, lambda=%e\n", iter, Chi2, oLam);

	if(bestChi2 < Chi2_thres){
	  if(verbose)
	    fprintf(stderr, "LevMar::fitData: Chi2 (%f) < Chi2_threshold (%f), finishing inversion", bestChi2, Chi2_thres);
	  break;
	}

	if(quit){
	  if(verbose)
	    fprintf(stderr, "LevMar::fitData: Chi2 improvement too small for 2-iterations, finishing inversion\n");
	  break;
	}

	iter++;
	if(iter >= max_iter){
	  break;
	}

	// --- compute gradient of the new model for next iteration --- //

	std::memcpy(m, bestModel, cPar*sizeof(T));
	fx_dx<T>(myData, nPar, m, syn, r, J);
      }

      std::memcpy(m, bestModel, cPar*sizeof(T));
      std::memcpy(syn, bestSyn, nDat*sizeof(T));

      for(int ii=0; ii<cPar; ++ii)
	Pinfo[ii].Scale(m[ii]);

      // --- Clean-up --- //

      delete [] bestModel;
      delete [] bestSyn;
      delete [] r;
      delete [] J;

      return bestChi2;
    }

  };

}

#endif


#ifndef ALEPH_GEOMETRY_DOWKER_COMPLEX_HH__
#define ALEPH_GEOMETRY_DOWKER_COMPLEX_HH__

#include <aleph/math/Combinations.hh>

#include <aleph/topology/filtrations/Data.hh>

#include <aleph/topology/Simplex.hh>
#include <aleph/topology/SimplicialComplex.hh>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>

#include <boost/graph/floyd_warshall_shortest.hpp>
#include <boost/graph/johnson_all_pairs_shortest.hpp>

#include <algorithm>
#include <iterator>
#include <limits>
#include <unordered_map>
#include <vector>

namespace aleph
{

namespace geometry
{

namespace detail
{

// FIXME: the weight type of an edge should be configurable as an
// additional template parameter
using EdgeWeightProperty = boost::property<boost::edge_weight_t, double>;

// FIXME: the data type of the graph should be configurable as an
// additional template parameter.
using Graph = boost::adjacency_list<
  boost::vecS,
  boost::vecS,
  boost::directedS,
  boost::no_property,
  EdgeWeightProperty
>;

using VertexDescriptor = boost::graph_traits<Graph>::vertex_descriptor;

template <class T, class I = std::size_t> struct Pair
{
  I p; // first index
  I q; // second index
  T w; // weight
};

template <class D, class V> struct Vertex
{
  V p; // vertex index
  D w; // weight
};

template <class D, class V> void swap( Vertex<D, V>& first, Vertex<D, V>& second )
{
  std::swap( first.p, second.p );
  std::swap( first.w, second.w );
}

} // namespace detail

/**
  Calculates a set of admissible pairs from a matrix of weights and
  a given distance threshold. The matrix of weights does *not* have
  to satisfy symmetry constraints.

  @param W Weighted adjacency matrix
  @param R Maximum weight
*/

template <class Matrix, class T> std::vector< detail::Pair<T> > admissiblePairs( const Matrix& W, T R )
{
  using namespace detail;

  // Convert matrix into a graph ---------------------------------------

  auto n          = W.size();
  using IndexType = decltype(n);

  detail::Graph G( n );

  for( IndexType i = 0; i < n; i++ )
  {
    for( IndexType j = 0; j < n; j++ )
    {
      if( W[i][j] > 0 )
      {
        EdgeWeightProperty weight = W[i][j];

        boost::add_edge( VertexDescriptor(i), VertexDescriptor(j),
                         weight,
                         G );
      }
    }
  }

  double density
    = static_cast<double>( boost::num_edges(G) ) / static_cast<double>( boost::num_vertices(G) * ( boost::num_vertices(G) - 1 ) );

  // This 'pseudo-matrix' contains the completion of the weight function
  // specified by the input matrix.
  std::vector< std::vector<double> > D( boost::num_vertices(G),
                                        std::vector<double>( boost::num_vertices(G) ) );

  if( density >= 0.5 )
    boost::floyd_warshall_all_pairs_shortest_paths( G, D );
  else
    boost::johnson_all_pairs_shortest_paths( G, D );

  std::vector< Pair<T> > pairs;

  // Create admissible pairs -------------------------------------------
  //
  // A pair is admissible if it satisfies a reachability property,
  // meaning that the induced graph distance permits to reach both
  // vertices under the specified distance threshold.

  for( IndexType i = 0; i < n; i++ )
  {
    for( IndexType j = 0; j < n; j++ )
    {
      if( D[i][j] <= R )
        pairs.push_back( {i, j, static_cast<T>( D[i][j] ) } );
    }
  }

  return pairs;
}

/**
  Creates a Dowker sink complex and a Dowker source complex from a given
  set of admissible pairs. A *general* Dowker complex contains a simplex
  if all of its vertices satisfy the admissibility condition.

  @param pairs     Set of admissible pairs
  @param dimension Maximum dimension for expansion. If set to zero, will
                   expand the complex to its maximum dimension.
*/

template <class V, class D, class T>
std::pair<
  topology::SimplicialComplex< topology::Simplex<D, V> >,
  topology::SimplicialComplex< topology::Simplex<D, V> >
> buildDowkerSinkSourceComplexes( const std::vector<detail::Pair<T> >& pairs,
                                  unsigned dimension = 0 )
{
  using namespace detail;

  using Simplex           = topology::Simplex<D, V>;
  using SimplicialComplex = topology::SimplicialComplex<Simplex>;

  using VertexType     = V;
  VertexType maxVertex = VertexType();

  for( auto&& pair : pairs )
  {
    maxVertex = std::max(maxVertex, VertexType(pair.p) );
    maxVertex = std::max(maxVertex, VertexType(pair.q) );
  }

  using Vertex = Vertex<D, V>;

  // Keep track of the mapping induced by fixing either the source
  // points or the sink points.
  std::unordered_map< VertexType, std::vector<Vertex> > sourceBasePointMap;
  std::unordered_map< VertexType, std::vector<Vertex> > sinkBasePointMap;

  for( auto&& pair : pairs )
  {
    auto&& p = pair.p;
    auto&& q = pair.q;

    sourceBasePointMap[ VertexType(p) ].push_back( { VertexType(q), pair.w } );
    sinkBasePointMap[ VertexType(q) ].push_back( { VertexType(p), pair.w } );
  }

  // Auxiliary weight calculation lambda function ----------------------
  //
  // This function calculates the *maximum* weight of a range of
  // vertices. It is used to determine the weight of a simplex.

  using Iterator = typename std::vector<Vertex>::const_iterator;
  auto getWeight = [] ( Iterator begin, Iterator end )
  {
    using DataType  = D;
    DataType weight = std::numeric_limits<DataType>::lowest();

    for( auto it = begin; it != end; ++it )
      weight = std::max( weight, it->w );

    return weight;
  };

  auto makeSimplices = [&dimension, &getWeight] ( const std::unordered_map< VertexType, std::vector<Vertex> >& map )
  {
    std::vector<Simplex> simplices;
    std::unordered_map<Simplex, D> simplex_to_weight;

    for( auto&& pair : map )
    {
      auto vertices            = pair.second;
      std::size_t maxDimension = 0;

      if( dimension == 0 )
        maxDimension = vertices.size();
      else
        maxDimension = dimension + 1;

      using DifferenceType = typename decltype(vertices)::difference_type;

      for( std::size_t d = std::min( vertices.size(), maxDimension ); d >= 1; d-- )
      {
        math::for_each_combination( vertices.begin(), vertices.begin() + DifferenceType(d), vertices.end(),
          [&simplex_to_weight, &getWeight] ( Iterator first, Iterator last )
          {
            std::vector<V> vertices_;
            vertices_.reserve( typename std::vector<V>::size_type( std::distance( first, last ) ) );

            for( auto it = first; it != last; ++it )
              vertices_.push_back( it->p );

            Simplex s( vertices_.begin(), vertices_.end() );

            if( simplex_to_weight.find(s) == simplex_to_weight.end() )
              simplex_to_weight[s] = getWeight(first, last);
            else
              simplex_to_weight[s] = std::min( simplex_to_weight[s], getWeight(first, last) );

            return false;
          }
        );
      }
    }

    for( auto&& pair : simplex_to_weight )
    {
      auto s = pair.first;
      s.setData( pair.second );

      simplices.push_back( s );
    }

    return simplices;
  };

  auto sourceEdges = makeSimplices( sourceBasePointMap );
  auto sinkEdges   = makeSimplices( sinkBasePointMap );

  SimplicialComplex dowkerSourceComplex( sourceEdges.begin(), sourceEdges.end() );
  SimplicialComplex dowkerSinkComplex  ( sinkEdges.begin()  , sinkEdges.end()   );

  dowkerSourceComplex.sort( topology::filtrations::Data<Simplex>() );
  dowkerSinkComplex.sort( topology::filtrations::Data<Simplex>() );

  return std::make_pair( dowkerSourceComplex, dowkerSinkComplex );
}

/**
  Given a matrix and a maximum radius, creates a Dowker source complex that
  contains a simplex if all of its vertices are admissible.

  @param matrix    Matrix of weighted adjacencies. The matrix is *not*
                   assumed to be symmetric.

  @param epsilon   Maximum radius for expansion. I refer to this as epsilon
                   in order to show the connection to other simplicial complex
                   creation algorithms.

  @param dimension Maximum dimension for expansion. If set to zero, will
                   expand the complex to its maximum dimension.
*/

template
<
  class Matrix,
  class VertexType,
  class DataType
> topology::SimplicialComplex< topology::Simplex<DataType, VertexType> >
    buildDowkerSourceCompplex( const Matrix& matrix,
                               DataType epsilon,
                               unsigned dimension = 0 )
{
  using namespace detail;

  auto pairs = admissiblePairs( matrix, epsilon );

  using Simplex           = topology::Simplex<DataType, VertexType>;
  using SimplicialComplex = topology::SimplicialComplex<Simplex>;

  VertexType maxVertex = VertexType();

  for( auto&& pair : pairs )
  {
    maxVertex = std::max(maxVertex, VertexType(pair.p) );
    maxVertex = std::max(maxVertex, VertexType(pair.q) );
  }

  using Vertex = Vertex<DataType, VertexType>;

  // Keep track of the mapping induced by fixing the source points. In
  // essence, this is a adjacency list representation of a matrix that
  // tracks the admissibility of vertices.
  std::unordered_map< VertexType, std::vector<Vertex> > sourceBasePointMap;

  for( auto&& pair : pairs )
  {
    auto&& p = pair.p;
    auto&& q = pair.q;

    sourceBasePointMap[ VertexType(p) ].push_back( { VertexType(q), pair.w } );
  }

  // Auxiliary weight calculation lambda function ----------------------
  //
  // This function calculates the *maximum* weight of a range of
  // vertices. It is used to determine the weight of a simplex.

  using Iterator = typename std::vector<Vertex>::const_iterator;
  auto getWeight = [] ( Iterator begin, Iterator end )
  {
    DataType weight = std::numeric_limits<DataType>::lowest();

    for( auto it = begin; it != end; ++it )
      weight = std::max( weight, it->w );

    return weight;
  };

  // Create all valid simplices ----------------------------------------
  //
  // All valid simplices with respect to the source point are created
  // by generating all combinations of admissible pairs, with respect
  // to the given source point.
  //
  std::vector<Simplex> simplices;

  {
    // The same simplex may occur multiple times because it is
    // 'observed' by multiple source points. We need to obtain
    // the *earliest* weight at which the simplex occurs!
    std::unordered_map<Simplex, DataType> simplex_to_weight;

    for( auto&& pair : sourceBasePointMap )
    {
      auto vertices            = pair.second;
      std::size_t maxDimension = 0;

      if( dimension == 0 )
        maxDimension = vertices.size();
      else
        maxDimension = dimension + 1;

      using DifferenceType = typename decltype(vertices)::difference_type;

      for( std::size_t d = std::min( vertices.size(), maxDimension ); d >= 1; d-- )
      {
        math::for_each_combination( vertices.begin(), vertices.begin() + DifferenceType(d), vertices.end(),
          [&simplex_to_weight, &getWeight] ( Iterator first, Iterator last )
          {
            std::vector<VertexType> vertices_;
            vertices_.reserve( typename std::vector<VertexType>::size_type( std::distance( first, last ) ) );

            for( auto it = first; it != last; ++it )
              vertices_.push_back( it->p );

            Simplex s( vertices_.begin(), vertices_.end() );

            // Ensures that we do not take the default weight, which is
            // zero, as the weight of the simplex---there does not seem
            // to be a way to solve this more efficiently...
            if( simplex_to_weight.find(s) == simplex_to_weight.end() )
              simplex_to_weight[s] = getWeight(first, last);
            else
              simplex_to_weight[s] = std::min( simplex_to_weight[s], getWeight(first, last) );

            return false;
          }
        );
      }
    }

    // Set the weights of all simplices prior to inserting them into the
    // simplicial complex.
    for( auto&& pair : simplex_to_weight )
    {
      auto s = pair.first;
      s.setData( pair.second );

      simplices.push_back( s );
    }
  }

  SimplicialComplex K( simplices.begin(), simplices.end() );
  K.sort( topology::filtrations::Data<Simplex>() );

  return K;
}

} // namespace geometry

} // namespace aleph

#endif


#ifndef __BETA_H__
#define __BETA_H__

#include <armadillo>

#include <vector>

/**
 * Estimates parameters for the beta distribution using the
 * method of moments. The method of moments estimates will have
 * a higher variance than maximum likelihood, but is faster and
 * simpler.
 *
 * @param samples A list of samples to fit a beta distribution to.
 *
 * @return The estimated parameters, if they could not be estimated,
 *         the parameters 1,1 will be returned.
 */
arma::vec mom_beta(const arma::vec &samples);

#endif /* End of __BETA_H__ */


#include <boost/math/special_functions/beta.hpp>
#include <deque>
#include <random>
#include <unordered_map>
#include "relles.h"

#include<iostream>
static uint64_t beta_bsearch(std::unordered_map<uint64_t, long double>& memo, long double value, uint64_t n) {
  uint64_t low = 0;
  uint64_t high = n;

  std::random_device rd;
  std::mt19937_64 gen(rd());
  std::uniform_real_distribution<> unif_dis(0.0, 1.0);

  while (low < high - 1) {
    uint64_t idx = low + (high - low) / 2;
    if (memo.find(idx) == memo.end()) {
      long double beta = boost::math::ibeta_inv(idx - low + 1, high - idx, unif_dis(gen), boost::math::policies::policy<boost::math::policies::digits2<25>>());
      memo[idx] = memo[low] + (memo[high] - memo[low]) * beta;
    }

    if (memo[idx] < value) {
      low = idx;
    } else {
      high = idx;
    }
  }

  assert(low == high - 1);

  return low;
}

static uint64_t beta_isearch(std::unordered_map<uint64_t, long double>& memo, std::deque<std::pair<uint64_t, long double>>& prev_points, long double value, uint64_t n) {
  uint64_t low = 0;
  while (prev_points.back().second < value) {
    low = prev_points.back().first;
    prev_points.pop_back();
  }
  long double low_val = memo[low];
  uint64_t high = prev_points.back().first;
  long double high_val = memo[high];

  std::random_device rd;
  std::mt19937_64 gen(rd());
  std::uniform_real_distribution<> unif_dis(0.0, 1.0);

  while (low < high - 2) {
    uint64_t idx = round((value - low_val) / (high_val - low_val) * (high - low - 2)) + low + 1;
    assert(idx >= 0);
    if (memo.find(idx) == memo.end()) {
      long double beta = boost::math::ibeta_inv(idx - low, high - idx + 1, unif_dis(gen), boost::math::policies::policy<boost::math::policies::digits2<25>>());
      memo[idx] = memo[low] + (memo[high] - memo[low]) * beta;
      prev_points.emplace_back(idx, memo[idx]);
    }

    if (memo[idx] < value) {
      low = idx;
      low_val = memo[idx];
    } else {
      high = idx;
      high_val = memo[idx];
    }
  }

  return low + 1;
}

/*
 * The O(k log n) algorithm for multinomial sampling.
 */
std::vector<uint64_t> relles(uint64_t n, const std::vector<long double>& dist) {
  std::unordered_map<uint64_t, long double> memo;
  std::vector<uint64_t> output(dist.size());

  memo[0] = 0;
  memo[n] = 1;

  long double cum = 0;
  uint64_t last = 0;

  for (int i = 0; i < dist.size(); i ++) {
    cum += dist[i];
    uint64_t loc = beta_bsearch(memo, cum, n);
    output[i] = loc - last - 1;
    last = loc;
  }

  return output;
}

/*
 * The O(k log log n) algorithm for multinomial sampling.
 */
std::vector<uint64_t> relles_enhanced(uint64_t n, const std::vector<long double>& dist) {
  std::unordered_map<uint64_t, long double> memo;
  std::deque<std::pair<uint64_t, long double>> prev_points = { { n, 1 } };
  std::vector<uint64_t> output(dist.size());

  memo[0] = 0;
  memo[n] = 1;

  long double cum = 0;
  uint64_t last = 0;

  for (int i = 0; i < dist.size(); i ++) {
    cum += dist[i];
    if (cum >= 1) {
      output[i] = n - last - 1;
      break;
    }
    uint64_t loc = beta_isearch(memo, prev_points, cum, n);
    output[i] = loc - last - 1;
    last = loc;
  }

  return output;
}