# pragma once
#define EIGEN_USE_MKL_ALL
#include"basis.hpp"
#include<cmath>
#include <Eigen/Sparse>
#include <type_traits>
#include"operators.hpp"
#ifdef MOM
using ValType= std::complex<double>;
#else
using ValType= double;
#endif
namespace Operators{
  using Many_Body::pi;
  using  Many_Body::GetLattice;
  using  Many_Body::Translate;
  using  Many_Body::Position;
  //   using  Many_Body::State();
  using Many_Body::RightId;
  using Many_Body::LeftId;

  using Mat= Eigen::SparseMatrix<ValType,Eigen::RowMajor>;
 template<class TotalBasis>
  Mat CdagTPOperator(const TotalBasis& totalBasis, int i,   const bool& PB=true)
{

   using LeftBasisIt= typename TotalBasis::LeftBasisIt;
      using RightBasisIt= typename TotalBasis::RightBasisIt;

 using Lattice=typename TotalBasis::LeftLattice;
    size_t dim=totalBasis.dim;

    Mat op(dim, dim);
       op.setZero();
 for( auto& tpState :totalBasis)
	     {

	        LeftBasisIt it2=totalBasis.lbasis.find(LeftId(tpState));

	       Lattice state=GetLattice(*it2);
	       //std::cout << " stat "<< state << "\n";
	       //std::cout << " i "<< i<< "  has "<< state[i]<< std::endl;

	       if(state[i]==0 and (state.Count()<totalBasis.lbasis.totalmaxPar) )
	         	 {

			   auto temp=state;
	         	   state.setPartNr(i, 1);
			   size_t signControl=CheckSign2(temp, i, 0);
			   // std::cout << "at i="<< i <<" changed to  "<< state << "\n"<< "with aign cont "<< signControl<< '\n';
	         	   it2= totalBasis.lbasis.find(state.GetId());
	        	   RightBasisIt it3=totalBasis.rbasis.find(RightId(tpState));
	     		   size_t newStateNr= Position(*it3)*totalBasis.lbasis.dim +Position(*it2);
	      		              if(signControl%2==0)
   	      	    	 {
	      		   op.coeffRef(newStateNr, Position(tpState))+= ValType{1};}
   	      	       else
   	      	    	 { op.coeffRef(newStateNr, Position(tpState))-= ValType{1};}
	      	    }

	      }
      return op;

       }
  template<class TotalBasis>
  Mat CTPOperator(const TotalBasis& totalBasis, int i,   const bool& PB=true)
{

   using LeftBasisIt= typename TotalBasis::LeftBasisIt;
      using RightBasisIt= typename TotalBasis::RightBasisIt;

 using Lattice=typename TotalBasis::LeftLattice;
    size_t dim=totalBasis.dim;

    Mat op(dim, dim);
       op.setZero();
 for( auto& tpState :totalBasis)
	     {

	        LeftBasisIt it2=totalBasis.lbasis.find(LeftId(tpState));

	       Lattice state=GetLattice(*it2);
	       // std::cout << " stat "<< state << "\n";
	       //std::cout << " i "<< i<< "  has "<< state[i]<< std::endl;

	       if(state[i]==1)
	         	 {

			   auto temp=state;
	         	   state.setPartNr(i, 0);
			   size_t signControl=CheckSign2(temp, i, 0);
			   // std::cout << "at i="<< i <<" changed to  "<< state << "\n"<< "with aign cont "<< signControl<< '\n';
	         	   it2= totalBasis.lbasis.find(state.GetId());
	        	   RightBasisIt it3=totalBasis.rbasis.find(RightId(tpState));
	     		   size_t newStateNr= Position(*it3)*totalBasis.lbasis.dim +Position(*it2);
	      		              if(signControl%2==0)
   	      	    	 {
	      		   op.coeffRef(newStateNr, Position(tpState))+= ValType{1};}
   	      	       else
   	      	    	 { op.coeffRef(newStateNr, Position(tpState))-= ValType{1};}
	      	    }

	      }
      return op;

       }
   template<class TotalBasis, class SubBasis>
 Mat NumberOperatorph_1(const TotalBasis& totalBasis, const SubBasis& subBasis, const double omega=1., size_t i=0, const bool& PB=true)

  {

    size_t dim=totalBasis.dim;
    size_t sites=totalBasis.sites;
    Mat op(dim, dim);
    //   op.setZero();

      for(const auto& tpState : totalBasis)
	{

	  // left vs right id ?
 op.coeffRef(Position(tpState), Position(tpState))+=(omega*subBasis.particlesAt(RightId(tpState), i));

	}
      return op;

       }
 template<class TotalBasis, class SubBasis>
 Mat NumberOperatore_1(const TotalBasis& totalBasis, const SubBasis& subBasis, const double omega=1., size_t i=0, const bool& PB=true)

  {

    size_t dim=totalBasis.dim;
    size_t sites=totalBasis.sites;
    Mat op(dim, dim);
    //   op.setZero();

      for(const auto& tpState : totalBasis)
	{

	  // left vs right id ?
 op.coeffRef(Position(tpState), Position(tpState))+=(omega*subBasis.particlesAt(LeftId(tpState), i));

	}
      return op;

       }
   template<class TotalBasis, class SubBasis>
  Mat NumberOperator(const TotalBasis& totalBasis, const SubBasis& subBasis, const double omega=1., const bool& PB=true)

  {

    size_t dim=totalBasis.dim;
    size_t sites=totalBasis.sites;
    Mat op(dim, dim);
    //   op.setZero();

      for(const auto& tpState : totalBasis)
	{

	  for(size_t i=0; i<sites; i++)
	    {

 op.coeffRef(Position(tpState), Position(tpState))+=(omega*subBasis.particlesAt(RightId(tpState), i));

	  }
	}
      return op;

       }

 template<class TotalBasis, class SubBasis >
  Mat CurOperatorL(const TotalBasis& totalBasis, const SubBasis& subBasis, double var=1. , const bool& PB=true)
     {

	      using LeftBasisIt= typename TotalBasis::LeftBasisIt;
     using RightBasisIt= typename TotalBasis::RightBasisIt;

using Lattice=typename SubBasis::Lattice;

   size_t dim=totalBasis.dim;
    size_t sites=totalBasis.sites;
    Mat op(dim, dim);
    //      op.setZero();

	   for( auto& tpState : totalBasis)
	     {

	       	    for(size_t i=0; i<Operators::Length( sites, PB); i++)
                 {

	       LeftBasisIt it2=subBasis.find(LeftId(tpState));

	       Lattice state=GetLattice(*it2);
                     size_t j=Operators::NextWithBC(i, sites, PB);

	   	    		    if(state[i]==state[j])
   	       	     {
	  //

   	   	     }
   	   	    else{

		      Lattice temp=state;

		       state.setPartNr(j, temp[i]);
		      state.setPartNr(i, temp[j]);

	  	     size_t signControl=CheckSign(temp, i, j);

		     it2= subBasis.find(state.GetId());

	  	       RightBasisIt it3=totalBasis.rbasis.find(RightId(tpState));

	  	       size_t newStateNr= Position(*it3)*totalBasis.lbasis.dim +Position(*it2);
		      double otherSign=(state[j]==1)?+1:-1;
	   	              if(signControl%2==0)
   	   	    	 {
	   		   op.coeffRef(newStateNr, Position(tpState))-= otherSign*ValType{var};}
   	   	       else
   	   	    	 { op.coeffRef(newStateNr, Position(tpState))+=otherSign*ValType{var};}
		    }

	   	     }
 	      	     }

  return op;
  }

  template<class TotalBasis, class SubBasis >
  Mat EKinOperatorL(const TotalBasis& totalBasis, const SubBasis& subBasis, double var=1. , const bool& PB=true)
     {

	      using LeftBasisIt= typename TotalBasis::LeftBasisIt;
     using RightBasisIt= typename TotalBasis::RightBasisIt;

using Lattice=typename SubBasis::Lattice;

   size_t dim=totalBasis.dim;
    size_t sites=totalBasis.sites;
    Mat op(dim, dim);
    //      op.setZero();

	   for( auto& tpState : totalBasis)
	     {

	       	    for(size_t i=0; i<Operators::Length( sites, PB); i++)
                 {

	       LeftBasisIt it2=subBasis.find(LeftId(tpState));

	       Lattice state=GetLattice(*it2);
                     size_t j=Operators::NextWithBC(i, sites, PB);
		     //		   std::cout<< i << "  " << j <<std::endl;
	   	    		    if(state[i]==state[j])
   	       	     {
	  //

   	   	     }
   	   	    else{

		      Lattice temp=state;

		       state.setPartNr(j, temp[i]);
		      state.setPartNr(i, temp[j]);

	  	     size_t signControl=CheckSign(temp, i, j);

		     it2= subBasis.find(state.GetId());

	  	       RightBasisIt it3=totalBasis.rbasis.find(RightId(tpState));

	  	       size_t newStateNr= Position(*it3)*totalBasis.lbasis.dim +Position(*it2);

	   	              if(signControl%2==0)
   	   	    	 {
	   		   op.coeffRef(newStateNr, Position(tpState))-= ValType{var};}
   	   	       else
   	   	    	 { op.coeffRef(newStateNr, Position(tpState))+= ValType{var};}
		    }

	   	     }
 	      	     }

  return op;
  }

  template<class TotalBasis, class SubBasis >
  Mat HadamardOperator(const TotalBasis& totalBasis, const SubBasis& subBasis, double var=1. , const bool& PB=true)
     {

	      using LeftBasisIt= typename TotalBasis::LeftBasisIt;
     using RightBasisIt= typename TotalBasis::RightBasisIt;

using Lattice=typename SubBasis::Lattice;

   size_t dim=totalBasis.dim;
    size_t sites=totalBasis.sites;
    Mat op(dim, dim);
    //      op.setZero();

	   for( auto& tpState : totalBasis)
	     {

	       	    for(size_t i=0; i<Operators::Length( sites, PB); i++)
                 {

	       LeftBasisIt it2=subBasis.find(LeftId(tpState));

	       Lattice state=GetLattice(*it2);
                     size_t j=Operators::NextWithBC(i, sites, PB);

	   	    		    if(state[i]==1)
				      {
					op.coeffRef(Position(tpState), Position(tpState))+= 1;
}

	   	    		    if(state[i]==state[j])
   	       	     {
	  //

   	   	     }
   	   	    else{

		      Lattice temp=state;

		       state.setPartNr(j, temp[i]);
		      state.setPartNr(i, temp[j]);

	  	     size_t signControl=CheckSign(temp, i, j);

		     it2= subBasis.find(state.GetId());

	  	       RightBasisIt it3=totalBasis.rbasis.find(RightId(tpState));

	  	       size_t newStateNr= Position(*it3)*totalBasis.lbasis.dim +Position(*it2);

	   	              if(signControl%2==0)
   	   	    	 {
	   		   op.coeffRef(newStateNr, Position(tpState))+= 1;}
   	   	       else
   	   	    	 { op.coeffRef(newStateNr, Position(tpState))-= 1;}
		    }
}
				    if(!PB)
				      {
LeftBasisIt it2=subBasis.find(LeftId(tpState));

	       Lattice state=GetLattice(*it2);
if(state[sites-1]==1)
				      {
									op.coeffRef(Position(tpState), Position(tpState))+= var*1;
}
}

 	      	     }

	   op*=1./std::sqrt(sites);

  return op;
  }

 template<class TotalBasis, class SubBasis >
  Mat EKinOperatorLNNN(const TotalBasis& totalBasis, const SubBasis& subBasis, double var=1. , size_t m=0)
     {

	      using LeftBasisIt= typename TotalBasis::LeftBasisIt;
     using RightBasisIt= typename TotalBasis::RightBasisIt;

using Lattice=typename SubBasis::Lattice;

   size_t dim=totalBasis.dim;
    size_t sites=totalBasis.sites;
    Mat op(dim, dim);
      op.setZero();

	   for( auto& tpState : totalBasis)
	     {
	       	    for(size_t i=0; i<sites; i++)
                 {
	       LeftBasisIt it2=subBasis.find(LeftId(tpState));

	       Lattice state=GetLattice(*it2);
                     size_t j=(i+2)%sites;
	   	    		    if(state[i]==state[j])
   	       	     {
	  //

   	   	     }
   	   	    else{
		        	       Lattice temp=state;
				       state.setPartNr(j, temp[i]);
				       state.setPartNr(i, temp[j]);

	  		       size_t signControl=CheckSign(temp, i, j);

	  		       it2= subBasis.find(state.GetId());
	  	       RightBasisIt it3=totalBasis.rbasis.find(RightId(tpState));
	  	       size_t newStateNr= Position(*it3)*totalBasis.lbasis.dim +Position(*it2);
	   	              if(signControl%2==0)
   	   	    	 {
	   		   op.coeffRef(newStateNr, Position(tpState))-= var;}
   	   	       else
   	   	    	 { op.coeffRef(newStateNr, Position(tpState))+= var;}
		    }

	   	     }
 	      	     }

  return op;
  }

