#ifndef LIMEX_HH
#define LIMEX_HH

#include <fstream>
#include <iostream>

#include <boost/timer/timer.hpp>

#include "dune/istl/solvers.hh"

#include "fem/embedded_errorest.hh"
#include "fem/iterate_grid.hh"
#include "fem/hierarchicErrorEstimator.hh"
#include "fem/hierarchicspace.hh"
#include "fem/istlinterface.hh"

#include "dune/istl/solvers.hh"
#include "dune/istl/preconditioners.hh"

#include "timestepping/extrapolation.hh"
#include "timestepping/semieuler.hh"

#include "linalg/factorization.hh"
#include "linalg/umfpack_solve.hh"
// #include "linalg/mumps_solve.hh"
// #include "linalg/superlu_solve.hh"

#include "linalg/trivialpreconditioner.hh"
#include "linalg/direct.hh"
#include "linalg/triplet.hh"
#include "linalg/iluprecond.hh"
#include "linalg/additiveschwarz.hh"
// #include "linalg/hyprecond.hh"
#include "linalg/jacobiPreconditioner.hh"

#include "utilities/enums.hh"

#include "io/vtk.hh"

bool fabscompare( double a, double b ) { return fabs( a ) < fabs( b ) ; }

DirectType xyz = DirectType::UMFPACK;

namespace Kaskade
{

  template <class Functional>
  struct CardioD2
  {
    template <int row, int col>
    class D2: public Functional::template D2<row,col>
    {
      typedef typename Functional::template D2<row,col> d2;

      public:
	static bool const present = d2::present && (row==0) && (col==0);
	static bool const lumped = true;
    };
  };

/**
 * \ingroup timestepping
 * \brief Extrapolated linearly implicit Euler method.
 *
 * This class implements the extrapolated linearly implicit Euler
 * method for integrating time-dependent evolution problems. The
 * implementation follows Deuflhard/Bornemann Chapter 6.4.3.
 */
template <class Eq>
class Limex
{
public:
  typedef Eq EvolutionEquation;
  typedef typename EvolutionEquation::AnsatzVars::VariableSet State;

private:
  typedef SemiLinearizationAt<SemiImplicitEulerStep<EvolutionEquation> > Linearization;
  typedef VariationalFunctionalAssembler<Linearization> Assembler;

  // for hierarchic error estimator
  typedef FEFunctionSpace<ContinuousHierarchicExtensionMapper<double,typename Eq::AnsatzVars::Grid::LeafGridView> > SpaceEx;
  typedef boost::fusion::vector< typename SpaceType<typename Eq::AnsatzVars::Spaces,0>::type const*, SpaceEx const*> ExSpaces;

//   two components, 1 space -- how to generalize?
//   typedef boost::fusion::vector<VariableDescription<1,1,0>, VariableDescription<1,1,1> > ExVariableDescriptions;

  // 1 component, 1 space -- how to generalize?
  typedef boost::fusion::vector<VariableDescription<1,1,0> > ExVariableDescriptions;

  typedef VariableSetDescription<ExSpaces,ExVariableDescriptions> ExVariableSet;
  typedef HierarchicErrorEstimator<Linearization,ExVariableSet,ExVariableSet,CardioD2<Linearization> > ErrorEstimator;
  typedef VariationalFunctionalAssembler<ErrorEstimator> EstimatorAssembler;

//   typedef typename EstimatorAssembler::template AnsatzVariableRepresentation<> Ansatz;
//   typedef typename EstimatorAssembler::template TestVariableRepresentation<> Test;
/*  typedef typename ExVariableSet::template CoefficientVectorRepresentation<0,1>::type ExCoefficientVectors;*/

  typedef typename Eq::AnsatzVars::Grid::Traits::LeafIndexSet IS ;

public:
  /**
   * Constructs an ODE integrator. The arguments eq and ansatzVars
   * have to exist during the lifetime of the integrator.
   */
  Limex(GridManager<typename EvolutionEquation::AnsatzVars::Grid>& gridManager_,
        EvolutionEquation& eq_, typename EvolutionEquation::AnsatzVars const& ansatzVars_, DirectType st=DirectType::UMFPACK):
    gridManager(gridManager_), ansatzVars(ansatzVars_), eq(&eq_,0),
    assembler(gridManager,ansatzVars.spaces),
    iteSteps(10000), iteEps(1e-6), extrap(0),
    rhsAssemblyTime(0.0), matrixAssemblyTime(0.0), factorizationTime(0.0), solutionTime(0.0), adaptivityTime(0.0),
    solverType(st)
    {}

  /**
   * In order to maintain compatibility with existing code,
   * this overload is needed, as non const reference parameters (refinements) can not
   * have default values. All ist does is call the original method with a temporary
   * parameter.
   */
  State const& step(State const& x, double dt, int order,
                    std::vector<std::pair<double,double> > const& tolX)
  {
    std::vector<std::vector<bool> > tmp ;
    return step(x,dt,order,tolX,tmp);
  }

