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


/*
 * kinovalib.cpp
 *
 *  Created on: Mar 10, 2013
 *      Author: mdedonato
 */

#include <kinova_driver/kinova_arm_kinematics.h>
#include <kinova_driver/kinova_ros_types.h>
#include <boost/lexical_cast.hpp>

namespace kinova
{

std::string concatTfName(const std::string& prefix, const std::string name)
{
    std::stringstream ss;
    ss << prefix << name;
    return ss.str();
}
std::string concatTfName(const std::string& prefix, const std::string name, const int index)
{
    std::stringstream ss;
    ss << prefix << name << index;
    return ss.str();
}
std::vector<double> array2vector(double* array, int length)
{
    std::vector<double> result;
    for (int i=0; i<length; i++)
    {
        result.push_back(array[i]);
    }
    return result;
}

KinovaKinematics::KinovaKinematics(const ros::NodeHandle &node_handle, std::string& kinova_robotType)
    : kinova_robotType_(kinova_robotType)
{
    if (valid_kinovaRobotType(kinova_robotType_) == false)
    {
        ROS_WARN("Invalid kinova_robotType error! Obtained: %s.", kinova_robotType_.c_str());
        return;
    }

//    tf_prefix_ = kinova_robotType_ + "_" + boost::lexical_cast<string>(same_type_index); // in case of multiple same_type robots
    tf_prefix_ = kinova_robotType_ + "_";

    // Maximum number of joints on Kinova-like robots:
    robot_category_ = kinova_robotType_[0];
    robot_category_version_ = kinova_robotType_[1]-'0';
    wrist_type_ = kinova_robotType_[2];
    arm_joint_number_ = kinova_robotType_[3]-'0';
    robot_mode_ = kinova_robotType_[4];
    finger_number_ = kinova_robotType_[5]-'0';
    int joint_total_number_ = arm_joint_number_ + finger_number_;

    if (kinova_robotType_.substr(0,4) == "j2n4" || kinova_robotType_.substr(0,4) == "m1n4")
        {
            // parameters stored in DSP chip
            node_handle.param<double>("D1", D1_, 0.2755);
            if(robot_category_ == 'j') {
                node_handle.param<double>("D2", D2_, 0.41);
                node_handle.param<double>("D3", D3_, 0.2073);
                node_handle.param<double>("e2", e2_, 0.0098);
            }
            else
            {
                node_handle.param<double>("D2", D2_, 0.29);
                node_handle.param<double>("D3", D3_, 0.1233);
                node_handle.param<double>("e2", e2_, 0.0070);
            }
            node_handle.param<double>("D4", D4_, 0.160);
            node_handle.param<double>("wrist_deg", wrist_deg_, 60.0);

            double aa = wrist_deg_/2 * M_PI/180.0, sa = sin(aa), s2a = sin(2*aa); // temp parameters

            // avoid use dynamic array DH_a[arm_joint_number_], but vector for DH parameters.
            double DH_a[4] = {0,  D2_, 0, 0};
            double DH_d[4] = {D1_, 0, -e2_, -(D3_ + D4_)};
            double DH_alpha[4] = {M_PI/2, M_PI, M_PI/2, M_PI};
            // DH_theta = DH_theta_sign*Q + DH_theta_offset
            double DH_theta_sign[4] = {-1, 1, 1, 1};
            double DH_theta_offset[4] = {0, -M_PI/2, M_PI/2, 3/2*M_PI};

            // copy local array values to class-scope vector.
            DH_a_ = array2vector(DH_a, arm_joint_number_);
            DH_d_ = array2vector(DH_d, arm_joint_number_);
            DH_alpha_ = array2vector(DH_alpha, arm_joint_number_);
            DH_theta_sign_ = array2vector(DH_theta_sign, arm_joint_number_);
            DH_theta_offset_ = array2vector(DH_theta_offset, arm_joint_number_);
        }
    else if (kinova_robotType_.substr(0,4) == "j2n6" || kinova_robotType_.substr(0,4) == "m1n6" )
    {
        // parameters stored in DSP chip
        node_handle.param<double>("D1", D1_, 0.2755);
        if(robot_category_ == 'j') {
            node_handle.param<double>("D2", D2_, 0.41);
            node_handle.param<double>("D3", D3_, 0.2073);
            node_handle.param<double>("e2", e2_, 0.0098);
        }
        else
        {
            node_handle.param<double>("D2", D2_, 0.29);
            node_handle.param<double>("D3", D3_, 0.1233);
            node_handle.param<double>("e2", e2_, 0.0070);
        }
        node_handle.param<double>("D4", D4_, 0.0741);
        node_handle.param<double>("D5", D5_, 0.0741);
        node_handle.param<double>("D6", D6_, 0.160);
        node_handle.param<double>("wrist_deg", wrist_deg_, 60.0);

        double aa = wrist_deg_/2 * M_PI/180.0, sa = sin(aa), s2a = sin(2*aa); // temp parameters

        // avoid use dynamic array DH_a[arm_joint_number_], but vector for DH parameters.
        double DH_a[6] = {0,  D2_, 0, 0, 0, 0};
        double DH_d[6] = {D1_, 0, -e2_, -(D3_ + sa/s2a * D4_), -(sa/s2a * D4_ + sa/s2a * D5_), -(sa/s2a * D5_ + D6_)};
        double DH_alpha[6] = {M_PI/2, M_PI, M_PI/2, 2*aa, 2*aa, M_PI};
        // DH_theta = DH_theta_sign*Q + DH_theta_offset
        double DH_theta_sign[6] = {-1, 1, 1, 1, 1, 1};
        double DH_theta_offset[6] = {0, -M_PI/2, +M_PI/2, 0, -M_PI, +M_PI/2};

        // copy local array values to class-scope vector.
        DH_a_ = array2vector(DH_a, arm_joint_number_);
        DH_d_ = array2vector(DH_d, arm_joint_number_);
        DH_alpha_ = array2vector(DH_alpha, arm_joint_number_);
        DH_theta_sign_ = array2vector(DH_theta_sign, arm_joint_number_);
        DH_theta_offset_ = array2vector(DH_theta_offset, arm_joint_number_);
    }
    else if (kinova_robotType_.substr(0,4) == "j2s6")
    {
        // parameters stored in DSP chip
        node_handle.param<double>("D1", D1_, 0.2755);
        node_handle.param<double>("D2", D2_, 0.41);
        node_handle.param<double>("D3", D3_, 0.2073);
        node_handle.param<double>("D4", D4_, 0.1038);
        node_handle.param<double>("D5", D5_, 0.1038);
        node_handle.param<double>("D6", D6_, 0.160);
        node_handle.param<double>("e2", e2_, 0.0098);

        // avoid use dynamic array DH_a[arm_joint_number_], but vector for DH parameters.
        double DH_a[6] = {0,  D2_, 0, 0, 0, 0};
        double DH_d[6] = {-D1_, 0, -e2_, -(D3_ + D4_), 0, -(D5_ + D6_)};
        double DH_alpha[6] = {M_PI/2, M_PI, M_PI/2, M_PI/2, M_PI/2, M_PI};
        // DH_theta = DH_theta_sign*Q + DH_theta_offset
        double DH_theta_sign[6] = {1, 1, 1, 1, 1, 1};
        double DH_theta_offset[6] = {-M_PI, M_PI/2, M_PI/2, 0, 0, -M_PI/2};

        // copy local array values to class-scope vector.
        DH_a_ = array2vector(DH_a, arm_joint_number_);
        DH_d_ = array2vector(DH_d, arm_joint_number_);
        DH_alpha_ = array2vector(DH_alpha, arm_joint_number_);
        DH_theta_sign_ = array2vector(DH_theta_sign, arm_joint_number_);
        DH_theta_offset_ = array2vector(DH_theta_offset, arm_joint_number_);
    }
    else if (kinova_robotType_.substr(0,4) == "j2s7")   //@wql 20211011 dh of j2s7s300
    {
        // parameters stored in DSP chip
        node_handle.param<double>("D1", D1_, 0.2755);
        node_handle.param<double>("D2", D2_, 0.205);
        node_handle.param<double>("D3", D3_, 0.205);
        node_handle.param<double>("D4", D4_, 0.2073);
        node_handle.param<double>("D5", D5_, 0.1038);
        node_handle.param<double>("D6", D6_, 0.1038);
        node_handle.param<double>("D7", D7_, 0.160);
        node_handle.param<double>("e2", e2_, 0.0098);

        // avoid use dynamic array DH_a[arm_joint_number_], but vector for DH parameters.
        double DH_a[7] = {0,  0, 0, 0, 0, 0, 0};
        double DH_d[7] = {-D1_, 0, -(D2_+ D3_), -e2_, -(D4_ + D5_), 0, -(D6_ + D7_)};
        double DH_alpha[7] = {M_PI/2, M_PI/2, M_PI/2, M_PI/2, M_PI/2, M_PI/2, M_PI};
        // DH_theta = DH_theta_sign*Q + DH_theta_offset
        double DH_theta_sign[7] = {-1, 1, 1, 1, 1, 1, 1};
        double DH_theta_offset[7] = {0, 0, 0, 0, 0, 0, 0};

        // copy local array values to class-scope vector.
        DH_a_ = array2vector(DH_a, arm_joint_number_);
        DH_d_ = array2vector(DH_d, arm_joint_number_);
        DH_alpha_ = array2vector(DH_alpha, arm_joint_number_);
        DH_theta_sign_ = array2vector(DH_theta_sign, arm_joint_number_);
        DH_theta_offset_ = array2vector(DH_theta_offset, arm_joint_number_);
    }
    else
    {
        // special parameters for custom robot or other cases
        printf("Please specify the kinematic of robots other than jaco and mico!\n");
    }

    node_handle.param<std::string>("base_frame", baseFrame, "root");
}

tf::Transform KinovaKinematics::DHParam2Transform(float d, float theta, float a, float alpha)
{
    tf::Transform transform;
    tf::Quaternion rotation_q(0, 0, 0, 1);
    tf::Matrix3x3 rot_matrix(1, 0, 0, 0, 1, 0, 0, 0, 1);
    tf::Vector3 translation_v(0, 0, 0);

    rot_matrix.setValue(cos(theta), -sin(theta)*cos(alpha),  sin(theta)*sin(alpha),
                        sin(theta),  cos(theta)*cos(alpha), -cos(theta)*sin(alpha),
                        0,           sin(alpha),             cos(alpha));
    rot_matrix.getRotation(rotation_q);
    transform.setRotation(rotation_q.normalize());

    translation_v.setValue(a*cos(theta), a*sin(theta), d);
    transform.setOrigin(translation_v);
    return transform;
}

/**************************************/
/*                                    */
/*  Computer and publish transform    */
/*                                    */
/**************************************/
// IMPORTANT!!! In the robot DSP chip, the classical D-H parameters are used to define the robot. Therefore, the frame definition is different comparing with the frames in URDF model.
void KinovaKinematics::updateForward(float* Q)
{
    tf::Transform transform;
    // the orientation of frame0 is differently defined starting from Jaco 6 spherical robot.
    if(kinova_robotType_.substr(0,4) == "j2s6" || kinova_robotType_.substr(0,4) == "j2s7")
    {
        transform = DHParam2Transform(0, 0, 0, M_PI);
    }
    else
    {
        transform = DHParam2Transform(0, 0, 0, 0);
    }
    broadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(),
                                                    baseFrame,
                                                    concatTfName(tf_prefix_, "link_base")));
    double DH_theta_i;
    for (int i = 0; i<arm_joint_number_; i++)
    {
        DH_theta_i = DH_theta_sign_[i]*Q[i] + DH_theta_offset_[i];
        transform = DHParam2Transform(DH_d_[i], DH_theta_i, DH_a_[i], DH_alpha_[i]);
        if(i==0)
        {
            broadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(), concatTfName(tf_prefix_, "link_base"), concatTfName(tf_prefix_, "link_1")));
        }
        else
        {
            broadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(),                                                            concatTfName(tf_prefix_, "link_", i), concatTfName(tf_prefix_, "link_", i+1)));
        }
    }

}

}  // namespace kinova