  template<class TotalBasis, class SubBasis >
  Mat EKinOperatorRNNN(const TotalBasis& totalBasis, const SubBasis& subBasis, double var=1. , size_t m=0)
     {

       using LeftBasisIt= typename TotalBasis::LeftBasisIt;
       using RightBasisIt= typename TotalBasis::RightBasisIt;

       using Lattice=typename SubBasis::Lattice;

   size_t dim=totalBasis.dim;
    size_t sites=totalBasis.sites;
    Mat op(dim, dim);
      op.setZero();

	  for( auto& tpState : totalBasis)
	    {
	      	    for(size_t i=0; i<sites; i++)
                {
	       RightBasisIt it2=subBasis.find(RightId(tpState));

	       Lattice state=GetLattice(*it2);
                     size_t j=(i+2)%sites;
	   	    		    if(state[i]==state[j])
   	       	     {

   	   	     }
   	   	    else{
		      Lattice temp=state;
			     state.setPartNr(j, temp[i]);
 state.setPartNr(i, temp[j]);

	  		       size_t signControl=CheckSign(temp, i, j);

	  		       it2= subBasis.find(state.GetId());
	  	       LeftBasisIt it3=totalBasis.lbasis.find(LeftId(tpState));
	  	       size_t newStateNr= Position(*it2)*totalBasis.lbasis.dim +Position(*it3);
	  	              if(signControl%2==0)
   	  	    	 {
	   		   op.coeffRef(newStateNr, Position(tpState))-= var;}
   	   	       else
   	   	    	 { op.coeffRef(newStateNr, Position(tpState))+= var;}

	   	     }
 	     	     }

   	  }

  return op;
  }

  template<class TotalBasis, class SubBasis >
  Mat EKinOperatorR(const TotalBasis& totalBasis, const SubBasis& subBasis, double var=1. , size_t m=0)
     {

       using LeftBasisIt= typename TotalBasis::LeftBasisIt;
       using RightBasisIt= typename TotalBasis::RightBasisIt;

       using Lattice=typename SubBasis::Lattice;

   size_t dim=totalBasis.dim;
    size_t sites=totalBasis.sites;
    Mat op(dim, dim);
      op.setZero();

	  for( auto& tpState : totalBasis)
	    {
	      	    for(size_t i=0; i<sites; i++)
                {
	       RightBasisIt it2=subBasis.find(RightId(tpState));

	       Lattice state=GetLattice(*it2);
                     size_t j=(i+1)%sites;
	   	    		    if(state[i]==state[j])
   	       	     {

   	   	     }
   	   	    else{
		      Lattice temp=state;
		      state.setPartNr(j, temp[i]);
 state.setPartNr(i, temp[j]);

	  		       size_t signControl=CheckSign(temp, i, j);

	  		       it2= subBasis.find(state.GetId());
	  	       LeftBasisIt it3=totalBasis.lbasis.find(LeftId(tpState));
	  	       size_t newStateNr= Position(*it2)*totalBasis.lbasis.dim +Position(*it3);
	  	              if(signControl%2==0)
   	  	    	 {
	   		   op.coeffRef(newStateNr, Position(tpState))-= var;}
   	   	       else
   	   	    	 { op.coeffRef(newStateNr, Position(tpState))+= var;}

	   	     }
 	     	     }

   	  }

  return op;
  }
 template<class TotalBasis, class SubBasis >
 Mat EKinOperatorRPH(const TotalBasis& totalBasis, const SubBasis& subBasis, double var=1. , const bool& PB=true)
     {

       using LeftBasisIt= typename TotalBasis::LeftBasisIt;
       using RightBasisIt= typename TotalBasis::RightBasisIt;

       using Lattice=typename SubBasis::Lattice;

   size_t dim=totalBasis.dim;
    size_t sites=totalBasis.sites;
    Mat op(dim, dim);
      op.setZero();

	  for( auto& tpState : totalBasis)
	    {
	      	    for(size_t i=0; i<Operators::Length( sites, PB); i++)
                {
	       RightBasisIt it2=subBasis.find(RightId(tpState));

	       Lattice state=GetLattice(*it2);
	       size_t j=Operators::NextWithBC(i, sites, PB);
	       size_t particleNumber1=subBasis.particlesAt(RightId(tpState), i);
	       size_t particleNumber2=subBasis.particlesAt(RightId(tpState), j);

  	     if( particleNumber1==0 or particleNumber2==subBasis.maxParticles )
  	       {
  	       }
   	   	 else{
		      Lattice temp=state;
		      state.setPartNr(j, temp[j]+1);
 state.setPartNr(i, temp[i]-1);

 it2= subBasis.find(state.GetId());
 LeftBasisIt it3=totalBasis.lbasis.find(LeftId(tpState));
 size_t newStateNr= Position(*it2)*totalBasis.lbasis.dim +Position(*it3);
// 	  	              if(signControl%2==0)
//    	  	    	 {
	   		   op.coeffRef(newStateNr, Position(tpState))+= var*(std::sqrt(state[i]+1))*(std::sqrt(state[j]));
			   op.coeffRef( Position(tpState), newStateNr)+= var*(std::sqrt(state[i]+1))*(std::sqrt(state[j]));
}
  	     	     }

   	  }

  return op;
  }
  template<typename TotalBasis, typename Basis>
    Mat CalculateCouplungOperator( const TotalBasis& totalBasis, const Basis& subBasis,   const double u=1.)
    {

    using LeftLattice=typename TotalBasis::LeftLattice;
    using RightLattice=typename TotalBasis::RightLattice;
    size_t dim=totalBasis.dim;
    Mat op(dim, dim);
    op.setZero();
    size_t sites=totalBasis.sites;

      for(const auto& tpState : totalBasis)
  	{
auto it21=totalBasis.lbasis.find(LeftId(tpState));
	  LeftLattice lState=GetLattice(*it21);
	  auto it22=totalBasis.rbasis.find(RightId(tpState));
	  RightLattice rState=GetLattice(*it22);
	  // RightLattice rState=GetLattice(RightId(tpState));
  	  for(int i=0; i<sites; i++)
	    {
	      op.coeffRef(Position(tpState), Position(tpState))+= (u*static_cast<double>(lState[i])*static_cast<double>(rState[i]));
	  }

  	}
      return op;

       }
  // Holstein model obc, for now this is n_0(n)
   template<typename TotalBasis, typename Basis>
   Mat BosonCOperator_1(const TotalBasis& totalBasis, Basis& subBasis, double var=1. , size_t i=0, const bool& PB=true)
  {

      using LeftBasisIt= typename TotalBasis::LeftBasisIt;
      using LeftLattice=  typename    TotalBasis::LeftLattice;
      using RightBasisIt= typename TotalBasis::RightBasisIt;
      using RightLattice=typename TotalBasis::RightLattice;

   size_t dim=totalBasis.dim;
    size_t sites=totalBasis.sites;
    Mat op(dim, dim);
    //      op.setZero();
        	for(const auto& tpState : totalBasis)
  	  {

		  	    RightBasisIt it2=subBasis.find(RightId(tpState));

	    	    LeftBasisIt it33=totalBasis.lbasis.find(LeftId(tpState));
	     LeftLattice stateEl(GetLattice(*it33));

  	    size_t particleNumber=subBasis.particlesAt(RightId(tpState), i);

  	     if( particleNumber==subBasis.maxParticles)
  	       {
  	       }
  	     else{

  	       LeftBasisIt it3=totalBasis.lbasis.find(LeftId(tpState));
  	       RightLattice state(GetLattice(*it2));

			     state.setPartNr(i, particleNumber+1);

  	       it2= subBasis.find(state.GetId());

  	       size_t newStateNr= Position(*it2) *totalBasis.lbasis.dim +Position(*it3) ;

  	       op.coeffRef(newStateNr, Position(tpState))+= ValType{var*(std::sqrt(state[i]))};

  		    }

  	  }
  		return op;
  }

 template<typename TotalBasis, typename Basis>
 Mat BosonDOperator_1(const TotalBasis& totalBasis, Basis& subBasis,   double var=1. , size_t i=0,  const bool& PB=true)
  {

      using LeftBasisIt= typename TotalBasis::LeftBasisIt;
      using LeftLattice=  typename    TotalBasis::LeftLattice;
      using RightBasisIt= typename TotalBasis::RightBasisIt;
      using RightLattice=typename TotalBasis::RightLattice;

   size_t dim=totalBasis.dim;
    size_t sites=totalBasis.sites;
    Mat op(dim, dim);
    //      op.setZero();
  	for(const auto& tpState : totalBasis)
  	  {

		RightBasisIt it2=subBasis.find(RightId(tpState));

	    	    LeftBasisIt it33=totalBasis.lbasis.find(LeftId(tpState));
	     LeftLattice stateEl(GetLattice(*it33));

	    size_t particleNumber=subBasis.particlesAt(RightId(tpState), i);

  	     if( particleNumber==0)
  	       {
  	       }
  	     else{

  	       LeftBasisIt it3=totalBasis.lbasis.find(LeftId(tpState));
  	       RightLattice state(GetLattice(*it2));

	       state.setPartNr(i, particleNumber-1);

  	       it2= subBasis.find(state.GetId());

  	       size_t newStateNr= Position(*it2)*totalBasis.lbasis.dim +Position(*it3);

  	       op.coeffRef(newStateNr, Position(tpState))+= ValType{var*(std::sqrt(state[i]+1))};

  		    }

	  }
  		return op;
   }

   template<typename TotalBasis, typename Basis>
  Mat NBosonCOperator(const TotalBasis& totalBasis, Basis& subBasis, double var=1. , const bool& PB=true)
  {

      using LeftBasisIt= typename TotalBasis::LeftBasisIt;
      using LeftLattice=  typename    TotalBasis::LeftLattice;
      using RightBasisIt= typename TotalBasis::RightBasisIt;
      using RightLattice=typename TotalBasis::RightLattice;

   size_t dim=totalBasis.dim;
    size_t sites=totalBasis.sites;
    Mat op(dim, dim);
    //      op.setZero();
        	for(const auto& tpState : totalBasis)
  	  {

	    for(size_t i=0; i<sites; i++)
	      {

		  	    RightBasisIt it2=subBasis.find(RightId(tpState));

	    	    LeftBasisIt it33=totalBasis.lbasis.find(LeftId(tpState));
	     LeftLattice stateEl(GetLattice(*it33));

	     if(stateEl[i]==0){

	       continue;}
  	    size_t particleNumber=subBasis.particlesAt(RightId(tpState), i);

  	     if( particleNumber==subBasis.maxParticles)
  	       {
  	       }
  	     else{

  	       LeftBasisIt it3=totalBasis.lbasis.find(LeftId(tpState));
  	       RightLattice state(GetLattice(*it2));

	       //	       std::cout<<" at site " << i << " we  started with e "<< stateEl<< "and ph  started with satte " << state << std::endl;
			     state.setPartNr(i, particleNumber+1);

			     //			      std::cout<< "and got " << state << std::endl;

  	       it2= subBasis.find(state.GetId());

  	       size_t newStateNr= Position(*it2) *totalBasis.lbasis.dim +Position(*it3) ;

  	       op.coeffRef(newStateNr, Position(tpState))+= ValType{var*(std::sqrt(state[i]))};

  		    }
	      }
  	  }
  		return op;
  }

 template<typename TotalBasis, typename Basis>
 Mat NBosonDOperator(const TotalBasis& totalBasis, Basis& subBasis,   double var=1. ,  const bool& PB=true)
  {

      using LeftBasisIt= typename TotalBasis::LeftBasisIt;
      using LeftLattice=  typename    TotalBasis::LeftLattice;
      using RightBasisIt= typename TotalBasis::RightBasisIt;
      using RightLattice=typename TotalBasis::RightLattice;

   size_t dim=totalBasis.dim;
    size_t sites=totalBasis.sites;
    Mat op(dim, dim);
    //      op.setZero();
  	for(const auto& tpState : totalBasis)
  	  {

	    for(size_t i=0; i<sites; i++)
	      {
		RightBasisIt it2=subBasis.find(RightId(tpState));
		//		std::cout<< "at "<<Position(tpState) <<" havinfg state BD " << i <<std::endl;

	    	    LeftBasisIt it33=totalBasis.lbasis.find(LeftId(tpState));
	     LeftLattice stateEl(GetLattice(*it33));
	     //std::cout<< "acted on e state " << stateEl<< std::endl;
	     if(stateEl[i]==0){
	       //	       std::cout<< " evaluateing "<<stateEl << "at " << i << " gave " <<stateEl[i] <<std::endl;
										 continue;}
	     //std::cout<< "and got " << satte << std::endl;
	    size_t particleNumber=subBasis.particlesAt(RightId(tpState), i);

  	     if( particleNumber==0)
  	       {
  	       }
  	     else{

  	       LeftBasisIt it3=totalBasis.lbasis.find(LeftId(tpState));
  	       RightLattice state(GetLattice(*it2));
	       //std::cout<<" at site " << i << " we  started with e "<< stateEl<< "and ph  started with satte " << state << std::endl;

	       state.setPartNr(i, particleNumber-1);
	       //      std::cout<< "and got " << state << std::endl;
  	       it2= subBasis.find(state.GetId());

  	       size_t newStateNr= Position(*it2)*totalBasis.lbasis.dim +Position(*it3);

  	       op.coeffRef(newStateNr, Position(tpState))+= ValType{var*(std::sqrt(state[i]+1))};

  		    }
  	  }
	  }
  		return op;
   }