  /**
   * Computes a state increment that advances the given state in
   * time. The time in the given evolution equation is increased by
   * dt.
   *
   * \param x the initial state to be evolved
   * \param dt the time step
   * \param order the extrapolation order >= 0 (0 corresponds to the linearly implicit Euler)
   * \param tolX
   * \param refinements keep track of the cells marked for refinement
   * \return the state increment (references an internal variable that will be
   *         invalidated by a subsequent call of step)
   *
   * \todo (i) check for B constant, do not reassemble matrix in this case
   *       (ii) implement fixed point iteration instead of new factorization
   *            in case B is not constant
   */
  State const& step(State const& x, double dt, int order,
                    std::vector<std::pair<double,double> > const& tolX,

		    std::vector< std::vector<bool> > &refinements )
  {
    boost::timer::cpu_timer timer;

    std::vector<double> stepFractions(order+1);
    for (int i=0; i<=order; ++i) stepFractions[i] = 1.0/(i+1); // harmonic sequence
    extrap.clear();

    typedef typename EvolutionEquation::AnsatzVars::Grid Grid;

    int const dim = EvolutionEquation::AnsatzVars::Grid::dimension;
    int const nvars = EvolutionEquation::AnsatzVars::noOfVariables;
    int const neq = EvolutionEquation::TestVars::noOfVariables;
    size_t  nnz = assembler.nnz(0,neq,0,nvars,false);
    size_t  size = ansatzVars.degreesOfFreedom(0,nvars);

    std::vector<double> rhs(size), sol(size);

    State dx(x), dxsum(x), tmp(x);
    double const t = eq.time();

    double estTime = 0 ;

    eq.temporalEvaluationRange(t,t+dt);

    typedef AssembledGalerkinOperator<Assembler,0,neq,0,neq> Op;

    bool iterative = true ;
    int fill_lev = /*0*/ /*1*/2;

    typedef typename EvolutionEquation::AnsatzVars::template CoefficientVectorRepresentation<0,neq>::type
      CoefficientVectors;
    typedef typename EvolutionEquation::TestVars::template CoefficientVectorRepresentation<0,neq>::type
      LinearSpaceX;

    for (int i=0; i<=order; ++i) {
      double const tau = stepFractions[i]*dt;
      eq.setTau(tau);
      eq.time(t);

      // mesh adaptation loop. Just once if i>0.
      bool accurate = true;
      int refinement_count = 0 ;

      do {
        // Evaluate and factorize matrix B(t)-tau*J
        dx/* * */= 0;
        timer.start();

// 	assembler.assemble(Linearization(eq,x,x,dx)/*,(Assembler::VALUE|Assembler::RHS|Assembler::MATRIX),4*/);

// #ifndef KASKADE_SEQUENTIAL
// 	gridManager.enforceConcurrentReads(std::is_same<Grid,Dune::UGGrid<dim> >::value);
// 	assembler.setNSimultaneousBlocks(40);
// 	assembler.setRowBlockFactor(2.0);
// #endif
// 	std::cout << "assemble linear system ..." ; std::cout.flush();
	assembler.assemble(Linearization(eq,x,x,dx)/*,(Assembler::VALUE|Assembler::RHS|Assembler::MATRIX),4*/);
// 	std::cout << " done\n"; std::cout.flush();

	matrixAssemblyTime += (double)(timer.elapsed().user)/1e9;

	Op A(assembler);
// 	std::cout << "solve linear system\n"; std::cout.flush();

	if (iterative)
	{
	  timer.start();

	  CoefficientVectors  solution(EvolutionEquation::AnsatzVars::template CoefficientVectorRepresentation<0,neq>::init(ansatzVars.spaces)
	    );
	  solution = 0;
	  CoefficientVectors rhs(assembler.rhs());

	  ILUKPreconditioner<Op> p(A,fill_lev,0);
// 	  ILUTPreconditioner<Op> p(A,240,1e-2,0);
// 	  JacobiPreconditioner<Op> p(A,1.0);

	  Dune::BiCGSTABSolver<LinearSpaceX> cg(A,p,1e-7,2000,0); //verbosity: 0-1-2 (nothing-start/final-every it.)
	  Dune::InverseOperatorResult res;
	  cg.apply(solution,rhs,res);
	  if ( !(res.converged) || (res.iterations == 2001) ) {
	    std::cout << "   no of iterations in cg = " << res.iterations << std::endl;
	    std::cout << " convergence status of cg = " << res.converged  << std::endl;
	    assert(0);
	  }
	  dx.data = solution.data;
	  solutionTime += (double)(timer.elapsed().user)/1e9;
	}
	else
	{
	  // direct solution
	  timer.start();
// 	  assembler.toTriplet(0,neq,0,nvars,ridx.begin(),cidx.begin(),data.begin(),false);
	  MatrixAsTriplet<double> triplet = A.template get<MatrixAsTriplet<double> >();

	  Factorization<double> *matrix = 0;
	  switch (solverType) {
	    case DirectType::UMFPACK:
	      matrix = new UMFFactorization<double>(size,0,triplet.ridx,triplet.cidx,triplet.data);
	      break;
	    case DirectType::UMFPACK3264:
	      matrix = new UMFFactorization<double>(size,0,triplet.ridx,triplet.cidx,triplet.data);
	      break;
// 	    case MUMPS:
// 	      matrix = new MUMPSFactorization<double>(size,0,triplet.ridx,triplet.cidx,triplet.data);
// 	      break;
// 	    case SUPERLU:
// 	      matrix = new SUPERLUFactorization<double>(size,0,triplet.ridx,triplet.cidx,triplet.data);
// 	      break;
	    default:
	      throw -321;
	      break;
	  }
	  factorizationTime += (double)(timer.elapsed().user)/1e9;

	  // First right hand side (j=0) has been assembled together with matrix.
	  timer.start();
	  A.getAssembler().toSequence(0,neq,rhs.begin());
	  for (int k=0; k<rhs.size(); ++k) assert(std::isfinite(rhs[k]));
	  matrix->solve(rhs,sol);
	  delete matrix;
	  for (int k=0; k<sol.size(); ++k) assert(std::isfinite(sol[k]));
	  dx.read(sol.begin());
	  solutionTime += (double)(timer.elapsed().user)/1e9;

// 	  Factorization<double> *matrix = 0;
// 	  switch (solverType)
// 	    {
// 	      case UMFPACK:
// 		matrix = new UMFFactorization<double>(size,0,ridx,cidx,data);
// 		break;
// 	      case UMFPACK3264:
// 		matrix = new UMFFactorization<double>(size,0,ridx,cidx,data);
// 		break;
// 	      case MUMPS:
// 		matrix = new MUMPSFactorization<double>(size,0,ridx,cidx,data);
// 		break;
// 	      case SUPERLU:
// 		matrix = new SUPERLUFactorization<double>(size,0,ridx,cidx,data);
// 		break;
// 	      default:
// 		throw -321;
// 		break;
// 	    }
// 	  factorizationTime += (double)(timer.elapsed().user)/1e9;
//
// 	  // First right hand side (j=0) has been assembled together with matrix.
// 	  timer.start();
// 	  assembler.toSequence(0,neq,rhs.begin());
// 	  for (size_t k=0; k<rhs.size(); ++k) assert(finite(rhs[k]));
// 	  matrix->solve(rhs,sol);
// 	  delete matrix;
// 	  for (size_t k=0; k<sol.size(); ++k) assert(finite(sol[k]));
// 	  dx.read(sol.begin());
// 	  solutionTime += (double)(timer.elapsed().user)/1e9;
	  //end: direct solution
	}

        // Mesh adaptation only if requested and only for the full
        // implicit Euler step (first stage).

	typedef typename EvolutionEquation::AnsatzVars::GridView::template Codim<0>::Iterator CellIterator ;

	accurate = true ;
	timer.start();
	IS const& is = gridManager.grid().leafIndexSet();
	std::vector<double> errorDistribution(is.size(0),0.0);
	double maxErr = 0.0, errNorm = 0.0 ;

        if (!tolX.empty() && i==0) {

	  // embeddedErrorEstimator can not be used for linear finite elements
	  // use HierarchicErrorEstimator instead
	  // preparation for HierarchicErrorEstimator
	  if(true) // to avoid mesh adaptation for exVariables
	  {
	  //TODO: this should be using the gridviews/index sets of the original space, not the leaf!
	  SpaceEx  spaceEx(gridManager,gridManager.grid().leafGridView(), boost::fusion::at_c<0>(ansatzVars.spaces)->mapper().getOrder()+1);
	  typename SpaceType<typename Eq::AnsatzVars::Spaces,0>::type spaceH1 = *(boost::fusion::at_c<0>(ansatzVars.spaces)) ;
	  ExSpaces exSpaces(&spaceH1,&spaceEx);
	  std::string exVarNames[2] = { "ev", "ew" };
	  ExVariableSet exVariableSet(exSpaces, exVarNames);
	  EstimatorAssembler estAssembler(gridManager,exSpaces);

	  tmp/* * */= 0 ;
// 	  estAssembler.assemble(ErrorEstimator(Linearization(eq,x,x,tmp/*dx*/),dx));
	  estAssembler.assemble(ErrorEstimator(semiLinearization(eq,x,x,/*dx*/tmp),dx));

	  int const estNvars = ErrorEstimator::AnsatzVars::noOfVariables;
// 	  std::cout << "estimator: nvars = " << estNvars << "\n";
	  int const estNeq = ErrorEstimator::TestVars::noOfVariables;
// 	  std::cout << "estimator:   neq = " << estNeq << "\n";
	  size_t  estNnz = estAssembler.nnz(0,estNeq,0,estNvars,false);
	  size_t  estSize = exVariableSet.degreesOfFreedom(0,estNvars);

	  std::vector<int> estRidx(estNnz), estCidx(estNnz);
	  std::vector<double> estData(estNnz), estRhs(estSize), estSolVec(estSize);

	  estAssembler.toSequence(0,estNeq,estRhs.begin());

	  typedef typename ExVariableSet::template CoefficientVectorRepresentation<0,estNeq>::type ExCoefficientVectors;

	  // iterative solution of error estimator
// 	  timer.start();
// 	  AssembledGalerkinOperator<EstimatorAssembler> E(estAssembler);
// 	  typename Dune::InverseOperatorResult estRes;
// 	  typename Test::type estRhside( Test::rhs(estAssembler) ) ;
// 	  typename Ansatz::type estSol( Ansatz::init(estAssembler) ) ;
// 	  estSol = 1.0 ;
// 	  JacobiPreconditioner<EstimatorAssembler> jprec(estAssembler, 1.0);
// 	  jprec.apply(estSol,estRhside); //single Jacobi iteration
// 	  estTime += (double)(timer.elapsed().user)/1e9;
// 	  estSol.write(estSolVec.begin());

	  typedef AssembledGalerkinOperator<EstimatorAssembler> AssEstOperator;
	  AssEstOperator agro(estAssembler);
	  Dune::InverseOperatorResult estRes;
	  ExCoefficientVectors estRhside(estAssembler.rhs());
	  ExCoefficientVectors estSol(ExVariableSet::template CoefficientVectorRepresentation<0,estNeq>::init(exVariableSet.spaces));
	  estSol = 1.0 ;
	  JacobiPreconditioner<AssEstOperator> jprec(agro, 1.0);
	  jprec.apply(estSol,estRhside); //single Jacobi iteration
// 	  estTime += (double)(timer.elapsed().user)/1e9;
	  estSol.write(estSolVec.begin());

	  //--

	  // Transfer error indicators to cells.
	  CellIterator ciEnd = ansatzVars.gridView.template end<0>() ;
	  for (CellIterator ci=ansatzVars.gridView.template begin<0>(); ci!=ciEnd; ++ci) {
	    typedef typename SpaceEx::Mapper::GlobalIndexRange GIR;
	    double err = 0.0;
	    GIR gix = spaceEx.mapper().globalIndices(*ci);
	    for (typename GIR::iterator j=gix.begin(); j!=gix.end(); ++j)
	      err += fabs(boost::fusion::at_c<0>(estSol.data)[*j]);
	    // only for 1st component -- second in monodomain is ODE
	    errorDistribution[is.index(*ci)] = err;
	    if (fabs(err)>maxErr) maxErr = fabs(err);
	  }
// 	  std::cout << "maxErr: " << maxErr << "\n";

// 	  for( size_t cno = 0 ; cno < is.size(0); cno++ )
// 	    std::cout << cno << "\t\t" << errorDistribution[cno] << "\n";
// 	  std::cout.flush();

	  //TODO: estimate only error in PDE, not ODE!
	  // what about relative accuracy?
	  for (size_t k = 0; k < estRhs.size() ; k++ ) errNorm +=  fabs( estRhs[k] * estSolVec[k] );
	  // this is for all variables?!
// 	  errNorm = tau/*dt*/*sqrt(errNorm);
	  std::cout << "errNorm = " << errNorm << std::endl ;
	  std::cout.flush();

	  }

	  double overallTol = tolX[0].first ;

// 	  for( int i = 0 ; i < 1 /*nvars*/ ; i++ )
// 	    overallTol += tolX[i].first*tolX[i].first ;
// 	  overallTol = sqrt(overallTol);

// 	  std::cout << "overallTol = " << overallTol << std::endl ;

// 	  alpha = 0 ; // for uniform refinement

	  int nRefMax = /*10*/7 ;/*7*//*5*//*3*//*15*/ /*4*/ /*5*/ /*10*/
	  double fractionOfCells = 0.05; /*0.1,0.2*/
	  unsigned long noToRefine = 0, noToCoarsen = 0;
	  double alpha = 1.0; /*0.5; //test april 13*/

	  double errLevel = 0.5*maxErr ; //0.75
// 	  double minRefine = 0.05;

// 	  if (minRefine>0.0)
// 	  {
// 	    std::vector<double> eSort(errorDistribution);
// 	    std::sort(eSort.begin(),eSort.end(),fabscompare);
// 	    int minRefineIndex = minRefine*(eSort.size()-1);
// 	    double minErrLevel = fabs(eSort[minRefineIndex])+1.0e-15;
// 	    if (minErrLevel<errLevel)
// 	      errLevel = minErrLevel;
// 	  }

// 	  double minRefine = fractionOfCells; //* gridManager.grid().size(0);
// 	  if (minRefine >0.0)
// 	  {
// 	    std::vector<double> eSort(errorDistribution);
// 	    std::sort(eSort.begin(),eSort.end(),fabscompare);
// 	    int minRefineIndex = minRefine*(eSort.size()-1);
// 	    double minErrLevel = fabs(eSort[minRefineIndex])+1.0e-15;
// 	    if (minErrLevel<errLevel)
// 	      errLevel = minErrLevel;
// 	  }

	  size_t maxNoOfVertices =/* 500000*/ /*2000000*//*12000*/30000;
	  size_t maxNoOfElements = 2000000;
	  int maxLevel = /*20*/6 /*25*/ /*18*/; // additional refinement levels

	  if( errNorm > overallTol && refinement_count < nRefMax
	    && gridManager.grid().size(/*dim*/0) < /*maxNoOfVertices*/maxNoOfElements )
	    //current setup: for 2D BFGS; for 3D fibrillation: use maxNoOfElements
	  {
// 	    accurate = false;
// 	    std::vector<bool> toRefine( is.size(0), false ) ;
// 	    size_t noCells = gridManager.grid().size(0);
// // 	    std::cout << is.size(0) << "   " << noCells << "\n"; std::cout.flush();
// 	    for( size_t cno = 0 ; cno < noCells ; cno++ )
// 	    {
// 	      if (fabs(errorDistribution[cno]) >= alpha*errLevel)
// 	      {
// 		noToRefine++;
// 		toRefine[cno] = true ;
// 	      }
// 	    }
	    std::vector<bool> toRefine( is.size(0), false ) ; //for adaptivity in compression

	    accurate = false ;
	    size_t noCells = gridManager.grid().size(0);

// 	    // Nagaiah paper:
// 	    unsigned minLevel = 10 ; // do not coarsen below that level
// 	    CellIterator ciEnd = ansatzVars.gridView.template end<0>() ;
// 	    for (CellIterator ci=ansatzVars.gridView.template begin<0>(); ci!=ciEnd; ++ci)
// 	    {
// 	      double vol = sqrt( ci->geometry().volume() );
// 	      if (fabs(errorDistribution[is.index(*ci)])/vol >= 0.2 /*0.1*/ )
// 	      {
// 		if( ci->level() < maxLevel && gridManager.grid().size(dim) < maxNoOfVertices )
// 		{
// 		  gridManager.mark(1,*ci);
// 		  noToRefine++;
// 		}
// 	      }
// 	      else if (fabs(errorDistribution[is.index(*ci)])/vol < 0.1 /* /2 */ && ci->level() > minLevel )
// 	      {
// 		gridManager.mark(-1,*ci);
// 		noToCoarsen++;
// 	      }
// 	    }
// 	    std::cout << "refine: " << noToRefine << "    coarsen: " << noToCoarsen << "\n";
// 	    gridManager.countMarked();

// 	    last implementation was the following:
	    unsigned long noToRefineOld = 0;
	    do{
	      noToRefineOld = noToRefine;
	      for( size_t cno = 0 ; cno < noCells ; cno++ )
	      {
		      if (fabs(errorDistribution[cno]) >= alpha*errLevel)
		      { // TODO: check and bugfix this!
			      if( !toRefine[cno] ) noToRefine++;
			      toRefine[cno] = true ;
		      }
	      }
// 		std::cout << "alpha: " << alpha << "\trefine: " << noToRefine << "\n";
// 		std::cout.flush();
		alpha *= 0.75; // refine more aggressively: reduce alpha by larger amount
	    } while ( noToRefine < fractionOfCells * noCells && (!noToRefineOld == noToRefine) ) ;
//   	    std::cout << "to refine: " << noToRefine << "\n";
	    std::cout.flush();

// 	    alpha = 0.5; //50
// 	    do {
// 	      for( size_t cno = 0 ; cno < noCells ; cno++ )
// 		if (fabs(errorDistribution[cno]) > alpha*maxErr/*overallTol/noCells*/) {
// 		    /*if( !toRefine[cno] )*/ noToRefine++;
// 		    toRefine[cno] = true;
// 		  }
//
// 		  alpha *=0.9;
// 	    } while (noToRefine < fractionOfCells * noCells);

// 	    long int nRefined = (long int) std::count( toRefine.begin(), toRefine.end(), true );
// 	    std::cout << "   noToRefine = " << nRefined << std::endl;
// 	    refinements.push_back(toRefine);

	    for (CellIterator ci=ansatzVars.gridView.template begin<0>(); ci!=ansatzVars.gridView.template end<0>(); ++ci)
	      if( toRefine[is.index(*ci)] && ci->level() < maxLevel )  gridManager.mark(1,*ci);

	    bool refok = gridManager.adaptAtOnce();  // something has been refined?
	    if( !refok )
	    {
	      // nothing has been refined
	      std::cout << "nothing has been refined (probably due to maxLevel constraint), stopping\n";
	      accurate = true ;
	    }

	    if (!accurate) {
	      refinement_count++ ;
	      nnz = assembler.nnz(0,neq,0,nvars,false);
	      size = ansatzVars.degreesOfFreedom(0,nvars);
	      rhs.resize(size);
	      sol.resize(size);
	    }
	  }
	  else
	  {
	   if( errNorm > overallTol && refinement_count > nRefMax )
	     std::cout << "max no of refinements exceeded\n";
// 	   if( errNorm > overallTol && gridManager.grid().size(2) > maxNoOfVertices )
// 	     std::cout << "max no of nodes exceeded\n";

	   accurate = true ;
	  }
        }
      } while (!accurate);
      adaptivityTime += (double)(timer.elapsed().user)/1e9;

//       std::cout << gridManager.grid().size(dim) << " nodes on " << gridManager.grid().maxLevel() << "levels\n";

      dxsum = dx;

      // propagate by linearly implicit Euler
      for (int j=1; j<=i; ++j) {
	// Assemble new right hand side tau*f(x_j)
        eq.time(eq.time()+tau);
	tmp = x; tmp += dxsum;
        State zero(x); zero /* * */= 0;
        timer.start();
// 	gridManager.enforceConcurrentReads(true);
// 	assembler.setNSimultaneousBlocks(40);
// 	assembler.setRowBlockFactor(2.0);
        assembler.assemble(Linearization(eq,tmp,x,zero),Assembler::RHS|Assembler::MATRIX,4);
        rhsAssemblyTime += (double)(timer.elapsed().user)/1e9;

	Op A(assembler);

	if( !iterative )
	{
	  timer.start();
	  //direct solution
	  A.getAssembler().toSequence(0,neq,rhs.begin());
	  MatrixAsTriplet<double> triplet = A.template get<MatrixAsTriplet<double> >();
	  Factorization<double> *matrix = 0;
	  switch (solverType) {
	    case DirectType::UMFPACK:
	      matrix = new UMFFactorization<double>(size,0,triplet.ridx,triplet.cidx,triplet.data);
	      break;
	    case DirectType::UMFPACK3264:
	      matrix = new UMFFactorization<double>(size,0,triplet.ridx,triplet.cidx,triplet.data);
	      break;
// 	    case MUMPS:
// 	      matrix = new MUMPSFactorization<double>(size,0,triplet.ridx,triplet.cidx,triplet.data);
// 	      break;
// 	    case SUPERLU:
// 	      matrix = new SUPERLUFactorization<double>(size,0,triplet.ridx,triplet.cidx,triplet.data);
// 	      break;
	    default:
	      throw -321;
	      break;
	  }
	  factorizationTime += (double)(timer.elapsed().user)/1e9;
	  timer.start();
	  matrix->solve(rhs,sol);
	  delete matrix;
	  for (int k=0; k<rhs.size(); ++k) assert(std::isfinite(rhs[k]));
	  for (int k=0; k<sol.size(); ++k) assert(std::isfinite(sol[k]));
	  dx.read(sol.begin());
	  solutionTime += (double)(timer.elapsed().user)/1e9;
	}
	else
	{
	  timer.start();
	  CoefficientVectors  solution(EvolutionEquation::AnsatzVars::template
		    CoefficientVectorRepresentation<0,neq>::init(ansatzVars.spaces) );
	  solution = 0;
	  // 	  assembler.assemble(linearization(F,x));
	  CoefficientVectors rhs(assembler.rhs());

	  ILUKPreconditioner<Op> p(A,fill_lev,0);
// 	  ILUTPreconditioner<Op> p(A,240,1e-2,0);
// 	  JacobiPreconditioner<Op> p(A,1.0);

	  Dune::BiCGSTABSolver<LinearSpaceX> cg(A,p,1e-7,2000,0); //verbosity: 0-1-2 (nothing-start/final-every it.)
	  Dune::InverseOperatorResult res;
	  cg.apply(solution,rhs,res);
	  if ( !(res.converged) || (res.iterations == 2001) ) {
	    std::cout << "   no of iterations in cg = " << res.iterations << std::endl;
	    std::cout << " convergence status of cg = " << res.converged  << std::endl;
	    assert(0);
	  }
	  dx.data = solution.data;
// 	// iterative solution
//         timer.start();
// 	typedef typename Assembler::template TestVariableRepresentation<>::type Rhs;
// 	typedef typename Assembler::template AnsatzVariableRepresentation<>::type Sol;
// 	Sol solution(Assembler::template AnsatzVariableRepresentation<>::init(assembler));
// 	Rhs rhside(Assembler::template TestVariableRepresentation<>::rhs(assembler));
// 	AssembledGalerkinOperator<Assembler,0,1,0,1> A(assembler, false);
// 	typename AssembledGalerkinOperator<Assembler,0,1,0,1>::matrix_type tri(A.getmat());
//
//
//         typedef typename Assembler::template TestVariableRepresentation<>::type LinearSpace;
// 	Dune::InverseOperatorResult res;
// 	solution = 1.0;
//      	JacobiPreconditioner<Assembler,0,1,0> p(assembler,1.0);
// // 	TrivialPreconditioner<LinearSpace> trivial;
// 	Dune::CGSolver<LinearSpace> cg(A,/*trivial*/p,iteEps,iteSteps,0);
// 	cg.apply(solution,rhside,res);
// 	A.applyscaleadd(-1.0,rhside,solution);
// 	double slntime = (double)(timer.elapsed().user)/1e9;
// 	solutionTime += slntime ;
// 	dx.data = solution.data ;
// 	// end: iterative solution
	}

        dxsum += dx;
        solutionTime += (double)(timer.elapsed().user)/1e9;
      }

      // insert into extrapolation tableau
      extrap.push_back(dxsum,stepFractions[i]);

      // restore initial time
      eq.time(t);
    }

    return extrap.back();
  }

