/*
    Copyright (c) 2013, Philipp Krähenbühl
    All rights reserved.

    Redistribution and use in source and binary forms, with or without
    modification, are permitted provided that the following conditions are met:
        * Redistributions of source code must retain the above copyright
        notice, this list of conditions and the following disclaimer.
        * Redistributions in binary form must reproduce the above copyright
        notice, this list of conditions and the following disclaimer in the
        documentation and/or other materials provided with the distribution.
        * Neither the name of the Stanford University nor the
        names of its contributors may be used to endorse or promote products
        derived from this software without specific prior written permission.

    THIS SOFTWARE IS PROVIDED BY Philipp Krähenbühl ''AS IS'' AND ANY
    EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
    WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
    DISCLAIMED. IN NO EVENT SHALL Philipp Krähenbühl BE LIABLE FOR ANY
    DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
    (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
    ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
    (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include "densecrf.h"
#include "permutohedral.h"
#include "util.h"
#include "pairwise.h"
#include <cmath>
#include <cstring>
#include <iostream>
#include <cstdio>
#include <ctime>
#include <algorithm>    // std::min
#include <boost/concept_check.hpp>

template<typename Derived>
inline bool is_finite(const Eigen::MatrixBase<Derived>& x)
{
	return ((x.array() == x.array())).all();
}

/////////////////////////////
/////  Alloc / Dealloc  /////
/////////////////////////////
DenseCRF::DenseCRF(int N, int M) : N_(N), M_(M), unary_(0), unary2_(0) {
  printf("DenseCRF  constructor get called\n");
}
DenseCRF::~DenseCRF() {
  printf("DenseCRF destructor  get called, pairwise size is %d\n", pairwise_.size());
	if (unary_)
		delete unary_;
	if (unary2_)
		delete unary2_;
        //if (pairwise_.size() > 4)
	//for( unsigned int i=0; i<pairwise_.size(); i++ )
        //  if (pairwise_[i])
	//	delete pairwise_[i];
}
DenseCRF2D::DenseCRF2D(int W, int H, int M) : DenseCRF(W*H,M), W_(W), H_(H) {
}
DenseCRF2D::~DenseCRF2D() {
}

DenseCRF3D::DenseCRF3D(int N,  int M) : DenseCRF(N, M) {
  printf("DenseCRF3d  constructor get called\n");
}
DenseCRF3D::~DenseCRF3D() {
}
/////////////////////////////////
/////  Pairwise Potentials  /////
/////////////////////////////////
void DenseCRF::addPairwiseEnergy (const MatrixXf & features, LabelCompatibility * function, KernelType kernel_type, NormalizationType normalization_type) {
// 	assert( features.cols() == N_ ); //HACK  comment by me, for hierarchical use
  addPairwiseEnergy(std::shared_ptr<PairwisePotential> (new PairwisePotential( features, function, kernel_type, normalization_type )) );
}
void DenseCRF::addPairwiseEnergy ( std::shared_ptr<PairwisePotential> potential ){
	pairwise_.push_back( potential );
}

void DenseCRF2D::addPairwiseGaussian ( float sx, float sy, LabelCompatibility * function, KernelType kernel_type, NormalizationType normalization_type ) {
	MatrixXf feature( 2, N_ );
	for( int j=0; j<H_; j++ )
		for( int i=0; i<W_; i++ ){
			feature(0,j*W_+i) = i / sx;
			feature(1,j*W_+i) = j / sy;
		}
	addPairwiseEnergy( feature, function, kernel_type, normalization_type );
}

void DenseCRF2D::addPairwiseBilateral ( float sx, float sy, float sr, float sg, float sb, const unsigned char* im, LabelCompatibility * function, KernelType kernel_type, NormalizationType normalization_type ) {
// 	MatrixXf feature_Bilateral( 5, N_ ); // some part of feature_Bilateral is fixed all the time
	if (!init_pose_pairwise)
	    addBilateral_pos(sx,sy);
	for( int j=0; j<H_; j++ )
		for( int i=0; i<W_; i++ ){
// 			feature_Bilateral(0,j*W_+i) = i / sx;
// 			feature_Bilateral(1,j*W_+i) = j / sy;
			feature_Bilateral(2,j*W_+i) = im[(i+j*W_)*3+0] / sr;
			feature_Bilateral(3,j*W_+i) = im[(i+j*W_)*3+1] / sg;
			feature_Bilateral(4,j*W_+i) = im[(i+j*W_)*3+2] / sb;
		}
	addPairwiseEnergy( feature_Bilateral, function, kernel_type, normalization_type );
}

void DenseCRF2D::addPairwiseBilateral ( float sx, float sy, float sr, float sg, float sb, VectorX8u im, LabelCompatibility * function, KernelType kernel_type, NormalizationType normalization_type ) {
// 	MatrixXf feature( 5, N_ );
	if (!init_pose_pairwise)
	    addBilateral_pos(sx,sy);
	for( int j=0; j<H_; j++ )
		for( int i=0; i<W_; i++ ){
// 			feature_Bilateral(0,j*W_+i) = i / sx;
// 			feature_Bilateral(1,j*W_+i) = j / sy;
			feature_Bilateral(2,j*W_+i) = im[(i+j*W_)*3+0] / sr;
			feature_Bilateral(3,j*W_+i) = im[(i+j*W_)*3+1] / sg;
			feature_Bilateral(4,j*W_+i) = im[(i+j*W_)*3+2] / sb;
		}
	addPairwiseEnergy( feature_Bilateral, function, kernel_type, normalization_type );
}

void DenseCRF2D::addBilateral_pos(float sx,float sy)  // this is fixed, doesn't change with color
{
	feature_Bilateral.resize( 5, N_ );
	for( int j=0; j<H_; j++ )
	      for( int i=0; i<W_; i++ ){
		      feature_Bilateral(0,j*W_+i) = i / sx;
		      feature_Bilateral(1,j*W_+i) = j / sy;
	      }
	init_pose_pairwise=true;
}

void DenseCRF3D::addPairwiseGaussian( float sx, float sy, float sz, const MatrixXf& posList,
					        LabelCompatibility * function, KernelType kernel_type, NormalizationType normalization_type ) {
// 	assert(posList.cols()==N_); // comment this for hierarchical crf
	MatrixXf feature( 3, posList.cols() );
	for (int i=0; i<posList.cols(); i++) {
		float x = posList(0,i);
		float y = posList(1,i);
		float z = posList(2,i);

		feature(0, i) = x / sx;
		feature(1, i) = y / sy;
		feature(2, i) = z / sz;
	}
	addPairwiseEnergy( feature, function, kernel_type, normalization_type );
}

void DenseCRF3D::addPairwiseBilateral ( float sx, float sy, float sz, float sr, float sg, float sb, const MatrixXf& posList,
					  const MatrixXf& colorList, LabelCompatibility * function, KernelType kernel_type, NormalizationType normalization_type ) {
//       assert(posList.cols()==N_);  // comment this for hierarchical crf
//       assert(colorList.cols()==N_);
      assert(colorList.cols()==posList.cols());
      MatrixXf feature_Bilateral(6, posList.cols() );  // some part could be precompute  by me
      for (int i=0; i<posList.cols(); i++) {
		float x = posList(0,i);
		float y = posList(1,i);
		float z = posList(2,i);

		feature_Bilateral(0, i) = x / sx;
		feature_Bilateral(1, i) = y / sy;
		feature_Bilateral(2, i) = z / sz;

		feature_Bilateral(3, i) = colorList(0,i) / sr;
		feature_Bilateral(4, i) = colorList(1,i) / sg;
		feature_Bilateral(5, i) = colorList(2,i) / sb;
	}
	addPairwiseEnergy( feature_Bilateral, function, kernel_type, normalization_type );
}

//////////////////////////////
/////  Unary Potentials  /////
//////////////////////////////
void DenseCRF::setUnaryEnergy ( UnaryEnergy * unary ) {
	if( unary_ ) delete unary_;
	unary_ = unary;
}
void DenseCRF::setUnaryEnergy( const MatrixXf & unary ) {
	setUnaryEnergy( new ConstUnaryEnergy( unary ) );
}
void DenseCRF::setUnaryEnergy2( const MatrixXf & unary ) {
	if( unary2_ ) delete unary2_;
	unary2_ = new ConstUnaryEnergy( unary );
}

void  DenseCRF::setUnaryEnergy( const MatrixXf & L, const MatrixXf & f ) {
	setUnaryEnergy( new LogisticUnaryEnergy( L, f ) );
}
///////////////////////
/////  Inference  /////
///////////////////////
void expAndNormalize ( MatrixXf & out, const MatrixXf & in ) {
	out.resize( in.rows(), in.cols() );
	for( int i=0; i<out.cols(); i++ ){
		VectorXf b = in.col(i);
		b.array() -= b.maxCoeff();
		b = b.array().exp();
		out.col(i) = b / b.array().sum();
	}
}

void sumAndNormalize( MatrixXf & out, const MatrixXf & in, const MatrixXf & Q ) {
	out.resize( in.rows(), in.cols() );
	for( int i=0; i<in.cols(); i++ ){
		VectorXf b = in.col(i);
		VectorXf q = Q.col(i);
		out.col(i) = b.array().sum()*q - b;
	}
}

MatrixXf DenseCRF::inference ( int n_iterations ) {
  	if (fuse_last_Q_dist)
	    return inference_fuse( n_iterations );

	if (addHO && hierarchical_high_order)
	    return inference_hierarchical( n_iterations );

	MatrixXf Q( M_, N_ ), tmp1, unary( M_, N_ ), tmp2;
	unary.fill(0);
	if( unary_ )
		unary = unary_->get();

	expAndNormalize( Q, -unary );

	for( int it=0; it<n_iterations; it++ )
	{
		tmp1 = -unary; //  tmp1, negative number  the higher, the larger prob
		if(addHO)
		{
			set_mem_init_higherorder();
			calculate_higherorder_pot(Q,tmp1);
			if (!decoupled_high_order)
			    tmp1 += high_order_sp_to_pix;  // minus high order potential energy
		}

		for( unsigned int k=0; k<pairwise_.size(); k++ ) {  // usually two. color and position
			pairwise_[k]->apply( tmp2, Q );  // each kind of features generate tmp2 (similary as unary, represent mutual energy)
			tmp1 -= tmp2;  // minus pairwise potential energy (minus a negative number)
		}
		expAndNormalize( Q, tmp1 ); //update Q (change energy into probability, each column independenty), doesn't change tmp1
	}
	return Q;
}

MatrixXf DenseCRF::inference_hierarchical( int n_iterations )
{
	int sp_num=all_3d_superpixels_.size();

	MatrixXf Q_low( M_, N_ ), tmp1_low, unary_low( M_, N_ ), tmp2_low;
	MatrixXf Q_high( M_, sp_num ), tmp1_high, unary_high( M_, sp_num), tmp2_high;

	std::cout<<"Hierarchical CRF low level size  "<<N_<<"  high level size  "<<sp_num<<std::endl;

	unary_low.fill(0);
	if( unary_ )
		unary_low = -unary_->get(); //NOTE, I keep a minus here, because all later operations use minus

	unary_high.fill(0); // high order unary set to 0, corresponding to clique's low cost is 0
	if( unary2_ )
		unary_high = -unary2_->get();

	std::clock_t start;

	expAndNormalize( Q_low,  unary_low);
	expAndNormalize( Q_high, unary_high);

	bool separate_two_level=true;
	if (separate_two_level)
	{
	      for( int it=0; it<n_iterations; it++ )
	      {
		      tmp1_low = unary_low;  // tmp1_low the higher, the larger prob
		      tmp1_high = unary_high;
// 		      set_mem_init_higherorder();  // if want to use  high_order_pix_to_sp etc.

		      // first optimize high level nodes's label
		      calculate_hierar_highlevel_pot(Q_low,Q_high,tmp1_high);
// 		      tmp1_high -= high_order_pix_to_sp;
		      for( unsigned int k=2; k<4; k++ ) {  // for high level pixels
			      pairwise_[k]->apply( tmp2_high, Q_high );  // each kind of features generate tmp2 (similary as unary, represent mutual energy)
			      tmp1_high -= tmp2_high;  // minus pairwise potential energy
		      }
		      expAndNormalize( Q_high, tmp1_high ); //update Q (change energy into probability, each column independenty), doesn't change tmp1
		      for (int sp_id=0;sp_id<all_3d_superpixels_.size();sp_id++)
			    all_3d_superpixels_[sp_id]->max_label_prob=Q_high.col(sp_id).maxCoeff(&all_3d_superpixels_[sp_id]->label);

		      // then fix high level nodes, only optimize low level nodes.
		      calculate_hierar_lowlevel_pot(Q_low,tmp1_low);
// 		      tmp1_low -= high_order_sp_to_pix;
		      for( unsigned int k=0; k<2; k++ ) {  // for low level pixels
			      pairwise_[k]->apply( tmp2_low, Q_low );  // each kind of features generate tmp2 (similary as unary, represent mutual energy)
			      tmp1_low -= tmp2_low;  // minus pairwise potential energy
		      }
		      expAndNormalize( Q_low, tmp1_low ); //update Q (change energy into probability, each column independenty), doesn't change tmp1
	      }
	}
	else
	{
	      for( int it=0; it<n_iterations; it++ )
	      {
		      tmp1_low = unary_low;
		      tmp1_high = unary_high;
// 		      set_mem_init_higherorder();

		      calculate_hierarchical_pot(Q_low,Q_high,tmp1_low,tmp1_high);
// 		      tmp1_low -= high_order_sp_to_pix;
// 		      tmp1_high -= high_order_pix_to_sp;

		      for( unsigned int k=0; k<2; k++ ) {  // for low level pixels
			      pairwise_[k]->apply( tmp2_low, Q_low );  // each kind of features generate tmp2 (similary as unary, represent mutual energy)
			      tmp1_low -= tmp2_low;  // minus pairwise potential energy
		      }

		      for( unsigned int k=2; k<4; k++ ) {  // for high level pixels
			      pairwise_[k]->apply( tmp2_high, Q_high );  // each kind of features generate tmp2 (similary as unary, represent mutual energy)
			      tmp1_high -= tmp2_high;  // minus pairwise potential energy
		      }
		      expAndNormalize( Q_low, tmp1_low ); //update Q (change energy into probability, each column independenty), doesn't change tmp1
		      expAndNormalize( Q_high, tmp1_high ); //update Q (change energy into probability, each column independenty), doesn't change tmp1
	      }
	}
	return Q_low;
}

void fuse_Q_dist(MatrixXf& curr_Q, const MatrixXf& last_Q, const float last_weight)
{
	curr_Q=last_weight*last_Q+(1-last_weight)*curr_Q;
	// normalize to sum to 1 in each column
	for( int i=0; i<curr_Q.cols(); i++ ){
		VectorXf b = curr_Q.col(i);
		curr_Q.col(i) = b / b.array().sum();
	}
}

MatrixXf DenseCRF::inference_fuse ( int n_iterations ) {
	MatrixXf Q, tmp1, unary( M_, N_ ), tmp2;
	unary.fill(0);
	if( unary_ )
	    unary = unary_->get();

	expAndNormalize( Q, -unary );
	if (Q_dist_initialized)  // fuse Q with last time
	    fuse_Q_dist(Q, Q_last, last_Q_weight);

	for( int it=0; it<n_iterations; it++ ) {
		tmp1 = -unary;  // right or not??? whether use Q
		for( unsigned int k=0; k<pairwise_.size(); k++ ) {
			pairwise_[k]->apply( tmp2, Q );
			tmp1 -= tmp2;
		}
		expAndNormalize( Q, tmp1 );
	}
	Q_last=Q;
	Q_dist_initialized=true;
	return Q;
}

VectorXi DenseCRF::map ( int n_iterations ) {
	// Run inference
	MatrixXf Q = inference( n_iterations );
	// Find the map
	return currentMap( Q );
}

// MatrixXf DenseCRF::map_prob( int n_iterations ) const{
//        return inference( n_iterations );
// }

///////////////////
/////  Debug  /////
///////////////////
VectorXf DenseCRF::unaryEnergy(const VectorXi & l) {
	assert( l.cols() == N_ );
	VectorXf r( N_ );
	r.fill(0.f);
	if( unary_ ) {
		MatrixXf unary = unary_->get();

		for( int i=0; i<N_; i++ )
			if ( 0 <= l[i] && l[i] < M_ )
				r[i] = unary( l[i], i );
	}
	return r;
}
VectorXf DenseCRF::pairwiseEnergy(const VectorXi & l, int term) {
	assert( l.cols() == N_ );
	VectorXf r( N_ );
	r.fill(0.f);

	if( term == -1 ) {
		for( unsigned int i=0; i<pairwise_.size(); i++ )
			r += pairwiseEnergy( l, i );
		return r;
	}

	MatrixXf Q( M_, N_ );
	// Build the current belief [binary assignment]
	for( int i=0; i<N_; i++ )
		for( int j=0; j<M_; j++ )
			Q(j,i) = (l[i] == j);
	pairwise_[ term ]->apply( Q, Q );
	for( int i=0; i<N_; i++ )
		if ( 0 <= l[i] && l[i] < M_ )
			r[i] =-0.5*Q(l[i],i );
		else
			r[i] = 0;
	return r;
}
MatrixXf DenseCRF::startInference() const{
	MatrixXf Q( M_, N_ );
	Q.fill(0);

	// Initialize using the unary energies
	if( unary_ )
		expAndNormalize( Q, -unary_->get() );
	return Q;
}
void DenseCRF::stepInference( MatrixXf & Q, MatrixXf & tmp1, MatrixXf & tmp2 ) const{
	tmp1.resize( Q.rows(), Q.cols() );
	tmp1.fill(0);
	if( unary_ )
		tmp1 -= unary_->get();

	// Add up all pairwise potentials
	for( unsigned int k=0; k<pairwise_.size(); k++ ) {
		pairwise_[k]->apply( tmp2, Q );
		tmp1 -= tmp2;
	}

	// Exponentiate and normalize
	expAndNormalize( Q, tmp1 );
}
VectorXi DenseCRF::currentMap( const MatrixXf & Q ) const{
	VectorXi r(Q.cols());
	// Find the map
	for( int i=0; i<N_; i++ ){
		int m;
		Q.col(i).maxCoeff( &m );
		r[i] = m;
	}
	return r;
}

// Compute the KL-divergence of a set of marginals
double DenseCRF::klDivergence( const MatrixXf & Q ) const {
	double kl = 0;
	// Add the entropy term
	for( int i=0; i<Q.cols(); i++ )
		for( int l=0; l<Q.rows(); l++ )
			kl += Q(l,i)*log(std::max( Q(l,i), 1e-20f) );
	// Add the unary term
	if( unary_ ) {
		MatrixXf unary = unary_->get();
		for( int i=0; i<Q.cols(); i++ )
			for( int l=0; l<Q.rows(); l++ )
				kl += unary(l,i)*Q(l,i);
	}

	// Add all pairwise terms
	MatrixXf tmp;
	for( unsigned int k=0; k<pairwise_.size(); k++ ) {
		pairwise_[k]->apply( tmp, Q );
		kl += (Q.array()*tmp.array()).sum();
	}
	return kl;
}

// Gradient computations
double DenseCRF::gradient( int n_iterations, const ObjectiveFunction & objective, VectorXf * unary_grad, VectorXf * lbl_cmp_grad, VectorXf * kernel_grad) const {
	// Run inference
	std::vector< MatrixXf > Q(n_iterations+1);
	MatrixXf tmp1, unary( M_, N_ ), tmp2;
	unary.fill(0);
	if( unary_ )
		unary = unary_->get();
	expAndNormalize( Q[0], -unary );
	for( int it=0; it<n_iterations; it++ ) {
		tmp1 = -unary;
		for( unsigned int k=0; k<pairwise_.size(); k++ ) {
			pairwise_[k]->apply( tmp2, Q[it] );
			tmp1 -= tmp2;
		}
		expAndNormalize( Q[it+1], tmp1 );
	}

	// Compute the objective value
	MatrixXf b( M_, N_ );
	double r = objective.evaluate( b, Q[n_iterations] );
	sumAndNormalize( b, b, Q[n_iterations] );

	// Compute the gradient
	if(unary_grad && unary_)
		*unary_grad = unary_->gradient( b );
	if( lbl_cmp_grad )
		*lbl_cmp_grad = 0*labelCompatibilityParameters();
	if( kernel_grad )
		*kernel_grad = 0*kernelParameters();

	for( int it=n_iterations-1; it>=0; it-- ) {
		// Do the inverse message passing
		tmp1.fill(0);
		int ip = 0, ik = 0;
		// Add up all pairwise potentials
		for( unsigned int k=0; k<pairwise_.size(); k++ ) {
			// Compute the pairwise gradient expression
			if( lbl_cmp_grad ) {
				VectorXf pg = pairwise_[k]->gradient( b, Q[it] );
				lbl_cmp_grad->segment( ip, pg.rows() ) += pg;
				ip += pg.rows();
			}
			// Compute the kernel gradient expression
			if( kernel_grad ) {
				VectorXf pg = pairwise_[k]->kernelGradient( b, Q[it] );
				kernel_grad->segment( ik, pg.rows() ) += pg;
				ik += pg.rows();
			}
			// Compute the new b
			pairwise_[k]->applyTranspose( tmp2, b );
			tmp1 += tmp2;
		}
		sumAndNormalize( b, tmp1.array()*Q[it].array(), Q[it] );

		// Add the gradient
		if(unary_grad && unary_)
			*unary_grad += unary_->gradient( b );
	}
	return r;
}
VectorXf DenseCRF::unaryParameters() const {
	if( unary_ )
		return unary_->parameters();
	return VectorXf();
}
void DenseCRF::setUnaryParameters( const VectorXf & v ) {
	if( unary_ )
		unary_->setParameters( v );
}
VectorXf DenseCRF::labelCompatibilityParameters() const {
	std::vector< VectorXf > terms;
	for( unsigned int k=0; k<pairwise_.size(); k++ )
		terms.push_back( pairwise_[k]->parameters() );
	int np=0;
	for( unsigned int k=0; k<pairwise_.size(); k++ )
		np += terms[k].rows();
	VectorXf r( np );
	for( unsigned int k=0,i=0; k<pairwise_.size(); k++ ) {
		r.segment( i, terms[k].rows() ) = terms[k];
		i += terms[k].rows();
	}
	return r;
}
void DenseCRF::setLabelCompatibilityParameters( const VectorXf & v ) {
	std::vector< int > n;
	for( unsigned int k=0; k<pairwise_.size(); k++ )
		n.push_back( pairwise_[k]->parameters().rows() );
	int np=0;
	for( unsigned int k=0; k<pairwise_.size(); k++ )
		np += n[k];

	for( unsigned int k=0,i=0; k<pairwise_.size(); k++ ) {
		pairwise_[k]->setParameters( v.segment( i, n[k] ) );
		i += n[k];
	}
}
VectorXf DenseCRF::kernelParameters() const {
	std::vector< VectorXf > terms;
	for( unsigned int k=0; k<pairwise_.size(); k++ )
		terms.push_back( pairwise_[k]->kernelParameters() );
	int np=0;
	for( unsigned int k=0; k<pairwise_.size(); k++ )
		np += terms[k].rows();
	VectorXf r( np );
	for( unsigned int k=0,i=0; k<pairwise_.size(); k++ ) {
		r.segment( i, terms[k].rows() ) = terms[k];
		i += terms[k].rows();
	}
	return r;
}
void DenseCRF::setKernelParameters( const VectorXf & v ) {
	std::vector< int > n;
	for( unsigned int k=0; k<pairwise_.size(); k++ )
		n.push_back( pairwise_[k]->kernelParameters().rows() );
	int np=0;
	for( unsigned int k=0; k<pairwise_.size(); k++ )
		np += n[k];

	for( unsigned int k=0,i=0; k<pairwise_.size(); k++ ) {
		pairwise_[k]->setKernelParameters( v.segment( i, n[k] ) );
		i += n[k];
	}
}

// start adding higher order potential related stuffs
void DenseCRF::set_ho(bool add_ho)
{
	addHO=add_ho;
}

void DenseCRF::set_mem_init_higherorder()
{
	if(addHO)
	{
		if (high_order_sp_to_pix.rows()!=M_)
		    high_order_sp_to_pix=MatrixXf::Zero(M_, N_);
		else
		    high_order_sp_to_pix.setZero();

		if (hierarchical_high_order)
		{
		    if (high_order_pix_to_sp.rows()!=M_)
			high_order_pix_to_sp=MatrixXf::Zero(M_,all_3d_superpixels_.size());
		    else
			high_order_pix_to_sp.setZero();
		}
	}
}

void DenseCRF::calculate_higherorder_pot(const MatrixXf& Q,MatrixXf& temp_energy)
{
	if(!decoupled_high_order)
	{
		int segment_count=all_3d_superpixels_.size();
		MatrixXf h_norm = MatrixXf::Ones(M_, segment_count);  // label distribution for each superpixels, directly multiply all pixels probability of that class

		int curr_pix_label = 0, curr_pix_index; // int x, y;

		VectorXf::Index max_label;

		float higher_order_prob;
		int pixels_num_in_superpixel = 0;
		for(int sp_id = 0; sp_id < segment_count; sp_id++)  // for each superpixel
		{
			pixels_num_in_superpixel = all_3d_superpixels_[sp_id]->pixel_indexes.size();
			for(int k = 0; k < M_; k++) // for all the class
			{
				higher_order_prob = 0.0;
				for(int j = 0; j < pixels_num_in_superpixel; j++) // for each pixel in the superpixel //TODO change to vector production
				{
					curr_pix_index = all_3d_superpixels_[sp_id]->pixel_indexes[j];
					higher_order_prob = higher_order_prob + Q(k,curr_pix_index); // change product to sum, avoid underflow
				}
				h_norm(k,sp_id) = higher_order_prob; // multiply all pixel prob in one superpixel
			}
		}

		double alpha = 0.5;
		float cost_l = 1;  //10
		float cost_max = 5;  //100
		float high_order_weight = 1; // weight relative to unary and pairwise. TODO should be large?
		for(int sp_id = 0; sp_id < segment_count; sp_id++)   // for each superpixel
		{
			pixels_num_in_superpixel = all_3d_superpixels_[sp_id]->pixel_indexes.size();
			for(int j = 0; j < pixels_num_in_superpixel; j++)   //  for each pixel in the superpixel
			{
			    VectorXf::Index max_label;
			    curr_pix_index = all_3d_superpixels_[sp_id]->pixel_indexes[j];
			    for(int k = 0; k < M_; k++)  //  for all the class
			    {
				higher_order_prob = h_norm(k,sp_id)-Q(k,curr_pix_index)+0.0001;
				higher_order_prob = higher_order_prob/float(pixels_num_in_superpixel);

				// higher_order_prob is very small, so this is very close to cost_max
				high_order_sp_to_pix(k,curr_pix_index) += higher_order_prob*cost_l+(1-higher_order_prob)*cost_max;

				// our proposed.   superpixel's label influce the clique,
// 				high_order_sp_to_pix(k,curr_pix_index) -= ( all_3d_superpixels_[sp_id]->label_distri).dot( Q.col(curr_pix_index) ) * 100; // 100 is a weght
// 				if (k==all_3d_superpixels_[sp_id]->label)
// 				    high_order_sp_to_pix(k,curr_pix_index) -= higher_order_prob*all_3d_superpixels_[sp_id]->label_distri[k]*100;
			    }
			}
		}
		high_order_sp_to_pix = high_order_sp_to_pix*high_order_weight;
	}
	else  // change robust Pn model into pairwise+auxiliary node. but assume auxiliary's node is fixed.
	{
		int curr_pix_label = 0, curr_pix_index; // int x, y;

		for(int sp_id = 0; sp_id < all_3d_superpixels_.size(); sp_id++)  // for each superpixel
		{
			for(int j = 0; j < all_3d_superpixels_[sp_id]->pixel_indexes.size(); j++) // for each pixel in the superpixel //TODO change to vector production
			{
				curr_pix_index = all_3d_superpixels_[sp_id]->pixel_indexes[j];
				float max_prob = Q.col(curr_pix_index).maxCoeff(&curr_pix_label);
				if (curr_pix_label==all_3d_superpixels_[sp_id]->label)
				    temp_energy(curr_pix_label,curr_pix_index) -= all_3d_superpixels_[sp_id]->max_label_prob*10.0;
				  // only one label (superpixel's label) has some energy.
// 				    high_order_sp_to_pix(curr_pix_label,curr_pix_index) -= 10.0; //if label the same, give negative energy
			}
		}
	}
}

//both minimize_x_y  E(x,y)
void DenseCRF::calculate_hierarchical_pot(const MatrixXf& Q_low,MatrixXf& Q_high,MatrixXf& temp_low_energy,MatrixXf& temp_high_energy)
{
	int curr_pix_label = 0, curr_pix_index; // int x, y;
	int curr_sp_label = 0;

	for(int sp_id = 0; sp_id < all_3d_superpixels_.size(); sp_id++)  // for each superpixel
	{
		float max_prob_sp = Q_high.col(sp_id).maxCoeff(&curr_sp_label);
		for(int j = 0; j < all_3d_superpixels_[sp_id]->pixel_indexes.size(); j++) // for each pixel in the superpixel
		{
			curr_pix_index = all_3d_superpixels_[sp_id]->pixel_indexes[j];
			float max_prob_pix = Q_low.col(curr_pix_index).maxCoeff(&curr_pix_label);
			if (curr_pix_label==curr_sp_label)
			{
// 			      high_order_sp_to_pix(curr_pix_label,curr_pix_index) -= max_prob_sp*0.2;
// 			      high_order_pix_to_sp(curr_sp_label,sp_id) -= max_prob_pix*0.2;
			      temp_low_energy(curr_pix_label,curr_pix_index) -= max_prob_sp*0.2;  // directly minus, faster
			      temp_high_energy(curr_sp_label,sp_id) -= max_prob_pix*0.2;
			}
		}
	}
}

//only minimize_y E(x,y)
void DenseCRF::calculate_hierar_highlevel_pot(const MatrixXf& Q_low,const MatrixXf& Q_high,MatrixXf& temp_high_energy)
{
	int curr_pix_label = 0, curr_pix_index; // int x, y;
	int curr_sp_label = 0;

	for(int sp_id = 0; sp_id < all_3d_superpixels_.size(); sp_id++)  // for each superpixel
	{
		float max_prob_sp = Q_high.col(sp_id).maxCoeff(&curr_sp_label);
		for(int j = 0; j < all_3d_superpixels_[sp_id]->pixel_indexes.size(); j++) // for each pixel in the superpixel
		{
			curr_pix_index = all_3d_superpixels_[sp_id]->pixel_indexes[j];
			float max_prob_pix = Q_low.col(curr_pix_index).maxCoeff(&curr_pix_label);
			if (curr_pix_label==curr_sp_label) // if the same label, encourage, give negative energy.  could also penalize for different label.
			{
// 			      high_order_pix_to_sp(curr_sp_label,sp_id) -= max_prob_pix*0.2;
			      temp_high_energy(curr_sp_label,sp_id) += 0.1;  // faster, directly add, originally, increase it to encourage it
			}
		}
	}
}

// using the minimal y, min_x E(x,y)
void DenseCRF::calculate_hierar_lowlevel_pot(const MatrixXf& Q_low,MatrixXf& temp_low_energy)
{
	int curr_pix_label = 0, curr_pix_index; // int x, y;

	std::clock_t begin = std::clock();
	for(int sp_id = 0; sp_id < all_3d_superpixels_.size(); sp_id++)  // for each superpixel
	{
		for(int j = 0; j < all_3d_superpixels_[sp_id]->pixel_indexes.size(); j++) // for each pixel in the superpixel
		{
			curr_pix_index = all_3d_superpixels_[sp_id]->pixel_indexes[j];
			float max_prob_pix = Q_low.col(curr_pix_index).maxCoeff(&curr_pix_label);
			if (curr_pix_label==all_3d_superpixels_[sp_id]->label)
			{
			      temp_low_energy(curr_pix_label,curr_pix_index) += 5.0;
			}
		}
	}
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2015 Johannes Göttker-Schnetmann
 Copyright (C) 2015 Klaus Spanderen

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file hestonslvmcmodel.cpp
*/