#include <boost/variant.hpp>
#include "geometry/Area.h"
#include "geometry/BoundingBox.h"
#include "geometry/Lanelet.h"
#include "geometry/LineString.h"
#include "geometry/RegulatoryElement.h"
#include "primitives/Traits.h"

namespace lanelet {
namespace geometry {
namespace {
using traits::to2D;
struct Bbox2dVisitor : public RuleParameterVisitor {  // NOLINT
  void operator()(const ConstPoint3d& p) override { bbox.extend(boundingBox2d(to2D(p))); }
  void operator()(const ConstLineString3d& l) override { bbox.extend(boundingBox2d(to2D(l))); }
  void operator()(const ConstPolygon3d& p) override { bbox.extend(boundingBox2d(to2D(p))); }
  void operator()(const ConstWeakLanelet& ll) override {
    if (ll.expired()) {
      return;
    }
    bbox.extend(boundingBox2d(ll.lock()));
  }
  void operator()(const ConstWeakArea& ar) override {
    if (ar.expired()) {
      return;
    }
    bbox.extend(boundingBox2d(ar.lock()));
  }
  BoundingBox2d bbox{};
};
struct Bbox3dVisitor : public RuleParameterVisitor {  // NOLINT
  void operator()(const ConstPoint3d& p) override { bbox.extend(boundingBox3d(p)); }
  void operator()(const ConstLineString3d& l) override { bbox.extend(boundingBox3d(l)); }
  void operator()(const ConstPolygon3d& p) override { bbox.extend(boundingBox3d(p)); }
  void operator()(const ConstWeakLanelet& ll) override {
    if (ll.expired()) {
      return;
    }
    bbox.extend(boundingBox3d(ll.lock()));
  }
  void operator()(const ConstWeakArea& ar) override {
    if (ar.expired()) {
      return;
    }
    bbox.extend(boundingBox3d(ar.lock()));
  }
  BoundingBox3d bbox;
};

struct DistanceVisitor : public RuleParameterVisitor {  // NOLINT
  explicit DistanceVisitor(BasicPoint2d p) : pdist{std::move(p)} {}
  void operator()(const ConstPoint3d& p) override { d = std::min(d, distance(traits::to2D(p), pdist)); }
  void operator()(const ConstLineString3d& l) override { d = std::min(d, distance2d(l, pdist)); }
  void operator()(const ConstPolygon3d& p) override { d = std::min(d, distance2d(p, pdist)); }
  void operator()(const ConstWeakLanelet& ll) override {
    if (ll.expired()) {
      return;
    }
    d = std::min(d, distance2d(ll.lock(), pdist));
  }
  void operator()(const ConstWeakArea& ar) override {
    if (ar.expired()) {
      return;
    }
    d = std::min(d, distance2d(ar.lock(), pdist));
  }
  BasicPoint2d pdist;
  double d{std::numeric_limits<double>::infinity()};
};
}  // namespace

BoundingBox2d boundingBox2d(const RegulatoryElement& regElem) {
  Bbox2dVisitor visitor;
  regElem.applyVisitor(visitor);
  return visitor.bbox;
}

BoundingBox3d boundingBox3d(const RegulatoryElement& regElem) {
  Bbox3dVisitor visitor;
  regElem.applyVisitor(visitor);
  return visitor.bbox;
}

double distance2d(const RegulatoryElement& regElem, const BasicPoint2d& p) {
  DistanceVisitor visitor(p);
  regElem.applyVisitor(visitor);
  return visitor.d;
}

}  // namespace geometry
}  // namespace lanelet


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_SSE2_FUNCTION_BROADCAST_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_SSE2_FUNCTION_BROADCAST_HPP_INCLUDED