  /**
   * Estimates the time discretization error of the previously
   * computed step by taking the difference between the diagonal and
   * subdiagonal extrapolation values of maximal order. This requires
   * that order>1 has been given for the last step.
   */
  std::vector<std::pair<double,double> > estimateError(State const& x,int i, int j) const
  {
    assert(extrap.size()>1);

    std::vector<std::pair<double,double> > e(ansatzVars.noOfVariables);

    relativeError(typename EvolutionEquation::AnsatzVars::Variables(),extrap[i].data,
                  extrap[j].data,x.data,
                  ansatzVars.spaces,eq.scaling(),e.begin());

    return e;
  }

  template <class OutStream>
  void reportTime(OutStream& out) const {
    out << "Limex time: " << matrixAssemblyTime << "s matrix assembly\n"
        << "            " << rhsAssemblyTime << "s rhs assembly\n"
        << "            " << factorizationTime << "s factorization\n"
        << "            " << solutionTime << "s solution\n"
	<< "            " << adaptivityTime << "s adaptivity\n";
  }

  void advanceTime(double dt) { eq.time(eq.time()+dt); }

private:
  GridManager<typename EvolutionEquation::AnsatzVars::Grid>& gridManager;
  typename EvolutionEquation::AnsatzVars const& ansatzVars;
  SemiImplicitEulerStep<EvolutionEquation>      eq;
  Assembler                                     assembler;
  int iteSteps ;
  double iteEps ;

public:
  ExtrapolationTableau<State>  extrap;
  double rhsAssemblyTime, matrixAssemblyTime, factorizationTime, solutionTime, adaptivityTime;
  DirectType solverType;
};

} //namespace Kaskade
#endif


#ifndef SCALARREWRITE_HPP
#define SCALARREWRITE_HPP

#include "scalarbase.hpp"
#include "exprrewrite.hpp"
#include "exprmatch.hpp"
#include <algorithm>
//#include <boost/logiclogic/tribool>
#include "scalarrange.hpp"

bool isgenexp(const expr &e) {
	if (e.isleaf()) {
		if (e.asleaf().type()==typeid(matchleaf)) {
			matchleaf ml = MYany_cast<matchleaf>(e.asleaf());
			if (std::dynamic_pointer_cast<matchany>(ml)
			    || std::dynamic_pointer_cast<matchvar>(ml)
			    || std::dynamic_pointer_cast<matchconstwrt>(ml)
			    || std::dynamic_pointer_cast<matchnonconstwrt>(ml))
				return true;
		}
		return false;
	}
	auto n = std::dynamic_pointer_cast<matchlabelop>(e.asnode());
	if (!n) return false;
	if (e.children().size()!=1) return false;
	return isgenexp(e.children()[0]);
}

expr chainpatternmod(const expr &ex) {
	return ex.map([](const expr &e) {
			if (e.isleaf()) return optional<expr>{};
			auto n = e.asnode();
			if (n==pluschain || n==multiplieschain) {
				auto bop = std::dynamic_pointer_cast<opchain>(n)->baseop;
				auto ch = e.children();
				auto last = ch.back();
				if (isgenexp(last))
					return optional<expr>{in_place,std::make_shared<matchassocop>(std::make_shared<matchremainderop>(bop)),
							ch};
				else return optional<expr>{in_place,std::make_shared<matchassocop>(bop),
							ch};
			}
			return optional<expr>{};
		});
}

struct sortchildren : public rewriterule {
	virtual ruleptr clone() const { return std::make_shared<sortchildren>(*this); }
	optional<vset> vars;
	std::vector<op> cops;

	sortchildren(std::vector<op> comops)
		: cops(comops), vars{} { }
	sortchildren(std::vector<op> comops, vset v)
		: cops(comops), vars(in_place,std::move(v)) {}

	int typeorder(const expr &e) const {
		// TODO: use map/unordered_map
		if (isconst(e)) return 0;
		int add = (isconstexpr(e,vars) ? 0 : 13);
		if (e.isleaf()) return 1+add;
		auto &op = e.asnode();
		if (op==plusop || op==pluschain) return 2+add;
		if (op==multipliesop || op==multiplieschain) return 3+add;
		if (op==powerop) return 4+add;
		if (op==logop) return 5+add;
		//if (op==switchop) return 6+add;
		if (op==condop) return 7+add;
		if (op==condeqop) return 8+add;
		if (op==absop) return 9+add;
		if (op==derivop) return 10+add;
		if (op==integrateop) return 11+add;
		if (op==evalatop) return 12+add;
		return 13+add;
	}

	virtual void setvars(const vset &v) {
		vars = v;
	}

	int secondordering(const expr &e1, const expr &e2) const {
		if (isconst(e1)) {
			scalarreal v1 = getconst<scalarreal>(e1);
			scalarreal v2 = getconst<scalarreal>(e2);
			if (v1<v2) return -1;
			if (v1>v2) return +1;
			return 0;
		}
		if (isvar(e1))
			return MYany_cast<var>(e1.asleaf())->name
					.compare(MYany_cast<var>(e2.asleaf())->name);
		if (e1.isleaf()) return 0;
		auto &ch1 = e1.children();
		auto &ch2 = e2.children();
		for(int i=std::min(ch1.size(),ch2.size())-1;i>=0;i--) {
			int res = exprcmp(ch1[i],ch2[i]);
			if (res!=0) return res;
		}
		return ch1.size()-ch2.size();
	}

	int exprcmp(const expr &e1, const expr &e2) const {
		int o1 = typeorder(e1), o2 = typeorder(e2);
		if (o1!=o2) return o1-o2;
		return secondordering(e1,e2);
	}

	virtual optional<expr> apply(const expr &e) const {
		if (e.isleaf()) return {};
		if (std::find(cops.begin(),cops.end(),e.asnode())==cops.end())
			return {};
		auto &ch = e.children();
		if (ch.size()<2) return {};
		for(int i=1;i<ch.size();i++) {
			if (exprcmp(ch[i-1],ch[i])>0) {
				std::vector<expr> che = ch;
				std::sort(che.begin(),che.end(),
						[this](const expr &e1, const expr &e2) {
							return exprcmp(e1,e2)<0;
						}
				);
				return optional<expr>{in_place,e.asnode(),che};
			}
		}
		return {};
	}
};