#include <ql/math/functional.hpp>
#include <ql/termstructures/volatility/equityfx/fixedlocalvolsurface.hpp>
#include <ql/experimental/models/hestonslvmcmodel.hpp>
#include <ql/experimental/processes/hestonslvprocess.hpp>

#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#endif
#include <boost/multi_array.hpp>
#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic pop
#endif

namespace QuantLib {
    HestonSLVMCModel::HestonSLVMCModel(
        const Handle<LocalVolTermStructure>& localVol,
        const Handle<HestonModel>& hestonModel,
        const ext::shared_ptr<BrownianGeneratorFactory>& brownianGeneratorFactory,
        const Date& endDate,
        Size timeStepsPerYear,
        Size nBins,
        Size calibrationPaths,
        const std::vector<Date>& mandatoryDates,
        const Real mixingFactor)
    : localVol_(localVol),
      hestonModel_(hestonModel),
      brownianGeneratorFactory_(brownianGeneratorFactory),
      endDate_(endDate),
      nBins_(nBins),
      calibrationPaths_(calibrationPaths),
      mixingFactor_(mixingFactor) {

        registerWith(localVol_);
        registerWith(hestonModel_);

        const DayCounter dc = hestonModel_->process()->riskFreeRate()->dayCounter();
        const Date refDate = hestonModel_->process()->riskFreeRate()->referenceDate();

        std::vector<Time> gridTimes;
        gridTimes.reserve(mandatoryDates.size()+1);
        for (Size i=0; i < mandatoryDates.size(); ++i) {
            gridTimes.push_back(dc.yearFraction(refDate, mandatoryDates[i]));

        }
        gridTimes.push_back(dc.yearFraction(refDate, endDate));

        timeGrid_ = ext::make_shared<TimeGrid>(gridTimes.begin(), gridTimes.end(),
                std::max(Size(2), Size(gridTimes.back()*timeStepsPerYear)));
    }