#include <boost/simd/detail/brigand.hpp>
#include <boost/simd/detail/overload.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = ::boost::dispatch;
  namespace bs = ::boost::simd;

  BOOST_DISPATCH_OVERLOAD ( broadcast_
                          , (typename A0, typename A1)
                          , bs::sse2_
                          , bs::pack_<bd::double_<A0>,bs::sse_>
                          , bd::constant_<bd::integer_<A1>>
                          )
  {
    BOOST_FORCEINLINE A0 operator()(A0 const& a0, A1 const&) const BOOST_NOEXCEPT
    {
      return do_(a0,brigand::bool_<(A1::value >= 0 && A1::value < 2)>{});
    }

    static BOOST_FORCEINLINE A0 do_(A0 const& a0, brigand::bool_<true> const&) BOOST_NOEXCEPT
    {
      return _mm_shuffle_pd(a0, a0, _MM_SHUFFLE2(A1::value,A1::value));
    }

    static BOOST_FORCEINLINE A0 do_(A0 const&, brigand::bool_<false> const&) BOOST_NOEXCEPT
    {
      return Zero<A0>();
    }
  };

  BOOST_DISPATCH_OVERLOAD ( broadcast_
                          , (typename A0, typename A1)
                          , bs::sse2_
                          , bs::pack_<bd::ints32_<A0>,bs::sse_>
                          , bd::constant_<bd::integer_<A1>>
                          )
  {
    BOOST_FORCEINLINE A0 operator()(A0 const& a0, A1 const&) const BOOST_NOEXCEPT
    {
      return do_(a0,brigand::bool_<(A1::value >= 0 && A1::value < 4)>{});
    }

    static BOOST_FORCEINLINE A0 do_(A0 const& a0, brigand::bool_<true> const&) BOOST_NOEXCEPT
    {
      return _mm_shuffle_epi32(a0, _MM_SHUFFLE ( A1::value, A1::value, A1::value, A1::value));
    }

    static BOOST_FORCEINLINE A0 do_(A0 const&, brigand::bool_<false> const&) BOOST_NOEXCEPT
    {
      return Zero<A0>();
    }
  };

  BOOST_DISPATCH_OVERLOAD ( broadcast_
                          , (typename A0, typename A1)
                          , bs::sse2_
                          , bs::pack_<bd::ints16_<A0>,bs::sse_>
                          , bd::constant_<bd::integer_<A1>>
                          )
  {
    BOOST_FORCEINLINE A0 operator()(A0 const& a0, A1 const&) const BOOST_NOEXCEPT
    {
      return do_(a0,brigand::bool_<(A1::value >= 0 && A1::value < 8)>{});
    }

#ifdef __clang__
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wshadow"
#endif // __clang__

    static BOOST_FORCEINLINE A0 do_(A0 const& a0, brigand::bool_<true> const&) BOOST_NOEXCEPT
    {
      // This shuffle the 32 bits packet of the broadcast so
      // we end up with [ In In+1 In In+1 In In+1 In In+1 ]
      using partial_t = std::integral_constant< int
                                              ,_MM_SHUFFLE( A1::value / 2,A1::value / 2
                                                          , A1::value / 2,A1::value / 2
                                                          )
                                              >;

      // This select either In or In+1 as 16 bits value and give us
      // [In In In In In In In In] or [In+1 In+1 In+1 In+1 In+1 In+1 In+1 In+1]
      using fix_t = std::integral_constant< int
                                          , _MM_SHUFFLE ( A1::value % 2,A1::value % 2
                                                        , A1::value % 2,A1::value % 2
                                                        )
                                          >;

      return _mm_shufflehi_epi16( _mm_shufflelo_epi16 ( _mm_shuffle_epi32( a0, partial_t::value)
                                                      , fix_t::value
                                                      )
                                , fix_t::value
                                );
    }

#ifdef __clang__
#pragma clang diagnostic pop
#endif // __clang__

    static BOOST_FORCEINLINE A0 do_(A0 const&, brigand::bool_<false> const&) BOOST_NOEXCEPT
    {
      return Zero<A0>();
    }
  };
} } }

#endif


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

Copyright 2019 Paolo G. Crosetto <pacrosetto@gmail.com>
SPDX-License-Identifier: Apache-2.0

---------------------------------------------------------------------*/
#include "../../src/cupq.h"
#include <boost/program_options.hpp>
#include <iostream>
#include <mutex>
#include <thread>
#include <vector>

// compile standalone with
// nvcc dijkstra_simple.cu -I.. -g -O3 -Xptxas -dlcm=ca -lineinfo
// -gencode=arch=compute_50,code=sm_50   -gencode=arch=compute_70,code=sm_70
// --ptxas-options=-v options:
// -DVERBOSE
// -DTIMINGS

// usage:

std::vector<float> dijkstra_cpu(int argc, char **argv,
                                std::vector<int> &origins_,
                                std::vector<int> &destinations_) {

  // parameters hardcoded
  // number of stream (= number of threads) for the GPU computation
  int nstreams = 1;
  // number of input points to consume per thread at once
  unsigned chunk = 1024;
  std::vector<float> finalCosts(origins_.size() * destinations_.size(), 0.);

  boost::program_options::options_description desc("Allowed options");
  desc.add_options()("help,h", "print usage message")(
      "graph,g", boost::program_options::value<std::string>(),
      "graph filename without extensions");

  boost::program_options::variables_map vm;
  boost::program_options::store(
      boost::program_options::parse_command_line(argc, argv, desc), vm);

  std::string graphname_;

  if (vm.count("graph")) {
    graphname_ = vm["graph"].as<std::string>();
  } else {
    std::cout << "specify a graph file";
    return finalCosts;
  }

  std::vector<std::thread> t_;

  // read graph
  using value_t = float;
  using index_t = unsigned;
  using graph_t = cupq::Graph<value_t, index_t, cupq::Backend::Host>;

  graph_t graph_;
  // read graph in DIMACS format
  graph_.readDIMACS((graphname_ + ".gr").c_str());

  {

    std::vector<cupq::DijkstraSolver<graph_t>> shortestPathSolver(nstreams);

#ifdef TIMINGS
    using time_t = std::chrono::duration<double>;
    std::vector<time_t> compute_time(nstreams, time_t(0.));
#endif

    std::cout << "starting computation on CPU \n";

    for (unsigned kk(0); kk < (float)origins_.size() / chunk; ++kk) {

      auto work = [&shortestPathSolver, &graph_, nstreams, kk, chunk, &origins_,
                   &destinations_, &finalCosts
#ifdef TIMINGS
                   ,
                   &compute_time
#endif
      ]() {
        if (origins_.size() && destinations_.size()) {

          auto &solver_ = shortestPathSolver[kk % nstreams];
          // std::range<int> origins_chunk_(origins_.begin()+kk,
          // std::max(origins_.begin()+kk+32, origins_.size_()));
          std::vector<int> origins_chunk_(
              origins_.begin() + kk * chunk,
              origins_.begin() +
                  std::min((std::size_t)kk * chunk + chunk, origins_.size()));
          solver_.setSources(origins_chunk_);
          solver_.setGraph(graph_);
#ifdef TIMINGS
          auto dstart = std::chrono::system_clock::now();
#endif
          solver_.computeDijkstra((index_t)-1, (index_t)-1);
          solver_.finalize();
          // result available on host in solver.out()
          for (int row = 0; row < origins_chunk_.size(); ++row)
            for (int col = 0; col < destinations_.size(); ++col)
              finalCosts[(row + kk * chunk) * destinations_.size() + col] =
                  solver_.out().first(row, destinations_[col]);

              // auto itCost=finalCosts.begin();
              // for(auto it=solver_.out().first.begin();
              // it!=solver_.out().first.end(); ++it) 	*itCost=*it;

#ifdef TIMINGS
          auto dend = std::chrono::system_clock::now();
          compute_time[kk % nstreams] += dend - dstart;
#endif
        }
      };

      if (kk < nstreams)
        t_.emplace_back(work);
      else { // blocking on a thread and reusing the resources
        t_[kk % nstreams].join();
        t_[kk % nstreams] = std::thread(work);
      }
    }

    for (auto &&i : t_)
      i.join();

#ifdef TIMINGS
    std::cout << "computation time per tread (computing shortest path)\n";
    for (int i = 0; i < compute_time.size(); ++i)
      std::cout << "thread " << i << " computing for "
                << compute_time[i].count() << " s\n";
#endif
    // result available in solver[i].out()
  } // RAII

  graph_.free();

  return finalCosts;
}


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*

    This example program shows how to use dlib's implementation of the paper:
        One Millisecond Face Alignment with an Ensemble of Regression Trees by
        Vahid Kazemi and Josephine Sullivan, CVPR 2014

    In particular, we will train a face landmarking model based on a small dataset
    and then evaluate it.  If you want to visualize the output of the trained
    model on some images then you can run the face_landmark_detection_ex.cpp
    example program with sp.dat as the input model.

    It should also be noted that this kind of model, while often used for face
    landmarking, is quite general and can be used for a variety of shape
    prediction tasks.  But here we demonstrate it only on a simple face
    landmarking task.
*/

// program na vytvaranie modeluhladania bodov na tvari
// upraveny subor - povodny je mozne najst na https://github.com/davisking/dlib/blob/master/examples/train_shape_predictor_ex.cpp

#include <dlib/image_processing.h>
#include <dlib/data_io.h>
#include <iostream>

using namespace dlib;
using namespace std;

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

std::vector<std::vector<double> > get_interocular_distances(
        const std::vector<std::vector<full_object_detection> > &objects
);
/*!
    ensures
        - returns an object D such that:
            - D[i][j] == the distance, in pixels, between the eyes for the face represented
              by objects[i][j].
!*/

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