scalarset<scalarreal> rangeprop(const expr &e) {
	if (isconst(e)) {
		return {getconst<scalarreal>(e)};
	}
	if (isop(e,pluschain)) {
		auto &ch = e.children();
		if (ch.empty()) return {-std::numeric_limits<scalarreal>::infinity(),
					std::numeric_limits<scalarreal>::infinity()};
		auto ret = rangeprop(ch[0]);
		for(int i=1;i<ch.size();i++)
			ret = ret.combine(rangeprop(ch[i]),
					[](const range<scalarreal> &a, const range<scalarreal> &b) {
						return range<scalarreal>{a.first+b.first,
									a.second+b.second}; });
		return ret;
	}
	if (isop(e,multiplieschain)) {
		auto &ch = e.children();
		if (ch.empty()) return {-std::numeric_limits<scalarreal>::infinity(),
					std::numeric_limits<scalarreal>::infinity()};
		auto ret = rangeprop(ch[0]);
		for(int i=1;i<ch.size();i++)
			ret = ret.combine(rangeprop(ch[i]),
					[](const range<scalarreal> &a, const range<scalarreal> &b) {
						auto v1=a.first*b.first, v2=a.second*b.second,
							v3 = a.first*b.second, v4=a.second*b.first;
						return range<scalarreal>{
							std::min(std::min(v1,v2),std::min(v3,v4)),
							std::max(std::max(v1,v2),std::max(v3,v4))};
							});
		return ret;
	}
	if (isop(e,absop)) {
		return rangeprop(e.children()[0]).modify(
				[](const range<scalarreal> &a) {
					if (a.first>=0) return a;
					if (a.second<=0) return range<scalarreal>{-a.second,-a.first};
					return range<scalarreal>{scalarreal{0},std::max(-a.first,a.second)};
					});
	}
	if (isop(e,powerop)) {
		return rangeprop(e.children()[0]).combinemult(
				rangeprop(e.children()[1]),
				[](const range<scalarreal> &b, const range<scalarreal> &p) {
					scalarset<scalarreal> ret;
					limpt<scalarreal> zero(scalarreal{0});
					if (b.first>zero) {
						auto v1=pow(b.first,p.first),
							v2=pow(b.second,p.second),
							v3=pow(b.first,p.second),
							v4=pow(b.second,p.first);
						ret.x.emplace(
							std::min(std::min(v1,v2),std::min(v3,v4)),
							std::max(std::max(v1,v2),std::max(v3,v4)));
						return ret;
					}
					if (b.second>zero) {
						auto v1=pow(zero,p.first),
							v2=pow(b.second,p.second),
							v3=pow(zero,p.second),
							v4=pow(b.second,p.first);
						v1.closed = v3.closed = false;
						ret.x.emplace(
							std::min(std::min(v1,v2),std::min(v3,v4)),
							std::max(std::max(v1,v2),std::max(v3,v4)));
					}
					// what to do with negative base???
					if (p.first.pt==p.second.pt) { // single exponent
						if (p.first.pt.isint()) {
							auto v1 = pow(zero,p.first);
							auto v2 = pow(b.first,p.first);
							ret.x.emplace(v1,v2);
						}
					} else {
						limpt<scalarreal> b2 = b.second.pt>=zero ? zero :
								(b.second.pt.isint() ? b.second :
									(limpt<scalarreal>{
										b.first.pt.floor()}));
						limpt<scalarreal> b1 = b2-limpt<scalarreal>{scalarreal{1}};
						if (b1>b.first) {
							auto v1 = pow(b2,p.first);
							auto v2 = pow(b1,p.first);
							auto v3 = pow(b2,p.second);
							auto v4 = pow(b1,p.second);
							ret.x.emplace(
								std::min(std::min(v1,v2),
									std::min(v3,v4)),
								std::max(std::max(v1,v2),
									std::max(v3,v4)));
						} else {
							auto v1 = pow(b2,p.first);
							auto v3 = pow(b2,p.second);
							ret.x.emplace(v1,v3);
						}
					}
					/* not sure why this was the code... seems very wrong
					 * p not even mentioned!
					 */
					/*
					for(auto ip = b.first.closed
								|| !b.first.pt.iseven() ?
							ceil(b.first.pt/2)*2
							: ceil(b.first.pt/2+1)*2;
							ip<0 && b.second>ip;ip+=2) {
						std::cout << "looking at " << tostring(b.first.pt) << ' ' << tostring(b.second.pt) << ' ' << tostring(ip) << std::endl;
						ret.x.emplace(
							b.first<=0 && b.second>=0 ?
								scalarreal{0} : pow(
									std::min(abs(b.first),
										abs(b.second)),ip),
							pow(std::max(abs(b.first),
									abs(b.second)),ip));
					}
					*/

				});
	}
	if (isop(e,logop)) {
		return rangeprop(e.children()[0]).modify(
				[](range<scalarreal> a) {
					a.first.pt = log(a.first.pt);
					a.second.pt = log(a.second.pt);
					return a;
					});
	}
	if (isop(e,heavisideop)) {
		return rangeprop(e.children()[0]).modify(
				[](const range<scalarreal> &a) {
					return range<scalarreal>{a.first < 0 ? 0 : 1,
								a.second < 0 ? 0 : 1};
					return a;
				});
	}
	if (isop(e,diracop)) {
		return rangeprop(e.children()[0]).modify(
				[](const range<scalarreal> &a) {
					return range<scalarreal>{scalarreal{0},
						a.overlap(range<scalarreal>(0,0)) ?
							std::numeric_limits<scalarreal>::infinity()
							: scalarreal{0}};
				});
	}
	return {-std::numeric_limits<scalarreal>::infinity(),
					std::numeric_limits<scalarreal>::infinity()};
}

// perhaps these should do their own simplification first...

bool ispos(const expr &e) {
	auto rs = rangeprop(e);
	if (rs.x.empty()) return false; //???
	for(auto &r : rs.x)
		if (r.first<=0) return false;
	return true;
}
bool isneg(const expr &e) {
	auto rs = rangeprop(e);
	if (rs.x.empty()) return false; //???
	for(auto &r : rs.x)
		if (r.second>=0) return false;
	return true;
}
bool isnonneg(const expr &e) {
	auto rs = rangeprop(e);
	if (rs.x.empty()) return false; //???
	for(auto &r : rs.x)
		if (r.first<0) return false;
	return true;
}
bool isnonpos(const expr &e) {
	auto rs = rangeprop(e);
	if (rs.x.empty()) return false; //???
	for(auto &r : rs.x)
		if (r.second>0) return false;
	return true;
}
bool isconst(const expr &e, scalarreal k) {
	auto rs = rangeprop(e);
	if (rs.x.empty()) return false; //???
	for(auto &r : rs.x)
		if (r.first!=k || r.second!=k) return false;
	return true;
}
bool iseven(const expr &e) {
	auto rs = rangeprop(e);
	if (rs.x.empty()) return false; //???
	for(auto &r : rs.x)
		if (r.first!=r.second || !r.first.pt.iseven()) return false;
	return true;
}
bool isodd(const expr &e) {
	auto rs = rangeprop(e);
	if (rs.x.empty()) return false; //???
	for(auto &r : rs.x)
		if (r.first!=r.second || !r.first.pt.isodd()) return false;
	return true;
}

struct simpcond : public rewriterule {
	virtual ruleptr clone() const { return std::make_shared<simpcond>(*this); }
	virtual optional<expr> apply(const expr &e) const {
		if (!isop(e,condop) && !isop(e,condeqop)) return {};
		auto rngset = rangeprop(e.children()[0]);
		if (isop(e,condop)) {
			bool hasless=false, hasgreq=false;
			for(auto &rng : rngset.x) {
				if (rng.first<0) {
					hasless=true;
					if (hasgreq) break;
				}
				if (rng.second>=0) {
					hasgreq=true;
					if (hasless) break;
				}
			}
			if (!hasgreq) return optional<expr>{in_place,e.children()[1]};
			if (!hasless) return optional<expr>{in_place,e.children()[2]};
			return {};
		} else {
			bool hasnotzero=false, haszero=false;
			for(auto &rng : rngset.x) {
				if (rng.first!=0 || rng.second!=0) {
					hasnotzero=true;
					if (haszero) break;
				}
				if (rng.first<=0 && rng.second>=0) {
					haszero=true;
					if (hasnotzero) break;
				}
			}
			if (!hasnotzero) return optional<expr>{in_place,e.children()[1]};
			if (!haszero) return optional<expr>{in_place,e.children()[2]};
			return {};
		}
	}
};

/*
struct normswitch : public rewriterule {
	virtual ruleptr clone() const { return std::make_shared<normswitch>(*this); }
	virtual optional<expr> apply(const expr &e) const {
		if (!isop(e,switchop)) return {};
		auto &c0 = e.children()[0];
		expr tosub = scalar(0);
		if (c0.isleaf()) {
			if (!isconst(c0) || getconst<scalarreal>(c0)==0)
				return {};
			tosub = c0;
		} else {
			if (!isop(c0,pluschain) || c0.children().empty()
					|| !isconst(c0.children()[0])
					|| getconst<scalarreal>(c0.children()[0])==0)
				return {};
			tosub = c0.children()[0];
		}
		std::vector<expr> newch(e.children());
		for (int i=0;i<newch.size();i+=2)
			newch[i] = newch[i] - tosub;
		return optional<expr>{in_place,e.asnode(),newch};
	}
};

struct liftswitch : public rewriterule {
	virtual ruleptr clone() const { return std::make_shared<liftswitch>(*this); }
	static expr replacech(const expr &e, int chi, const expr &ch) {
		auto &ech = e.children();
		std::vector<expr> newch;
		newch.reserve(ech.size());
		for(int i=0;i<ech.size();i++)
			if (i==chi) newch.emplace_back(ch);
			else newch.emplace_back(ech[i]);
		return {e.asnode(),newch};
	}

	virtual optional<expr> apply(const expr &e) const {
		if (e.isleaf() || e.asnode()==switchop) return {};
		auto &ch = e.children();
		for(int i=0;i<ch.size();i++)
			if (isop(ch[i],switchop)) {
				auto &sch = ch[i].children();
				std::vector<expr> newch;
				for(int j=0;j<sch.size();j+=2) {
					newch.emplace_back(sch[j]);
					newch.emplace_back(replacech(e,i,sch[j+1]));
				}
				return optional<expr>{in_place,switchop,newch};
			}
		return {};
	}
};

struct squeezeswitch : public rewriterule {
	virtual ruleptr clone() const { return std::make_shared<squeezeswitch>(*this); }
	virtual optional<expr> apply(expr &e) const {
		if (!isop(e,switchop)) return {};
		auto &ch = e.children();
		if (ch.size()<5) {
			if (ch[1]==ch[3]) // technically, this might expand
				// the domain (if ch[0] is undefined in places)
				return optional<expr>{in_place,ch[1]};
			return {};
		}
		for(int i=2;i<ch.size()-1;i+=2)
			if (ch[i-1]==ch[i+1]) {
				std::vector<expr> newch;
				newch.reserve(ch.size()-2);
				for(int j=0;j<i;j++) newch.emplace_back(ch[j]);
				for(int j=i+2;j<ch.size();j++) newch.emplace_back(ch[j]);
				return optional<expr>{in_place,e.asnode(),newch};
			}
		return {};
	}
};

struct mergeswitch : public rewriterule {
	virtual ruleptr clone() const { return std::make_shared<mergeswitch>(*this); }
	static bool ismergeable(const expr &e, const expr &te, bool checkte=true) {
		if (!isop(e,switchop)) return false;
		auto &ch = e.children();
		if (checkte && !(ch[0]==te)) return false;
		scalarreal lastk = 0;
		for(int i=2;i<ch.size();i+=2) {
			if (!isconst(ch[i])) return false;
			if (i>2) {
				scalarreal newk = getconst<scalarreal>(ch[i]);
				if (newk<lastk) return false;
				lastk = newk;
			}
		}
		return true;
	}

	virtual optional<expr> apply(expr &e) const {
		if (!ismergeable(e,e,false)) return {};
		auto &ch = e.children();
		int mchi = -1;
		scalarreal preth = -std::numeric_limits<scalarreal>::infinity();
		scalarreal postth = std::numeric_limits<scalarreal>::infinity();
		for(int i=1;i<ch.size();i+=2)
			if (ismergeable(ch[i],ch[0])) {
				mchi = i;
				if (i>1) preth = getconst<scalarreal>(ch[i-1]);
				if (i+1 < ch.size()) postth = getconst<scalarreal>(ch[i+1]);
				break;
			}
		if (mchi==-1) return {};

		auto &mch = ch[mchi].children();
		std::vector<expr> newch;
		for(int i=0;i<mchi;i++)
			newch.emplace_back(ch[i]);
		newch.reserve(ch.size()+mch.size()-2);
		for(int i=1;i<mch.size();i+=2) {
			scalarreal th = (i+1<mch.size() ? getconst<scalarreal>(mch[i+1])
							: std::numeric_limits<scalarreal>::infinity());
			if (th > preth) {
				newch.emplace_back(mch[i]);
				if (th < postth && i+1<mch.size()) newch.emplace_back(mch[i+1]);
				else break;
			}
		}
		for(int i=mchi+1;i<ch.size();i++)
			newch.emplace_back(ch[i]);
		return optional<expr>{in_place,e.asnode(),newch};
	}
};
*/