    ext::shared_ptr<HestonProcess> HestonSLVMCModel::hestonProcess() const {
        return hestonModel_->process();
    }

    ext::shared_ptr<LocalVolTermStructure> HestonSLVMCModel::localVol() const {
        return localVol_.currentLink();
    }

    ext::shared_ptr<LocalVolTermStructure>
    HestonSLVMCModel::leverageFunction() const {
        calculate();

        return leverageFunction_;
    }

    void HestonSLVMCModel::performCalculations() const {
        const ext::shared_ptr<HestonProcess> hestonProcess
            = hestonModel_->process();
        const ext::shared_ptr<Quote> spot
            = hestonProcess->s0().currentLink();

        const Real v0            = hestonProcess->v0();
        const DayCounter dc      = hestonProcess->riskFreeRate()->dayCounter();
        const Date referenceDate = hestonProcess->riskFreeRate()->referenceDate();

        const Volatility lv0
            = localVol_->localVol(0.0, spot->value())/std::sqrt(v0);

        const ext::shared_ptr<Matrix> L(new Matrix(nBins_, timeGrid_->size()));

        std::vector<ext::shared_ptr<std::vector<Real> > >
            vStrikes(timeGrid_->size());
        for (Size i=0; i < timeGrid_->size(); ++i) {
            const Integer u = nBins_/2;
            const Real dx = spot->value()*std::sqrt(QL_EPSILON);

            vStrikes[i] = ext::make_shared<std::vector<Real> >(nBins_);

            for (Integer j=0; j < Integer(nBins_); ++j)
                vStrikes[i]->at(j) = spot->value() + (j - u)*dx;
        }

        std::fill(L->column_begin(0),L->column_end(0), lv0);

        leverageFunction_ = ext::make_shared<FixedLocalVolSurface>(
            referenceDate,
            std::vector<Time>(timeGrid_->begin(), timeGrid_->end()),
            vStrikes, L, dc);

        const ext::shared_ptr<HestonSLVProcess> slvProcess
            = ext::make_shared<HestonSLVProcess>(hestonProcess, leverageFunction_, mixingFactor_);

        std::vector<std::pair<Real, Real> > pairs(
                calibrationPaths_, std::make_pair(spot->value(), v0));

        const Size k = calibrationPaths_ / nBins_;
        const Size m = calibrationPaths_ % nBins_;

        const Size timeSteps = timeGrid_->size()-1;

        typedef boost::multi_array<Real, 3> path_type;
        path_type paths(boost::extents[calibrationPaths_][timeSteps][2]);

        const ext::shared_ptr<BrownianGenerator> brownianGenerator =
            brownianGeneratorFactory_->create(2, timeSteps);

        for (Size i=0; i < calibrationPaths_; ++i) {
            brownianGenerator->nextPath();
            std::vector<Real> tmp(2);
            for (Size j=0; j < timeSteps; ++j) {
                brownianGenerator->nextStep(tmp);
                paths[i][j][0] = tmp[0];
                paths[i][j][1] = tmp[1];
            }
        }

        for (Size n=1; n < timeGrid_->size(); ++n) {
            const Time t = timeGrid_->at(n-1);
            const Time dt = timeGrid_->dt(n-1);

            Array x0(2), dw(2);

            for (Size i=0; i < calibrationPaths_; ++i) {
                x0[0] = pairs[i].first;
                x0[1] = pairs[i].second;

                dw[0] = paths[i][n-1][0];
                dw[1] = paths[i][n-1][1];

                x0 = slvProcess->evolve(t, x0, dt, dw);

                pairs[i].first = x0[0];
                pairs[i].second = x0[1];
            }

            std::sort(pairs.begin(), pairs.end());

            Size s = 0U, e = 0U;
            for (Size i=0; i < nBins_; ++i) {
                const Size inc = k + static_cast<unsigned long>(i < m);
                e = s + inc;

                Real sum=0.0;
                for (Size j=s; j < e; ++j) {
                    sum+=pairs[j].second;
                }
                sum/=inc;

                vStrikes[n]->at(i) = 0.5*(pairs[e-1].first + pairs[s].first);
                (*L)[i][n] = std::sqrt(square<Real>()(
                     localVol_->localVol(t, vStrikes[n]->at(i), true))/sum);

                s = e;
            }

            leverageFunction_->setInterpolation<Linear>();
        }
    }
}