int main(int argc, char **argv) {
    try {

        dlib::array<array2d<unsigned char> > images_train, images_test;
        std::vector<std::vector<full_object_detection> > faces_train, faces_test;

        load_image_dataset(images_train, faces_train,
                           "../../../ibug_300W_large_face_landmark_dataset/trainUsta+7.xml");

//        pre otestovanie by bolo potrebne upravit testovaciu metodu viac info na https://github.com/davisking/dlib/blob/master/examples/train_shape_predictor_ex.cpp
//        load_image_dataset(images_test, faces_test, "../../ibug_300W_large_face_landmark_dataset/testUsta+7.xml");

        shape_predictor_trainer trainer;

        trainer.set_cascade_depth(12);
        trainer.set_num_trees_per_cascade_level(600);
        trainer.set_tree_depth(4);
        trainer.set_nu(0.1); //shrinkage faktor
        trainer.set_oversampling_amount(550);
        trainer.set_feature_pool_size(750);
        trainer.set_lambda(0.2);
        trainer.set_num_test_splits(300);

        trainer.set_num_threads(8);
        trainer.be_verbose();

        shape_predictor sp = trainer.train(images_train, faces_train);

        serialize("../data/models/27BodovyModel.dat") << sp;

    }
    catch (exception &e) {
        cout << "\nexception thrown!" << endl;
        cout << e.what() << endl;
    }
}

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

double interocular_distance(
        const full_object_detection &det
) {
    dlib::vector<double, 2> l, r;
    double cnt = 0;
    // Find the center of the left eye by averaging the points around
    // the eye.
    for (unsigned long i = 36; i <= 41; ++i) {
        l += det.part(i);
        ++cnt;
    }
    l /= cnt;

    // Find the center of the right eye by averaging the points around
    // the eye.
    cnt = 0;
    for (unsigned long i = 42; i <= 47; ++i) {
        r += det.part(i);
        ++cnt;
    }
    r /= cnt;

    // Now return the distance between the centers of the eyes
    return length(l - r);
}

std::vector<std::vector<double> > get_interocular_distances(
        const std::vector<std::vector<full_object_detection> > &objects
) {
    std::vector<std::vector<double> > temp(objects.size());
    for (unsigned long i = 0; i < objects.size(); ++i) {
        for (unsigned long j = 0; j < objects[i].size(); ++j) {
            temp[i].push_back(interocular_distance(objects[i][j]));
        }
    }
    return temp;
}

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