ruleptr SRR(const expr &s, const expr &p) {
	return SR(chainpatternmod(s),p);
}

template<typename F>
ruleptr SRR(const expr &s, const expr &p, F &&f) {
	return SR(chainpatternmod(s),p,std::forward<F>(f));
}

template<typename T>
struct bigopexpand : public rewriterule {
	virtual ruleptr clone() const { return std::make_shared<bigopexpand>(*this); }
	int nterms;
	op bigop, chainop;
	T id;

	bigopexpand(int n, op bop, op cop, T i) : nterms(n), bigop(bop), chainop(cop), id(i) {}

	virtual optional<expr> apply(const expr &e) const {
		if (!isop(e,bigop)) return {};
		auto &ch = e.children();
		if (!isconst(ch[2]) || !isconst(ch[3])) return {};
		scalarreal x0 = getconst<scalarreal>(ch[2]);
		scalarreal x1 = getconst<scalarreal>(ch[3]);
		if (x1 < x0) return optional<expr>{in_place,scalar(id)};
		if (x1 >= x0+nterms) return {};
		std::vector<expr> terms;
		for(scalarreal x=x0;x<=x1;x+=1)
			terms.emplace_back(substitute(ch[1],ch[0],scalar(x)));
		return optional<expr>{in_place,chainop,terms};
	}
};

expr x_ = newvar<scalarreal>("x_");
expr notx_{makematchleaf<matchconstwrt>(vset{{getvar(x_)}})};
expr k1_ = L(1,notx_);
expr k2_ = L(2,notx_);
expr k3_ = L(3,notx_);
expr k4_ = L(4,notx_);
expr k5_ = L(5,notx_);
expr k6_ = L(6,notx_);
expr k7_ = L(7,notx_);
expr k8_ = L(8,notx_);
expr k9_ = L(9,notx_);

struct tableintegrate : public rewriterule {
	virtual ruleptr clone() const { return std::make_shared<tableintegrate>(*this); }
	std::vector<std::pair<expr,expr>> antiderivs;

	tableintegrate(std::vector<std::pair<expr,expr>> ader)
			: antiderivs(std::move(ader)) {}

	virtual optional<expr> apply(const expr &e) const {
		if (!isop(e,integrateop)) return {};
		auto &ch = e.children();
		expr newint = substitute(ch[1],ch[0],x_);
		for(auto &ad : antiderivs) {
			auto res = match(newint,ad.first);
			if (res) {
				expr ader = substitute(ad.second,*res);
				expr ret = substitute(ader,x_,ch[3])
					- substitute(ader,x_,ch[2]);
				return optional<expr>{in_place,std::move(ret)};
			}
		}
		return {};
	}
};

template<typename E1, typename E2>
std::pair<expr,expr> ADR(E1 &&e, E2 &&ad) {
	return std::make_pair(chainpatternmod(std::forward<E1>(e)),
			std::forward<E2>(ad));
}

std::vector<std::pair<expr,expr>> stdantiderivs
	{{
		 ADR(  k1_,
	            P1_*x_   ),

		 ADR(  x_,
		       x_*x_/2   ),

		 // TODO:
		 // when k1_<0 this doesn't work if the integral goes over the pole (@ x_=0)
		 // similar problem with the other rational function antiderivatives below!
		 ADR(  pow(x_,k1_),
			  ifeqthenelse(P1_+1,log(abs(x_)),pow(x_,P1_+1)/(P1_+1))   ),

		 ADR(  pow(x_ + k2_,k3_),
			  ifeqthenelse(P3_+1, log(abs(x_+P2_)),
							pow(x_+P2_,P3_+1)/(P3_+1))),

		 ADR(  pow(k1_*x_ + k2_,k3_),
			  ifeqthenelse(P1_,pow(P2_,P3_)*x_,
				  ifeqthenelse(P3_+1, log(abs(P1_*x_+P2_))/P1_,
					  			pow(P1_*x_+P2_,P3_+1)/(P3_+1)/P1_))),

		 // added to make example in proposal work... needs to be
		 // more general
		 ADR(  pow(k1_,k2_*x_),
				 ifeqthenelse(P2_,x_,
					 pow(P1_,P2_*x_)/(P2_*log(P1_)))),

	 }};

// all of the next few "helpers" should perhaps be moved elsewhere
// they may also need to be made more efficient and perhaps given
// their own algebraic symbols to be reasonable about directly
// (for instance, we probably don't want to be evaluating "factorial"
//  or "choose" directly)
expr factorial(const expr &n) {
	expr i = newvar<scalarreal>();
	return prod(i,i,scalar(1),n);
}

expr nchoosek(const expr &n, const expr k) {
	return factorial(n)/(factorial(k)*factorial(n-k));
}

expr B0(const expr &m) {
	expr k=newvar<scalarreal>(), v=newvar<scalarreal>();

	// pos series
	//return sum(sum(pow(scalar(-1),v)*nchoosek(k,v)*pow(v+1,m)/(k+1),v,scalar(0),k),k,scalar(0),m);
	// neg series
	return sum(sum(pow(scalar(-1),v)*nchoosek(k,v)*pow(v,m)/(k+1),v,scalar(0),k),k,scalar(0),m);
}

// Bernoulli polynomial
expr B(const expr &n, const expr &m) {
	expr k = newvar<scalarreal>();
	return sum(nchoosek(n,k)*B0(n-k)*pow(m,k),k,scalar(0),n);
}

expr psum(const expr &p, const expr &n) {
	expr k = newvar<scalarreal>();
	return sum(nchoosek(p,k)*B0(p-k)*pow(-1,p-k)/(k+1)*pow(n,k+1),k,scalar(0),p);
}

// TODO:  will need to be separated out into general and specific to scalars
//std::vector<ruleptr>

ruleset basicscalarrules
	{{toptr<trivialconsteval>(),
	  toptr<scopeeval>(),
	  toptr<sortchildren>(std::vector<op>{pluschain,multiplieschain}),

  SRR(E1_ - E2_                          ,  P1_ + -1*P2_                   ),
  SRR(-E1_                               ,  -1*P1_                         ),

  SRR(E1_ / E2_                          ,  P1_ * pow(P2_,-1)              ),

  /*
  SRR(E1_ + (E2_ + E3_)                  ,  P1_ + P2_ + P3_                  ),
  SRR( E1_ + (E2_ + E3_) + E4_           ,  P1_ + P2_ + P3_ + P4_            ),

  SRR(E1_ * (E2_ * E3_)                  ,  P1_ * P2_ * P3_                  ),
  SRR( E1_ * (E2_ * E3_) * E4_           ,  P1_ * P2_ * P3_ * P4_            ),
  */
  toptr<collapsechain>(pluschain,true),
  toptr<collapsechain>(multiplieschain,true),
  toptr<constchaineval>(pluschain),
  toptr<constchaineval>(multiplieschain),
  toptr<simpcond>(),
  /*
  toptr<normswitch>(),
  toptr<mergeswitch>(),
  toptr<squeezeswitch>(),
  toptr<liftswitch>(),
  */

  // some of these are only true "almost everywhere"
  // and might need to be removed for some applications
  // (or, we need a "domain" to be propagated with the expr)
  //
  SRR( 0 + E1_                          ,  P1_                             ),
  SRR( -0 + E1_                          ,  P1_                             ),
  SRR( 1 * E1_                          ,  P1_                             ),
  SRR( 0 * E1_                          ,  scalar(0)                   ),
  SRR( -0 * E1_                          ,  scalar(0)                   ),
  SRR( pow(E1_,1)                       ,  P1_                             ),
  SRR( pow(1,E1_)                       ,  scalar(1)                   ),
  SRR( pow(0,E1_)                       ,  ifthenelse(P1_,
				scalar(std::numeric_limits<scalarreal>::infinity()),
				scalar(0))                                           ),
  SRR( pow(E1_,0)                       ,  scalar(1)                   ),

  SRR( E1_ + E1_                          ,  2*P1_                           ),
  SRR( E1_ + E1_ + E2_                    ,  2*P1_ + P2_                     ),

  SRR( E1_ * E1_                          ,  pow(P1_,2)                    ),
  SRR( E1_ * E1_ * E2_                    ,  pow(P1_,2)*P2_                ),

  SRR( log(pow(E1_,E2_))                  ,  P2_*log(P1_)                    ),
  SRR( log(E1_*E2_)                       ,  log(P1_) + log(P2_)             ),

  SRR( (W2_ + W3_)*E1_                    ,  P1_*P2_ + P1_*P3_               ),
  SRR( (K2_ + W3_)*E1_                    ,  P1_*P2_ + P1_*P3_               ),
  SRR( (W2_ + K3_)*E1_                    ,  P1_*P2_ + P1_*P3_               ),

  SRR( K1_*E2_ + K3_*E2_                  ,  (P1_+P3_) * P2_                 ),
  SRR( K1_*E2_ + K3_*E2_ + E4_            ,  (P1_+P3_) * P2_ + P4_           ),
  SRR( E2_ + K3_*E2_                      ,  (scalar(1)+P3_) * P2_       ),
  SRR( E2_ + K3_*E2_ + E4_                ,  (scalar(1)+P3_) * P2_ + P4_ ),

  SRR( pow(E1_,E2_) * pow(E1_,E3_)        ,  pow(P1_,P2_+P3_)                ),
  SRR( pow(E1_,E2_) * pow(E1_,E3_) * E4_  ,  pow(P1_,P2_+P3_)*P4_            ),
  SRR( E1_ * pow(E1_,E3_)                 ,  pow(P1_,scalar(1)+P3_)      ),
  SRR( E1_ * pow(E1_,E3_) * E4_           ,  pow(P1_,scalar(1)+P3_)*P4_  ),

  SRR( pow(pow(W1_,E2_),E3_)              ,  pow(P1_,P2_*P3_)                ),
  SRR( pow(pow(K1_,W2_),W3_)              ,  pow(P1_,P2_*P3_)                ),
  SRR( pow(pow(K1_,W2_),K3_)              ,  pow(pow(P1_,P3_),P2_)           ),
  SRR( pow(K1_,K2_*E3_)                   ,  pow(pow(P1_,P2_),P3_)           ),

  SRR( pow(E1_*W2_,E3_)                   ,  pow(P1_,P3_)*pow(P2_,P3_)       ),
  SRR( pow(W1_*E2_,E3_)                   ,  pow(P1_,P3_)*pow(P2_,P3_)       ),
  SRR( pow(K1_,E3_)*pow(K2_,E3_)          ,  pow(P1_*P2_,P3_)                ),

  SRR( log(ifthenelse(E1_,E2_,E3_))    ,  ifthenelse(P1_,log(P2_),log(P3_))  ),

  SRR( abs(E1_)                           ,  P1_ ,
   [](const exprmap &m) { return isnonneg(m.at(1)); } ),

  SRR( abs(E1_)                           ,  -P1_ ,
   [](const exprmap &m) { return isnonpos(m.at(1)); } ),
	 }};