// Boost.Geometry

// Copyright (c) 2017-2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_SRS_PROJECTIONS_DPAR_HPP
#define BOOST_GEOMETRY_SRS_PROJECTIONS_DPAR_HPP

#include <boost/geometry/core/radius.hpp>
#include <boost/geometry/core/tag.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/srs/projections/exception.hpp>
#include <boost/geometry/srs/projections/par_data.hpp>
#include <boost/geometry/srs/sphere.hpp>
#include <boost/geometry/srs/spheroid.hpp>

#include <boost/mpl/assert.hpp>
#include <boost/mpl/if.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/variant/variant.hpp>
#include <boost/type_traits/integral_constant.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/is_void.hpp>

#include <string>
#include <vector>

namespace boost { namespace geometry { namespace srs
{

namespace detail
{

template
<
    typename Types,
    typename T,
    typename Iter = typename boost::mpl::begin<Types>::type,
    typename End = typename boost::mpl::end<Types>::type,
    int I = 0
>
struct find_type_index
{
    typedef typename boost::mpl::deref<Iter>::type type;
    static const int value = boost::is_same<type, T>::value
                           ? I
                           : find_type_index
                                <
                                    Types,
                                    T,
                                    typename boost::mpl::next<Iter>::type,
                                    End,
                                    I + 1
                                >::value;

};

template
<
    typename Types,
    typename T,
    typename End,
    int I
>
struct find_type_index<Types, T, End, End, I>
{
    static const int value = I;
};

template
<
    typename Range,
    typename ToValue,
    bool IsRange = boost::has_range_iterator<Range>::value
>
struct is_convertible_range
    : boost::is_convertible
        <
            typename boost::range_value<Range>::type,
            ToValue
        >
{};

template
<
    typename Range,
    typename ToValue
>
struct is_convertible_range<Range, ToValue, false>
    : boost::false_type
{};

} // namespace detail

namespace dpar
{

enum value_datum
{
    datum_wgs84 = 0,
    datum_ggrs87,
    datum_nad83,
    datum_nad27,
    datum_potsdam,
    datum_carthage,
    datum_hermannskogel,
    datum_ire65,
    datum_nzgd49,
    datum_osgb36,
};

enum value_ellps
{
    ellps_merit = 0,
    ellps_sgs85,
    ellps_grs80,
    ellps_iau76,
    ellps_airy,
    ellps_apl4_9,
    ellps_nwl9d,
    ellps_mod_airy,
    ellps_andrae,
    ellps_aust_sa,
    ellps_grs67,
    ellps_bessel,
    ellps_bess_nam,
    ellps_clrk66,
    ellps_clrk80,
    ellps_clrk80ign,
    ellps_cpm,
    ellps_delmbr,
    ellps_engelis,
    ellps_evrst30,
    ellps_evrst48,
    ellps_evrst56,
    ellps_evrst69,
    ellps_evrstss,
    ellps_fschr60,
    ellps_fschr60m,
    ellps_fschr68,
    ellps_helmert,
    ellps_hough,
    ellps_intl,
    ellps_krass,
    ellps_kaula,
    ellps_lerch,
    ellps_mprts,
    ellps_new_intl,
    ellps_plessis,
    ellps_seasia,
    ellps_walbeck,
    ellps_wgs60,
    ellps_wgs66,
    ellps_wgs72,
    ellps_wgs84,
    ellps_sphere
};

enum value_mode
{
    mode_plane = 0,
    mode_di,
    mode_dd,
    mode_hex
};

enum value_orient
{
    orient_isea = 0,
    orient_pole,
};

enum value_pm
{
    pm_greenwich = 0,
    pm_lisbon,
    pm_paris,
    pm_bogota,
    pm_madrid,
    pm_rome,
    pm_bern,
    pm_jakarta,
    pm_ferro,
    pm_brussels,
    pm_stockholm,
    pm_athens,
    pm_oslo
};

enum value_proj
{
    proj_unknown = 0,
    proj_aea, proj_leac,
    proj_aeqd,
    proj_airy,
    proj_aitoff, proj_wintri,
    proj_august,
    proj_apian, proj_ortel, proj_bacon,
    proj_bipc,
    proj_boggs,
    proj_bonne,
    proj_cass,
    proj_cc,
    proj_cea,
    proj_chamb,
    proj_collg,
    proj_crast,
    proj_denoy,
    proj_eck1,
    proj_eck2,
    proj_eck3, proj_putp1, proj_wag6, proj_kav7,
    proj_eck4,
    proj_eck5,
    proj_eqc,
    proj_eqdc,
    proj_etmerc, proj_utm,
    proj_fahey,
    proj_fouc_s,
    proj_gall,
    proj_geocent,
    proj_geos,
    proj_gins8,
    proj_gn_sinu, proj_sinu, proj_eck6, proj_mbtfps,
    proj_gnom,
    proj_goode,
    proj_gstmerc,
    proj_hammer,
    proj_hatano,
    proj_healpix,
    proj_rhealpix,
    proj_igh,
    proj_imw_p,
    proj_isea,
    proj_krovak,
    proj_labrd,
    proj_laea,
    proj_lagrng,
    proj_larr,
    proj_lask,
    proj_lonlat, proj_latlon, proj_latlong, proj_longlat,
    proj_lcc,
    proj_lcca,
    proj_loxim,
    proj_lsat,
    proj_mbt_fps,
    proj_mbtfpp,
    proj_mbtfpq,
    proj_merc,
    proj_mill,
    proj_mil_os, proj_lee_os, proj_gs48, proj_alsk, proj_gs50,
    proj_moll, proj_wag4, proj_wag5,
    proj_natearth,
    proj_nell,
    proj_nell_h,
    proj_nicol,
    proj_nsper, proj_tpers,
    proj_nzmg,
    proj_ob_tran,
    proj_ocea,
    proj_oea,
    proj_omerc,
    proj_ortho,
    proj_poly,
    proj_putp2,
    proj_putp3, proj_putp3p,
    proj_putp4p, proj_weren,
    proj_putp5, proj_putp5p,
    proj_putp6, proj_putp6p,
    proj_qsc,
    proj_robin,
    proj_rouss,
    proj_rpoly,
    proj_euler, proj_murd1, proj_murd2, proj_murd3, proj_pconic, proj_tissot, proj_vitk1,
    proj_somerc,
    proj_stere, proj_ups,
    proj_sterea,
    proj_kav5, proj_qua_aut, proj_fouc, proj_mbt_s,
    proj_tcc,
    proj_tcea,
    proj_tmerc,
    proj_tpeqd,
    proj_urm5,
    proj_urmfps, proj_wag1,
    proj_vandg,
    proj_vandg2, proj_vandg3,
    proj_vandg4,
    proj_wag2,
    proj_wag3,
    proj_wag7,
    proj_wink1,
    proj_wink2
};

enum value_sweep
{
    sweep_x = 0, sweep_y
};

enum value_units
{
    units_km = 0,
    units_m,
    units_dm,
    units_cm,
    units_mm,
    units_kmi,
    units_in,
    units_ft,
    units_yd,
    units_mi,
    units_fath,
    units_ch,
    units_link,
    units_us_in,
    units_us_ft,
    units_us_yd,
    units_us_ch,
    units_us_mi,
    units_ind_yd,
    units_ind_ft,
    units_ind_ch
};

enum name_f
{
    a = 0,
    b,
    e,
    es,
    f,
    h,
    //h_0, // currently not used
    k = 7,
    k_0,
    m, // also used for M
    n,
    //phdg_0, // currently not used
    //plat_0, // currently not used
    //plon_0, // currently not used
    q = 14,
    r, // originally R
    rf,
    to_meter,
    vto_meter,
    w, // originally W
    x_0,
    y_0
};

enum name_r
{
    alpha = 22,
    azi,
    gamma,
    lat_0,
    lat_1,
    lat_2,
    lat_3,
    lat_b,
    lat_ts, // 30
    lon_0,
    lon_1,
    lon_2,
    lon_3,
    lon_wrap,
    lonc,
    o_alpha,
    o_lat_1,
    o_lat_2,
    o_lat_c, // 40
    o_lat_p,
    o_lon_1,
    o_lon_2,
    o_lon_c,
    o_lon_p,
    r_lat_a, // originally R_lat_a
    r_lat_g, // originally R_lat_g
    theta,
    tilt
};

enum name_i
{
    aperture = 50,
    lsat,
    north_square,
    path,
    resolution,
    south_square,
    zone
};

enum name_be
{
    czech = 57,
    geoc,
    guam,
    no_cut, // 60
    no_defs,
    no_rot,
    ns,
    over,
    r_au, // originally R_A
    r_a, // originally R_a
    r_g, // originally R_g
    r_h, // originally R_h
    r_v, // originally R_V
    rescale, // 70
    south
};

/*enum name_catalog
{
    catalog = 72 // currently not used
};

enum name_date
{
    date = 73 // currently not used
};*/

enum name_datum
{
    datum = 74
};

enum name_ellps
{
    ellps = 75 // id, sphere or spheroid
};

/*enum name_geoidgrids
{
    geoidgrids = 76 // currently not used
};*/

enum name_mode
{
    mode = 77
};

enum name_nadgrids
{
    nadgrids = 78 // arbitrary-length list of strings
};

enum name_orient
{
    orient = 79
};

enum name_pm
{
    pm = 80 // id or angle
};

enum name_proj
{
    o_proj = 81,
    proj
};

enum name_sweep
{
    sweep = 83
};

enum name_towgs84
{
    towgs84 = 84 // 3 or 7 element list of numbers
};

enum name_units
{
    units = 85,
    vunits
};

template <typename T>
struct parameter
{
    parameter()
        : m_id(-1), m_value(false)
    {}