/*
Lightweight Automated Planning Toolkit
Copyright (C) 2012
Miquel Ramirez <miquel.ramirez@rmit.edu.au>
Nir Lipovetzky <nirlipo@gmail.com>

This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

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

You should have received a copy of the GNU General Public License
along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

// MRJ: In this example, we'll show how to create a search problem out of
// a planning problem we acquired from some external source
#include <iostream>
#include <fstream>

#include <ff_to_aptk.hxx>
#include <strips_prob.hxx>
#include <fluent.hxx>
#include <action.hxx>
#include <cond_eff.hxx>
#include <strips_state.hxx>
#include <fwd_search_prob.hxx>

#include <landmark_graph.hxx>
#include <landmark_graph_generator.hxx>
#include <landmark_graph_manager.hxx>
#include <landmark_count.hxx>
#include <h_2.hxx>
#include <h_1.hxx>
#include <layered_h_max.hxx>

#include <rp_heuristic.hxx>
#include <ff_rp_heuristic.hxx>

#include <aptk/open_list.hxx>
#include <aptk/string_conversions.hxx>

#include <boost/program_options.hpp>

#include "new_node_comparer.hxx"
#include "dynamic_novelty_partition.hxx"
#include "at_gbfs_2h.hxx"

#include "at_gbfs_3h.hxx"
#include "at_gbfs_2h_exp.hxx"
#include "2h_exp_node.hxx"

//Local Search
#include <novelty.hxx>
#include "iw.hxx"

namespace po = boost::program_options;

using	aptk::STRIPS_Problem;
using	aptk::agnostic::Fwd_Search_Problem;
using	aptk::Action;

using 	aptk::agnostic::Landmarks_Graph;
using 	aptk::agnostic::Landmarks_Graph_Generator;
using   aptk::agnostic::Landmarks_Graph_Manager;
using 	aptk::agnostic::Landmarks_Count_Heuristic;
using 	aptk::agnostic::H2_Heuristic;
using 	aptk::agnostic::H1_Heuristic;
using   aptk::agnostic::Layered_H_Max;

using	aptk::agnostic::H_Add_Evaluation_Function;
using	aptk::agnostic::H_Max_Evaluation_Function;
using	aptk::agnostic::Relaxed_Plan_Heuristic;
using   aptk::agnostic::FF_Relaxed_Plan_Heuristic;
using 	aptk::agnostic::Dynamic_Novelty_Partition;

using 	aptk::search::Open_List;
using	aptk::search::Node_Comparer_2H;

using	aptk::search::Node_Comparer_3H_FN;
using	aptk::search::gbfs_2h::AT_GBFS_2H;
using	aptk::search::gbfs_3h::AT_GBFS_3H;
using	aptk::search::gbfs_2h_exp::AT_GBFS_2H_EXP;

//using	aptk::search::gbfs_mh::Node;

using 	aptk::agnostic::Novelty;
using	aptk::search::brfs::IW;

typedef         H2_Heuristic<Fwd_Search_Problem>                  H2_Fwd;
typedef         Landmarks_Graph_Generator<Fwd_Search_Problem>     Gen_Lms_Fwd;
typedef         Landmarks_Count_Heuristic<Fwd_Search_Problem>     H_Lmcount_Fwd;
typedef         Landmarks_Graph_Manager<Fwd_Search_Problem>       Land_Graph_Man;

// NIR: We start defining the type of nodes for our planner
typedef		aptk::search::gbfs_2h_exp::Node< Fwd_Search_Problem, aptk::State >	Search_Node;
typedef		aptk::search::gbfs_3h::Node< Fwd_Search_Problem, aptk::State >	        Search_Node_3h;

typedef         Dynamic_Novelty_Partition<Fwd_Search_Problem, Search_Node>             H_Dyn_Novel_Fwd;
typedef         Dynamic_Novelty_Partition<Fwd_Search_Problem, Search_Node_3h>             H_Dyn_Novel_Fwd_3h;
typedef         Novelty<Fwd_Search_Problem, Search_Node>                               H_Novel_Fwd;

// NIR: Then we define the type of the tie-breaking algorithm
// for the open list we are going to use
typedef		Node_Comparer_2H< Search_Node >					Tie_Breaking_Algorithm;
typedef		Node_Comparer_3H_FN< Search_Node_3h >				Tie_Breaking_Algorithm_3h;

// NIR: Now we define the Open List type by combining the types we have defined before
typedef		Open_List< Tie_Breaking_Algorithm, Search_Node >		BFS_Open_List;
typedef		Open_List< Tie_Breaking_Algorithm_3h, Search_Node_3h >		BFS_Open_List_3h;

// NIR: Now we define the heuristics
typedef		H1_Heuristic<Fwd_Search_Problem, H_Add_Evaluation_Function>	H_Add_Fwd;
typedef		Relaxed_Plan_Heuristic< Fwd_Search_Problem, H_Add_Fwd >		H_Add_Rp_Fwd;

typedef         Layered_H_Max< Fwd_Search_Problem >				      Alt_H_Max;
typedef         FF_Relaxed_Plan_Heuristic< Fwd_Search_Problem, Alt_H_Max, unsigned >     Classic_FF_H_Max;

// MRJ: Now we're ready to define the BFS algorithm we're going to use
typedef		AT_GBFS_2H< Fwd_Search_Problem, H_Dyn_Novel_Fwd, H_Add_Rp_Fwd, BFS_Open_List >    GBFS_H_Add_Fwd;
typedef		AT_GBFS_2H< Fwd_Search_Problem, H_Dyn_Novel_Fwd, Classic_FF_H_Max, BFS_Open_List >    GBFS_FF_Rp_Fwd;
typedef		AT_GBFS_3H< Fwd_Search_Problem, H_Dyn_Novel_Fwd_3h, H_Lmcount_Fwd, H_Add_Rp_Fwd, BFS_Open_List_3h >    GBFS_H_Add_Rp_LM_Fwd;

//NIR: GBF with Local Search
typedef		IW< Fwd_Search_Problem, H_Novel_Fwd, H_Add_Rp_Fwd, BFS_Open_List >	          IW_Fwd;
typedef		AT_GBFS_2H_EXP< Fwd_Search_Problem, H_Dyn_Novel_Fwd, H_Add_Rp_Fwd, IW_Fwd, BFS_Open_List >    GBFS_IW_H_Add_Fwd;
typedef		AT_GBFS_2H_EXP< Fwd_Search_Problem, H_Dyn_Novel_Fwd,  Classic_FF_H_Max, IW_Fwd, BFS_Open_List >    GBFS_IW_FF_Rp_Fwd;

template <typename Search_Engine>
float do_search( Search_Engine& engine, STRIPS_Problem& plan_prob, std::ofstream& plan_stream ) {

	std::ofstream	details( "execution.details" );
	engine.start();

	std::vector< aptk::Action_Idx > plan;
	float				cost;

	float ref = aptk::time_used();
	float t0 = aptk::time_used();

	unsigned expanded_0 = engine.expanded();
	unsigned generated_0 = engine.generated();

	if ( engine.find_solution( cost, plan ) ) {
		details << "Plan found with cost: " << cost << std::endl;
		for ( unsigned k = 0; k < plan.size(); k++ ) {
			details << k+1 << ". ";
			const aptk::Action& a = *(plan_prob.actions()[ plan[k] ]);
			details << a.signature();
			details << std::endl;
			plan_stream << a.signature() << std::endl;
		}
		float tf = aptk::time_used();
		unsigned expanded_f = engine.expanded();
		unsigned generated_f = engine.generated();
		details << "Time: " << tf - t0 << std::endl;
		details << "Generated: " << generated_f - generated_0 << std::endl;
		details << "Expanded: " << expanded_f - expanded_0 << std::endl;
		t0 = tf;
		expanded_0 = expanded_f;
		generated_0 = generated_f;
		plan.clear();
	}
 	float total_time = aptk::time_used() - ref;
	std::cout << "Total time: " << total_time << std::endl;
	std::cout << "Nodes generated during search: " << engine.generated() << std::endl;
	std::cout << "Nodes expanded during search: " << engine.expanded() << std::endl;
	std::cout << "Plan found with cost: " << cost << std::endl;
	details.close();

	return total_time;
}

void process_command_line_options( int ac, char** av, po::variables_map& vars ) {
	po::options_description desc( "Options:" );

	desc.add_options()
		( "help", "Show help message" )
		( "domain", po::value<std::string>(), "Input PDDL domain description" )
		( "problem", po::value<std::string>(), "Input PDDL problem description" )
		( "output", po::value<std::string>(), "Output file for plan" )
		( "ignore_novelty", po::value<bool>()->default_value(false),
			"ignore novelty in gbfs" )
		( "ignore_local_novelty", po::value<bool>()->default_value(false),
			"ignore novelty within the local search" )
	        ( "heuristic", po::value<std::string>()->default_value("hadd"),
			"hadd/hff" )
		( "max_novel", po::value<unsigned>()->default_value(1),
			"1,2,3" )
	;

	try {
		po::store( po::parse_command_line( ac, av, desc ), vars );
		po::notify( vars );
	}
	catch ( std::exception& e ) {
		std::cerr << "Error: " << e.what() << std::endl;
		std::exit(1);
	}
	catch ( ... ) {
		std::cerr << "Exception of unknown type!" << std::endl;
		std::exit(1);
	}

	if ( vars.count("help") ) {
		std::cout << desc << std::endl;
		std::exit(0);
	}

}

void report_no_solution( std::string reason, std::ofstream& plan_stream ) {
	plan_stream << ";; No solution found" << std::endl;
	plan_stream << ";; " << reason << std::endl;
	plan_stream.close();
}

int main( int argc, char** argv ) {

	po::variables_map vm;

	process_command_line_options( argc, argv, vm );

	if ( !vm.count( "domain" ) ) {
		std::cerr << "No PDDL domain was specified!" << std::endl;
		std::exit(1);
	}

	if ( !vm.count( "problem" ) ) {
		std::cerr << "No PDDL problem was specified!" << std::endl;
		std::exit(1);
	}

	std::ofstream	plan_stream;

	if ( !vm.count( "output" ) ) {
		std::cerr << "No output plan file specified, defaulting to 'plan.ipc'" << std::endl;
		plan_stream.open( "plan.ipc" );
	}
	else
		plan_stream.open( vm["output"].as<std::string>() );

	STRIPS_Problem	prob;
	bool ignore_costs = true;

	aptk::FF_Parser::get_problem_description( vm["domain"].as<std::string>(), vm["problem"].as<std::string>(), prob, ignore_costs  );
	std::cout << "PDDL problem description loaded: " << std::endl;
	std::cout << "\tDomain: " << prob.domain_name() << std::endl;
	std::cout << "\tProblem: " << prob.problem_name() << std::endl;
	std::cout << "\t#Actions: " << prob.num_actions() << std::endl;
	std::cout << "\t#Fluents: " << prob.num_fluents() << std::endl;

	Fwd_Search_Problem	search_prob( &prob );

	//if ( !prob.has_conditional_effects() ) {
	// 	H2_Fwd    h2( search_prob );
	// 	h2.compute_edeletes( prob );
	// 	if ( h2.eval( prob.goal() ) == infty ) {
	// 		std::ofstream	details( "execution.details" );
	// 		details << "Problem has no solution!" << std::endl;
	// 		details.close();
	// 		report_no_solution( "h2(s0) = infty", plan_stream );
	// 		return 0;
	// 	}
	//
	//}
	//else
	// 	prob.compute_edeletes();

	if( vm["heuristic"].as<std::string>() == "hff" ){
		std::cout << "Starting search with GBFS(novel,h_ff)..." << std::endl;

		GBFS_FF_Rp_Fwd bfs_engine( search_prob );

		bfs_engine.set_ignore_novelty(  vm["ignore_novelty"].as<bool>() );
		bfs_engine.set_arity(  vm["max_novel"].as<unsigned>() );

		float bfs_t = do_search( bfs_engine, prob,  plan_stream );

		std::cout << "BFS search completed in " << bfs_t << " secs" << std::endl;

	}
	else if( vm["heuristic"].as<std::string>() == "hadd" ){
		std::cout << "Starting search with GBFS(novel,h_add)..." << std::endl;

		GBFS_H_Add_Fwd bfs_engine( search_prob );

		bfs_engine.set_ignore_novelty(  vm["ignore_novelty"].as<bool>() );
		bfs_engine.set_arity(  vm["max_novel"].as<unsigned>() );

		float bfs_t = do_search( bfs_engine, prob,  plan_stream );

		std::cout << "BFS search completed in " << bfs_t << " secs" << std::endl;
	}
	else if( vm["heuristic"].as<std::string>() == "hadd_local" ){
		std::cout << "Starting search with GBFS(novel,h_add)..." << std::endl;

		GBFS_IW_H_Add_Fwd bfs_engine( search_prob );

		bfs_engine.set_ignore_novelty(  vm["ignore_novelty"].as<bool>() );
		bfs_engine.set_ignore_local_novelty(  vm["ignore_local_novelty"].as<bool>() );
		bfs_engine.set_arity(  vm["max_novel"].as<unsigned>() );
		bfs_engine.set_stall_size( 1000 );
		bfs_engine.set_max_expanded( 1000 );
		bfs_engine.set_local_tries( 100 );

		float bfs_t = do_search( bfs_engine, prob,  plan_stream );

		std::cout << "BFS search completed in " << bfs_t << " secs" << std::endl;
	}
	else if( vm["heuristic"].as<std::string>() == "hff_local" ){
		std::cout << "Starting search with GBFS(novel,h_add)..." << std::endl;

		GBFS_IW_FF_Rp_Fwd bfs_engine( search_prob );

		bfs_engine.set_ignore_novelty(  vm["ignore_novelty"].as<bool>() );
		bfs_engine.set_ignore_local_novelty(  vm["ignore_local_novelty"].as<bool>() );
		bfs_engine.set_arity(  vm["max_novel"].as<unsigned>() );
		bfs_engine.set_stall_size( 1000 );
		bfs_engine.set_max_expanded( 1000 );
		bfs_engine.set_local_tries( 100 );

		float bfs_t = do_search( bfs_engine, prob,  plan_stream );

		std::cout << "BFS search completed in " << bfs_t << " secs" << std::endl;
	}
	else if( vm["heuristic"].as<std::string>() == "mix" ){

		Gen_Lms_Fwd    gen_lms( search_prob );
		Landmarks_Graph graph( prob );

		gen_lms.compute_lm_graph_set_additive( graph );

		std::cout << "Landmarks found: " << graph.num_landmarks() << std::endl;

		std::cout << "Starting search with GBFS(novel,h_add_rp,hlm)..." << std::endl;

		GBFS_H_Add_Rp_LM_Fwd bfs_engine( search_prob );

		bfs_engine.set_arity(  vm["max_novel"].as<unsigned>() );
		bfs_engine.h3().set_one_HA_per_fluent( false );

		Land_Graph_Man lgm( search_prob, &graph);
		bfs_engine.use_land_graph_manager( &lgm );

		float bfs_t = do_search( bfs_engine, prob,  plan_stream );

		std::cout << "BFS search completed in " << bfs_t << " secs" << std::endl;
	}

	plan_stream.close();

	return 0;
}


/*
 Copyright (C) 2018 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <ored/portfolio/builders/cmsspread.hpp>
#include <ored/utilities/log.hpp>
#include <ored/utilities/parsers.hpp>
#include <qle/termstructures/flatcorrelation.hpp>

#include <boost/make_shared.hpp>

using namespace QuantLib;

namespace ore {
namespace data {

boost::shared_ptr<FloatingRateCouponPricer>
CmsSpreadCouponPricerBuilder::engineImpl(const Currency& ccy, const string& index1, const string& index2,
                                         const boost::shared_ptr<CmsCouponPricer>& cmsPricer) {

    QuantLib::Handle<QuantExt::CorrelationTermStructure> corrCurve(
        boost::make_shared<QuantExt::FlatCorrelation>(0, NullCalendar(), 0.0, Actual365Fixed()));
    try {
        corrCurve = market_->correlationCurve(index1, index2, configuration(MarketContext::pricing));
    } catch (...) {
        WLOG("no correlation curve for " << index1 << ", " << index2 << " found, fall back to zero correlation.");
    }

    const string& ccyCode = ccy.code();
    return boost::make_shared<QuantExt::LognormalCmsSpreadPricer>(
        cmsPricer, corrCurve, market_->discountCurve(ccyCode, configuration(MarketContext::pricing)),
        parseInteger(engineParameter("IntegrationPoints")));
}

} // namespace data
} // namespace ore


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


#include "TemporalConstraintNetwork.hpp"
#include <graph_analysis/WeightedEdge.hpp>
#include <graph_analysis/algorithms/FloydWarshall.hpp>
#include <graph_analysis/GraphIO.hpp>
#include <limits>
#include <boost/lexical_cast.hpp>
#include <numeric/Combinatorics.hpp>

#include "point_algebra/TimePointComparator.hpp"
#include "../csp/TemporalConstraintNetwork.hpp"
#include "QualitativeTemporalConstraintNetwork.hpp"

using namespace templ::solvers::temporal::point_algebra;
using namespace graph_analysis;

namespace templ {
namespace solvers {
namespace temporal {

TemporalConstraintNetwork::TemporalConstraintNetwork()
    : mpDistanceGraph( BaseGraph::getInstance() )
{}

TemporalConstraintNetwork::~TemporalConstraintNetwork()
{}

TemporalConstraintNetwork::TemporalConstraintNetwork(const TemporalConstraintNetwork& other)
    : ConstraintNetwork(other)
    , mpDistanceGraph(other.mpDistanceGraph->cloneEdges())
{}

/**
 * Get the known and consolidated constraint between two timepoints
 * \return consolidated timepoint constraint
 */