   template<class TotalBasis, class SubBasis >
  Mat EKinOperatorR(const TotalBasis& totalBasis, const SubBasis& subBasis, double var=1. , const bool& PB=true)
     {

	      using LeftBasisIt= typename TotalBasis::LeftBasisIt;
     using RightBasisIt= typename TotalBasis::RightBasisIt;

using Lattice=typename SubBasis::Lattice;

   size_t dim=totalBasis.dim;
    size_t sites=totalBasis.sites;
    Mat op(dim, dim);
      op.setZero();

	   for( auto& tpState : totalBasis)
	     {

	       	    for(size_t i=0; i<Operators::Length( sites, PB); i++)
                 {

	       RightBasisIt it2=subBasis.find(RightId(tpState));
	       LeftBasisIt iL=totalBasis.lbasis.find(LeftId(tpState));
	       Lattice state=GetLattice(*it2);
                     size_t j=Operators::NextWithBC(i, sites, PB);
		     //		   std::cout<< i << "  " << j <<std::endl;
	   	    		    if(state[i]== 0 or state[j]== totalBasis.rbasis.maxParticles)
   	       	     {
	  //

   	   	     }
    	   	    else{

		      Lattice temp=state;

		       state.setPartNr(j, temp[j]+1);
		      state.setPartNr(i, temp[i]-1);

// 		      //	  	     size_t signControl=CheckSign(temp, i, j);

		     it2= subBasis.find(state.GetId());

	  	       RightBasisIt it3=totalBasis.rbasis.find(state.id);

	  	       size_t newStateNr= Position(*it3)*totalBasis.lbasis.dim +Position(*iL);

		       op.coeffRef(newStateNr, Position(tpState))-= ValType{var}*std::sqrt(temp[j]+1)*std::sqrt(temp[i]);
//
		    }
				    	     it2=subBasis.find(RightId(tpState));
 iL=totalBasis.lbasis.find(LeftId(tpState));
	 state=GetLattice(*it2);
				    	   	    		    if(state[j]== 0 or state[i]== totalBasis.rbasis.maxParticles)
   	       	     {
	  //

   	   	     }
    	   	    else{

		      Lattice temp=state;

		       state.setPartNr(j, temp[j]-1);
		      state.setPartNr(i, temp[i]+1);

// 		      //	  	     size_t signControl=CheckSign(temp, i, j);

		     it2= subBasis.find(state.GetId());

	  	       RightBasisIt it3=totalBasis.rbasis.find(state.id);

	  	       size_t newStateNr= Position(*it3)*totalBasis.lbasis.dim +Position(*iL);

		       op.coeffRef(newStateNr, Position(tpState))-= ValType{var}*std::sqrt(temp[j])*std::sqrt(temp[i]+1);
//
		    }
	   	     }
 	      	     }

  return op;
  }
     template<class TotalBasis, class SubBasis >
  Mat EKinOperatorRNN(const TotalBasis& totalBasis, const SubBasis& subBasis, double var=1. , const bool& PB=true)
     {

	      using LeftBasisIt= typename TotalBasis::LeftBasisIt;
     using RightBasisIt= typename TotalBasis::RightBasisIt;

using Lattice=typename SubBasis::Lattice;

   size_t dim=totalBasis.dim;
    size_t sites=totalBasis.sites;
    Mat op(dim, dim);
      op.setZero();

	   for( auto& tpState : totalBasis)
	     {

	       	    for(size_t i=0; i<Operators::Length( sites, PB); i++)
                 {

	       RightBasisIt it2=subBasis.find(RightId(tpState));
	       LeftBasisIt iL=totalBasis.lbasis.find(LeftId(tpState));
	       Lattice state=GetLattice(*it2);
	       size_t j=(i+2)%sites;
	       //Operators::NextWithBC(i, sites, PB);
		     //		   std::cout<< i << "  " << j <<std::endl;
	   	    		    if(state[i]== 0 or state[j]== totalBasis.rbasis.maxParticles)
   	       	     {
	  //

   	   	     }
    	   	    else{

		      Lattice temp=state;

		       state.setPartNr(j, temp[j]+1);
		      state.setPartNr(i, temp[i]-1);

// 		      //	  	     size_t signControl=CheckSign(temp, i, j);

		     it2= subBasis.find(state.GetId());

	  	       RightBasisIt it3=totalBasis.rbasis.find(state.id);

	  	       size_t newStateNr= Position(*it3)*totalBasis.lbasis.dim +Position(*iL);

		       op.coeffRef(newStateNr, Position(tpState))-= ValType{var}*std::sqrt(temp[j]+1)*std::sqrt(temp[i]);
//
		    }
				    	     it2=subBasis.find(RightId(tpState));
 iL=totalBasis.lbasis.find(LeftId(tpState));
	 state=GetLattice(*it2);
				    	   	    		    if(state[j]== 0 or state[i]== totalBasis.rbasis.maxParticles)
   	       	     {
	  //

   	   	     }
    	   	    else{

		      Lattice temp=state;

		       state.setPartNr(j, temp[j]-1);
		      state.setPartNr(i, temp[i]+1);

// 		      //	  	     size_t signControl=CheckSign(temp, i, j);

		     it2= subBasis.find(state.GetId());

	  	       RightBasisIt it3=totalBasis.rbasis.find(state.id);

	  	       size_t newStateNr= Position(*it3)*totalBasis.lbasis.dim +Position(*iL);

		       op.coeffRef(newStateNr, Position(tpState))-= ValType{var}*std::sqrt(temp[j])*std::sqrt(temp[i]+1);
//
		    }
	   	     }
 	      	     }

  return op;
  }
   template<class TotalBasis, class SubBasis >
   Mat eMOMOperator(const TotalBasis& totalBasis, const SubBasis& subBasis, double var=1. , int k=0, const bool& PB=true)
     {
  using Many_Body::pi;
  using Many_Body::im;

	      using LeftBasisIt= typename TotalBasis::LeftBasisIt;
     using RightBasisIt= typename TotalBasis::RightBasisIt;

using Lattice=typename SubBasis::Lattice;

   size_t dim=totalBasis.dim;
    size_t sites=totalBasis.sites;
    Mat op(dim, dim);
      op.setZero();
 auto K=2*pi/sites;

	   for( auto& tpState : totalBasis)
	     {

	       	    for(size_t i=0; i<sites; i++)
                 {

                       for(size_t j=0; j<sites; j++)
                 {
		       RightBasisIt it2=totalBasis.rbasis.find(RightId(tpState));
	       LeftBasisIt iL=totalBasis.lbasis.find(LeftId(tpState));
	       Lattice state=GetLattice(*iL);
	       if(j==i)
		 {
		   	      Lattice temp=state;
		     if(state[i]!= 0)
		       {
		       op.coeffRef(Position(tpState), Position(tpState))+= ValType{var}/sites;
		       }
		 }
	       else{
		     //		   std::cout<< i << "  " << j <<std::endl;
	   	    		    if(state[i]== 0)
   	       	     {
	  //

   	   	     }
     	   	    else{

		      Lattice temp=state;

		       state.setPartNr(j, 1);
		      state.setPartNr(i, 0);

// 		      //	  	     size_t signControl=CheckSign(temp, i, j);

		      //iL= subBasis.find(state.GetId());

	  	       LeftBasisIt it3=totalBasis.lbasis.find(state.id);

	  	       size_t newStateNr= Position(*it2)*totalBasis.lbasis.dim +Position(*it3);

		       std::complex<double> value=exp(-im*static_cast<std::complex<double>>(K*(j-i)));
		       op.coeffRef(newStateNr, Position(tpState))+= ValType{var}/sites;
		       //*std::sqrt(temp[j]+1)*std::sqrt(temp[i]);
//
 		    }
	       }

 	   	     }
  	      	     }

	     }

  return op;
  }

     template<class TotalBasis, class SubBasis >
   Mat phMOMOperator(const TotalBasis& totalBasis, const SubBasis& subBasis, double var=1. , int k=0, const bool& PB=true)
     {

	      using LeftBasisIt= typename TotalBasis::LeftBasisIt;
     using RightBasisIt= typename TotalBasis::RightBasisIt;

using Lattice=typename SubBasis::Lattice;

   size_t dim=totalBasis.dim;
    size_t sites=totalBasis.sites;
    Mat op(dim, dim);
      op.setZero();

	   for( auto& tpState : totalBasis)
	     {

	       	    for(size_t i=0; i<sites; i++)
                 {

                       for(size_t j=0; j<sites; j++)
                 {
		       RightBasisIt it2=totalBasis.rbasis.find(RightId(tpState));
	       LeftBasisIt iL=totalBasis.lbasis.find(LeftId(tpState));
	       Lattice state=GetLattice(*it2);
	       if(j==i)
		 {
		   	      Lattice temp=state;
		     if(state[i]!= 0)
		       {
		       op.coeffRef(Position(tpState), Position(tpState))+=state[i]*ValType{var}/sites;
		       }
		 }
	       else{
		     //		   std::cout<< i << "  " << j <<std::endl;
	   	    		    if(state[i]== 0 or state[j]== totalBasis.rbasis.maxParticles)
   	       	     {
	  //

   	   	     }
     	   	    else{

		      Lattice temp=state;

		       state.setPartNr(j, temp[j]+1);
		       state.setPartNr(i, temp[i]-1);

// 		      //	  	     size_t signControl=CheckSign(temp, i, j);

		      //iL= subBasis.find(state.GetId());

	  	       RightBasisIt it3=totalBasis.rbasis.find(state.id);

	  	       size_t newStateNr= Position(*it3)*totalBasis.lbasis.dim +Position(*iL);

		       op.coeffRef(newStateNr, Position(tpState))+= std::sqrt(temp[j]+1)*std::sqrt(temp[i])*ValType{var}/sites;
		       //*std::sqrt(temp[j]+1)*std::sqrt(temp[i]);
//
 		    }
	       }

 	   	     }
  	      	     }

	     }

  return op;
  }
template<class TotalBasis>
Mat HolsteinHam(const TotalBasis& totalBasis,  const double t0=1., const double omega=1., const double gamma=1., const bool& PB=true)

  {

       Mat EK=Operators::EKinOperatorL(totalBasis, totalBasis.lbasis,  t0,PB);
       Mat Ebdag=Operators::NBosonCOperator(totalBasis, totalBasis.rbasis, gamma, PB);
       Mat Eb=Operators::NBosonDOperator(totalBasis, totalBasis.rbasis, gamma, PB);
       Mat Eph=Operators::NumberOperator(totalBasis, totalBasis.rbasis, omega,  PB);
      return EK+Eb+Eph+Ebdag;
  }
  // template<class TotalBasis, class SubBasis>
 //  Mat NumberOperatore(const TotalBasis& totalBasis, const SubBasis& subBasis, const double omega=1., const bool& PB=true)
 // {    using BasisIt= typename TotalBasis::BasisIt;
 //    using Const_BasisIt= typename TotalBasis::Const_BasisIt;
 //    size_t dim=totalBasis.dim;
 //    size_t sites=totalBasis.sites;
 //    Mat op(dim, dim);
 //   op.setZero();

 //      for(const auto& tpState : totalBasis)
 // 	{

 // 	  for(size_t i=0; i<Operators::Length( sites, PB); i++)
 // 	    {

 // 	      op.coeffRef(Position(tpState), Position(tpState))+=ValType(omega*subBasis.particlesAt(LeftId(tpState), i));

 // 	  }
 // 	}
      // return op;

      //  }
}


//==================================================================================================
/*!

  Copyright 2015 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)
*/
//==================================================================================================
#include <boost/simd/function/scalar/inbtrue.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>

STF_CASE_TPL (" inbtrue real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::inbtrue;

  // specific values tests
  STF_EQUAL(inbtrue(bs::Inf<T>()) , 1u);
  STF_EQUAL(inbtrue(bs::Minf<T>()), 1u);
  STF_EQUAL(inbtrue(bs::Mone<T>()), 1u);
  STF_EQUAL(inbtrue(bs::Nan<T>()) , 1u);
  STF_EQUAL(inbtrue(bs::One<T>()) , 1u);
  STF_EQUAL(inbtrue(bs::Zero<T>()), 0u);
} // end of test for floating_