    parameter(name_f id, T const& v)
        : m_id(id), m_value(v)
    {}

    // TODO various angle units
    parameter(name_r id, T const& v)
        : m_id(id), m_value(v)
    {}

    parameter(name_i id, int v)
        : m_id(id), m_value(v)
    {}

    parameter(name_be id)
        : m_id(id), m_value(true)
    {}

    parameter(name_be id, bool v)
        : m_id(id), m_value(v)
    {}

    parameter(name_datum id, value_datum v)
        : m_id(id), m_value(int(v))
    {}

    parameter(value_datum v)
        : m_id(datum), m_value(int(v))
    {}

    // TODO: store model at this point?
    parameter(name_ellps id, value_ellps v)
        : m_id(id), m_value(int(v))
    {}
    // TODO: store model at this point?
    parameter(value_ellps v)
        : m_id(ellps), m_value(int(v))
    {}

    template <typename Sphere>
    parameter(name_ellps id, Sphere const& v,
              typename boost::enable_if_c
                <
                    boost::is_same<typename geometry::tag<Sphere>::type, srs_sphere_tag>::value
                >::type * = 0)
        : m_id(id)
        , m_value(T(get_radius<0>(v)))
    {}

    template <typename Spheroid>
    parameter(name_ellps id, Spheroid const& v,
              typename boost::enable_if_c
                <
                    boost::is_same<typename geometry::tag<Spheroid>::type, srs_spheroid_tag>::value
                >::type * = 0)
        : m_id(id)
        , m_value(srs::spheroid<T>(get_radius<0>(v), get_radius<2>(v)))
    {}

    parameter(name_mode id, value_mode v)
        : m_id(id), m_value(int(v))
    {}

    parameter(value_mode v)
        : m_id(mode), m_value(int(v))
    {}

    template <typename Range>
    parameter(name_nadgrids id, Range const& v,
              typename boost::enable_if_c
                <
                    detail::is_convertible_range<Range const, std::string>::value
                >::type * = 0)
        : m_id(id)
        , m_value(srs::detail::nadgrids(boost::begin(v), boost::end(v)))
    {}

#ifndef BOOST_NO_CXX11_HDR_INITIALIZER_LIST
    parameter(name_nadgrids id, std::initializer_list<std::string> v)
        : m_id(id)
        , m_value(srs::detail::nadgrids(v))
    {}
#endif

    parameter(name_orient id, value_orient v)
        : m_id(id), m_value(int(v))
    {}

    parameter(value_orient v)
        : m_id(orient), m_value(int(v))
    {}

    // TODO: store to_meters at this point?
    parameter(name_pm id, value_pm v)
        : m_id(id), m_value(int(v))
    {}
    // TODO: store to_meters at this point?
    parameter(value_pm v)
        : m_id(pm), m_value(int(v))
    {}

    // TODO angle units
    parameter(name_pm id, T const& v)
        : m_id(id), m_value(v)
    {}

    parameter(name_proj id, value_proj v)
        : m_id(id), m_value(int(v))
    {}

    parameter(value_proj v)
        : m_id(proj), m_value(int(v))
    {}

    parameter(name_sweep id, value_sweep v)
        : m_id(id), m_value(int(v))
    {}

    parameter(value_sweep v)
        : m_id(sweep), m_value(int(v))
    {}

    template <typename Range>
    parameter(name_towgs84 id, Range const& v,
              typename boost::enable_if_c
                <
                    detail::is_convertible_range<Range const, T>::value
                >::type * = 0)
        : m_id(id)
        , m_value(srs::detail::towgs84<T>(boost::begin(v), boost::end(v)))
    {
        std::size_t n = boost::size(v);
        if (n != 3 && n != 7)
        {
            BOOST_THROW_EXCEPTION( projection_exception("Invalid number of towgs84 elements. Should be 3 or 7.") );
        }
    }

#ifndef BOOST_NO_CXX11_HDR_INITIALIZER_LIST
    parameter(name_towgs84 id, std::initializer_list<T> v)
        : m_id(id)
        , m_value(srs::detail::towgs84<T>(v))
    {
        std::size_t n = v.size();
        if (n != 3 && n != 7)
        {
            BOOST_THROW_EXCEPTION( projection_exception("Invalid number of towgs84 elements. Should be 3 or 7.") );
        }
    }
#endif

    parameter(name_units id, value_units v)
        : m_id(id), m_value(int(v))
    {}

    parameter(value_units v)
        : m_id(units), m_value(int(v))
    {}

private:
    typedef boost::variant
        <
            bool,
            int,
            T,
            srs::spheroid<T>,
            srs::detail::nadgrids,
            srs::detail::towgs84<T>
        > variant_type;

public:
    bool is_id_equal(name_f const& id) const { return m_id == int(id); }
    bool is_id_equal(name_r const& id) const { return m_id == int(id); }
    bool is_id_equal(name_i const& id) const { return m_id == int(id); }
    bool is_id_equal(name_be const& id) const { return m_id == int(id); }
    bool is_id_equal(name_datum const& id) const { return m_id == int(id); }
    bool is_id_equal(name_ellps const& id) const { return m_id == int(id); }
    bool is_id_equal(name_mode const& id) const { return m_id == int(id); }
    bool is_id_equal(name_nadgrids const& id) const { return m_id == int(id); }
    bool is_id_equal(name_orient const& id) const { return m_id == int(id); }
    bool is_id_equal(name_pm const& id) const { return m_id == int(id); }
    bool is_id_equal(name_proj const& id) const { return m_id == int(id); }
    bool is_id_equal(name_sweep const& id) const { return m_id == int(id); }
    bool is_id_equal(name_towgs84 const& id) const { return m_id == int(id); }
    bool is_id_equal(name_units const& id) const { return m_id == int(id); }

    template <typename V>
    V const& get_value() const
    {
        return boost::get<V>(m_value);
    }

    template <typename V>
    bool is_value_set() const
    {
        return m_value.which() == srs::detail::find_type_index
            <
                typename variant_type::types,
                V
            >::value;
    }

private:
    int m_id;
    variant_type m_value;
};

template <typename T = double>
class parameters
{
    typedef std::vector<parameter<T> > container_type;

public:
    typedef typename container_type::value_type value_type;
    typedef typename container_type::const_iterator const_iterator;
    typedef typename container_type::const_reference const_reference;
    typedef typename container_type::size_type size_type;

    BOOST_DEFAULTED_FUNCTION(parameters(), {})

#if defined(BOOST_NO_CXX11_RVALUE_REFERENCES) || defined(BOOST_NO_CXX11_VARIADIC_TEMPLATES)
    template <typename Id>
    explicit parameters(Id id)
    {
        add(id);
    }

    template <typename Id>
    parameters & add(Id id)
    {
        m_params.push_back(parameter<T>(id));
        return *this;
    }

    template <typename Id>
    parameters & operator()(Id id)
    {
        return add(id);
    }

    template <typename Id, typename V>
    parameters(Id id, V const& value)
    {
        add(id, value);
    }

    template <typename Id, typename V>
    parameters & add(Id id, V const& value)
    {
        m_params.push_back(parameter<T>(id, value));
        return *this;
    }

    template <typename Id, typename V>
    parameters & operator()(Id id, V const& value)
    {
        return add(id, value);
    }

#ifndef BOOST_NO_CXX11_HDR_INITIALIZER_LIST
    template <typename Id, typename V>
    parameters(Id id, std::initializer_list<V> value)
    {
        add(id, value);
    }

    template <typename Id, typename V>
    parameters & add(Id id, std::initializer_list<V> value)
    {
        m_params.push_back(parameter<T>(id, value));
        return *this;
    }

    template <typename Id, typename V>
    parameters & operator()(Id id, std::initializer_list<V> value)
    {
        return add(id, value);
    }
#endif // BOOST_NO_CXX11_HDR_INITIALIZER_LIST
#else // BOOST_NO_CXX11_RVALUE_REFERENCES || BOOST_NO_CXX11_RVALUE_REFERENCES
    template <typename Id>
    explicit parameters(Id id)
    {
        add(id);
    }

    template <typename Id>
    parameters & add(Id id)
    {
        m_params.emplace_back(id);
        return *this;
    }

    template <typename Id>
    parameters & operator()(Id id)
    {
        return add(id);
    }