ruleset derivscalarrules{{
  SRR( deriv(E1_,V2_,V3_)                 ,  scalar(0),
      [](const exprmap &m) { return isconstexpr(m.at(1),getvar(m.at(2))); }),
  SRR( deriv(V1_,V1_,V3_)                 ,  scalar(1)                   ),

  SRR( deriv(E1_+E2_,V3_,V4_)             ,
		                           deriv(P1_,P3_,P4_) + deriv(P2_,P3_,P4_) ),

  SRR( deriv(E1_*E2_,V3_,V4_)             ,
		     deriv(P1_,P3_,P4_)*evalat(P2_,P3_,P4_)
		  + evalat(P1_,P3_,P4_)* deriv(P2_,P3_,P4_)                        ),

  SRR( deriv(pow(E1_,E2_),V3_,V4_)        ,
	  evalat(pow(P1_,P2_),P3_,P4_)*evalat(log(P1_),P3_,P4_)*deriv(P2_,P3_,P4_)
   + evalat(P2_,P3_,P4_)*evalat(pow(P1_,P2_-1),P3_,P4_)*deriv(P1_,P3_,P4_) ),

  SRR( deriv(log(E1_),V2_,V3_)            ,
		                          deriv(P1_,P2_,P3_) / evalat(P1_,P2_,P3_) ),

  SRR( deriv(abs(E1_),V2_,V3_)            ,
		  deriv(P1_,P2_,P3_)*ifthenelse(P1_,scalar(-1),scalar(1)) ),

  SRR( deriv(expr{heavisideop,E1_},V2_,V3_),
				 evalat(expr{diracop,P1_},P2_,P3_)*deriv(P1_,P2_,P3_) ),

  // not sure this is right with diracop
  SRR( deriv(ifthenelse(E1_,E2_,E3_),V4_,V5_)   ,
		  evalat(expr{diracop,P1_},P4_,P5_)*deriv(P1_,P4_,P5_)*(E2_-E3_)
		  + ifthenelse(P1_,deriv(P2_,P4_,P5_),deriv(P3_,P4_,P5_))           ),

  SRR( deriv(ifeqthenelse(E1_,E2_,E3_),V4_,V5_)  ,
		  ifeqthenelse(evalat(P1_,P4_,P5_),
			  deriv(P2_,P4_,P5_),deriv(P3_,P4_,P5_))                       ),

  SRR( deriv(integrate(E1_,E2_,E3_,E4_),E5_,E6_)  ,
		  integrate(deriv(P1_,P5_,P6_),P2_,
					evalat(P3_,P5_,P6_),evalat(P4_,P5_,P6_))
		  + deriv(P4_,P5_,P6_)*evalat(evalat(P1_,P2_,P4_),P5_,P6_)
		  - deriv(P3_,P5_,P6_)*evalat(evalat(P1_,P2_,P3_),P5_,P6_)          ),
}};

ruleset integralscalarrules {{
  SRR( integrate(E1_+E2_,E3_,E4_,E5_), integrate(P1_,P3_,P4_,P5_) + integrate(P2_,P3_,P4_,P5_) ),

  SRR( integrate(E1_*E2_,E3_,E4_,E5_), P1_ * integrate(P2_,P3_,P4_,P5_) ,
  		[](const exprmap &m) { return isconstexpr(m.at(1),getvar(m.at(3))); } ),

  SRR( integrate(E1_*E2_,E3_,E4_,E5_), integrate(P1_,P3_,P4_,P5_) * P2_ ,
  		[](const exprmap &m) { return isconstexpr(m.at(2),getvar(m.at(3))); } ),

  SRR( integrate(ifthenelse(E1_+E2_*E3_,E4_,E5_),E3_,E6_,E7_) ,
		  ifthenelse(-P1_/P2_ - P6_,
			  		integrate(P5_,P3_,P6_,P7_),
					ifthenelse(-P1_/P2_ - P7_,
						integrate(P5_,P3_,P6_,-P1_/P2_)
						+ integrate(P4_,P3_,-P1_/P2_,P7_),
						integrate(P4_,P3_,P6_,P7_))),
		  [](const exprmap &m) {
		  	return isconstexpr(m.at(1),getvar(m.at(3)))
					&& isconstexpr(m.at(2),getvar(m.at(3))); } ),

  toptr<tableintegrate>(stdantiderivs),

  SRR( sum(E1_,V2_,E3_,E4_)               , scalar(0),
		  [](const exprmap &m) { return isneg(E4_-E3_); } ),
  SRR( prod(E1_,V2_,E3_,E4_)               , scalar(1),
		  [](const exprmap &m) { return isneg(E4_-E3_); } ),

  // perhaps need "max(0,P4_-P3_+1)"??
  SRR( sum(E1_,V2_,E3_,E4_)               , P1_*(P4_-P3_+1),
      [](const exprmap &m) { return isconstexpr(m.at(1),getvar(m.at(2))); }   ),

  SRR( sum(E1_*E2_,V3_,E4_,E5_)           , P1_*sum(P2_,P3_,P4_,P5_),
	 [](const exprmap &m) { return isconstexpr(m.at(1),getvar(m.at(3))); }   ),
  SRR( sum(E1_*E2_,V3_,E4_,E5_)           , sum(P1_,P3_,P4_,P5_)*P2_,
	 [](const exprmap &m) { return isconstexpr(m.at(2),getvar(m.at(3))); }   ),
}};

ruleset sumprodscalarrules {{
  toptr<bigopexpand<scalarreal>>(3,sumop,pluschain,0),
  toptr<bigopexpand<scalarreal>>(3,prodop,multiplieschain,1),

  SRR( sum(V2_,V2_,E3_,E4_)      , ifthenelse(P4_-P3_,scalar(0),(P4_+1-P3_)*(P4_+P3_)/2)                  ),
  SRR( sum((V2_+E1_),V2_,E3_,E4_) , ifthenelse(P4_-P3_,scalar(0),(P4_+1-P3_)*(P4_+P3_+2*P1_)/2)              ,
	  [](const exprmap &m) { return isconstexpr(m.at(1),getvar(m.at(2))); } ),

  SRR( sum(pow(V2_,2),V2_,E3_,E4_)
	  , ifthenelse(P4_-P3_,scalar(0),scalarreal{1,6}*(P4_-P3_+1) + scalarreal{1,2}*(pow(P4_,2)-pow(P3_-1,2))
	  		+ scalarreal{1,3}*(pow(P4_,3)-pow(P3_-1,3)))                   ),
  SRR( sum(pow(V2_+E1_,2),V2_,E3_,E4_)
	  , ifthenelse(P4_-P3_,scalar(0),scalarreal{1,6}*(P4_-P3_+1) + scalarreal{1,2}*(pow(P4_,2)-pow(P3_-1,2))
	  		+ scalarreal{1,3}*(pow(P4_,3)-pow(P3_-1,3))
			+ pow(P1_,2)*(P4_-P3_) + P1_*(pow(P4_,2)-pow(P3_,2))          ),
	  [](const exprmap &m) { return isconstexpr(m.at(1),getvar(m.at(2))); } ),

  // Faulhaber's formula
  SRR( sum(pow(V1_,E2_),V1_,E3_,E4_)      ,
	//	 			(B(P2_+1,P4_+1) - B(P2_+1,P3_+1))/(P2_+1) ,
		ifthenelse(P4_-P3_,scalar(0),psum(P2_,P4_) - psum(P2_,P3_-1)),
  	[](const exprmap &m) { return isconstexpr(m.at(2),getvar(m.at(1))); }   ),
  SRR( sum(pow((V1_+E5_),E2_),V1_,E3_,E4_)      ,
	//	 			(B(P2_+1,P4_+1) - B(P2_+1,P3_+1))/(P2_+1) ,
		ifthenelse(P4_-P3_,scalar(0),psum(P2_,P4_+P5_) - psum(P2_,P3_+P5_-1)),
  	[](const exprmap &m) { return isconstexpr(m.at(2),getvar(m.at(1)))
				&& isconstexpr(m.at(5),getvar(m.at(1))); }   ),
}};

ruleset numericevalrules {{
		  toptr<consteval>(),
	 }};

ruleset scalarruleset = basicscalarrules
			+ derivscalarrules
			+ integralscalarrules
			+ sumprodscalarrules
			+ numericevalrules;
#endif


#include <boost/test/unit_test.hpp>
#include <sudoku/groupwise_block_counter.h>
#include <sudoku/standard.h>

BOOST_AUTO_TEST_CASE(GroupwiseBlockCounter_setCellValue_getNextAvailableValue)
{
    sudoku::standard::Dimensions dims;
    sudoku::GroupwiseBlockCounter counter(dims);

    // 1 x 0 ... <-- 1 in same row
    // 0 0 2 ... <-- 2 in same subsquare
    // 0 0 0
    // 0 3 0 ... <-- 3 in same column
    // ...
    counter.setCellValue(0, 1);
    counter.setCellValue(11, 2);
    counter.setCellValue(28, 3);

    BOOST_REQUIRE_EQUAL(counter.getNextAvailableValue(1, 0), 4);
    BOOST_REQUIRE_EQUAL(counter.getNextAvailableValue(1, 1), 4);
    BOOST_REQUIRE_EQUAL(counter.getNextAvailableValue(1, 2), 4);
    BOOST_REQUIRE_EQUAL(counter.getNextAvailableValue(1, 3), 4);
    BOOST_REQUIRE_EQUAL(counter.getNextAvailableValue(1, 4), 5);
}

BOOST_AUTO_TEST_CASE(GroupwiseBlockCounter_getNextAvailableValue_noMoreValues)
{
    sudoku::square::Dimensions dims(2);
    sudoku::GroupwiseBlockCounter counter(dims);
    // 1 2 3 x <-- x has one remaining available value = 4
    // 0 0 0 0
    // 0 0 0 0
    // 0 0 0 0
    counter.setCellValue(0, 1);
    counter.setCellValue(1, 2);
    counter.setCellValue(2, 3);
    BOOST_REQUIRE_EQUAL(counter.getNextAvailableValue(3, 0), 4);

    // 1 2 3 x <-- x has no more available values
    // 0 0 0 4
    // 0 0 0 0
    // 0 0 0 0
    counter.setCellValue(7, 4);
    BOOST_REQUIRE_EQUAL(counter.getNextAvailableValue(3, 0), 0);
}


#ifndef RODIN_CORE_GEOMETRY_SEGMENT2D_IPP
#define RODIN_CORE_GEOMETRY_SEGMENT2D_IPP

#include <Eigen/Core>

#include "Point2D.h"
#include "Circle.h"

#include "LineSegment2D.h"

namespace Rodin::Plot::Geometry
{

  template <class T>
  constexpr
  LineSegment2D<T>::LineSegment2D(
      const Point2D<T>& start, const Point2D<T>& end)
    : m_start(start), m_end(end)
  {
    assert(start != end);
  }

  template <class T>
  inline
  constexpr
  Point2D<T> LineSegment2D<T>::operator()(const T& t) const
  {
    return start() + t * (end() - start());
  }

  template <class T>
  inline
  constexpr
  T LineSegment2D<T>::length() const
  {
    return (m_end - m_start).norm();
  }

  template <class T>
  inline
  constexpr
  Magnum::Math::Vector2<T> LineSegment2D<T>::direction() const
  {
    return (m_end - m_start).normalized();
  }