point_algebra::QualitativeTimePointConstraint::Type TemporalConstraintNetwork::getQualitativeConstraint(const point_algebra::TimePoint::Ptr& t1, const point_algebra::TimePoint::Ptr& t2)
{
    if(t1 == t2)
    {
        return point_algebra::QualitativeTimePointConstraint::Universal;
    } else {
        return point_algebra::QualitativeTimePointConstraint::Empty;
    }
}

// stp(N) is generated by upper-lower bounds of range on disjunctive intervals
void TemporalConstraintNetwork::stpWithConjunctiveIntervals()
{
    BaseGraph::Ptr graph = mpDistanceGraph->copy();

    TemporalConstraintNetwork tcn;

    EdgeIterator::Ptr edgeIt = graph->getEdgeIterator();

    IntervalConstraint::Ptr edge;
    double max, min;
    while (edgeIt->next())
    {
        // for each edge of the temp.const.network, check each interval and find the
        // highest min bound and the lowest max bound
        edge = dynamic_pointer_cast<IntervalConstraint>( edgeIt->current() );
        max = std::numeric_limits<double>::max();
        min = std::numeric_limits<double>::min();

        std::vector<Bounds> intervals = edge->getIntervals();
        std::vector<Bounds>::const_iterator cit = intervals.begin();
        for( ; cit != intervals.end(); ++cit)
        {
            min = std::max(cit->getLowerBound(), min);
            max = std::min(cit->getUpperBound(), max);
        }

        // create a new interval constraint using only the interval [min,max]
        IntervalConstraint::Ptr i(new IntervalConstraint(edge->getSourceTimePoint(),edge->getTargetTimePoint()));
        i->addInterval(Bounds(min,max));
        tcn.addIntervalConstraint(i);
    }
    // update mpDistanceGraph with the one that we just created (tcn)
    mpDistanceGraph = tcn.mpDistanceGraph->copy();
}

// stp(N) is generated by upper-lower bounds of range on disjunctive intervals
void TemporalConstraintNetwork::stp()
{
    // see Processing disjunctions in temporal constraint networks, (Schwalb and Dechter, 1997)
    // Upper-Lower-Tightening

    BaseGraph::Ptr graph = mpDistanceGraph->copy();

    TemporalConstraintNetwork tcn;

    EdgeIterator::Ptr edgeIt = graph->getEdgeIterator();

    IntervalConstraint::Ptr edge;
    double max, min;
    while (edgeIt->next())
    {
        // for each edge of the temp.const.network, check each interval and find the
        // highest min bound and the lowest max bound
        edge = dynamic_pointer_cast<IntervalConstraint>( edgeIt->current() );

        min = std::numeric_limits<double>::max();
        max = std::numeric_limits<double>::min();

        std::vector<Bounds> intervals = edge->getIntervals();
        std::vector<Bounds>::const_iterator cit = intervals.begin();
        for( ; cit != intervals.end(); ++cit)
        {
            min = std::min(cit->getLowerBound(), min);
            max = std::max(cit->getUpperBound(), max);
        }

        // create a new interval constraint using only the interval [min,max]
        IntervalConstraint::Ptr i = make_shared<IntervalConstraint>(edge->getSourceTimePoint(),edge->getTargetTimePoint());
        i->addInterval(Bounds(min, std::max(max, min + 1E-06)) );
        tcn.addIntervalConstraint(i);
    }
    // update mpDistanceGraph with the one that we just created (tcn)
    mpDistanceGraph = tcn.mpDistanceGraph->copy();
}

// the intersection between a temporal constraint network and a simple temporal constraint network
graph_analysis::BaseGraph::Ptr TemporalConstraintNetwork::intersection(graph_analysis::BaseGraph::Ptr other)
{
    //// complex temporal constraint network
    //BaseGraph::Ptr graph0 = mpDistanceGraph->copy();

    //// simple temporal constraint network
    //BaseGraph::Ptr graph1 = other->copy();

    // the intersection result
    TemporalConstraintNetwork tcn;

    EdgeIterator::Ptr edgeIt1 = other->getEdgeIterator();
    TimePoint::Ptr source,target;
    IntervalConstraint::Ptr edge0,edge1;

    // iterate through the simple temporal constraint network
    while (edgeIt1->next())
    {
        edge1 = dynamic_pointer_cast<IntervalConstraint>( edgeIt1->current() );
        source = edge1 -> getSourceTimePoint();
        target = edge1 -> getTargetTimePoint();

        std::vector<Bounds> intervals = edge1->getIntervals();
        std::vector<Bounds>::iterator interval = intervals.begin();

        EdgeIterator::Ptr edgeIt0 = mpDistanceGraph->getEdgeIterator();
        // for each edge from the simple temporal constraint network; iterate through the complex temporal constraint network
        while (edgeIt0->next())
        {
            // if we find two edges (one from each graph) which have the same source and target variables
            // then we look at their upper and lower bounds
            edge0 = dynamic_pointer_cast<IntervalConstraint>( edgeIt0->current() );
            if (target == edge0->getTargetTimePoint() && source == edge0->getSourceTimePoint())
            {

                std::vector<Bounds> v = edge0->getIntervals();
                std::vector<Bounds>::iterator it = v.begin();
                IntervalConstraint::Ptr i(new IntervalConstraint(source, target));
                while (it!=v.end())
                {
                    /* edge1: [x--------------y]
                        edge0:   [a-------b]
                        => add:  [a-------b]
                    */
                    if (interval->getLowerBound() <= it->getLowerBound() && interval->getUpperBound() >= it->getUpperBound())
                    {
                        i->addInterval(Bounds(it->getLowerBound(),it->getUpperBound()));
                    }
                    else
                    /*  edge1: [x------------y]
                        edge0:      [a------------b]
                        => add:     [a-------y]
                    */
                    if (interval->getLowerBound() <= it->getLowerBound() && interval->getUpperBound() <= it->getUpperBound())
                    {
                        if (it->getLowerBound() <= interval->getUpperBound())
                        {
                            i->addInterval(Bounds(it->getLowerBound(),interval->getUpperBound()));
                        }
                    }
                    else
                    /*  edge1:        [x-----------y]
                        edge0:    [a---------b]
                        => add:       [x-----b]
                    */
                    if (interval->getLowerBound() >= it->getLowerBound() && interval->getUpperBound() >= it->getUpperBound())
                    {
                        if (it->getUpperBound() >= interval->getLowerBound())
                        {
                            i->addInterval(Bounds(interval->getLowerBound(),it->getUpperBound()));
                        }
                    }
                    else
                    /*  edge1:     [x----y]
                        edge0:  [a-----------b]
                        => add:    [x----y]
                    */
                    if (interval->getLowerBound() >= it->getLowerBound() && interval->getUpperBound() <= it->getUpperBound())
                    {
                        i->addInterval(Bounds(interval->getLowerBound(),interval->getUpperBound()));
                    }
                    // otherwise:
                    /*  edge1:  [x----y]
                        edge0:            [a----b]
                        or
                        edge1:            [x----y]
                        edge0:  [a----b]
                        => we don't add anything
                    */
                    it++;
                }
                tcn.addIntervalConstraint(i);
            }
        }
    }
    mpDistanceGraph = tcn.mpDistanceGraph->copy();
    // update mpDistanceGraph with the one that we just created (tcn)
    return mpDistanceGraph;
}