    template <typename Id, typename V>
    parameters(Id id, V && value)
    {
        add(id, std::forward<V>(value));
    }

    template <typename Id, typename V>
    parameters & add(Id id, V && value)
    {
        m_params.emplace_back(id, std::forward<V>(value));
        return *this;
    }

    template <typename Id, typename V>
    parameters & operator()(Id id, V && value)
    {
        return add(id, std::forward<V>(value));
    }

#ifndef BOOST_NO_CXX11_HDR_INITIALIZER_LIST
    template <typename Id, typename V>
    parameters(Id id, std::initializer_list<V> value)
    {
        add(id, value);
    }

    template <typename Id, typename V>
    parameters & add(Id id, std::initializer_list<V> value)
    {
        m_params.emplace_back(id, value);
        return *this;
    }

    template <typename Id, typename V>
    parameters & operator()(Id id, std::initializer_list<V> value)
    {
        return add(id, value);
    }
#endif // BOOST_NO_CXX11_HDR_INITIALIZER_LIST
#endif // BOOST_NO_CXX11_RVALUE_REFERENCES || BOOST_NO_CXX11_RVALUE_REFERENCES

    const_iterator begin() const { return m_params.begin(); }
    const_iterator end() const { return m_params.end(); }
    const_reference operator[](size_type i) const { return m_params[i]; }
    size_type size() { return m_params.size(); }
    bool empty() { return m_params.empty(); }

private:
    container_type m_params;
};

} // namespace dpar

}}} // namespace boost::geometry::srs

#endif // BOOST_GEOMETRY_SRS_PROJECTIONS_DPAR_HPP


// Distributed under the MIT License.
// See LICENSE.txt for details.

/// \file
/// Defines class AdamsBashforthN

#pragma once

#include <algorithm>
#include <boost/iterator/transform_iterator.hpp>
#include <cstddef>
#include <iosfwd>
#include <iterator>
#include <limits>
#include <map>
#include <pup.h>
#include <tuple>
#include <type_traits>
#include <vector>

#include "NumericalAlgorithms/Interpolation/LagrangePolynomial.hpp"
#include "Options/Options.hpp"
#include "Parallel/CharmPupable.hpp"
#include "Time/EvolutionOrdering.hpp"
#include "Time/Time.hpp"
#include "Time/TimeStepId.hpp"
#include "Time/TimeSteppers/TimeStepper.hpp"  // IWYU pragma: keep
#include "Utilities/CachedFunction.hpp"
#include "Utilities/ErrorHandling/Assert.hpp"
#include "Utilities/ErrorHandling/Error.hpp"
#include "Utilities/Gsl.hpp"
#include "Utilities/MakeWithValue.hpp"
#include "Utilities/Overloader.hpp"
#include "Utilities/TMPL.hpp"

/// \cond
namespace TimeSteppers {
template <typename LocalVars, typename RemoteVars, typename CouplingResult>
class BoundaryHistory;  // IWYU pragma: keep
template <typename Vars, typename DerivVars>
class History;
}  // namespace TimeSteppers
/// \endcond

namespace TimeSteppers {

/*!
 * \ingroup TimeSteppersGroup
 *
 * An Nth order Adams-Bashforth time stepper.
 *
 * The stable step size factors for different orders are given by:
 *
 *

 *

 *     %Order
 *     CFL Factor
 *

 *

 *     1
 *     1
 *

 *

 *     2
 *     1 / 2
 *

 *

 *     3
 *     3 / 11
 *

 *

 *     4
 *     3 / 20
 *

 *

 *     5
 *     45 / 551
 *

 *

 *     6
 *     5 / 114
 *

 *

 *     7
 *     945 / 40663
 *

 *

 *     8
 *     945 / 77432
 *

 *

 */
class AdamsBashforthN : public LtsTimeStepper::Inherit {
 public:
  static constexpr const size_t maximum_order = 8;

  struct Order {
    using type = size_t;
    static constexpr Options::String help = {"Convergence order"};
    static type lower_bound() noexcept { return 1; }
    static type upper_bound() noexcept { return maximum_order; }
  };
  using options = tmpl::list<Order>;
  static constexpr Options::String help = {
      "An Adams-Bashforth Nth order time-stepper."};

  AdamsBashforthN() = default;
  explicit AdamsBashforthN(size_t order) noexcept;
  AdamsBashforthN(const AdamsBashforthN&) noexcept = default;
  AdamsBashforthN& operator=(const AdamsBashforthN&) noexcept = default;
  AdamsBashforthN(AdamsBashforthN&&) noexcept = default;
  AdamsBashforthN& operator=(AdamsBashforthN&&) noexcept = default;
  ~AdamsBashforthN() noexcept override = default;

  template <typename Vars, typename DerivVars>
  void update_u(gsl::not_null<Vars*> u,
                gsl::not_null<History<Vars, DerivVars>*> history,
                const TimeDelta& time_step) const noexcept;

  template <typename Vars, typename ErrVars, typename DerivVars>
  bool update_u(gsl::not_null<Vars*> u, gsl::not_null<ErrVars*> u_error,
                gsl::not_null<History<Vars, DerivVars>*> history,
                const TimeDelta& time_step) const noexcept;

  template <typename Vars, typename DerivVars>
  bool dense_update_u(gsl::not_null<Vars*> u,
                      const History<Vars, DerivVars>& history,
                      double time) const noexcept;

  // This is defined as a separate type alias to keep the doxygen page
  // width somewhat under control.
  template <typename LocalVars, typename RemoteVars, typename Coupling>
  using BoundaryHistoryType =
      BoundaryHistory<LocalVars, RemoteVars,
                      std::result_of_t<const Coupling&(LocalVars, RemoteVars)>>;

  /*!
   * An explanation of the computation being performed by this
   * function:
   * \f$\newcommand\tL{t^L}\newcommand\tR{t^R}\newcommand\tU{\tilde{t}\!}
   * \newcommand\mat{\mathbf}\f$
   *
   * Suppose the local and remote sides of the interface are evaluated
   * at times \f$\ldots, \tL_{-1}, \tL_0, \tL_1, \ldots\f$ and
   * \f$\ldots, \tR_{-1}, \tR_0, \tR_1, \ldots\f$, respectively, with
   * the starting location of the numbering arbitrary in each case.
   * Let the step we wish to calculate the effect of be the step from
   * \f$\tL_{m_S}\f$ to \f$\tL_{m_S+1}\f$.  We call the sequence
   * produced from the union of the local and remote time sequences
   * \f$\ldots, \tU_{-1}, \tU_0, \tU_1, \ldots\f$.  For example, one
   * possible sequence of times is:
   * \f{equation}
   *   \begin{aligned}
   *     \text{Local side:} \\ \text{Union times:} \\ \text{Remote side:}
   *   \end{aligned}
   *   \cdots
   *   \begin{gathered}
   *     \, \\ \tU_1 \\ \tR_5
   *   \end{gathered}
   *   \leftarrow \Delta \tU_1 \rightarrow
   *   \begin{gathered}
   *     \tL_4 \\ \tU_2 \\ \,
   *   \end{gathered}
   *   \leftarrow \Delta \tU_2 \rightarrow
   *   \begin{gathered}
   *     \, \\ \tU_3 \\ \tR_6
   *   \end{gathered}
   *   \leftarrow \Delta \tU_3 \rightarrow
   *   \begin{gathered}
   *    \, \\ \tU_4 \\ \tR_7
   *   \end{gathered}
   *   \leftarrow \Delta \tU_4 \rightarrow
   *   \begin{gathered}
   *     \tL_5 \\ \tU_5 \\ \,
   *   \end{gathered}
   *   \cdots
   * \f}
   * We call the indices of the step's start and end times in the
   * union time sequence \f$n_S\f$ and \f$n_E\f$, respectively.  We
   * define \f$n^L_m\f$ to be the union-time index corresponding to
   * \f$\tL_m\f$ and \f$m^L_n\f$ to be the index of the last local
   * time not later than \f$\tU_n\f$ and similarly for the remote
   * side.  So for the above example, \f$n^L_4 = 2\f$ and \f$m^R_2 =
   * 5\f$, and if we wish to compute the step from \f$\tL_4\f$ to
   * \f$\tL_5\f$ we would have \f$m_S = 4\f$, \f$n_S = 2\f$, and
   * \f$n_E = 5\f$.
   *
   * If we wish to evaluate the change over this step to \f$k\f$th
   * order, we can write the change in the value as a linear
   * combination of the values of the coupling between the elements at
   * unequal times:
   * \f{equation}
   *   \mat{F}_{m_S} =
   *   \mspace{-10mu}
   *   \sum_{q^L = m_S-(k-1)}^{m_S}
   *   \,
   *   \sum_{q^R = m^R_{n_S}-(k-1)}^{m^R_{n_E-1}}
   *   \mspace{-10mu}
   *   \mat{D}_{q^Lq^R}
   *   I_{q^Lq^R},
   * \f}
   * where \f$\mat{D}_{q^Lq^R}\f$ is the coupling function evaluated
   * between data from \f$\tL_{q^L}\f$ and \f$\tR_{q^R}\f$.  The
   * coefficients can be written as the sum of three terms,
   * \f{equation}
   *   I_{q^Lq^R} = I^E_{q^Lq^R} + I^R_{q^Lq^R} + I^L_{q^Lq^R},
   * \f}
   * which can be interpreted as a contribution from equal-time
   * evaluations and contributions related to the remote and local
   * evaluation times.  These are given by
   * \f{align}
   *   I^E_{q^Lq^R} &=
   *   \mspace{-10mu}
   *   \sum_{n=n_S}^{\min\left\{n_E, n^L+k\right\}-1}
   *   \mspace{-10mu}
   *   \tilde{\alpha}_{n,n-n^L} \Delta \tU_n
   *   &&\text{if $\tL_{q^L} = \tR_{q^R}$, otherwise 0}
   *   \\
   *   I^R_{q^Lq^R} &=
   *   \ell_{q^L - m_S + k}\!\left(
   *     \tU_{n^R}; \tL_{m_S - (k-1)}, \ldots, \tL_{m_S}\right)
   *   \mspace{-10mu}
   *   \sum_{n=\max\left\{n_S, n^R\right\}}
   *       ^{\min\left\{n_E, n^R+k\right\}-1}
   *   \mspace{-10mu}
   *   \tilde{\alpha}_{n,n-n^R} \Delta \tU_n
   *   &&\text{if $\tR_{q^R}$ is not in $\{\tL_{\vphantom{|}\cdots}\}$,
   *     otherwise 0}
   *   \\
   *   I^L_{q^Lq^R} &=
   *   \mspace{-10mu}
   *   \sum_{n=\max\left\{n_S, n^R\right\}}
   *       ^{\min\left\{n_E, n^L+k, n^R_{q^R+k}\right\}-1}
   *   \mspace{-10mu}
   *   \ell_{q^R - m^R_n + k}\!\left(\tU_{n^L};
   *     \tR_{m^R_n - (k-1)}, \ldots, \tR_{m^R_n}\right)
   *   \tilde{\alpha}_{n,n-n^L} \Delta \tU_n
   *   &&\text{if $\tL_{q^L}$ is not in $\{\tR_{\vphantom{|}\cdots}\}$,
   *     otherwise 0,}
   * \f}
   * where for brevity we write \f$n^L = n^L_{q^L}\f$ and \f$n^R =
   * n^R_{q^R}\f$, and where \f$\ell_a(t; x_1, \ldots, x_k)\f$ a
   * Lagrange interpolating polynomial and \f$\tilde{\alpha}_{nj}\f$
   * is the \f$j\f$th coefficient for an Adams-Bashforth step over the
   * union times from step \f$n\f$ to step \f$n+1\f$.
   */
  template <typename LocalVars, typename RemoteVars, typename Coupling>
  std::result_of_t<const Coupling&(LocalVars, RemoteVars)>
  compute_boundary_delta(
      const Coupling& coupling,
      gsl::not_null<BoundaryHistoryType<LocalVars, RemoteVars, Coupling>*>
          history,
      const TimeDelta& time_step) const noexcept;