  template <class T>
  inline
  constexpr
  Point2D<T> LineSegment2D<T>::start() const
  {
    return m_start;
  }

  template <class T>
  inline
  constexpr
  Point2D<T> LineSegment2D<T>::end() const
  {
    return m_end;
  }

  template <class T>
  inline
  constexpr
  LineSegment2D<T>& LineSegment2D<T>::setStart(const Point2D<T>& start)
  {
    m_start = start;
    return *this;
  }

  template <class T>
  inline
  constexpr
  LineSegment2D<T>& LineSegment2D<T>::setEnd(const Point2D<T>& end)
  {
    m_end = end;
    return *this;
  }

  template <class T>
  inline
  constexpr
  LineSegment2D<T> LineSegment2D<T>::reverse() const
  {
    return LineSegment2D<T>(end(), start());
  }

  template <class T>
  inline
  constexpr
  std::optional<Point2D<T>> LineSegment2D<T>::intersect(const
      LineSegment2D<T>& other)
  const
  {
    auto det = (end().x() - start().x()) * (other.start().y() - other.end().y()) -
      (end().y() - start().y()) * (other.start().x() - other.end().x());

    if (det == T{0})
      return {};
    else
    {
      auto s0 = (other.start().y() - other.end().y()) * (other.start().x() - start().x())
            + (other.end().x() - other.start().x()) * (other.start().y() - start().y());
      s0 /= det;

      auto t0 = (start().y() - end().y()) * (other.start().x() - start().x())
            + (end().x() - start().x()) * (other.start().y() - start().y());
      t0 /= det;

      if (T{0} <= s0 && s0 <= T{1} && T{0} <= t0 && t0 <= T{1})
        return other(t0);
      else
        return {};
    }
  }
}

#endif


/**
 * @brief  This file is part of Template Matching Implementation using OpenCV C++ Library
 * @file  template_matching.cpp
 * @date  3/12/19 -- Revised 18/01/2021
 * @copyright
 * @author  Arslan Ali  <marslanali800@gmail.com>
 */

#include <armadillo>
#include <iostream>
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc/imgproc.hpp>

/**
 * @brief Template function for converting openCV Mat to Armadillo Mat
 * @tparam T
 * @param opencv_mat
 * @return arma::Mat<T>
 */
template <typename T>
arma::Mat<T> opencv_to_arma2(const cv::Mat& opencv_mat)
{
  arma::Mat<T> arma_mat(opencv_mat.rows, opencv_mat.cols);

  for (size_t i = 0; i < opencv_mat.rows; ++i)
  {
    for (size_t j = 0; j < opencv_mat.cols; ++j)
    {
      arma_mat(i, j) = (T)opencv_mat.at<uchar>(i, j);
    }
  }

  return arma_mat;
}

/**
 * main function
 * @param argc  Total number of command line arguments
 * @param argv  Pointer to store command line arguments
 * @return 0
 */