// Change A ------[lowerBound,uppperBound]------> B into:
/*
 *           A --- weight:   upper bound --> B
 *           B --- weight: - lower bound --> A
 *  Upper and lower bounds of each interval are added as edges in forward and backward direction between two edges
 *   Returns the weighted graph
**/
graph_analysis::BaseGraph::Ptr TemporalConstraintNetwork::toWeightedGraph()
{
    BaseGraph::Ptr graph = BaseGraph::getInstance();
    BaseGraph::Ptr tcn = mpDistanceGraph->copy();
    EdgeIterator::Ptr edgeIt = tcn->getEdgeIterator();

    while (edgeIt->next())
    {
        IntervalConstraint::Ptr edge0 = dynamic_pointer_cast<IntervalConstraint>( edgeIt->current() );

        std::vector<Bounds> intervals = edge0->getIntervals();
        std::vector<Bounds>::const_iterator cit = intervals.begin();
        for( ; cit != intervals.end(); ++cit)
        {
            WeightedEdge::Ptr forwardEdge(new WeightedEdge(cit->getUpperBound()));
            forwardEdge->setSourceVertex(edge0->getSourceTimePoint());
            forwardEdge->setTargetVertex(edge0->getTargetTimePoint());
            graph->addEdge(forwardEdge);

            WeightedEdge::Ptr reverseEdge(new WeightedEdge(-cit->getLowerBound()));
            reverseEdge->setSourceVertex(edge0->getTargetTimePoint());
            reverseEdge->setTargetVertex(edge0->getSourceTimePoint());
            graph->addEdge(reverseEdge);
        }
    }

    return graph;
}

//compute the minimal network of a simple temporal network using Floyd-Warshall
void TemporalConstraintNetwork::minNetwork()
{
    // Input: a simple temporal constraint network
    TemporalConstraintNetwork tcn;
    // Change its graph into a weighted graph and then apply Floyd-Warshall
    BaseGraph::Ptr graph = (toWeightedGraph())->copy();

    BaseGraph::Ptr oldGraph = mpDistanceGraph->copy();
    algorithms::DistanceMatrix distanceMatrix = algorithms::FloydWarshall::allShortestPaths(graph, [](Edge::Ptr e) -> double
                {
                    return dynamic_pointer_cast<WeightedEdge>(e)->getWeight();
                });
    EdgeIterator::Ptr edgeIt = oldGraph->getEdgeIterator();

    // Change again the computed graph into a graph using Interval Constraint representation
    TimePoint::Ptr v1,v2;
    while (edgeIt->next())
    {
        IntervalConstraint::Ptr edge = dynamic_pointer_cast<IntervalConstraint>(edgeIt->current());
        v1 = edge->getSourceTimePoint();
        v2 = edge->getTargetTimePoint();

        double distance12 = distanceMatrix[std::pair<TimePoint::Ptr, TimePoint::Ptr>(v1,v2)];
        double distance21 = distanceMatrix[std::pair<TimePoint::Ptr, TimePoint::Ptr>(v2,v1)];
        distance21 = (-1)*distance21;
        IntervalConstraint::Ptr i(new IntervalConstraint(v1, v2));
        i->addInterval(Bounds(distance21,distance12));
        tcn.addIntervalConstraint(i);
    }
    // update mpDistanceGraph with the one that we just created (a simple temporal constraint network with the shortest paths computed)
    mpDistanceGraph = tcn.mpDistanceGraph->copy();
}

bool TemporalConstraintNetwork::equals(const graph_analysis::BaseGraph::Ptr& other)
{
    // check if the graphs have the same number of vertices and edges
    if (mpDistanceGraph->size() != other->size() || mpDistanceGraph->order() != other->order())
    {
        LOG_WARN_S << "Expected size: " << mpDistanceGraph->size() << " order: " << mpDistanceGraph->order()
            << " got other with: size:" << other->size() << " order: " << other->order();
        return false;
    }

    Vertex::PtrList allVertices = mpDistanceGraph->getAllVertices();
    numeric::Combination<Vertex::Ptr> combination(allVertices, 2, numeric::EXACT);
    do
    {
        const Vertex::PtrList& edge = combination.current();
        const point_algebra::TimePoint::Ptr& sourceTimePoint = dynamic_pointer_cast<point_algebra::TimePoint>(edge[0]);
        const point_algebra::TimePoint::Ptr& targetTimePoint = dynamic_pointer_cast<point_algebra::TimePoint>(edge[1]);

        std::vector<graph_analysis::Edge::Ptr> thisEdgeList, otherEdgeList;
        thisEdgeList = mpDistanceGraph->getEdges(sourceTimePoint,targetTimePoint);
        otherEdgeList = other->getEdges(sourceTimePoint,targetTimePoint);

        Bounds::List thisIntervals;
        Bounds::List otherIntervals;

        if(!thisEdgeList.empty())
        {
            IntervalConstraint::Ptr thisEdge = dynamic_pointer_cast<IntervalConstraint>(thisEdgeList.front());
            thisIntervals.insert(thisIntervals.begin(), thisEdge->getIntervals().begin(), thisEdge->getIntervals().end());
        }
        if(!otherEdgeList.empty())
        {
            IntervalConstraint::Ptr otherEdge = dynamic_pointer_cast<IntervalConstraint>(otherEdgeList.front());
            otherIntervals.insert(otherIntervals.begin(), otherEdge->getIntervals().begin(), otherEdge->getIntervals().end());
        }

        // check reverse
        thisEdgeList = mpDistanceGraph->getEdges(targetTimePoint, sourceTimePoint);
        otherEdgeList = other->getEdges(targetTimePoint, sourceTimePoint);
        if(!thisEdgeList.empty())
        {
            IntervalConstraint::Ptr edge = dynamic_pointer_cast<IntervalConstraint>(thisEdgeList.front());
            Bounds::List intervals = Bounds::reverse( edge->getIntervals() );
            thisIntervals.insert(thisIntervals.begin(), intervals.begin(), intervals.end());
        }
        if(!otherEdgeList.empty())
        {
            IntervalConstraint::Ptr edge = dynamic_pointer_cast<IntervalConstraint>(otherEdgeList.front());
            Bounds::List intervals = Bounds::reverse( edge->getIntervals() );
            otherIntervals.insert(otherIntervals.begin(), intervals.begin(), intervals.end());
        }

        if(!Bounds::equals(thisIntervals, otherIntervals))
        {
            LOG_WARN_S << "From: " << sourceTimePoint->getLabel()
                << " to: " << targetTimePoint->getLabel() << std::endl << " BOUNDS are not equal: " << Bounds::toString(thisIntervals) << " vs "
                << Bounds::toString(otherIntervals) << std::endl;

            return false;
        }

    } while(combination.next());

    return true;
}