  template <typename LocalVars, typename RemoteVars, typename Coupling>
  std::result_of_t<const Coupling&(LocalVars, RemoteVars)>
  boundary_dense_output(
      const Coupling& coupling,
      const BoundaryHistoryType<LocalVars, RemoteVars, Coupling>& history,
      double time) const noexcept;

  size_t order() const noexcept override;

  size_t error_estimate_order() const noexcept override;

  size_t number_of_past_steps() const noexcept override;

  double stable_step() const noexcept override;

  TimeStepId next_time_id(const TimeStepId& current_id,
                          const TimeDelta& time_step) const noexcept override;

  template <typename Vars, typename DerivVars>
  bool can_change_step_size(
      const TimeStepId& time_id,
      const TimeSteppers::History<Vars, DerivVars>& history) const noexcept;

  WRAPPED_PUPable_decl_template(AdamsBashforthN);  // NOLINT

  explicit AdamsBashforthN(CkMigrateMessage* /*unused*/) noexcept {}

  // clang-tidy: do not pass by non-const reference
  void pup(PUP::er& p) noexcept override;  // NOLINT

 private:
  friend bool operator==(const AdamsBashforthN& lhs,
                         const AdamsBashforthN& rhs) noexcept;

  // Some of the private methods take a parameter of type "Delta" or
  // "TimeType".  Delta is expected to be a TimeDelta or an
  // ApproximateTimeDelta, and TimeType is expected to be a Time or an
  // ApproximateTime.  The former cases will detect and optimize the
  // constant-time-step case, while the latter are necessary for dense
  // output.

  template <typename UpdateVars, typename Vars, typename DerivVars,
            typename Delta>
  void update_u_impl(gsl::not_null<UpdateVars*> u,
                     const History<Vars, DerivVars>& history,
                     const Delta& time_step, size_t order) const noexcept;

  template <typename LocalVars, typename RemoteVars, typename Coupling,
            typename TimeType>
  std::result_of_t<const Coupling&(LocalVars, RemoteVars)> boundary_impl(
      const Coupling& coupling,
      const BoundaryHistoryType<LocalVars, RemoteVars, Coupling>& history,
      const TimeType& end_time) const noexcept;

  /// Get coefficients for a time step.  Arguments are an iterator
  /// pair to past times, oldest to newest, and the time step to take.
  template <typename Iterator, typename Delta>
  static std::vector<double> get_coefficients(const Iterator& times_begin,
                                              const Iterator& times_end,
                                              const Delta& step) noexcept;

  static std::vector<double> get_coefficients_impl(
      const std::vector<double>& steps) noexcept;

  static std::vector<double> variable_coefficients(
      const std::vector<double>& steps) noexcept;

  static std::vector<double> constant_coefficients(size_t order) noexcept;

  struct ApproximateTimeDelta;

  // Time-like interface to a double used for dense output
  struct ApproximateTime {
    double time = std::numeric_limits<double>::signaling_NaN();
    double value() const noexcept { return time; }

    // Only the operators that are actually used are defined.
    friend ApproximateTimeDelta operator-(const ApproximateTime& a,
                                          const Time& b) noexcept {
      return {a.value() - b.value()};
    }

    friend bool operator<(const Time& a, const ApproximateTime& b) noexcept {
      return a.value() < b.value();
    }

    friend bool operator<(const ApproximateTime& a, const Time& b) noexcept {
      return a.value() < b.value();
    }

    friend std::ostream& operator<<(std::ostream& s,
                                    const ApproximateTime& t) noexcept {
      return s << t.value();
    }
  };

  // TimeDelta-like interface to a double used for dense output
  struct ApproximateTimeDelta {
    double delta = std::numeric_limits<double>::signaling_NaN();
    double value() const noexcept { return delta; }
    bool is_positive() const noexcept { return delta > 0.; }

    // Only the operators that are actually used are defined.
    friend bool operator<(const ApproximateTimeDelta& a,
                          const ApproximateTimeDelta& b) noexcept {
      return a.value() < b.value();
    }
  };