int main(int argc, char** agrv)
{
  cv::Mat input_image, input_filter;
  cv::Mat gray_scale_image(input_image.rows, input_image.cols, CV_8UC1);
  cv::Mat gray_scale_filter(input_filter.rows, input_filter.cols, CV_8UC1);

  input_image = cv::imread("../img/clutter.png");
  input_filter = cv::imread("../img/waldo.png");

  cv::imshow("Input Image", input_image);
  cv::imshow("Input Template", input_filter);

  if (input_image.empty() || input_filter.empty())
  {
    std::cout << "Can't find image" << std::endl;
    return -1;
  }

  cv::cvtColor(input_image, gray_scale_image, cv::COLOR_BGR2GRAY);
  cv::cvtColor(input_filter, gray_scale_filter, cv::COLOR_BGR2GRAY);

  cv::imshow("GrayScale Image", gray_scale_image);
  cv::imshow("GrayScale Filter", gray_scale_filter);

  cv::Mat final_image(
      gray_scale_image.rows - gray_scale_filter.rows + 1, gray_scale_image.cols - gray_scale_filter.cols + 1, CV_8UC1);
  cv::matchTemplate(gray_scale_image, gray_scale_filter, final_image, 3);
  cv::normalize(final_image, final_image, 0, 1, cv::NORM_MINMAX, -1, cv::Mat());

  /// Localizing the best match with minMaxLoc
  double min_val, max_val;
  cv::Point min_loc, max_loc, match_loc;

  minMaxLoc(final_image, &min_val, &max_val, &min_loc, &max_loc, cv::Mat());

  // For SQDIFF and SQDIFF_NORMED, the best matches are lower values. For all the other methods, the higher the better
  match_loc = max_loc;
  std::cout << match_loc << std::endl;

  /// Show what you got
  cv::rectangle(input_image,
                match_loc,
                cv::Point(match_loc.x + gray_scale_filter.cols, match_loc.y + gray_scale_filter.rows),
                cv::Scalar::all(0),
                2,
                8,
                0);
  cv::rectangle(final_image,
                match_loc,
                cv::Point(match_loc.x + gray_scale_filter.cols, match_loc.y + gray_scale_filter.rows),
                cv::Scalar::all(0),
                2,
                8,
                0);

  cv::imshow("Final Image", input_image);

  cv::imwrite("../img/Final-Image.png", input_image);

  // Convert OpenCV Mat to Armadillo Mat<double>
  arma::Mat<double> arma_gray_scale_image(gray_scale_image.rows, gray_scale_image.cols);
  arma::Mat<double> arma_gray_scale_filter(gray_scale_filter.rows, gray_scale_filter.cols);

  // arma_gray_scale_filter = opencv_to_arma2<double>(gray_scale_filter);

  for (size_t i = 0; i < gray_scale_image.rows; i++)
  {
    for (size_t j = 0; j < gray_scale_image.cols; j++)
    {
      // cv::Vec3b pixelColor = testData.at<cv::Vec3b>(i);
      arma_gray_scale_image(i, j) = (double)gray_scale_image.at<uchar>(i, j);
    }
  }

  for (size_t i = 0; i < gray_scale_filter.rows; i++)
  {
    for (size_t j = 0; j < gray_scale_filter.cols; j++)
    {
      arma_gray_scale_filter(i, j) = (double)gray_scale_filter.at<uchar>(i, j);
    }
  }

  // arma_gray_scale_image += 50;
  // arma_gray_scale_filter += 50;

  double value = arma::sqrt(arma::sum(arma::sum(arma::pow(arma_gray_scale_filter, 2), 0), 1)).eval()(0, 0);
  arma_gray_scale_filter = arma_gray_scale_filter / value;

  // convert Armadillo Mat <double> to openCV Mat
  cv::Mat image(arma_gray_scale_image.n_rows, arma_gray_scale_image.n_cols, CV_8UC1);
  cv::Mat filter(arma_gray_scale_filter.n_rows, arma_gray_scale_filter.n_cols, CV_8UC1);

  // final_image = arma_to_opencv<uchar>(arma_mat_filter);

  for (size_t i = 0; i < gray_scale_image.rows; ++i)
  {
    for (size_t j = 0; j < gray_scale_image.cols; ++j)
    {
      image.at<uchar>(i, j) = (uchar)arma_gray_scale_image(i, j);
    }
  }

  for (size_t i = 0; i < gray_scale_filter.rows; ++i)
  {
    for (size_t j = 0; j < gray_scale_filter.cols; ++j)
    {
      filter.at<uchar>(i, j) = (uchar)arma_gray_scale_filter(i, j);
    }
  }

  // imshow("Final Image 2 ", image);
  // imshow("Final Filter", filter);

  cv::waitKey();
  return 0;
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "test_loss"

#include <boost/test/unit_test.hpp>
#include "nanocv/nanocv.h"
#include "nanocv/optimizer.h"
#include "nanocv/math/random.hpp"
#include "nanocv/math/epsilon.hpp"

namespace test
{
        using namespace ncv;

        void check_grad(const string_t& loss_id, size_t n_dims, size_t n_tests)
        {
                const rloss_t loss = ncv::get_losses().get(loss_id);

                const vector_t target = ncv::class_target(n_dims / 2, n_dims);

                // optimization problem: size
                auto opt_fn_size = [&] ()
                {
                        return n_dims;
                };

                // optimization problem: function value
                auto opt_fn_fval = [&] (const vector_t& x)
                {
                        const vector_t& output = x;

                        return loss->value(target, output);
                };

                // optimization problem: function value & gradient
                auto opt_fn_grad = [&] (const vector_t& x, vector_t& gx)
                {
                        const vector_t& output = x;

                        gx = loss->vgrad(target, output);

                        return loss->value(target, output);
                };

                // construct optimization problem
                const opt_problem_t problem(opt_fn_size, opt_fn_fval, opt_fn_grad);

                // check the gradient using random parameters
                for (size_t t = 0; t < n_tests; t ++)
                {
                        random_t<scalar_t> rgen(-1.0, +1.0);

                        vector_t x(n_dims);
                        rgen(x.data(), x.data() + n_dims);

                        BOOST_CHECK_GE(problem(x), 0.0);
                        BOOST_CHECK_LE(problem.grad_accuracy(x), math::epsilon1<scalar_t>());
                }
        }
}

BOOST_AUTO_TEST_CASE(test_loss)
{
        ncv::init();

        using namespace ncv;

        const strings_t loss_ids = ncv::get_losses().ids();

        const size_t cmd_min_dims = 2;
        const size_t cmd_max_dims = 10;
        const size_t cmd_tests = 128;

        // evaluate the analytical gradient vs. the finite difference approximation
        for (const string_t& loss_id : loss_ids)
        {
                for (size_t cmd_dims = cmd_min_dims; cmd_dims <= cmd_max_dims; cmd_dims ++)
                {
                        test::check_grad(loss_id, cmd_dims, cmd_tests);
                }
        }
}


/*
 * Copyright 2009-2020 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 *
 *     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.
 *
 */
#define BOOST_TEST_MAIN

#define BOOST_TEST_MODULE sigma_test

// Standard includes
#include <fstream>

// Third party includes
#include <boost/test/unit_test.hpp>

// VOTCA includes
#include <votca/tools/eigenio_matrixmarket.h>

// Local VOTCA includes
#include "votca/xtp/aobasis.h"
#include "votca/xtp/logger.h"
#include "votca/xtp/orbitals.h"
#include "votca/xtp/ppm.h"
#include "votca/xtp/rpa.h"
#include "votca/xtp/sigma_ppm.h"
#include "votca/xtp/threecenter.h"

using namespace votca::xtp;
using namespace std;

BOOST_AUTO_TEST_SUITE(sigma_test)

BOOST_AUTO_TEST_CASE(sigma_full) {

  Orbitals orbitals;
  orbitals.QMAtoms().LoadFromFile(std::string(XTP_TEST_DATA_FOLDER) +
                                  "/sigma_ppm/molecule.xyz");
  BasisSet basis;
  basis.Load(std::string(XTP_TEST_DATA_FOLDER) + "/sigma_ppm/3-21G.xml");

  AOBasis aobasis;
  aobasis.Fill(basis, orbitals.QMAtoms());

  Eigen::MatrixXd MOs = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/sigma_ppm/MOs.mm");

  Eigen::VectorXd mo_energy = Eigen::VectorXd::Zero(17);
  mo_energy << -0.612601, -0.341755, -0.341755, -0.341755, 0.137304, 0.16678,
      0.16678, 0.16678, 0.671592, 0.671592, 0.671592, 0.974255, 1.01205,
      1.01205, 1.01205, 1.64823, 19.4429;
  Logger log;
  TCMatrix_gwbse Mmn{log};
  Mmn.Initialize(aobasis.AOBasisSize(), 0, 16, 0, 16);
  Mmn.Fill(aobasis, aobasis, MOs);

  RPA rpa(log, Mmn);
  rpa.configure(4, 0, 16);
  rpa.setRPAInputEnergies(mo_energy);

  Sigma_PPM sigma = Sigma_PPM(Mmn, rpa);

  Sigma_PPM::options opt;
  opt.homo = 4;
  opt.qpmin = 0;
  opt.qpmax = 16;
  opt.rpamin = 0;
  opt.rpamax = 16;
  sigma.configure(opt);

  Eigen::MatrixXd x = sigma.CalcExchangeMatrix();

  Eigen::MatrixXd x_ref = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/sigma_ppm/x_ref.mm");

  bool check_x = x_ref.isApprox(x, 1e-5);
  if (!check_x) {
    cout << "Sigma X" << endl;
    cout << x << endl;
    cout << "Sigma X ref" << endl;
    cout << x_ref << endl;
  }
  BOOST_CHECK_EQUAL(check_x, true);

  sigma.PrepareScreening();
  Eigen::MatrixXd c = sigma.CalcCorrelationOffDiag(mo_energy);
  c.diagonal() = sigma.CalcCorrelationDiag(mo_energy);

  Eigen::MatrixXd c_ref = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/sigma_ppm/c_ref.mm");

  bool check_c_diag = c.diagonal().isApprox(c_ref.diagonal(), 1e-5);
  if (!check_c_diag) {
    cout << "Sigma C" << endl;
    cout << c.diagonal() << endl;
    cout << "Sigma C ref" << endl;
    cout << c_ref.diagonal() << endl;
  }
  BOOST_CHECK_EQUAL(check_c_diag, true);

  bool check_c = c.isApprox(c_ref, 1e-5);
  if (!check_c) {
    cout << "Sigma C" << endl;
    cout << c << endl;
    cout << "Sigma C ref" << endl;
    cout << c_ref << endl;
  }
  BOOST_CHECK_EQUAL(check_c, true);
}

BOOST_AUTO_TEST_SUITE_END()


#include <iomanip>
#include <Eigen/Dense>
#include <Exception.hh>
#include "HartreeFock.hh"
#include "HFConverger.hh"
#include "exceptions.hh"

const int HartreeFock::default_max_iter = 100;
const double HartreeFock::default_tolerance = 1.e-10;

HartreeFock::HartreeFock(): _max_iter(default_max_iter),
	_tolerance(default_tolerance), _conv_method("diis"),  _status(),
	_orbitals(), _orb_ener(), _density() {}

HartreeFock::HartreeFock(const Basis& basis, const Geometry& geometry,
	int multiplicity, bool restricted): _max_iter(default_max_iter),
	_tolerance(default_tolerance), _conv_method("diis"), _status(),
	_orbitals(), _orb_ener(), _density()
{
	iterate(basis, geometry, multiplicity, restricted);
}

void HartreeFock::iterate(const Basis& basis, const Geometry& geometry,
	int multiplicity, bool restricted)
{
	_status.reset();
	_restricted = restricted;
	setMultiplicity(geometry, multiplicity);

	double nuc_rep = geometry.nuclearRepulsion();
	std::cout << "nuclear repulsion = " << nuc_rep << "\n";

	Eigen::MatrixXd H = basis.kineticEnergy()
		+ basis.nuclearAttraction(geometry.positions(), geometry.charges());
	const Eigen::MatrixXd& S = basis.overlap();

	if (restricted)
		iterateRestricted(basis, H, S, nuc_rep);
	else
		iterateUnrestricted(basis, H, S, nuc_rep);
}

void HartreeFock::iterateRestricted(const Basis& basis,
	const Eigen::MatrixXd& H, const Eigen::MatrixXd& S,
	double nuc_rep)
{
	calcOrbitals(H, S);

	_energy = nuc_rep;
	if (_nr_double > 0) _energy += 2 * _orb_ener.head(_nr_double).sum();
	if (_nr_single > 0) _energy += _orb_ener.segment(_nr_double, _nr_single).sum();
	std::cout << "Energy: " << _energy << "\n";

	Eigen::MatrixXd J, K;
	HFConverger::Ptr converger = HFConverger::create(_conv_method, *this,
		basis);
	for (int iter = 0; iter < _max_iter; ++iter)
	{
		double last_energy = _energy;

		const Eigen::MatrixXd& D = density();
		basis.twoElectron(D, J, K);
		_energy = (H + 0.5*(J - 0.5*K)).cwiseProduct(D).sum() + nuc_rep;
		std::cout << std::setw(3) << iter << " "
			<< std::setw(20) << std::setprecision(15) << std::fixed << _energy << " "
			<< std::setw(13) << std::setprecision(6) << std::scientific << converger->error()
			<< "\n";
		//std::cout << "orbital energies: " << _orb_ener.transpose() << "\n";

		if (std::abs(_energy - last_energy) < _tolerance)
		{
			_status.set(ENERGY_CONVERGED);
			return;
		}

		Eigen::MatrixXd F = H + J - 0.5*K;
		converger->step(F, _energy);
		calcOrbitals(F, S);
	}

	throw NoConvergence();
}

void HartreeFock::iterateUnrestricted(const Basis& basis,
	const Eigen::MatrixXd& H, const Eigen::MatrixXd& S,
	double nuc_rep)
{
	int nr_func = basis.size();

	calcOrbitals(H, H, S);
	_energy = nuc_rep + _orb_ener.head(_nr_alpha).sum()
		+ _orb_ener.segment(nr_func, _nr_beta).sum();
	std::cout << "Energy: " << _energy << "\n";

	Eigen::MatrixXd Ja, Ka, Jb, Kb;
	HFConverger::Ptr converger = HFConverger::create(_conv_method, *this,
		basis);
	for (int iter = 0; iter < _max_iter; ++iter)
	{
		double last_energy = _energy;

		Eigen::MatrixXd::ConstColsBlockXpr Da = density().leftCols(nr_func);
		Eigen::MatrixXd::ConstColsBlockXpr Db = density().rightCols(nr_func);
		basis.twoElectron(Da, Ja, Ka);
		basis.twoElectron(Db, Jb, Kb);

		double energyA = (H + 0.5*(Ja+Jb-Ka)).cwiseProduct(Da).sum();
		double energyB = (H + 0.5*(Ja+Jb-Kb)).cwiseProduct(Db).sum();
		_energy = energyA + energyB + nuc_rep;

		std::cout << std::setw(3) << iter << " "
			<< std::setw(20) << std::setprecision(15) << std::fixed << _energy << " "
			<< std::setw(13) << std::setprecision(6) << std::scientific << converger->error() << "\n";
		//std::cout << "orbital energies: " << _orb_ener.transpose() << "\n";

		if (std::abs(_energy - last_energy) < _tolerance)
		{
			_status.set(ENERGY_CONVERGED);
			return;
		}

		Eigen::MatrixXd Fa = H + Ja + Jb - Ka;
		Eigen::MatrixXd Fb = H + Ja + Jb - Kb;
		converger->step(Fa, Fb, _energy);
		calcOrbitals(Fa, Fb, S);
	}

	throw NoConvergence();
}

double HartreeFock::energy()
{
	return _energy;
}

void HartreeFock::setMultiplicity(const Geometry& geometry, int multiplicity)
{
	int nr_elec = geometry.totalCharge();

	if (multiplicity < 1)
		// Use lowest possible spin multiplicity
		multiplicity = 1 + nr_elec % 2;
	else if (nr_elec % 2 == multiplicity % 2 || multiplicity > nr_elec+1)
		throw Li::Exception("Multiplicity does not match number of electrons");

	_nr_single = multiplicity - 1;
	_nr_double = (nr_elec - _nr_single) / 2;
	if (!_restricted)
	{
		_nr_alpha = _nr_double + _nr_single;
		_nr_beta = _nr_double;
	}
}

void HartreeFock::calcDensity()
{
	if (!_status.test(ORBITALS_CURRENT))
		throw Li::Exception("No orbitals to compute density matrix with");

	int nr_func = _orbitals.rows();
	if (_restricted)
	{
		_density.resize(nr_func, nr_func);
		for (int j = 0; j < nr_func; j++)
		{
			for (int i = 0; i <= j; i++)
			{
				double sum = 0.0;
				if (_nr_double)
					sum += 2 * _orbitals.row(i).head(_nr_double).dot(
						_orbitals.row(j).head(_nr_double));
				if (_nr_single)
					sum += _orbitals.row(i).segment(_nr_double, _nr_single).dot(
						_orbitals.row(j).segment(_nr_double, _nr_single));
				_density(j,i) = _density(i,j) = sum;
			}
		}
        }
        else
	{
		_density.resize(nr_func, 2*nr_func);
		if (_nr_alpha == 0)
		{
			_density.leftCols(nr_func).setZero();
		}
		else
		{
			Eigen::MatrixXd::ColsBlockXpr occ
				= _orbitals.leftCols(_nr_alpha);
			_density.leftCols(nr_func) = occ * occ.transpose();
		}
		if (_nr_beta == 0)
		{
			_density.rightCols(nr_func).setZero();
		}
		else
		{
			Eigen::Block<Eigen::MatrixXd> occ
				= _orbitals.block(0, nr_func, nr_func, _nr_beta);
			_density.rightCols(nr_func) = occ * occ.transpose();
		}
	}

	_status.set(DENSITY_CURRENT);
}

void HartreeFock::calcOrbitals(const Eigen::MatrixXd& F,
	const Eigen::MatrixXd& S)
{
	Eigen::GeneralizedSelfAdjointEigenSolver<Eigen::MatrixXd> solver(F, S);
	_orbitals = solver.eigenvectors();
	_orb_ener = solver.eigenvalues();
	_status.set(ORBITALS_CURRENT);
	_status.reset(DENSITY_CURRENT);
}

void HartreeFock::calcOrbitals(const Eigen::MatrixXd& Fa,
	const Eigen::MatrixXd& Fb, const Eigen::MatrixXd& S)
{
	int nr_func = S.rows();
	_orbitals.resize(nr_func, 2*nr_func);
	_orb_ener.resize(2*nr_func);

	Eigen::GeneralizedSelfAdjointEigenSolver<Eigen::MatrixXd> solver(Fa, S);
	_orbitals.leftCols(nr_func) = solver.eigenvectors();
	_orb_ener.head(nr_func) = solver.eigenvalues();
	solver.compute(Fb, S);
	_orbitals.rightCols(nr_func) = solver.eigenvectors();
	_orb_ener.tail(nr_func) = solver.eigenvalues();

	_status.set(ORBITALS_CURRENT);
	_status.reset(DENSITY_CURRENT);
}


/**
 * @file contourplot.cc
 * @brief NPDE homework ContourPlot code
 * @author Unknown, Oliver Rietmann
 * @date 25.03.2021
 * @copyright Developed at ETH Zurich
 */

#include "contourplot.h"

#include <Eigen/Core>

namespace ContourPlot {

/* SAM_LISTING_BEGIN_0 */
Eigen::Matrix<double, 2, Eigen::Dynamic> crookedEgg() {
  //====================
  // Your code goes here
  // Replace the following dummy return value
  // by the matrix containing the isoline points:
  return Eigen::Matrix<double, 2, 42>::Zero();
  //====================
}
/* SAM_LISTING_END_0 */

}  // namespace ContourPlot


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

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

#include <boost/simd/detail/enforce_precision.hpp>
#include <boost/simd/function/if_zero_else.hpp>
#include <boost/simd/function/is_inf.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/plus.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>
#include <utility>

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

  BOOST_DISPATCH_OVERLOAD ( two_add_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_< bd::floating_<A0> >
                          , bd::generic_< bd::floating_<A0> >
                          )
  {
    using result_t = std::pair<A0,A0>                                  ;

    BOOST_FORCEINLINE result_t operator() ( A0 const& a,A0 const& b) const BOOST_NOEXCEPT
    {
      detail::enforce_precision<A0> enforcer;
      A0 r0 = a  + b;
      A0 z = r0 - a;
#if defined(BOOST_SIMD_NO_INFINITIES)
      A0 r1 = a-(r0-z)+(b-z);
#else
      A0 r1 = if_zero_else(is_inf(r0), (a-(r0-z))+(b-z));
#endif
      return {r0, r1};
    }
  };
} } }

#endif