STF_CASE_TPL (" inbtrue integer",  STF_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::inbtrue;

  // specific values tests
  STF_EQUAL(inbtrue(bs::Mone<T>()), 1u);
  STF_EQUAL(inbtrue(bs::One<T>()) , 1u);
  STF_EQUAL(inbtrue(bs::Zero<T>()), 0u);
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2016
// Mehdi Goli    Codeplay Software Ltd.
// Ralph Potter  Codeplay Software Ltd.
// Luke Iwanski  Codeplay Software Ltd.
// Contact: <eigen@codeplay.com>
// Benoit Steiner <benoit.steiner.goog@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#define EIGEN_TEST_NO_LONGDOUBLE
#define EIGEN_TEST_NO_COMPLEX
#define EIGEN_TEST_FUNC cxx11_tensor_patch_sycl
#define EIGEN_DEFAULT_DENSE_INDEX_TYPE int64_t
#define EIGEN_USE_SYCL

#include "main.h"

#include <Eigen/CXX11/Tensor>

using Eigen::Tensor;

template <typename DataType, int DataLayout, typename IndexType>
static void test_simple_patch_sycl(const Eigen::SyclDevice& sycl_device){

  IndexType sizeDim1 = 2;
  IndexType sizeDim2 = 3;
  IndexType sizeDim3 = 5;
  IndexType sizeDim4 = 7;
  array<IndexType, 4> tensorRange = {{sizeDim1, sizeDim2, sizeDim3, sizeDim4}};
  array<IndexType, 5> patchTensorRange;
  if (DataLayout == ColMajor) {
   patchTensorRange = {{1, 1, 1, 1, sizeDim1*sizeDim2*sizeDim3*sizeDim4}};
  }else{
     patchTensorRange = {{sizeDim1*sizeDim2*sizeDim3*sizeDim4,1, 1, 1, 1}};
  }

  Tensor<DataType, 4, DataLayout,IndexType> tensor(tensorRange);
  Tensor<DataType, 5, DataLayout,IndexType> no_patch(patchTensorRange);

  tensor.setRandom();

  array<ptrdiff_t, 4> patch_dims;
  patch_dims[0] = 1;
  patch_dims[1] = 1;
  patch_dims[2] = 1;
  patch_dims[3] = 1;

  const size_t tensorBuffSize =tensor.size()*sizeof(DataType);
  size_t patchTensorBuffSize =no_patch.size()*sizeof(DataType);
  DataType* gpu_data_tensor  = static_cast<DataType*>(sycl_device.allocate(tensorBuffSize));
  DataType* gpu_data_no_patch  = static_cast<DataType*>(sycl_device.allocate(patchTensorBuffSize));

  TensorMap<Tensor<DataType, 4, DataLayout,IndexType>> gpu_tensor(gpu_data_tensor, tensorRange);
  TensorMap<Tensor<DataType, 5, DataLayout,IndexType>> gpu_no_patch(gpu_data_no_patch, patchTensorRange);

  sycl_device.memcpyHostToDevice(gpu_data_tensor, tensor.data(), tensorBuffSize);
  gpu_no_patch.device(sycl_device)=gpu_tensor.extract_patches(patch_dims);
  sycl_device.memcpyDeviceToHost(no_patch.data(), gpu_data_no_patch, patchTensorBuffSize);

  if (DataLayout == ColMajor) {
    VERIFY_IS_EQUAL(no_patch.dimension(0), 1);
    VERIFY_IS_EQUAL(no_patch.dimension(1), 1);
    VERIFY_IS_EQUAL(no_patch.dimension(2), 1);
    VERIFY_IS_EQUAL(no_patch.dimension(3), 1);
    VERIFY_IS_EQUAL(no_patch.dimension(4), tensor.size());
  } else {
    VERIFY_IS_EQUAL(no_patch.dimension(0), tensor.size());
    VERIFY_IS_EQUAL(no_patch.dimension(1), 1);
    VERIFY_IS_EQUAL(no_patch.dimension(2), 1);
    VERIFY_IS_EQUAL(no_patch.dimension(3), 1);
    VERIFY_IS_EQUAL(no_patch.dimension(4), 1);
  }

  for (int i = 0; i < tensor.size(); ++i) {
    VERIFY_IS_EQUAL(tensor.data()[i], no_patch.data()[i]);
  }

  patch_dims[0] = 2;
  patch_dims[1] = 3;
  patch_dims[2] = 5;
  patch_dims[3] = 7;

  if (DataLayout == ColMajor) {
   patchTensorRange = {{sizeDim1,sizeDim2,sizeDim3,sizeDim4,1}};
  }else{
     patchTensorRange = {{1,sizeDim1,sizeDim2,sizeDim3,sizeDim4}};
  }
  Tensor<DataType, 5, DataLayout,IndexType> single_patch(patchTensorRange);
  patchTensorBuffSize =single_patch.size()*sizeof(DataType);
  DataType* gpu_data_single_patch  = static_cast<DataType*>(sycl_device.allocate(patchTensorBuffSize));
  TensorMap<Tensor<DataType, 5, DataLayout,IndexType>> gpu_single_patch(gpu_data_single_patch, patchTensorRange);

  gpu_single_patch.device(sycl_device)=gpu_tensor.extract_patches(patch_dims);
  sycl_device.memcpyDeviceToHost(single_patch.data(), gpu_data_single_patch, patchTensorBuffSize);

  if (DataLayout == ColMajor) {
    VERIFY_IS_EQUAL(single_patch.dimension(0), 2);
    VERIFY_IS_EQUAL(single_patch.dimension(1), 3);
    VERIFY_IS_EQUAL(single_patch.dimension(2), 5);
    VERIFY_IS_EQUAL(single_patch.dimension(3), 7);
    VERIFY_IS_EQUAL(single_patch.dimension(4), 1);
  } else {
    VERIFY_IS_EQUAL(single_patch.dimension(0), 1);
    VERIFY_IS_EQUAL(single_patch.dimension(1), 2);
    VERIFY_IS_EQUAL(single_patch.dimension(2), 3);
    VERIFY_IS_EQUAL(single_patch.dimension(3), 5);
    VERIFY_IS_EQUAL(single_patch.dimension(4), 7);
  }

  for (int i = 0; i < tensor.size(); ++i) {
    VERIFY_IS_EQUAL(tensor.data()[i], single_patch.data()[i]);
  }
  patch_dims[0] = 1;
  patch_dims[1] = 2;
  patch_dims[2] = 2;
  patch_dims[3] = 1;

  if (DataLayout == ColMajor) {
   patchTensorRange = {{1,2,2,1,2*2*4*7}};
  }else{
     patchTensorRange = {{2*2*4*7, 1, 2,2,1}};
  }
  Tensor<DataType, 5, DataLayout,IndexType> twod_patch(patchTensorRange);
  patchTensorBuffSize =twod_patch.size()*sizeof(DataType);
  DataType* gpu_data_twod_patch  = static_cast<DataType*>(sycl_device.allocate(patchTensorBuffSize));
  TensorMap<Tensor<DataType, 5, DataLayout,IndexType>> gpu_twod_patch(gpu_data_twod_patch, patchTensorRange);

  gpu_twod_patch.device(sycl_device)=gpu_tensor.extract_patches(patch_dims);
  sycl_device.memcpyDeviceToHost(twod_patch.data(), gpu_data_twod_patch, patchTensorBuffSize);

  if (DataLayout == ColMajor) {
    VERIFY_IS_EQUAL(twod_patch.dimension(0), 1);
    VERIFY_IS_EQUAL(twod_patch.dimension(1), 2);
    VERIFY_IS_EQUAL(twod_patch.dimension(2), 2);
    VERIFY_IS_EQUAL(twod_patch.dimension(3), 1);
    VERIFY_IS_EQUAL(twod_patch.dimension(4), 2*2*4*7);
  } else {
    VERIFY_IS_EQUAL(twod_patch.dimension(0), 2*2*4*7);
    VERIFY_IS_EQUAL(twod_patch.dimension(1), 1);
    VERIFY_IS_EQUAL(twod_patch.dimension(2), 2);
    VERIFY_IS_EQUAL(twod_patch.dimension(3), 2);
    VERIFY_IS_EQUAL(twod_patch.dimension(4), 1);
  }

  for (int i = 0; i < 2; ++i) {
    for (int j = 0; j < 2; ++j) {
      for (int k = 0; k < 4; ++k) {
        for (int l = 0; l < 7; ++l) {
          int patch_loc;
          if (DataLayout == ColMajor) {
            patch_loc = i + 2 * (j + 2 * (k + 4 * l));
          } else {
            patch_loc = l + 7 * (k + 4 * (j + 2 * i));
          }
          for (int x = 0; x < 2; ++x) {
            for (int y = 0; y < 2; ++y) {
              if (DataLayout == ColMajor) {
                VERIFY_IS_EQUAL(tensor(i,j+x,k+y,l), twod_patch(0,x,y,0,patch_loc));
              } else {
                VERIFY_IS_EQUAL(tensor(i,j+x,k+y,l), twod_patch(patch_loc,0,x,y,0));
              }
            }
          }
        }
      }
    }
  }

  patch_dims[0] = 1;
  patch_dims[1] = 2;
  patch_dims[2] = 3;
  patch_dims[3] = 5;

  if (DataLayout == ColMajor) {
   patchTensorRange = {{1,2,3,5,2*2*3*3}};
  }else{
     patchTensorRange = {{2*2*3*3, 1, 2,3,5}};
  }
  Tensor<DataType, 5, DataLayout,IndexType> threed_patch(patchTensorRange);
  patchTensorBuffSize =threed_patch.size()*sizeof(DataType);
  DataType* gpu_data_threed_patch  = static_cast<DataType*>(sycl_device.allocate(patchTensorBuffSize));
  TensorMap<Tensor<DataType, 5, DataLayout,IndexType>> gpu_threed_patch(gpu_data_threed_patch, patchTensorRange);

  gpu_threed_patch.device(sycl_device)=gpu_tensor.extract_patches(patch_dims);
  sycl_device.memcpyDeviceToHost(threed_patch.data(), gpu_data_threed_patch, patchTensorBuffSize);

  if (DataLayout == ColMajor) {
    VERIFY_IS_EQUAL(threed_patch.dimension(0), 1);
    VERIFY_IS_EQUAL(threed_patch.dimension(1), 2);
    VERIFY_IS_EQUAL(threed_patch.dimension(2), 3);
    VERIFY_IS_EQUAL(threed_patch.dimension(3), 5);
    VERIFY_IS_EQUAL(threed_patch.dimension(4), 2*2*3*3);
  } else {
    VERIFY_IS_EQUAL(threed_patch.dimension(0), 2*2*3*3);
    VERIFY_IS_EQUAL(threed_patch.dimension(1), 1);
    VERIFY_IS_EQUAL(threed_patch.dimension(2), 2);
    VERIFY_IS_EQUAL(threed_patch.dimension(3), 3);
    VERIFY_IS_EQUAL(threed_patch.dimension(4), 5);
  }

  for (int i = 0; i < 2; ++i) {
    for (int j = 0; j < 2; ++j) {
      for (int k = 0; k < 3; ++k) {
        for (int l = 0; l < 3; ++l) {
          int patch_loc;
          if (DataLayout == ColMajor) {
            patch_loc = i + 2 * (j + 2 * (k + 3 * l));
          } else {
            patch_loc = l + 3 * (k + 3 * (j + 2 * i));
          }
          for (int x = 0; x < 2; ++x) {
            for (int y = 0; y < 3; ++y) {
              for (int z = 0; z < 5; ++z) {
                if (DataLayout == ColMajor) {
                  VERIFY_IS_EQUAL(tensor(i,j+x,k+y,l+z), threed_patch(0,x,y,z,patch_loc));
                } else {
                  VERIFY_IS_EQUAL(tensor(i,j+x,k+y,l+z), threed_patch(patch_loc,0,x,y,z));
                }
              }
            }
          }
        }
      }
    }
  }
  sycl_device.deallocate(gpu_data_tensor);
  sycl_device.deallocate(gpu_data_no_patch);
  sycl_device.deallocate(gpu_data_single_patch);
  sycl_device.deallocate(gpu_data_twod_patch);
  sycl_device.deallocate(gpu_data_threed_patch);
}

template<typename DataType, typename dev_Selector> void sycl_tensor_patch_test_per_device(dev_Selector s){
  QueueInterface queueInterface(s);
  auto sycl_device = Eigen::SyclDevice(&queueInterface);
  test_simple_patch_sycl<DataType, RowMajor, int64_t>(sycl_device);
  test_simple_patch_sycl<DataType, ColMajor, int64_t>(sycl_device);
}
void test_cxx11_tensor_patch_sycl()
{
  for (const auto& device :Eigen::get_sycl_supported_devices()) {
    CALL_SUBTEST(sycl_tensor_patch_test_per_device<float>(device));
  }
}


#ifndef _PATHFINDINGMAPPERTURBATORUTILS_UTILS__
#define _PATHFINDINGMAPPERTURBATORUTILS_UTILS__

#include <cpp-utils/adjacentGraph.hpp>
#include <boost/filesystem.hpp>
#include <pathfinding-utils/MovingAIGridMapReader.hpp>
#include <pathfinding-utils/GridMapGraphConverter.hpp>
#include <pathfinding-utils/GridMap.hpp>
#include <pathfinding-utils/GridBranching.hpp>

namespace pathfinding::map_perturbator::utils {

	using namespace cpp_utils;
	using namespace cpp_utils::graphs;
	using namespace pathfinding::maps;

	/**
	 * @brief load a map
	 *
	 * by default we load a moving AI map with the following weights:
	 *
	 * @li . is 1000
	 * @li T is 1500
	 * @li S is 2000
	 * @li W is 2500
	 * @li @ is invalicable
	 *
	 * The map build is 8-connected
	 *
	 * @param mapFilename  filename of the map to load
	 * @return AdjacentGraph<std::string, xyLoc, cost_t> the graph representing the map
	 */
	static AdjacentGraph<std::string, xyLoc, cost_t> loadMap(const boost::filesystem::path& mapFilename) {
		MovingAIGridMapReader reader{
			'.', 1000, color_t::WHITE,
			'T', 1500, color_t::GREEN,
			'S', 2000, color_t::CYAN,
			'W', 2500, color_t::BLUE,
			'@', cost_t::INFTY, color_t::BLACK
		};
		pathfinding::maps::GridMap gridMap = reader.load(mapFilename);
		GridMapGraphConverter converter{GridBranching::EIGHT_CONNECTED};
		return AdjacentGraph<std::string, xyLoc, cost_t>(*converter.toGraph(gridMap));
	}

}

#endif


//Link to Boost
#define BOOST_TEST_DYN_LINK

//VERY IMPORTANT - include this last
//#include <boost/test/included/unit_test.hpp>
#include <boost/test/unit_test.hpp>

#include "test.h"
#include "../BetheRootCache.h"

// test suite
BOOST_FIXTURE_TEST_SUITE(BetheRootCache_suite, SimpleTestFixture, * utf::label("BetheRootCache"))

BOOST_AUTO_TEST_CASE(Demo1)
{
    BetheRootCache cache(6, 2);
    for (int i = 0; i < cache.numberOfRoots(); i++) {
        std::cout << i << ": " << cache.getRoot(i) << ", momentum=" << cache.momentum(i) << std::endl;
    }
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright Abel Sinkovics (abel@sinkovics.hu) 2013.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <mpllibs/metamonad/eval_syntax.hpp>
#include <mpllibs/metamonad/syntax.hpp>
#include <mpllibs/metamonad/returns.hpp>

#include <boost/test/unit_test.hpp>

#include <boost/mpl/plus.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/assert.hpp>

#include "common.hpp"

BOOST_AUTO_TEST_CASE(test_eval_syntax)
{
  using mpllibs::metamonad::syntax;
  using mpllibs::metamonad::eval_syntax;
  using mpllibs::metamonad::returns;

  using boost::mpl::equal_to;
  using boost::mpl::plus;

  // test_evaluating_a_syntax
  BOOST_MPL_ASSERT((
    equal_to<int13, eval_syntax<syntax<plus<int11, int2> > >::type>
  ));

  // test_laziness_of_eval_syntax
  BOOST_MPL_ASSERT((
    equal_to<int13, eval_syntax<returns<syntax<plus<int11, int2> > > >::type>
  ));
}


// Copyright (c) 2016-2021 Fernando Pelliccioni.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

// #include <boost/hana/product.hpp>
// #include <boost/hana/tuple.hpp>

#include <cstdlib>
#include <iostream>
#include <random>
#include <tuple>

#include <tao/benchmark/instrumented.hpp>
#include <tao/algorithm/selection/selection_i_1_3.hpp>

#define Integer typename

template <Integer I = int, I From = std::numeric_limits<I>::min(), I To = std::numeric_limits<I>::max()>
struct random_int_generator {
	using value_type = I;
	using dis_t = std::uniform_int_distribution<value_type>;
	static constexpr value_type from = From;
	static constexpr value_type to = To;

	random_int_generator()
		: mt{ rd() }
		, dis{ from, to }  // closed range [1, 1000]
	{}

	random_int_generator(random_int_generator const&) = default;

	auto operator()() {
		return dis(mt);
	}

	std::random_device rd;
	std::mt19937 mt;
	dis_t dis;
}; // Models: RandomIntGenerator

int main() {
    using namespace tao::algorithm;

    size_t const max = 100000;

    instrumented<int>::initialize(0);
    random_int_generator<int> gen;

    // instrumented<int> a(1);
    // instrumented<int> b(2);
    // instrumented<int> c(3);

    // select_1_3(a, b, c);

    for (size_t i = 0; i < max; ++i) {
        instrumented<int> a(gen());
        instrumented<int> b(gen());
        instrumented<int> c(gen());

        select_1_3(a, b, c);
    }

    double* count_p = instrumented<int>::counts;
    std::cout << "comparisons: " << count_p[instrumented_base::comparison] << std::endl;

    return 0;
}


#include <sys/stat.h> // stat
#include <unistd.h> // getopt
#include <chrono> // high_resolution_clock
#include <iostream> // cout
#include <cstdio> // printf
#include <fstream> // ifstream, ofstream
#include <sstream> // stringstream
#include <ratio>  // milli

#include <boost/graph/johnson_all_pairs_shortest.hpp> // seq algorithm, distance_map

#ifdef _OPENMP
#include "omp.h" // omp_set_num_threads
#endif

#include "util.hpp"
#include "johnson.hpp"
#include "floyd_warshall.hpp"

void bench_floyd_warshall(int interations, unsigned long seed, int block_size, bool check_correctness);
void bench_johnson(int iterations, unsigned long seed, bool check_correctness);

int main(int argc, char* argv[]) {
  // parameter defaults
  unsigned long seed = 0;
  int n = 1024;
  double p = 0.5;
  bool use_floyd_warshall = true;
  bool benchmark = false;
  bool check_correctness = false;
  int block_size = 32;
  int thread_count = 1;

  extern char* optarg;
  int opt;
  while ((opt = getopt(argc, argv, "ha:n:p:s:bd:ct:")) != -1) {
    switch (opt) {
      case 'h':
      case '?': // illegal command
      case ':': // forgot command's argument
        print_usage();
        return 0;

      case 'a':
        if (optarg[0] == 'j') {
          use_floyd_warshall = false;
        } else if (optarg[0] != 'f') {
          std::cerr << "Illegal algorithm argument, must be f or j\n";
          return -1;
        }
        break;

      case 'p':
        p = std::stod(optarg);
        break;

      case 's':
        seed = std::stoul(optarg);
        break;

      case 'b':
        benchmark = true;
        break;

      case 'n':
        n = std::stoi(optarg);
        break;

      case 'd':
        block_size = std::stoi(optarg);
        break;

      case 'c':
        check_correctness = true;
        break;

      case 't':
	thread_count = std::stoi(optarg);
        break;

    }
  }

#ifdef _OPENMP
  omp_set_num_threads(thread_count);
#else
  (void)thread_count; // suppress unused warning
#endif

  int* solution = nullptr; // both algorithms share the same solution
  if (!benchmark && check_correctness) {
    bool write_solution_to_file = true;

    // have we cached the solution before?
    std::string solution_filename = get_solution_filename("apsp", n, p, seed);
    struct stat file_stat;
    bool solution_available = stat(solution_filename.c_str(), &file_stat) != -1 || errno != ENOENT;

    solution = new int[n * n];
    if (solution_available) {
      std::cout << "Reading reference solution from file: " << solution_filename << "\n";

      std::ifstream in(solution_filename, std::ios::in | std::ios::binary);
      in.read(reinterpret_cast<char*>(solution), n * n * sizeof(int));
      in.close();
    } else {
      int* matrix = floyd_warshall_init(n, p, seed);

      auto start = std::chrono::high_resolution_clock::now();

      floyd_warshall(matrix, solution, n);

      auto end = std::chrono::high_resolution_clock::now();
      std::chrono::duration<double, std::milli> start_to_end = end - start;
      std::cout << "Algorithm runtime: " << start_to_end.count() << "ms\n";

      if (write_solution_to_file) {
        std::cout << "Writing solution to file: " << solution_filename << "\n";

        if (system("mkdir -p solution_cache") == -1) {
          std::cerr << "mkdir failed!";
          return -1;
        }

        std::ofstream out(solution_filename, std::ios::out | std::ios::binary);
        out.write(reinterpret_cast<const char*>(solution), n * n * sizeof(int));
        out.close();
      }
    }
  }

  if (use_floyd_warshall) {
    int* matrix = nullptr;
    int* output = nullptr;
    if (benchmark) {
      bench_floyd_warshall(1, seed, block_size, check_correctness);
    } else {
      matrix = floyd_warshall_blocked_init(n, block_size, p, seed);
      int n_blocked = n;
      int block_remainder = n % block_size;
      if (block_remainder != 0) {
	n_blocked = n + block_size - block_remainder;
      }
      output = new int[n_blocked * n_blocked];

      std::cout << "Using Floyd-Warshall's on " << n_blocked << "x" << n_blocked
		<< " with p=" << p << " and seed=" << seed << "\n";
      auto start = std::chrono::high_resolution_clock::now();
#ifdef CUDA
      floyd_warshall_blocked_cuda(matrix, output, n_blocked);
#else
      floyd_warshall_blocked(matrix, output, n_blocked, block_size);
#endif
      auto end = std::chrono::high_resolution_clock::now();
      std::chrono::duration<double, std::milli> start_to_end = end - start;
      std::cout << "Algorithm runtime: " << start_to_end.count() << "ms\n\n";

      if (check_correctness) {
        correctness_check(output, n_blocked, solution, n);
      }

      delete[] matrix;
      delete[] output;
    }
  } else {  // Using Johnson's Algorithm
    if (benchmark) {
      bench_johnson(1, seed, check_correctness);
    } else {
      int *output = new int[n * n];
      std::cout << "Using Johnson's on " << n << "x" << n
                << " with p=" << p << " and seed=" << seed << "\n";
#ifdef CUDA
      std::cout << "CUDA!\n";
      graph_cuda_t* cuda_gr = johnson_cuda_init(n, p, seed);
      auto start = std::chrono::high_resolution_clock::now();
      johnson_cuda(cuda_gr, output);
      auto end = std::chrono::high_resolution_clock::now();
      free_cuda_graph(cuda_gr);
#else
      graph_t *gr = johnson_init(n, p, seed);
      auto start = std::chrono::high_resolution_clock::now();
      johnson_parallel(gr, output);
      auto end = std::chrono::high_resolution_clock::now();
#endif
      std::chrono::duration<double, std::milli> start_to_end = end - start;
      std::cout << "Algorithm runtime: " << start_to_end.count() << "ms\n\n";

      if (check_correctness) correctness_check(output, n, solution, n);

      //free_graph(gr);
      delete[] output;
    }
  }

  if (check_correctness) {
    delete[] solution;
  }

  return 0;
}

void bench_floyd_warshall(int iterations, unsigned long seed, int block_size, bool check_correctness) {
  std::cout << "\n\nFloyd-Warshall's Algorithm benchmarking results for seed=" << seed << " and block size=" << block_size << "\n";

  print_table_header(check_correctness);
  for (double p = 0.25; p < 1.0; p += 0.25) {
    for (int v = 64; v <= 1024; v *= 2) {
      int* matrix = floyd_warshall_init(v, p, seed);
      int* solution = new int[v * v];

      int* matrix_blocked = matrix; // try to reuse inputs
      int v_blocked = v;
      int block_remainder = v % block_size;
      if (block_remainder != 0) {
	// we may have to add some verts to fit to a multiple of block_size
        matrix_blocked = floyd_warshall_blocked_init(v, block_size, p, seed);
	    v_blocked = v + block_size - block_remainder;
      }
      int* output = new int[v_blocked * v_blocked];

      bool correct = false;

      double seq_total_time = 0.0;
      double total_time = 0.0;
      for (int b = 0; b < iterations; b++) {
        // clear solution
        std::memset(solution, 0, v*v*sizeof(int));

        auto seq_start = std::chrono::high_resolution_clock::now();
        floyd_warshall(matrix, solution, v);
        auto seq_end = std::chrono::high_resolution_clock::now();

        std::chrono::duration<double, std::milli> seq_start_to_end = seq_end - seq_start;
        seq_total_time += seq_start_to_end.count();

        // clear output
        std::memset(output, 0, v_blocked*v_blocked*sizeof(int));
        auto start = std::chrono::high_resolution_clock::now();
        floyd_warshall_blocked(matrix_blocked, output, v_blocked, block_size);
        auto end = std::chrono::high_resolution_clock::now();

        if (check_correctness) {
          correct = correct || correctness_check(output, v_blocked, solution, v);
        }

        std::chrono::duration<double, std::milli> start_to_end = end - start;
        total_time += start_to_end.count();
      }
      delete[] matrix;

      print_table_row(p, v, seq_total_time, total_time, check_correctness, correct);
    }
    print_table_break(check_correctness);
  }
  std::cout << "\n\n";
}

void bench_johnson(int iterations, unsigned long seed, bool check_correctness) {
  std::cout << "\n\nJohnson's Algorithm benchmarking results for seed=" << seed << "\n";

  print_table_header(check_correctness);
  for (double p = 0.25; p < 1.0; p += 0.25) {
    for (int v = 64; v <= 2048; v *= 2) {
      // johnson init
      graph_t* gr = johnson_init(v, p, seed);
      int* matrix = floyd_warshall_init(v, p, seed);
      int* output = new int[v* v];

      int* solution = new int[v*v];
      int** out_sol = new int*[v];
      for (int i = 0; i < v; i++) out_sol[i] = &solution[i*v];
      Graph G(gr->edge_array, gr->edge_array + gr->E, gr->weights, gr->V);
      std::vector<int> d(num_vertices(G));

      bool correct = false;

      double seq_total_time = 0.0;
      double total_time = 0.0;
      for (int b = 0; b < iterations; b++) {
        // clear solution
        std::memset(solution, 0, v*v*sizeof(int));

        auto seq_start = std::chrono::high_resolution_clock::now();
        johnson_all_pairs_shortest_paths(G, out_sol, distance_map(&d[0]));
        auto seq_end = std::chrono::high_resolution_clock::now();

        std::chrono::duration<double, std::milli> seq_start_to_end = seq_end - seq_start;
        seq_total_time += seq_start_to_end.count();

        // clear output
        std::memset(output, 0, v*v*sizeof(int));
        auto start = std::chrono::high_resolution_clock::now();
        // TODO: johnson parallel -- temporarily putting floyd_warshall here
        //floyd_warshall_blocked(matrix, output, v, block_size);
        johnson_parallel(gr, output);
        auto end = std::chrono::high_resolution_clock::now();

        if (check_correctness) {
          correct = correct || correctness_check(output, v, solution, v);
        }

        std::chrono::duration<double, std::milli> start_to_end = end - start;
        total_time += start_to_end.count();
      }
      delete[] solution;
      delete[] out_sol;
      delete[] output;
      delete[] matrix;

      print_table_row(p, v, seq_total_time, total_time, check_correctness, correct);
    }
    print_table_break(check_correctness);
  }
  std::cout << "\n\n";
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2014-2017, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_TEST_IS_VALID_HPP
#define BOOST_GEOMETRY_TEST_IS_VALID_HPP

#include <iostream>
#include <sstream>
#include <string>

#include <boost/core/ignore_unused.hpp>
#include <boost/range.hpp>
#include <boost/variant/variant.hpp>

#include <boost/geometry/core/closure.hpp>
#include <boost/geometry/core/exterior_ring.hpp>
#include <boost/geometry/core/interior_rings.hpp>
#include <boost/geometry/core/point_order.hpp>
#include <boost/geometry/core/ring_type.hpp>
#include <boost/geometry/core/tag.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/segment.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/ring.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/multi_point.hpp>
#include <boost/geometry/geometries/multi_linestring.hpp>
#include <boost/geometry/geometries/multi_polygon.hpp>

#include <boost/geometry/strategies/strategies.hpp>

#include <boost/geometry/io/wkt/wkt.hpp>

#include <boost/geometry/algorithms/convert.hpp>
#include <boost/geometry/algorithms/num_points.hpp>
#include <boost/geometry/algorithms/is_valid.hpp>

#include <boost/geometry/algorithms/detail/check_iterator_range.hpp>

#include <from_wkt.hpp>

#ifdef BOOST_GEOMETRY_TEST_DEBUG
#include "pretty_print_geometry.hpp"
#endif

namespace bg = ::boost::geometry;

typedef bg::model::point<double, 2, bg::cs::cartesian> point_type;
typedef bg::model::segment<point_type>                 segment_type;
typedef bg::model::box<point_type>                     box_type;
typedef bg::model::linestring<point_type>              linestring_type;
typedef bg::model::multi_linestring<linestring_type>   multi_linestring_type;
typedef bg::model::multi_point<point_type>             multi_point_type;

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

// returns true if a geometry can be converted to closed
template
<
    typename Geometry,
    typename Tag = typename bg::tag<Geometry>::type,
    bg::closure_selector Closure = bg::closure<Geometry>::value
>
struct is_convertible_to_closed
{
    static inline bool apply(Geometry const&)
    {
        return false;
    }
};

template <typename Ring>
struct is_convertible_to_closed<Ring, bg::ring_tag, bg::open>
{
    static inline bool apply(Ring const& ring)
    {
        return boost::size(ring) > 0;
    }
};

template <typename Polygon>
struct is_convertible_to_closed<Polygon, bg::polygon_tag, bg::open>
{
    typedef typename bg::ring_type<Polygon>::type ring_type;

    template <typename InteriorRings>
    static inline
    bool apply_to_interior_rings(InteriorRings const& interior_rings)
    {
        return bg::detail::check_iterator_range
            <
                is_convertible_to_closed<ring_type>
            >::apply(boost::begin(interior_rings),
                     boost::end(interior_rings));
    }

    static inline bool apply(Polygon const& polygon)
    {
        return boost::size(bg::exterior_ring(polygon)) > 0
            && apply_to_interior_rings(bg::interior_rings(polygon));
    }
};

template <typename MultiPolygon>
struct is_convertible_to_closed<MultiPolygon, bg::multi_polygon_tag, bg::open>
{
    typedef typename boost::range_value<MultiPolygon>::type polygon;

    static inline bool apply(MultiPolygon const& multi_polygon)
    {
        return bg::detail::check_iterator_range
            <
                is_convertible_to_closed<polygon>,
                false // do not allow empty multi-polygon
            >::apply(boost::begin(multi_polygon),
                     boost::end(multi_polygon));
    }
};

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

// returns true if a geometry can be converted to cw
template
<
    typename Geometry,
    typename Tag = typename bg::tag<Geometry>::type,
    bg::order_selector Order = bg::point_order<Geometry>::value
>
struct is_convertible_to_cw
{
    static inline bool apply(Geometry const&)
    {
        return bg::point_order<Geometry>::value == bg::counterclockwise;
    }
};

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

// returns true if geometry can be converted to polygon
template
<
    typename Geometry,
    typename Tag = typename bg::tag<Geometry>::type
>
struct is_convertible_to_polygon
{
    typedef Geometry type;
    static bool const value = false;
};

template <typename Ring>
struct is_convertible_to_polygon<Ring, bg::ring_tag>
{
    typedef bg::model::polygon
        <
            typename bg::point_type<Ring>::type,
            bg::point_order<Ring>::value == bg::clockwise,
            bg::closure<Ring>::value == bg::closed
        > type;

    static bool const value = true;
};

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

// returns true if geometry can be converted to multi-polygon
template
<
    typename Geometry,
    typename Tag = typename bg::tag<Geometry>::type
>
struct is_convertible_to_multipolygon
{
    typedef Geometry type;
    static bool const value = false;
};

template <typename Ring>
struct is_convertible_to_multipolygon<Ring, bg::ring_tag>
{
    typedef bg::model::multi_polygon
        <
            typename is_convertible_to_polygon<Ring>::type
        > type;

    static bool const value = true;
};

template <typename Polygon>
struct is_convertible_to_multipolygon<Polygon, bg::polygon_tag>
{
    typedef bg::model::multi_polygon<Polygon> type;
    static bool const value = true;
};

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

template <typename ValidityTester>
struct validity_checker
{
    template <typename Geometry>
    static inline bool apply(std::string const& case_id,
                             Geometry const& geometry,
                             bool expected_result,
                             std::string& reason)
    {
        bool valid = ValidityTester::apply(geometry);
        std::string const reason_valid
            = bg::validity_failure_type_message(bg::no_failure);
        reason = ValidityTester::reason(geometry);
        std::string reason_short = reason.substr(0, reason_valid.length());

        BOOST_CHECK_MESSAGE(valid == expected_result,
            "case id: " << case_id
            << ", Expected: " << expected_result
            << ", detected: " << valid
            << "; wkt: " << bg::wkt(geometry));

        BOOST_CHECK_MESSAGE(reason != "",
            "case id (empty reason): " << case_id
            << ", Expected: " << valid
            << ", detected reason: " << reason
            << "; wkt: " << bg::wkt(geometry));

        BOOST_CHECK_MESSAGE((valid && reason == reason_valid)
                            ||
                            (! valid && reason != reason_valid)
                            ||
                            (! valid && reason_short != reason_valid),
            "case id (reason): " << case_id
            << ", Expected: " << valid
            << ", detected reason: " << reason
            << "; wkt: " << bg::wkt(geometry));

        return valid;
    }
};

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

struct default_validity_tester
{
    template <typename Geometry>
    static inline bool apply(Geometry const& geometry)
    {
        return bg::is_valid(geometry);
    }

    template <typename Geometry>
    static inline std::string reason(Geometry const& geometry)
    {
        std::string message;
        bg::is_valid(geometry, message);
        return message;
    }
};

template <bool AllowSpikes>
struct validity_tester_linear
{
    template <typename Geometry>
    static inline bool apply(Geometry const& geometry)
    {
        bool const irrelevant = true;
        bg::is_valid_default_policy<irrelevant, AllowSpikes> visitor;
        return bg::is_valid(geometry, visitor, bg::default_strategy());
    }

    template <typename Geometry>
    static inline std::string reason(Geometry const& geometry)
    {
        bool const irrelevant = true;
        std::ostringstream oss;
        bg::failing_reason_policy<irrelevant, AllowSpikes> visitor(oss);
        bg::is_valid(geometry, visitor, bg::default_strategy());
        return oss.str();
    }
};

template <bool AllowDuplicates>
struct validity_tester_areal
{
    template <typename Geometry>
    static inline bool apply(Geometry const& geometry)
    {
        bg::is_valid_default_policy<AllowDuplicates> visitor;
        return bg::is_valid(geometry, visitor, bg::default_strategy());
    }

    template <typename Geometry>
    static inline std::string reason(Geometry const& geometry)
    {
        std::ostringstream oss;
        bg::failing_reason_policy<AllowDuplicates> visitor(oss);
        bg::is_valid(geometry, visitor, bg::default_strategy());
        return oss.str();
    }

};

template <bool AllowDuplicates>
struct validity_tester_geo_areal
{
    template <typename Geometry>
    static inline bool apply(Geometry const& geometry)
    {
        bg::is_valid_default_policy<AllowDuplicates> visitor;
        bg::strategy::intersection::geographic_segments<> s;
        return bg::is_valid(geometry, visitor, s);
    }

    template <typename Geometry>
    static inline std::string reason(Geometry const& geometry)
    {
        std::ostringstream oss;
        bg::failing_reason_policy<AllowDuplicates> visitor(oss);
        bg::strategy::intersection::geographic_segments<> s;
        bg::is_valid(geometry, visitor, s);
        return oss.str();
    }

};

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

template
<
    typename ValidityTester,
    typename Geometry,
    typename ClosedGeometry = Geometry,
    typename CWGeometry = Geometry,
    typename CWClosedGeometry = Geometry,
    typename Tag = typename bg::tag<Geometry>::type
>
class test_valid
{
protected:
    template <typename G>
    static inline void base_test(std::string const& case_id,
                                 G const& g,
                                 bool expected_result)
    {
#ifdef BOOST_GEOMETRY_TEST_DEBUG
        std::cout << "=======" << std::endl;
#endif

        std::string reason;
        bool valid = validity_checker
            <
                ValidityTester
            >::apply(case_id, g, expected_result, reason);
        boost::ignore_unused(valid);

#ifdef BOOST_GEOMETRY_TEST_DEBUG
        std::cout << "case id: " << case_id << ", Geometry: ";
        pretty_print_geometry<G>::apply(std::cout, g);
        std::cout << std::endl;
        std::cout << "wkt: " << bg::wkt(g) << std::endl;
        std::cout << std::boolalpha;
        std::cout << "is valid? " << valid << std::endl;
        std::cout << "expected result: " << expected_result << std::endl;
        std::cout << "reason: " << reason << std::endl;
        std::cout << "=======" << std::endl;
        std::cout << std::noboolalpha;
#endif
    }

public:
    static inline void apply(std::string const& case_id,
                             Geometry const& geometry,
                             bool expected_result)
    {
        std::stringstream sstr;
        sstr << case_id << "-original"; // which is: CCW open
        base_test(sstr.str(), geometry, expected_result);

        if ( is_convertible_to_closed<Geometry>::apply(geometry) )
        {
#ifdef BOOST_GEOMETRY_TEST_DEBUG
            std::cout << "...checking closed geometry..."
                      << std::endl;
#endif
            ClosedGeometry closed_geometry;
            bg::convert(geometry, closed_geometry);
            sstr.str("");
            sstr << case_id << "-2closed";
            base_test(sstr.str(), closed_geometry, expected_result);
        }
        if ( is_convertible_to_cw<Geometry>::apply(geometry) )
        {
#ifdef BOOST_GEOMETRY_TEST_DEBUG
            std::cout << "...checking cw open geometry..."
                      << std::endl;
#endif
            CWGeometry cw_geometry;
            bg::convert(geometry, cw_geometry);
            sstr.str("");
            sstr << case_id << "-2CW";
            base_test(sstr.str(), cw_geometry, expected_result);
            if ( is_convertible_to_closed<CWGeometry>::apply(cw_geometry) )
            {
#ifdef BOOST_GEOMETRY_TEST_DEBUG
                std::cout << "...checking cw closed geometry..."
                          << std::endl;
#endif
                CWClosedGeometry cw_closed_geometry;
                bg::convert(cw_geometry, cw_closed_geometry);
                sstr.str("");
                sstr << case_id << "-2CWclosed";
                base_test(sstr.str(), cw_closed_geometry, expected_result);
            }
        }

        if ( BOOST_GEOMETRY_CONDITION(is_convertible_to_polygon<Geometry>::value) )
        {
#ifdef BOOST_GEOMETRY_TEST_DEBUG
            std::cout << "...checking geometry converted to polygon..."
                      << std::endl;
#endif
            typename is_convertible_to_polygon<Geometry>::type polygon;
            bg::convert(geometry, polygon);
            sstr.str("");
            sstr << case_id << "-2Polygon";
            base_test(sstr.str(), polygon, expected_result);
        }

        if ( BOOST_GEOMETRY_CONDITION(is_convertible_to_multipolygon<Geometry>::value) )
        {
#ifdef BOOST_GEOMETRY_TEST_DEBUG
            std::cout << "...checking geometry converted to multi-polygon..."
                      << std::endl;
#endif
            typename is_convertible_to_multipolygon
                <
                    Geometry
                >::type multipolygon;

            bg::convert(geometry, multipolygon);
            sstr.str("");
            sstr << case_id << "-2MultiPolygon";
            base_test(sstr.str(), multipolygon, expected_result);
        }

#ifdef BOOST_GEOMETRY_TEST_DEBUG
        std::cout << std::endl << std::endl << std::endl;
#endif
    }

    static inline void apply(std::string const& case_id,
                             std::string const& wkt,
                             bool expected_result)
    {
        apply(case_id, from_wkt<Geometry>(wkt), expected_result);
    }
};

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

template <typename VariantGeometry>
class test_valid_variant
    : test_valid<default_validity_tester, VariantGeometry>
{
private:
    typedef test_valid<default_validity_tester, VariantGeometry> base_type;

public:
    static inline void apply(std::string const& case_id,
                             VariantGeometry const& vg,
                             bool expected_result)
    {
        std::ostringstream oss;
        base_type::base_test(case_id, vg, expected_result);
    }
};

#endif // BOOST_GEOMETRY_TEST_IS_VALID_HPP


#pragma once

/* ORIGINAL LICENSE TEXT:
 *
 * Copyright (c) 2011, Georgia Tech Research Corporation
 * All rights reserved.
 *
 * Author: Tobias Kunz <tobias@gatech.edu>
 * Date: 05/2012
 *
 * Humanoid Robotics Lab      Georgia Institute of Technology
 * Director: Mike Stilman     http://www.golems.org
 *
 * Algorithm details and publications:
 * http://www.golems.org/node/1570
 *
 * This file is provided under the following "BSD-style" License:
 *   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.
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 */

#include <stdlib.h>
#include <iostream>
#include <functional>
#include <Eigen/Geometry>
#include <vector>
#include <chrono>
#include <list>

namespace common_robotics_utilities
{
namespace time_optimal_trajectory_parametrization
{
class PathSegment
{
public:

  PathSegment(const double length = 0.0) : length_(length) {}

  virtual ~PathSegment() {}

  double Length() const { return length_; }

  virtual Eigen::VectorXd GetConfig(const double s) const = 0;

  virtual Eigen::VectorXd GetTangent(const double s) const = 0;

  virtual Eigen::VectorXd GetCurvature(const double s) const = 0;

  virtual std::list<double> GetSwitchingPoints() const = 0;

  virtual PathSegment* Clone() const = 0;

  double position;

protected:

  double length_;
};

class Path
{
public:

  Path(const std::list<Eigen::VectorXd>& path,
       const double maxDeviation = 0.0);

  Path(const Path& path);

  ~Path();

  double Length() const;

  Eigen::VectorXd GetConfig(const double s) const;

  Eigen::VectorXd GetTangent(const double s) const;

  Eigen::VectorXd GetCurvature(const double s) const;

  double GetNextSwitchingPoint(const double s, bool& discontinuity) const;

  std::list<std::pair<double, bool>> SwitchingPoints() const;

private:

  PathSegment* GetPathSegment(double& s) const;

  double length_;

  std::list<std::pair<double, bool>> switching_points_;

  std::list<PathSegment*> path_segments_;
};

class Trajectory
{
public:
  // Generates a time-optimal trajectory
  Trajectory(const std::list<Eigen::VectorXd>& waypoints,
             const Eigen::VectorXd& max_velocity,
             const Eigen::VectorXd& max_acceleration,
             const double max_deviation = 0.0,
             const double timestep = 0.001);

  Trajectory(const Path& path,
             const Eigen::VectorXd& max_velocity,
             const Eigen::VectorXd& max_acceleration,
             const double timestep = 0.001);

  // Returns the optimal duration of the trajectory
  double Duration() const;

  // Return the position vector and velocity vector of the
  // robot for a given point in time within the trajectory.
  // !!! NOT THREAD SAFE - MUTABLE CACHE INSIDE !!!
  std::pair<Eigen::VectorXd, Eigen::VectorXd> GetPositionVelocity(
      const double time) const;

  // Outputs the phase trajectory and the velocity limit curve
  // in 2 files for debugging purposes.
  void OutputPhasePlaneTrajectory() const;

private:

  struct TrajectoryStep
  {
    TrajectoryStep() {}

    TrajectoryStep(double path_pos, double path_vel)
      : path_pos_(path_pos), path_vel_(path_vel) {}

    double path_pos_;
    double path_vel_;
    double time_;
  };

  bool GetNextSwitchingPoint(const double path_pos,
                             TrajectoryStep& next_switching_point,
                             double& before_acceleration,
                             double& after_acceleration);

  bool GetNextAccelerationSwitchingPoint(const double path_pos,
                                         TrajectoryStep& next_switching_point,
                                         double& before_acceleration,
                                         double& after_acceleration);

  bool GetNextVelocitySwitchingPoint(const double path_pos,
                                     TrajectoryStep& next_switching_point,
                                     double& before_acceleration,
                                     double& after_acceleration);

  bool IntegrateForward(std::list<TrajectoryStep>& trajectory,
                        const double acceleration);

  void IntegrateBackward(std::list<TrajectoryStep>& start_trajectory,
                         const double path_pos,
                         const double path_vel,
                         const double acceleration);

  double GetMinMaxPathAcceleration(const double path_position,
                                   const double path_velocity,
                                   const bool max);

  double GetMinMaxPhaseSlope(const double path_position,
                             const double path_velocity,
                             const bool max);

  double GetAccelerationMaxPathVelocity(const double path_pos) const;

  double GetVelocityMaxPathVelocity(const double path_pos) const;

  double GetAccelerationMaxPathVelocityDeriv(const double path_pos);

  double GetVelocityMaxPathVelocityDeriv(const double path_pos);

  std::list<TrajectoryStep>::const_iterator GetTrajectorySegment(
      const double time) const;

  Path path_;
  Eigen::VectorXd max_velocity_;
  Eigen::VectorXd max_acceleration_;
  uint32_t n_ = 0u;
  std::list<TrajectoryStep> trajectory_;

  static const double eps;
  const double time_step_;

  mutable double cached_time_;
  mutable std::list<TrajectoryStep>::const_iterator cached_trajectory_segment_;
};
}  // namespace time_optimal_trajectory_parametrization
}  // namespace common_robotics_utilities


/*
 * point.hpp
 *
 *  Created on: Jun 23, 2015
 *      Author: sontd
 */

#ifndef WANDRIAN_INCLUDE_COMMON_POINT_HPP_
#define WANDRIAN_INCLUDE_COMMON_POINT_HPP_

#include <stdlib.h>
#include <cmath>
#include <boost/shared_ptr.hpp>
#include "global.hpp"

namespace wandrian {
namespace common {

struct Point {

  double x, y;

  Point();
  Point(double, double);
  Point(const Point&);
  Point(const boost::shared_ptr<Point>);
};

typedef boost::shared_ptr<Point> PointPtr;
typedef boost::shared_ptr<Point const> PointConstPtr;

inline double operator%(PointPtr p1, PointPtr p2) {
  return std::sqrt(std::pow(p1->x - p2->x, 2) + std::pow(p1->y - p2->y, 2));
}

inline bool operator<(PointConstPtr p1, PointConstPtr p2) {
  return
      std::abs(p1->x - p2->x) > EPSILON ?
          p1->x - p2->x < -EPSILON : p1->y - p2->y < -EPSILON;
}

inline bool operator<(PointPtr p1, PointPtr p2) {
  return
      std::abs(p1->x - p2->x) > EPSILON ?
          p1->x - p2->x < -EPSILON : p1->y - p2->y < -EPSILON;
}

inline bool operator!=(PointPtr p1, PointPtr p2) {
  return p1 < p2 || p2 < p1;
}

inline bool operator==(PointPtr p1, PointPtr p2) {
  return !(p1 != p2);
}

inline bool operator>(PointPtr p1, PointPtr p2) {
  return p1 != p2 && !(p1 < p2);
}

struct PointComp {
  bool operator()(PointConstPtr p1, PointConstPtr p2) const {
    return p1 < p2;
  }
};

}
}

#endif /* WANDRIAN_INCLUDE_COMMON_POINT_HPP_ */


// Copyright 2015 Conrad Sanderson (http://conradsanderson.id.au)
// Copyright 2015 National ICT Australia (NICTA)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// ------------------------------------------------------------------------

#include <armadillo>
#include "catch.hpp"

using namespace arma;

TEST_CASE("instantiation_mat_1")
  {
  const uword n_rows = 5;
  const uword n_cols = 6;

   mat A(n_rows,n_cols);
  fmat B(n_rows,n_cols);
  umat C(n_rows,n_cols);
  imat D(n_rows,n_cols);

  cx_mat  E(n_rows,n_cols);
  cx_fmat F(n_rows,n_cols);

  mat::fixed<5,6> G;
  }

// TODO: colvec_instantiation
// TODO: rowvec_instantiation

TEST_CASE("instantiation_cube_1")
  {
  const uword n_rows   = 5;
  const uword n_cols   = 6;
  const uword n_slices = 2;

   cube A(n_rows,n_cols,n_slices);
  fcube B(n_rows,n_cols,n_slices);
  ucube C(n_rows,n_cols,n_slices);
  icube D(n_rows,n_cols,n_slices);

  cx_cube  E(n_rows,n_cols,n_slices);
  cx_fcube F(n_rows,n_cols,n_slices);

  cube::fixed<5,6,2> G;
  }

// TODO: field_instantiation 1D 2D 3D
// TODO: spmat_instantiation


//          Copyright Carl Philipp Reh 2009 - 2016.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <fcppt/container/grid/make_pos_ref_crange.hpp>
#include <fcppt/container/grid/make_pos_ref_crange_start_end.hpp>
#include <fcppt/container/grid/make_pos_ref_range.hpp>
#include <fcppt/container/grid/make_pos_ref_range_start_end.hpp>
#include <fcppt/container/grid/min_from_pos.hpp>
#include <fcppt/container/grid/object.hpp>
#include <fcppt/container/grid/pos.hpp>
#include <fcppt/container/grid/sup_from_pos.hpp>
#include <fcppt/math/vector/comparison.hpp>
#include <fcppt/math/vector/null.hpp>
#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/test/unit_test.hpp>
#include <fcppt/config/external_end.hpp>

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	container_grid_pos_ref_range
)
{
FCPPT_PP_POP_WARNING

	typedef
	fcppt::container::grid::pos<
		std::size_t,
		2
	>
	element_type;

	typedef
	fcppt::container::grid::object<
		element_type,
		2
	>
	dim2_grid;

	typedef
	fcppt::container::grid::min_from_pos<
		dim2_grid::pos
	>
	min;

	typedef
	fcppt::container::grid::sup_from_pos<
		dim2_grid::pos
	>
	sup;

	typedef
	dim2_grid::pos
	pos;

	dim2_grid grid(
		dim2_grid::dim(
			4u,
			6u
		),
		fcppt::math::vector::null<
			pos
		>()
	);

	for(
		auto const element
		:
		fcppt::container::grid::make_pos_ref_range(
			grid
		)
	)
		element.value()
		=
		element.pos();

	for(
		auto const element
		:
		fcppt::container::grid::make_pos_ref_crange(
			grid
		)
	)
		BOOST_CHECK(
			element.value()
			==
			element.pos()
		);

	grid =
		dim2_grid(
			dim2_grid::dim(
				4u,
				6U
			),
			fcppt::math::vector::null<
				element_type
			>()
		);

	for(
		auto const element
		:
		fcppt::container::grid::make_pos_ref_range_start_end(
			grid,
			min{
				pos(
					1u,
					1U
				)
			},
			sup{
				pos(
					3u,
					3u
				)
			}
		)
	)
		element.value()
		=
		element.pos();

	for(
		auto const element
		:
		fcppt::container::grid::make_pos_ref_crange_start_end(
			grid,
			min{
				pos(
					1u,
					1u
				)
			},
			sup{
				pos(
					3u,
					3u
				)
			}
		)
	)
		BOOST_CHECK(
			element.value()
			==
			element.pos()
		);

	bool loop_empty(
		true
	);

	for(
		auto const element
		:
		fcppt::container::grid::make_pos_ref_range_start_end(
			grid,
			min{
				fcppt::math::vector::null<
					pos
				>()
			},
			sup{
				fcppt::math::vector::null<
					pos
				>()
			}
		)
	)
		loop_empty = false;

	BOOST_CHECK(
		loop_empty
	);

	for(
		auto const element
		:
		fcppt::container::grid::make_pos_ref_range_start_end(
			grid,
			min{
				fcppt::math::vector::null<
					pos
				>()
			},
			sup{
				pos(
					0u,
					1u
				)
			}
		)
	)
		loop_empty = false;

	BOOST_CHECK(
		loop_empty
	);

	for(
		auto const element
		:
		fcppt::container::grid::make_pos_ref_range_start_end(
			grid,
			min{
				fcppt::math::vector::null<
					pos
				>()
			},
			sup{
				pos(
					1u,
					0u
				)
			}
		)
	)
		loop_empty = false;

	BOOST_CHECK(
		loop_empty
	);
}


#include "heg_solver.h"

#include <boost/functional/hash.hpp>
#include <boost/range/adaptor/reversed.hpp>
#include "../array_math.h"
#include "../config.h"
#include "../parallel.h"
#include "../time.h"
#include "k_points_util.h"
#include "omp.h"

void HEGSolver::solve() {
  n_up = Config::get<size_t>("n_up");
  n_dn = Config::get<size_t>("n_dn");
  rcut_vars = Config::get_array<double>("rcut_vars");
  eps_vars = Config::get_array<double>("eps_vars");

  Time::start("variation");
  for (size_t i = 0; i < rcut_vars.size(); i++) {
    if (i > 0 && rcut_vars[i] == rcut_vars[i - 1]) continue;
    const double rcut_var = rcut_vars[i];
    wf.clear();
    std::string rcut_var_event = str(boost::format("rcut_var: %#.4g") % rcut_var);
    Time::start(rcut_var_event);

    Time::start("setup");
    setup(rcut_var);
    Time::end();

    for (size_t j = 0; j < eps_vars.size(); j++) {
      if (j > 0 && eps_vars[j] == eps_vars[j - 1]) continue;
      const double eps_var = eps_vars[j];
      std::string eps_var_event = str(boost::format("eps_var: %#.4g") % eps_var);
      Time::start(eps_var_event);
      const std::string filename = str(boost::format("var_%#.4g_%#.4g.txt") % eps_var % rcut_var);
      if (!load_variation_result(filename)) {
        variation(eps_var);
        save_variation_result(filename);
      }
      Time::end();
    }
    Time::end();
  }
  Time::end();

  const bool variation_only = Config::get<bool>("variation_only", false);
  if (variation_only) return;

  Time::start("perturbation");
  // Start from the largest PT so that it fails earlier upon insufficient memory.
  for (const double rcut_var : rcut_vars | boost::adaptors::reversed) {
    std::string rcut_var_event = str(boost::format("rcut_var: %#.4g") % rcut_var);
    Time::start(rcut_var_event);
    Time::start("setup");
    const double rcut_pt_ratio = Config::get<double>("rcut_pt_ratio", 1.26);
    const double rcut_pt = rcut_var * rcut_pt_ratio;
    if (Parallel::is_master()) {
      printf("rcut_pt: %#.4g (%#.4g * %#.4g)\n", rcut_pt, rcut_var, rcut_pt_ratio);
    }
    setup(rcut_pt);
    Time::end();
    for (const double eps_var : eps_vars | boost::adaptors::reversed) {
      std::string eps_var_event = str(boost::format("eps_var: %#.4g") % eps_var);
      Time::start(eps_var_event);
      const double eps_pt_ratio = Config::get<double>("eps_pt_ratio", 0.01);
      const double eps_pt = eps_var * eps_pt_ratio;
        if (Parallel::is_master()) {
          printf("rcut_pt: %#.4g (%zu orbs)\n", rcut_pt, k_points.size() * 2);
          printf("eps_pt: %#.4g (%#.4g * %#.4g)\n", eps_pt, eps_var, eps_pt_ratio);
        }
      const std::string filename = str(boost::format("var_%#.4g_%#.4g.txt") % eps_var % rcut_var);
      assert(load_variation_result(filename));
      // perturbation();
      Time::end();
    }
    Time::end();
  }
  Time::end();
}

void HEGSolver::setup(const double rcut) {
  const double r_s = Config::get<double>("r_s");
  const double density = 3.0 / (4.0 * M_PI * pow(r_s, 3));
  const double cell_length = pow((n_up + n_dn) / density, 1.0 / 3);
  k_unit = 2 * M_PI / cell_length;
  H_unit = 1.0 / (M_PI * cell_length);
  k_points = KPointsUtil::generate_k_points(rcut);
  for (size_t i = 0; i < k_points.size(); i++) k_lut[k_points[i]] = i;
  if (Parallel::is_master()) {
    printf("number of orbitals: %d\n", static_cast<int>(k_points.size() * 2));
  }
  Time::checkpoint("k points generated");

  generate_hci_queue(rcut);
  Time::checkpoint("hci queue generated");
}

void HEGSolver::generate_hci_queue(const double rcut) {
  same_spin_hci_queue.clear();
  opposite_spin_hci_queue.clear();
  max_abs_H = 0.0;

  // Common dependencies.
  const auto& k_diffs = KPointsUtil::get_k_diffs(k_points);

  // Same spin.
  for (const auto& diff_pq : k_diffs) {
    for (const auto& diff_pr : k_diffs) {
      const auto& diff_sr = diff_pr + diff_pr - diff_pq;  // Momentum conservation.
      if (diff_sr == 0 || norm(diff_sr) > rcut * 2) continue;
      const auto& diff_ps = diff_pr - diff_sr;
      if (diff_ps == 0) continue;
      if (squared_norm(diff_pr) == squared_norm(diff_ps)) continue;
      const double abs_H = fabs(1.0 / squared_norm(diff_pr) - 1.0 / squared_norm(diff_ps));
      if (abs_H < DBL_EPSILON) continue;
      const auto& item = std::make_pair(diff_pr, abs_H * H_unit);
      same_spin_hci_queue[diff_pq].push_back(item);
    }
  }
  for (auto& kv : same_spin_hci_queue) {
    auto& items = kv.second;
    std::stable_sort(
        items.begin(),
        items.end(),
        [](const std::pair<std::array<int8_t, 3>, double>& a,
           const std::pair<std::array<int8_t, 3>, double>& b) -> bool {
          return a.second > b.second;
        });
    max_abs_H = std::max(max_abs_H, items.front().second);
  }

  // Opposite spin.
  for (const auto& diff_pr : k_diffs) {
    const double abs_H = 1.0 / sum(square(diff_pr));
    if (abs_H < DBL_EPSILON) continue;
    const auto& item = std::make_pair(diff_pr, abs_H * H_unit);
    opposite_spin_hci_queue.push_back(item);
  }
  std::stable_sort(
      opposite_spin_hci_queue.begin(),
      opposite_spin_hci_queue.end(),
      [](const std::pair<std::array<int8_t, 3>, double>& a,
         const std::pair<std::array<int8_t, 3>, double>& b) -> bool {
        return a.second > b.second;
      });
  max_abs_H = std::max(max_abs_H, opposite_spin_hci_queue.front().second);
}

double HEGSolver::hamiltonian(const Det& det_pq, const Det& det_rs) const {
  double H = 0.0;

  if (det_pq == det_rs) {
    const auto& occ_pq_up = det_pq.up.get_elec_orbs();
    const auto& occ_pq_dn = det_pq.dn.get_elec_orbs();

    // One electron operator.
    for (const auto p : occ_pq_up) H += squared_norm(k_points[p] * k_unit) * 0.5;
    for (const auto p : occ_pq_dn) H += squared_norm(k_points[p] * k_unit) * 0.5;

    // Two electrons operator.
    for (size_t i = 0; i < n_up; i++) {
      const auto p = occ_pq_up[i];
      for (size_t j = i + 1; j < n_up; j++) {
        const auto q = occ_pq_up[j];
        H -= H_unit / squared_norm(k_points[p] - k_points[q]);
      }
    }
    for (size_t i = 0; i < n_dn; i++) {
      const auto p = occ_pq_dn[i];
      for (size_t j = i + 1; j < n_dn; j++) {
        const auto q = occ_pq_dn[j];
        H -= H_unit / squared_norm(k_points[p] - k_points[q]);
      }
    }
  } else {
    // Off-diagonal elements.
    Det det_eor;
    det_eor.from_eor(det_pq, det_rs);
    const size_t n_eor_up = det_eor.up.get_n_elecs();
    const size_t n_eor_dn = det_eor.dn.get_n_elecs();
    if (n_eor_up + n_eor_dn != 4) return 0.0;
    const auto& eor_up_set_bits = det_eor.up.get_elec_orbs();
    const auto& eor_dn_set_bits = det_eor.dn.get_elec_orbs();
    bool k_p_set = false, k_r_set = false;
    uint16_t orb_p = 0, orb_r = 0, orb_s = 0;

    // Obtain p, q, s.
    std::array<int8_t, 3> k_change;
    k_change.fill(0);
    for (const auto orb_i : eor_up_set_bits) {
      if (det_pq.up.get_orb(orb_i)) {
        k_change -= k_points[orb_i];
        if (!k_p_set) {
          orb_p = orb_i;
          k_p_set = true;
        }
      } else {
        k_change += k_points[orb_i];
        if (!k_r_set) {
          orb_r = orb_i;
          k_r_set = true;
        } else {
          orb_s = orb_i;
        }
      }
    }
    for (const auto orb_i : eor_dn_set_bits) {
      if (det_pq.dn.get_orb(orb_i)) {
        k_change -= k_points[orb_i];
        if (!k_p_set) {
          orb_p = orb_i;
          k_p_set = true;
        }
      } else {
        k_change += k_points[orb_i];
        if (!k_r_set) {
          orb_r = orb_i;
          k_r_set = true;
        } else {
          orb_s = orb_i;
        }
      }
    }

    // Check for momentum conservation.
    if (k_change != 0) return 0.0;

    H = H_unit / squared_norm(k_points[orb_p] - k_points[orb_r]);
    if (n_eor_up != 2) H -= H_unit / squared_norm(k_points[orb_p] - k_points[orb_s]);

    const int gamma_exp =
        get_gamma_exp(det_pq.up, eor_up_set_bits) + get_gamma_exp(det_pq.dn, eor_dn_set_bits) +
        get_gamma_exp(det_rs.up, eor_up_set_bits) + get_gamma_exp(det_rs.dn, eor_dn_set_bits);
    if ((gamma_exp & 1) == 1) H = -H;
  }
  return H;
}

int HEGSolver::get_gamma_exp(const SpinDet& spin_det, const std::vector<uint16_t>& eor) const {
  int gamma_exp = 0;
  int ptr = 0;
  const auto& occ = spin_det.get_elec_orbs();
  for (const uint16_t orb_id : eor) {
    if (!spin_det.get_orb(orb_id)) continue;
    ptr = std::lower_bound(occ.begin() + ptr, occ.end(), orb_id) - occ.begin();
    gamma_exp += ptr;
  }
  return gamma_exp;
}

std::list<OrbitalPair> HEGSolver::get_pq_pairs(const Det& det, const Orbital dn_offset) const {
  const auto& occ_up = det.up.get_elec_orbs();
  const auto& occ_dn = det.dn.get_elec_orbs();
  const size_t n_up = det.up.get_n_elecs();
  const size_t n_dn = det.dn.get_n_elecs();

  std::list<OrbitalPair> pq_pairs;

  for (size_t i = 0; i < n_up; i++) {
    for (size_t j = i + 1; j < n_up; j++) {
      pq_pairs.push_back(std::make_pair(occ_up[i], occ_up[j]));
    }
  }
  for (size_t i = 0; i < n_dn; i++) {
    for (size_t j = i + 1; j < n_dn; j++) {
      pq_pairs.push_back(std::make_pair(occ_dn[i] + dn_offset, occ_dn[j] + dn_offset));
    }
  }
  for (size_t i = 0; i < n_up; i++) {
    for (size_t j = 0; j < n_dn; j++) {
      pq_pairs.push_back(std::make_pair(occ_up[i], occ_dn[j] + dn_offset));
    }
  }

  return pq_pairs;
}

std::list<Det> HEGSolver::find_connected_dets(const Det& det, const double eps) const {
  std::list<Det> connected_dets;
  connected_dets.push_back(det);

  if (max_abs_H < eps) return connected_dets;

  const size_t dn_offset = k_points.size();
  const auto& pq_pairs = get_pq_pairs(det, dn_offset);

  for (const auto& pq_pair : pq_pairs) {
    const Orbital p = pq_pair.first;
    const Orbital q = pq_pair.second;

    // Get rs pairs.
    Orbital pp = p, qq = q;
    if (p >= dn_offset && q >= dn_offset) {
      pp -= dn_offset;
      qq -= dn_offset;
    } else if (p < dn_offset && q >= dn_offset && p > q - dn_offset) {
      pp = q - dn_offset;
      qq = p + dn_offset;
    }
    bool same_spin = false;
    std::vector<std::pair<std::array<int8_t, 3>, double>> const* items_ptr;
    if (pp < dn_offset && qq < dn_offset) {
      same_spin = true;
      const auto& diff_pq = k_points[qq] - k_points[pp];
      items_ptr = &(same_spin_hci_queue.find(diff_pq)->second);
    } else {
      items_ptr = &(opposite_spin_hci_queue);
    }
    const auto& items = *items_ptr;
    Orbital qs_offset = 0;
    if (!same_spin) qs_offset = dn_offset;

    for (const auto& item : items) {
      if (item.second < eps) break;
      const auto& diff_pr = item.first;
      const auto& it_r = k_lut.find(diff_pr + k_points[pp]);
      if (it_r == k_lut.end()) continue;
      Orbital r = it_r->second;
      const auto& it_s = k_lut.find(k_points[pp] + k_points[qq - qs_offset] - k_points[r]);
      if (it_s == k_lut.end()) continue;
      Orbital s = it_s->second;
      if (same_spin && s < r) continue;
      s += qs_offset;
      if (p >= dn_offset && q >= dn_offset) {
        r += dn_offset;
        s += dn_offset;
      } else if (p < dn_offset && q >= dn_offset && p > q - dn_offset) {
        const Orbital tmp = s;
        s = r + dn_offset;
        r = tmp - dn_offset;
      }

      // Test whether pqrs is a valid excitation for det.
      if (det.get_orb(r, dn_offset) || det.get_orb(s, dn_offset)) continue;
      connected_dets.push_back(det);
      Det& new_det = connected_dets.back();
      new_det.set_orb(p, dn_offset, false);
      new_det.set_orb(q, dn_offset, false);
      new_det.set_orb(r, dn_offset, true);
      new_det.set_orb(s, dn_offset, true);
    }
  }

  return connected_dets;
};


// Boost.Geometry Index
//
// n-dimensional margin value (hypersurface), 2d perimeter, 3d surface, etc...
//
// Copyright (c) 2011-2013 Adam Wulkiewicz, Lodz, Poland.
//
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_EXTENSIONS_NSPHERE_INDEX_DETAIL_ALGORITHMS_MARGIN_HPP
#define BOOST_GEOMETRY_EXTENSIONS_NSPHERE_INDEX_DETAIL_ALGORITHMS_MARGIN_HPP

#include <boost/geometry/index/detail/algorithms/margin.hpp>

namespace boost { namespace geometry { namespace index { namespace detail {

template <typename NSphere, size_t Dimension>
struct comparable_margin_nsphere
{
    BOOST_STATIC_ASSERT(1 < Dimension);
    //BOOST_STATIC_ASSERT(Dimension <= dimension<NSphere>::value);

    static inline typename default_margin_result<NSphere>::type apply(NSphere const& s)
    {
        return comparable_margin_nsphere<NSphere, Dimension-1>::apply(s)
            * geometry::get_radius<0>(s);
    }
};

template <typename NSphere>
struct comparable_margin_nsphere<NSphere, 1>
{
    static inline typename default_margin_result<NSphere>::type apply(NSphere const& )
    {
        return 1;
    }
};

namespace dispatch {

template <typename NSphere>
struct comparable_margin<NSphere, nsphere_tag>
{
    typedef typename default_margin_result<NSphere>::type result_type;

    static inline result_type apply(NSphere const& g)
    {
        return comparable_margin_nsphere<
            NSphere, dimension<NSphere>::value
        >::apply(g);
    }
};

} // namespace dispatch

}}}} // namespace boost::geometry::index::detail

#endif // BOOST_GEOMETRY_EXTENSIONS_NSPHERE_INDEX_DETAIL_ALGORITHMS_MARGIN_HPP