  size_t order_ = 3;
};

bool operator!=(const AdamsBashforthN& lhs,
                const AdamsBashforthN& rhs) noexcept;

template <typename Vars, typename DerivVars>
void AdamsBashforthN::update_u(
    const gsl::not_null<Vars*> u,
    const gsl::not_null<History<Vars, DerivVars>*> history,
    const TimeDelta& time_step) const noexcept {
  ASSERT(history->size() >= history->integration_order(),
         "Insufficient data to take an order-" << history->integration_order()
         << " step.  Have " << history->size() << " times, need "
         << history->integration_order());
  history->mark_unneeded(
      history->end() -
      static_cast<typename decltype(history->end())::difference_type>(
          history->integration_order()));
  update_u_impl(u, *history, time_step, history->integration_order());
}

template <typename Vars, typename ErrVars, typename DerivVars>
bool AdamsBashforthN::update_u(
    const gsl::not_null<Vars*> u, const gsl::not_null<ErrVars*> u_error,
    const gsl::not_null<History<Vars, DerivVars>*> history,
    const TimeDelta& time_step) const noexcept {
  ASSERT(history->size() >= history->integration_order(),
         "Insufficient data to take an order-" << history->integration_order()
         << " step.  Have " << history->size() << " times, need "
         << history->integration_order());
  history->mark_unneeded(
      history->end() -
      static_cast<typename decltype(history->end())::difference_type>(
          history->integration_order()));
  update_u_impl(u, *history, time_step, history->integration_order());
  // the error estimate is only useful once the history has enough elements to
  // do more than one order of step
  update_u_impl(u_error, *history, time_step, history->integration_order() - 1);
  *u_error = *u - *u_error;
  return true;
}

template <typename Vars, typename DerivVars>
bool AdamsBashforthN::dense_update_u(const gsl::not_null<Vars*> u,
                                     const History<Vars, DerivVars>& history,
                                     const double time) const noexcept {
  ASSERT(history.integration_order() == order_,
         "Dense output is only supported at full order");
  const ApproximateTimeDelta time_step{time - history.back().value()};
  update_u_impl(u, history, time_step, order_);
  return true;
}

template <typename UpdateVars, typename Vars, typename DerivVars,
          typename Delta>
void AdamsBashforthN::update_u_impl(const gsl::not_null<UpdateVars*> u,
                                    const History<Vars, DerivVars>& history,
                                    const Delta& time_step,
                                    const size_t order) const noexcept {
  ASSERT(
      history.size() > 0,
      "Cannot meaningfully update the evolved variables with an empty history");
  ASSERT(order <= order_,
         "Requested integration order higher than integrator order");

  const auto history_start =
      history.end() -
      static_cast<typename History<Vars, DerivVars>::difference_type>(order);
  const auto coefficients =
      get_coefficients(history_start, history.end(), time_step);

  *u = (history.end() - 1).value();
  auto coefficient = coefficients.rbegin();
  for (auto history_entry = history_start;
       history_entry != history.end();
       ++history_entry, ++coefficient) {
    *u += time_step.value() * *coefficient * history_entry.derivative();
  }
}

template <typename LocalVars, typename RemoteVars, typename Coupling>
std::result_of_t<const Coupling&(LocalVars, RemoteVars)>
AdamsBashforthN::compute_boundary_delta(
    const Coupling& coupling,
    const gsl::not_null<BoundaryHistoryType<LocalVars, RemoteVars, Coupling>*>
        history,
    const TimeDelta& time_step) const noexcept {
  const auto signed_order =
      static_cast<typename decltype(history->local_end())::difference_type>(
          history->integration_order());

  ASSERT(history->local_size() >= history->integration_order(),
         "Insufficient data to take an order-" << history->integration_order()
         << " step.  Have " << history->local_size() << " times, need "
         << history->integration_order());
  history->local_mark_unneeded(history->local_end() - signed_order);

  if (std::equal(history->local_begin(), history->local_end(),
                 history->remote_end() - signed_order)) {
    // GTS
    ASSERT(history->remote_size() >= history->integration_order(),
           "Insufficient data to take an order-" << history->integration_order()
           << " step.  Have " << history->remote_size() << " times, need "
           << history->integration_order());
    history->remote_mark_unneeded(history->remote_end() - signed_order);
  } else {
    const auto remote_step_for_step_start =
        std::upper_bound(history->remote_begin(), history->remote_end(),
                         *(history->local_end() - 1),
                         evolution_less<Time>{time_step.is_positive()});
    ASSERT(remote_step_for_step_start - history->remote_begin() >= signed_order,
           "Insufficient data to take an order-" << history->integration_order()
           << " step.  Have "
           << remote_step_for_step_start - history->remote_begin()
           << " times before the step, need " << history->integration_order());
    history->remote_mark_unneeded(remote_step_for_step_start - signed_order);
  }

  return boundary_impl(coupling, *history,
                       *(history->local_end() - 1) + time_step);
}

template <typename LocalVars, typename RemoteVars, typename Coupling>
std::result_of_t<const Coupling&(LocalVars, RemoteVars)>
AdamsBashforthN::boundary_dense_output(
    const Coupling& coupling,
    const BoundaryHistoryType<LocalVars, RemoteVars, Coupling>& history,
    const double time) const noexcept {
  return boundary_impl(coupling, history, ApproximateTime{time});
}

template <typename LocalVars, typename RemoteVars, typename Coupling,
          typename TimeType>
std::result_of_t<const Coupling&(LocalVars, RemoteVars)>
AdamsBashforthN::boundary_impl(
    const Coupling& coupling,
    const BoundaryHistoryType<LocalVars, RemoteVars, Coupling>& history,
    const TimeType& end_time) const noexcept {
  // Might be different from order_ during self-start.
  const auto current_order = history.integration_order();

  ASSERT(current_order <= order_,
         "Local history is too long for target order (" << current_order
         << " should not exceed " << order_ << ")");
  ASSERT(history.remote_size() >= current_order,
         "Remote history is too short (" << history.remote_size()
         << " should be at least " << current_order << ")");

  // Avoid billions of casts
  const auto order_s = static_cast<typename BoundaryHistoryType<
      LocalVars, RemoteVars, Coupling>::remote_iterator::difference_type>(
      current_order);

  // Start and end of the step we are trying to take
  const Time start_time = *(history.local_end() - 1);
  const auto time_step = end_time - start_time;

  // Result variable.  We evaluate the coupling only for the
  // structure.  This evaluation may be expensive, but by choosing the
  // most recent times on both sides we should guarantee that it is a
  // result we need later, so this will serve to get it into the
  // coupling cache so we don't have to compute it when we actually use it.
  auto accumulated_change =
      make_with_value<std::result_of_t<const Coupling&(LocalVars, RemoteVars)>>(
          history.coupling(coupling, history.local_end() - 1,
                           history.remote_end() - 1),
          0.);

  // We define the local_begin and remote_begin variables as the start
  // of the part of the history relevant to this calculation.
  // Boundary history cleanup happens immediately before the step, but
  // boundary dense output happens before that, so there may be data
  // left over that was needed for the previous step and has not been
  // cleaned out yet.
  const auto local_begin = history.local_end() - order_s;

  if (std::equal(local_begin, history.local_end(),
                 history.remote_end() - order_s)) {
    // No local time-stepping going on.
    const auto coefficients =
        get_coefficients(local_begin, history.local_end(), time_step);

    auto local_it = local_begin;
    auto remote_it = history.remote_end() - order_s;
    for (auto coefficients_it = coefficients.rbegin();
         coefficients_it != coefficients.rend();
         ++coefficients_it, ++local_it, ++remote_it) {
      accumulated_change +=
          *coefficients_it * history.coupling(coupling, local_it, remote_it);
    }
    accumulated_change *= time_step.value();

    return accumulated_change;
  }

  ASSERT(current_order == order_,
         "Cannot perform local time-stepping while self-starting.");

  const evolution_less<> less{time_step.is_positive()};
  const auto remote_begin =
      std::upper_bound(history.remote_begin(), history.remote_end(),
                       start_time, less) -
      order_s;

  ASSERT(std::is_sorted(local_begin, history.local_end(), less),
         "Local history not in order");
  ASSERT(std::is_sorted(remote_begin, history.remote_end(), less),
         "Remote history not in order");
  ASSERT(not less(start_time, *(remote_begin + (order_s - 1))),
         "Remote history does not extend far enough back");
  ASSERT(less(*(history.remote_end() - 1), end_time),
         "Please supply only older data: " << *(history.remote_end() - 1)
         << " is not before " << end_time);

  // Union of times of all step boundaries on any side.
  const auto union_times = [&history, &local_begin, &remote_begin,
                            &less]() noexcept {
    std::vector<Time> ret;
    ret.reserve(history.local_size() + history.remote_size());
    std::set_union(local_begin, history.local_end(), remote_begin,
                   history.remote_end(), std::back_inserter(ret), less);
    return ret;
  }();

  using UnionIter = typename decltype(union_times)::const_iterator;

  // Find the union times iterator for a given time.
  const auto union_step = [&union_times, &less](const Time& t) noexcept {
    return std::lower_bound(union_times.cbegin(), union_times.cend(), t, less);
  };

  // The union time index for the step start.
  const auto union_step_start = union_step(start_time);

  // min(union_times.end(), it + order_s) except being careful not
  // to create out-of-range iterators.
  const auto advance_within_step =
      [order_s, &union_times](const UnionIter& it) noexcept {
    return union_times.end() - it >
                   static_cast<typename decltype(union_times)::difference_type>(
                       order_s)
               ? it + static_cast<typename decltype(
                          union_times)::difference_type>(order_s)
               : union_times.end();
  };

  // Calculating the Adams-Bashforth coefficients is somewhat
  // expensive, so we cache them.  ab_coefs(it, step) returns the
  // coefficients used to step from *it to *it + step.
  auto ab_coefs = make_overloader(
      make_cached_function<std::tuple<UnionIter, TimeDelta>,
                           std::map>([order_s](
          const std::tuple<UnionIter, TimeDelta>& args) noexcept {
        return get_coefficients(
            std::get<0>(args) -
                static_cast<typename UnionIter::difference_type>(order_s - 1),
            std::get<0>(args) + 1, std::get<1>(args));
      }),
      make_cached_function<std::tuple<UnionIter, ApproximateTimeDelta>,
                           std::map>([order_s](
          const std::tuple<UnionIter, ApproximateTimeDelta>& args) noexcept {
        return get_coefficients(
            std::get<0>(args) -
                static_cast<typename UnionIter::difference_type>(order_s - 1),
            std::get<0>(args) + 1, std::get<1>(args));
      }));

  // The value of the coefficient of `evaluation_step` when doing
  // a standard Adams-Bashforth integration over the union times
  // from `step` to `step + 1`.
  const auto base_summand = [&ab_coefs, &end_time, &union_times](
      const UnionIter& step, const UnionIter& evaluation_step) noexcept {
    if (step + 1 != union_times.end()) {
      const TimeDelta step_size = *(step + 1) - *step;
      return step_size.value() *
             ab_coefs(std::make_tuple(
                 step, step_size))[static_cast<size_t>(step - evaluation_step)];
    } else {
      const auto step_size = end_time - *step;
      return step_size.value() *
             ab_coefs(std::make_tuple(
                 step, step_size))[static_cast<size_t>(step - evaluation_step)];
    }
  };

  for (auto local_evaluation_step = local_begin;
       local_evaluation_step != history.local_end();
       ++local_evaluation_step) {
    const auto union_local_evaluation_step = union_step(*local_evaluation_step);
    for (auto remote_evaluation_step = remote_begin;
         remote_evaluation_step != history.remote_end();
         ++remote_evaluation_step) {
      double deriv_coef = 0.;

      if (*local_evaluation_step == *remote_evaluation_step) {
        // The two elements stepped at the same time.  This gives a
        // standard Adams-Bashforth contribution to each segment
        // making up the current step.
        const auto union_step_upper_bound =
            advance_within_step(union_local_evaluation_step);
        for (auto step = union_step_start;
             step < union_step_upper_bound;
             ++step) {
          deriv_coef += base_summand(step, union_local_evaluation_step);
        }
      } else {
        // In this block we consider a coupling evaluation that is not
        // performed at equal times on the two sides of the mortar.

        // Makes an iterator with a map to give time as a double.
        const auto make_lagrange_iterator = [](const auto& it) noexcept {
          return boost::make_transform_iterator(
              it, [](const Time& t) noexcept { return t.value(); });
        };

        const auto union_remote_evaluation_step =
            union_step(*remote_evaluation_step);
        const auto union_step_lower_bound =
            std::max(union_step_start, union_remote_evaluation_step);

        // Compute the contribution to an interpolation over the local
        // times to `remote_evaluation_step->value()`, which we will
        // use as the coupling value for that time.  If there is an
        // actual evaluation at that time then skip this because the
        // Lagrange polynomial will be zero.
        if (not std::binary_search(local_begin, history.local_end(),
                                   *remote_evaluation_step, less)) {
          const auto union_step_upper_bound =
              advance_within_step(union_remote_evaluation_step);
          for (auto step = union_step_lower_bound;
               step < union_step_upper_bound;
               ++step) {
            deriv_coef += base_summand(step, union_remote_evaluation_step);
          }
          deriv_coef *=
              lagrange_polynomial(make_lagrange_iterator(local_evaluation_step),
                                  remote_evaluation_step->value(),
                                  make_lagrange_iterator(local_begin),
                                  make_lagrange_iterator(history.local_end()));
        }

        // Same qualitative calculation as the previous block, but
        // interpolating over the remote times.  This case is somewhat
        // more complicated because the latest remote time that can be
        // used varies for the different segments making up the step.
        if (not std::binary_search(remote_begin, history.remote_end(),
                                   *local_evaluation_step, less)) {
          auto union_step_upper_bound =
              advance_within_step(union_local_evaluation_step);
          if (history.remote_end() - remote_evaluation_step > order_s) {
            union_step_upper_bound = std::min(
                union_step_upper_bound,
                union_step(*(remote_evaluation_step + order_s)));
          }

          auto control_points = make_lagrange_iterator(
              remote_evaluation_step - remote_begin >= order_s
                  ? remote_evaluation_step - (order_s - 1)
                  : remote_begin);
          for (auto step = union_step_lower_bound;
               step < union_step_upper_bound;
               ++step, ++control_points) {
            deriv_coef +=
                base_summand(step, union_local_evaluation_step) *
                lagrange_polynomial(
                    make_lagrange_iterator(remote_evaluation_step),
                    local_evaluation_step->value(), control_points,
                    control_points +
                        static_cast<typename decltype(
                            control_points)::difference_type>(order_s));
          }
        }
      }

      if (deriv_coef != 0.) {
        // Skip the (potentially expensive) coupling calculation if
        // the coefficient is zero.
        accumulated_change +=
            deriv_coef * history.coupling(coupling, local_evaluation_step,
                                          remote_evaluation_step);
      }
    }  // for remote_evaluation_step
  }  // for local_evaluation_step

  return accumulated_change;
}

template <typename Vars, typename DerivVars>
bool AdamsBashforthN::can_change_step_size(
    const TimeStepId& time_id,
    const TimeSteppers::History<Vars, DerivVars>& history) const noexcept {
  // We need to forbid local time-stepping before initialization is
  // complete.  The self-start procedure itself should never consider
  // changing the step size, but we need to wait during the main
  // evolution until the self-start history has been replaced with
  // "real" values.
  const evolution_less<Time> less{time_id.time_runs_forward()};
  return history.size() == 0 or
         (less(history.back(), time_id.step_time()) and
          std::is_sorted(history.begin(), history.end(), less));
}

template <typename Iterator, typename Delta>
std::vector<double> AdamsBashforthN::get_coefficients(
    const Iterator& times_begin, const Iterator& times_end,
    const Delta& step) noexcept {
  if (times_begin == times_end) {
    return {};
  }
  std::vector<double> steps;
  // This may be slightly more space than we need, but we can't get
  // the exact amount without iterating through the iterators, which
  // is not necessarily cheap depending on the iterator type.
  steps.reserve(maximum_order);
  for (auto t = times_begin; std::next(t) != times_end; ++t) {
    steps.push_back((*std::next(t) - *t).value());
  }
  steps.push_back(step.value());
  return get_coefficients_impl(steps);
}
}  // namespace TimeSteppers


/* Copyright 2020 Oinam Romesh Meitei

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
 */

#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <pybind11/eigen.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <cmath>
#include <vector>
#include "getham.h"

namespace py = pybind11;

Eigen::MatrixXcd solve_func(double &t, std::vector<std::complex<double> > &y,
			    pulsec &pobj,
			    std::vector< std::vector< Eigen::SparseMatrix<double,0,ptrdiff_t> > > &hdrive,
			    std::vector< std::complex<double> > &dsham){

  int dsham_len = dsham.size();
  Eigen::SparseMatrix<std::complex<double> >
    matexp_(dsham_len, dsham_len);

  Eigen::SparseMatrix<std::complex<double> > H =
    getham(t, pobj, hdrive, dsham, dsham_len, matexp_);

  Eigen::Map<Eigen::VectorXcd> y_(y.data(), y.size());
  auto H_ = std::complex<double>(0.0,-1.0) * H * y_;
  return H_;
}

PYBIND11_MODULE(solve,m){
  m.def("solve_func", &solve_func, "solve_func ");
  py::class_<pulsec>(m, "pulsec")
    .def(py::init<
         std::vector< std::vector< double > > &,
         std::vector< std::vector< double > > &,
         std::vector< double > &, double &, int &,
         int & > ())
    .def_readonly("amp", &pulsec::amp)
    .def_readonly("tseq", &pulsec::tseq)
    .def_readonly("freq", &pulsec::freq)
    .def_readonly("duration", &pulsec::duration)
    .def_readonly("nqubit", &pulsec::nqubit)
    .def_readonly("nwindow", &pulsec::nwindow);
}