// Upper-Lower-Tightening Algorithm
void TemporalConstraintNetwork::upperLowerTightening()
{
    BaseGraph::Ptr oldGraph;
    do
    {
        // oldGraph = newGraph
        oldGraph = mpDistanceGraph->copy();
        stp();
        minNetwork();

        // copy oldGraph into a temporal constraint network
        TemporalConstraintNetwork tcn;

        // newGraph becomes the minimal network
        EdgeIterator::Ptr edgeIt = oldGraph->getEdgeIterator();
        while (edgeIt->next())
        {
            IntervalConstraint::Ptr intervalConstraint = dynamic_pointer_cast<IntervalConstraint>(edgeIt->current());
            tcn.addIntervalConstraint(intervalConstraint);
        }

        // apply intersection for oldGraph and newGraph(minimal network)
        tcn.intersection(mpDistanceGraph);
        // newGraph becomes the resulted graph obtained from the intersection between the oldGraph and the minimal network
        mpDistanceGraph = tcn.mpDistanceGraph->copy();
    } while (!this->equals(oldGraph)); // until newGraph = oldGraph
}

int TemporalConstraintNetwork::getEdgeNumber()
{
    return mpDistanceGraph->size();
}

void TemporalConstraintNetwork::save(const std::string& filename) const
{
    if(!getGraph()->empty())
    {
        graph_analysis::io::GraphIO::write(filename, getGraph(), graph_analysis::representation::GRAPHVIZ);
        graph_analysis::io::GraphIO::write(filename, getGraph(), graph_analysis::representation::GEXF);
    }

    if(!getDistanceGraph()->empty())
    {
        graph_analysis::io::GraphIO::write(filename + "-distance_graph", getDistanceGraph(), graph_analysis::representation::GRAPHVIZ);
        graph_analysis::io::GraphIO::write(filename + "-distance_graph", getDistanceGraph(), graph_analysis::representation::GEXF);
    }
}

void TemporalConstraintNetwork::sort(std::vector<point_algebra::TimePoint::Ptr>& timepoints) const
{
    const QualitativeTemporalConstraintNetwork* qtcn = dynamic_cast<const QualitativeTemporalConstraintNetwork*>(this);
    if(qtcn)
    {
        solvers::csp::TemporalConstraintNetwork::sort(*qtcn, timepoints);
    } else {
        // allow using of shared pointer like normal pointers, when auto-deleting is not
        // desirable
        TemporalConstraintNetwork::Ptr tcn(const_cast<TemporalConstraintNetwork*>(this), [](TemporalConstraintNetwork*){});
        TimePointComparator comparator(tcn);
        comparator.sort(timepoints);
    }
}

void TemporalConstraintNetwork::setConsistentNetwork(const graph_analysis::BaseGraph::Ptr& baseGraph)
{
    setGraph(baseGraph);
}

point_algebra::TimePoint::Ptr TemporalConstraintNetwork::getTimePoint(const std::string& name) const
{
    using namespace solvers::temporal::point_algebra;

    graph_analysis::VertexIterator::Ptr it = getVariableIterator();
    while(it->next())
    {
        QualitativeTimePoint::Ptr t = dynamic_pointer_cast<QualitativeTimePoint>(it->current());
        if(t && t->getLabel() == name)
        {
            return t;
        }
    }
    throw std::invalid_argument("templ::solvers::temporal::TemporalConstraintNetwork::getTimePoint: timepoint with label '" + name + "'"
            " not found");
}

point_algebra::TimePoint::Ptr TemporalConstraintNetwork::getOrCreateTimePoint(const std::string& name) const
{
    using namespace solvers::temporal::point_algebra;
    TimePoint::Ptr timepoint;
    try {
        return getTimePoint(name);
    } catch(const std::invalid_argument& e)
    {
        try {
            if(name == "inf")
            {
                return TimePoint::create(std::numeric_limits<uint64_t>::infinity(), std::numeric_limits<uint64_t>::infinity());
            } else {
                uint64_t exactTime = boost::lexical_cast<uint64_t>(name);
                return TimePoint::create(exactTime, exactTime);
            }
        } catch(const std::bad_cast& e)
        {
            return TimePoint::create(name);
        }
    }

    throw std::runtime_error("templ::solvers::temporal::TemporalConstraintNetwork::getOrCreateTimePoint: timepoint with label/value '" + name + "' could neither be found nor created");
}

std::string TemporalConstraintNetwork::toString(uint32_t indent) const
{
    std::stringstream ss;
    std::string hspace(indent,' ');

    using namespace solvers::temporal::point_algebra;
    graph_analysis::EdgeIterator::Ptr it = getConstraintIterator();
    while(it->next())
    {
        QualitativeTimePointConstraint::Ptr t = dynamic_pointer_cast<QualitativeTimePointConstraint>(it->current());
        if(t)
        {
            ss << t->toString(indent);
        }
    }
    return ss.str();
}

point_algebra::TimePoint::PtrList TemporalConstraintNetwork::getTimepoints() const
{
    using namespace graph_analysis;
    std::vector<solvers::temporal::point_algebra::TimePoint::Ptr> timepoints;

    VertexIterator::Ptr vertexIt = getGraph()->getVertexIterator();
    while(vertexIt->next())
    {
        solvers::temporal::point_algebra::TimePoint::Ptr tp =
            dynamic_pointer_cast<solvers::temporal::point_algebra::TimePoint>(vertexIt->current());
        timepoints.push_back(tp);
    }

    sort(timepoints);
    return timepoints;
}

TemporalConstraintNetwork::Assignment TemporalConstraintNetwork::getAssignment() const
{
    TimePoint::PtrList timepoints;
    std::map<point_algebra::TimePoint::Ptr, double> lowerBounds;
    TimePoint::Ptr startPoint;
    VertexIterator::Ptr vertexIt = mpDistanceGraph->getVertexIterator();
    while(vertexIt->next())
    {
        TimePoint::Ptr tp = dynamic_pointer_cast<TimePoint>(vertexIt->current());
        timepoints.push_back(tp);

        if(mpDistanceGraph->getInEdges(tp).empty())
        {
            if(!startPoint)
            {
                startPoint = tp;
            } else
            {
                throw
                    std::runtime_error("TemporalConstraintNetwork::getAssignment:" \
                            "start point already set" + startPoint->toString() +
                            " vs. " + tp->toString() );
            }
        }
    }

    if(timepoints.empty())
    {
        throw std::invalid_argument("TemporalConstraintNetwork::getAssignment: empty list of timepoints");
    }

    if(!startPoint)
    {
        throw std::invalid_argument("TemporalConstraintNetwork::getAssignment:" \
                "no start timepoint identified");
    }

    // Compute the time horizon
    double timeHorizonInS = 0.0;
    TimePoint::Ptr evalTimepoint = startPoint;
    while(true)
    {
        lowerBounds[ evalTimepoint ] = timeHorizonInS;
        {
            Edge::PtrList edges =
                mpDistanceGraph->getOutEdges(evalTimepoint);

            if(edges.empty())
            {
                // final timepoint found
                break;
            }

            TimePoint::Ptr nextTp;
            double nextTpDistance = std::numeric_limits<double>::max();
            for(const Edge::Ptr& e : edges)
            {
                IntervalConstraint::Ptr intervalConstraint = dynamic_pointer_cast<IntervalConstraint>(e);
                double interval = intervalConstraint->getLowerBound();

                if(interval < nextTpDistance)
                {
                    nextTpDistance = interval;
                    nextTp = intervalConstraint->getTargetTimePoint();
                }
            }
            evalTimepoint = nextTp;
            timeHorizonInS += nextTpDistance;
        }
    }
    return lowerBounds;
}

double TemporalConstraintNetwork::getTimeHorizon(const Assignment& assignments)
{
    double horizon = 0;
    for(const Assignment::value_type& p : assignments)
    {
        if(p.second > horizon)
        {
            horizon = p.second;
        }
    }
    return horizon;
}

void TemporalConstraintNetwork::addIntervalConstraint(const IntervalConstraint::Ptr& i)
{
    if( !i->getSourceVertex()->associated( mpDistanceGraph->getId()) ||
            !i->getTargetVertex()->associated( mpDistanceGraph->getId()) )
    {
        mpDistanceGraph->addEdge(i);
        return;
    }

    Edge::PtrList edges = mpDistanceGraph->getEdges(i->getSourceVertex(), i->getTargetVertex());
    if(edges.empty())
    {
        mpDistanceGraph->addEdge(i);
    } else {
        IntervalConstraint::Ptr ic = dynamic_pointer_cast<IntervalConstraint>(*edges.begin());
        if(!ic)
        {
            throw std::runtime_error("TemporalConstraintNetwork: conversion to interval constraint failed");
        }

        for(const Bounds& bounds : i->getIntervals())
        {
            ic->addInterval(bounds);
        }
    }
}

} // end namespace temporal
} // end namespace solvers
} // end namespace templ