/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2011 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifndef SDC__HH
#define SDC__HH

#include <complex>
#include <fstream>
#include <iostream>
#include <iomanip>
#include <cstdlib>

#include <boost/timer/timer.hpp>

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

#include "fem/embedded_errorest.hh"
#include "fem/iterate_grid.hh"
#include "timestepping/extrapolation.hh"
#include "timestepping/semieuler.hh"

#include "fem/assemble.hh"
#include "fem/istlinterface.hh"
#include "fem/functional_aux.hh"
#include "fem/hierarchicspace.hh"
#include "fem/lagrangespace.hh"
#include "fem/norms.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/jacobiPreconditioner.hh"
#include "linalg/iluprecond.hh"
#include "linalg/additiveschwarz.hh"
#include "linalg/hyprecond.hh"

#include "utilities/enums.hh"

#include "sdc.hh"

#include <iostream>
#include "dune/common/dynmatrix.hh"
#include "dune/common/dynvector.hh"

#include "utilities/enums.hh"

namespace Kaskade
{

template <class Eq>
class Sdc_
{
  public:
    typedef Eq                                                     EvolutionEquation;
    typedef typename EvolutionEquation::AnsatzVars::VariableSet State;

  private:
    typedef SemiLinearizationAt<SemiImplicitEulerStep<EvolutionEquation> > Linearization;
    typedef VariationalFunctionalAssembler<Linearization> Assembler;
    typedef typename EvolutionEquation::AnsatzVars::template CoefficientVectorRepresentation<0,EvolutionEquation::TestVars::noOfVariables>::type CoefficientVectors;

  public:
    /**
     * Constructs an ODE integrator. The arguments eq and ansatzVars
     * have to exist during the lifetime of the integrator.
     */
  Sdc_(GridManager<typename EvolutionEquation::AnsatzVars::Grid>& gridManager_,
          EvolutionEquation& eq_, typename EvolutionEquation::AnsatzVars const& ansatzVars_,
          typename EvolutionEquation::AnsatzVars::VariableSet x_,
          DirectType st=DirectType::MUMPS, PrecondType precondType_ = PrecondType::ILUK,
          int verbosity_ = 1):
          gridManager(gridManager_), ansatzVars(ansatzVars_), eq(&eq_,0),
          assembler(gridManager,ansatzVars.spaces) , u(x_),dx(x_),
          rhsAssemblyTime(0.0), matrixAssemblyTime(0.0), precAssemblyTime(0.0), factorizationTime(0.0), solutionTime(0.0),
          initSolutionTime(0.0), estimateTime(0), directType(st), precondType(precondType_), verbosity(verbosity_)

    {}

  State const& stepCardio(State const& x, double const dt, std::vector<std::pair<double,double> > const& tolX)
  {
    boost::timer::cpu_timer timer;

    int const n = 3, kEnd = 3;
    //std::cout << "SDC method: n = " << n << ",  kEnd = " << kEnd << std::endl;

    //LobattoTimeGrid grid(n,eq.time(),eq.time()+dt);
    RadauTimeGrid grid(n,eq.time(),eq.time()+dt);
    std::cout << "SDC-Matrix method, reaction terms evaluated only in nodes: n = " << n << ",  kEnd = " << kEnd << std::endl;

    Dune::DynamicVector<double> const& pts = grid.points();

    // S is provided an used in sdcIterationStep()
    //Dune::DynamicMatrix<double> const& S = grid.integrationMatrix();

    // Shat is computed in sdcIterationStep(...) called below
    //SDCTimeGrid::RealMatrix Shat;
    //eulerIntegrationMatrix(grid, Shat);
    //luIntegrationMatrix(grid, Shat);
    //std::cout << "Shat = \n" << Shat << std::endl;

    double tau1=0;

    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::cout << "nvars = " << nvars << std::endl;
    std::cout << "  neq = " <<   neq << std::endl;
    std::cout << "  dof = " <<  size << std::endl;

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

    double const t = eq.time();
    eq.temporalEvaluationRange(t,t+dt);

    double const tau = dt;
    eq.setTau(tau);

    // mesh adaptation loop. Just once if i>0.
    gridManager.setVerbosity(verbosity);
    bool accurate = false;

    bool const iterative = true;       // false: use a direct solver,      true: use an iterative solver
    std::vector<double> rhs(size), sol(size);

    // Evaluate and factorize matrix B(t)-tau*J

    std::vector<State> currSol(n+1,x);
    std::vector<State> lastSol(n+1,x);

      typedef typename Op::field_type field_type;
      typedef Dune::FieldMatrix<field_type,1,1> Field_Matrix;
      typedef Dune::BCRSMatrix<Field_Matrix> BCRS_Matrix;
      BCRS_Matrix  matM;
      BCRS_Matrix  matStiff;

    //for (int kk=0; kk<kEnd; kk++)
    int kk=-1;
    do
    {
      kk++;
      size_t  dofs = ansatzVars.degreesOfFreedom(0,nvars);
      std::cout << "sweep: k = " << kk << ", degree of freedom = " << dofs << std::endl;

      eq.time(t);

      CoefficientVectors rU(EvolutionEquation::AnsatzVars::template CoefficientVectorRepresentation<0,neq>::init(ansatzVars));

      //for (int k=0; k< boost::fusion::at_c<0>(rhs1[0].data).size(); k++)
      //std::cout << " size of rU(0) = " << boost::fusion::at_c<0>(rU.data).size() << std::endl;
      //std::cout << " size of rU(1) = " << boost::fusion::at_c<1>(rU.data).size() << std::endl;

      rU = 0;
      std::vector<CoefficientVectors> rUiMat(n+1,rU);
      std::vector<CoefficientVectors> rJiMat(n+1,rU);
      std::vector<CoefficientVectors> integMatS(n,rU);
      std::vector<CoefficientVectors> dUiMat(n+1,rU);
      CoefficientVectors sHatDu(rU);
      CoefficientVectors currsol(rU);
      CoefficientVectors lastsol(rU);

      CoefficientVectors solution(EvolutionEquation::AnsatzVars::template CoefficientVectorRepresentation<0,neq>::init(ansatzVars));
      solution=0;
      dUiMat[0] = solution;

//       typedef typename Op::field_type field_type;
//       typedef Dune::FieldMatrix<field_type,1,1> Field_Matrix;
//       typedef Dune::BCRSMatrix<Field_Matrix> BCRS_Matrix;

      if (!accurate)
      {
      // mass matrix M
        boost::timer::cpu_timer massMatrixTimer;
        timer.start();
        dx *= 0;
        eq.setSecondRHS(false);
        eq.setOnlyJacobian(false);
        eq.setOnlyStiffnessMatrix(false);
        eq.setOnlyF_uMatrix(false);
        eq.setOnlyMassMatrix(true);
        assembler.assemble(Linearization(eq,lastSol[0],lastSol[0],dx),Assembler::MATRIX);
        eq.setOnlyMassMatrix(false);
        Op M(assembler);
        //BCRS_Matrix  matM(M.template get<BCRS_Matrix>());    //  wenn matM lokal in loop
        matM = M.template get<BCRS_Matrix>();
        std::cout << "computing time for mass matrix: " << boost::timer::format(massMatrixTimer.elapsed()) << "\n";
        matrixAssemblyTime += (double)(timer.elapsed().wall)/1e9;

        // stiffness matrix A
        boost::timer::cpu_timer stiffMatrixTimer;
        timer.start();
        dx *= 0;
        eq.setSecondRHS(false);
        eq.setOnlyJacobian(false);
        eq.setOnlyMassMatrix(false);
        eq.setOnlyF_uMatrix(false);
        eq.setOnlyStiffnessMatrix(true);
        assembler.assemble(Linearization(eq,lastSol[0],lastSol[0],dx),Assembler::MATRIX);
        eq.setOnlyStiffnessMatrix(false);
        Op Stiff(assembler);
        //BCRS_Matrix  matStiff(Stiff.template get<BCRS_Matrix>());
        matStiff = Stiff.template get<BCRS_Matrix>();
        //std::cout << "Stiff = \n" << matStiff << std::endl;
        matrixAssemblyTime += (double)(timer.elapsed().wall)/1e9;
        std::cout << "computing time for stiffness matrix: " << boost::timer::format(stiffMatrixTimer.elapsed()) << "\n";
      }

      // pointwise evaluation of RHS  rUi = f(u)
      timer.start();
      boost::timer::cpu_timer reactionTimer;
      for (int ii=0; ii<n+1; ii++)
      {
        if (ii<n) tau1=pts[ii+1]-pts[ii];
        double tt = eq.time();
        eq.setTau(tau1);
		CoefficientVectors rU(solution);
        int dofsV = boost::fusion::at_c<1>(u.data).space().degreesOfFreedom();
        //std::cout << "\n dofs(v) = " << dofsV << std::endl;
        std::vector<double> uVec(dofsV), vVec(dofsV), fVec(dofsV), gVec(dofsV);
        for (int j=0; j<dofsV; ++j)
        {
          uVec[j] = (boost::fusion::at_c<0>(lastSol[ii].data)).coefficients()[j];
          vVec[j] = (boost::fusion::at_c<1>(lastSol[ii].data)).coefficients()[j];
        }
        for (int k=0; k<dofsV; ++k)
        {
          fVec[k] = eq.f(uVec[k],vVec[k]);
          gVec[k] = eq.g(uVec[k],vVec[k]);
        }
        CoefficientVectors rhs(rU);
        for (int k=0; k<dofsV; ++k) boost::fusion::at_c<0>(rhs.data)[k] = fVec[k];
        for (int k=0; k<dofsV; ++k) boost::fusion::at_c<1>(rhs.data)[k] = gVec[k];
        rUiMat[ii]=rhs;

        eq.time(eq.time()+tau1);
      }
      std::cout << "computing time for reaction terms: " << boost::timer::format(reactionTimer.elapsed()) << "\n";
      rhsAssemblyTime += (double)(timer.elapsed().wall)/1e9;

      std::vector<BCRS_Matrix> allMatF_u(n+1,matStiff);

      // pointwise evaluation of diagonals in Jacobian matrices
      timer.start();
      boost::timer::cpu_timer jacobianTimer;
      for (int ii=0; ii<n+1; ii++)
      {
        if (ii<n) tau1=pts[ii+1]-pts[ii];
        double tt = eq.time();
        eq.setTau(tau1);
		CoefficientVectors rU(solution);
        int dofsV = boost::fusion::at_c<1>(u.data).space().degreesOfFreedom();
        std::vector<double> uVec(dofsV), vVec(dofsV), fuVec(dofsV), gvVec(dofsV);
        for (int j=0; j<dofsV; ++j)
        {
          uVec[j] = (boost::fusion::at_c<0>(lastSol[ii].data)).coefficients()[j];
          vVec[j] = (boost::fusion::at_c<1>(lastSol[ii].data)).coefficients()[j];
        }
        for (int k=0; k<dofsV; ++k)
        {
          fuVec[k] = eq.fu(uVec[k],vVec[k]);
          gvVec[k] = eq.gv(uVec[k],vVec[k]);
        }
        CoefficientVectors rhs(rU);
        for (int k=0; k<dofsV; ++k) boost::fusion::at_c<0>(rhs.data)[k] = fuVec[k];
        for (int k=0; k<dofsV; ++k) boost::fusion::at_c<1>(rhs.data)[k] = gvVec[k];
        rJiMat[ii]=rhs;

        BCRS_Matrix  matF_u(matM);
        matF_u *= 0;
        for (int j=0; j<dofsV; ++j) matF_u[j][j]             = matM[j][j] * fuVec[j];
        for (int j=0; j<dofsV; ++j) matF_u[j+dofsV][j+dofsV] = matM[j+dofsV][j+dofsV] * gvVec[j];
        allMatF_u[ii] = matF_u;

        eq.time(eq.time()+tau1);
      }
      std::cout << "computing time for diagonals of Jacobian matrices: " << boost::timer::format(jacobianTimer.elapsed()) << "\n";
      matrixAssemblyTime += (double)(timer.elapsed().wall)/1e9;

      // Reaction matrix f_u for u=Sol[ii-1]
      /*
      boost::timer::cpu_timer jacobianTimer;
      for (int ii=0; ii<n+1; ii++)
      {
        dx *= 0;
        eq.setSecondRHS(false);
        eq.setOnlyJacobian(false);
        eq.setOnlyMassMatrix(false);
        eq.setOnlyStiffnessMatrix(false);
        eq.setOnlyF_uMatrix(true);
        assembler.assemble(Linearization(eq,lastSol[ii],lastSol[ii],dx),Assembler::MATRIX);
        eq.setOnlyF_uMatrix(false);
        Op F_u(assembler);
        //BCRS_Matrix  matF_u(F_u.template get<BCRS_Matrix>());
        allMatF_u[ii] = F_u.template get<BCRS_Matrix>();
        //std::cout << "ii = " << ii << ", F_u[ii] = \n" << allMatF_u[ii] << std::endl;

      }
      std::cout << "computing time for Jacobian matrices: " << boost::timer::format(jacobianTimer.elapsed()) << "\n";
      */

      const int blocksize = 1;
      typedef Dune::FieldVector<double,blocksize> BlockType;
      typedef typename Dune::BlockVector<BlockType> B_Vector;

      CoefficientVectors u(rU);
      u = lastSol[0];
      //size_t  dofs = ansatzVars.degreesOfFreedom(0,nvars);
      B_Vector uVec(dofs);
      std::vector<double> uStdVec(dofs);
      IstlInterfaceDetail::toVector(u, uStdVec);
      for(int j=0;j<dofs;j++) uVec[j] = uStdVec[j];

      std::vector<B_Vector> rUi(n+1,uVec);
      for (int ii=0; ii<n+1; ii++)
      {
        u = rUiMat[ii];
        IstlInterfaceDetail::toVector(u, uStdVec);
        for(int j=0;j<dofs;j++) uVec[j] = uStdVec[j];
        rUi[ii] = uVec;
      }

      std::vector<B_Vector> solLast(n+1,uVec);
      for (int ii=0; ii<n+1; ii++)
      {
        u = lastSol[ii];
        IstlInterfaceDetail::toVector(u, uStdVec);
        for(int j=0;j<dofs;j++) uVec[j] = uStdVec[j];
        solLast[ii] = uVec;
      }

      B_Vector dxVec(dofs);
      dxVec = 0;

      timer.start();
      sdcIterationStepCardio(grid, matM, matStiff, rUi, allMatF_u, solLast, dxVec);
      solutionTime += (double)(timer.elapsed().wall)/1e9;

      eq.time(t);

      //currSol <--- solLast  :  after calling sdcIterationStep we have the corrected solution in solLast
      for (int ii=0; ii<n+1; ii++)
      {
        for(int j=0;j<dofs;j++) uStdVec[j] = solLast[ii][j];
        IstlInterfaceDetail::fromVector(uStdVec, u);
        currSol[ii] = u;
      }
      lastSol=currSol;

      eq.time(eq.time()+dt);

      for(int j=0;j<dofs;j++) uStdVec[j] = dxVec[j];
      IstlInterfaceDetail::fromVector(uStdVec, dx);

      // estimate spatial error
      // dx is the defect in the right Lobatto/Radau point
      //if (!tolX.empty() && kk==0)
      //accurate = true;
      if (!tolX.empty() && !accurate)
      {
        State tmp(dx);
        projectHierarchically(ansatzVars,tmp);
        tmp -= dx;

        // perform mesh adaptation
        //accurate = embeddedErrorEstimator(ansatzVars,tmp,x,eq.scaling(),tolX,gridManager,verbosity);
        accurate = embeddedErrorEstimator(ansatzVars,tmp,x,eq.scaling(),tolX,gridManager,1);
        if (!accurate) {
          nnz = assembler.nnz(0,neq,0,nvars,false);
          size = ansatzVars.degreesOfFreedom(0,nvars);
          rhs.resize(size);
          sol.resize(size);
          std::cout << "accurate = " << accurate << ",   dofs after mesh refinement: " << size << std::endl;
        }
      }
    }
    while ( ( kk<(kEnd-1) ) || !accurate );

    u = currSol[n];
    return u;

  }

  State const& stepMOL(State const& x, double const dt, std::vector<std::pair<double,double> > const& tolX)
  {
    int const n = 6, kEnd = 6;
    //std::cout << "SDC method: n = " << n << ",  kEnd = " << kEnd << std::endl;

    //LobattoTimeGrid grid(n,eq.time(),eq.time()+dt);
    RadauTimeGrid grid(n,eq.time(),eq.time()+dt);
    std::cout << "SDC-Matrix method: n = " << n << ",  kEnd = " << kEnd << std::endl;

    Dune::DynamicVector<double> const& pts = grid.points();

    // S is provided an used in sdcIterationStep()
    //Dune::DynamicMatrix<double> const& S = grid.integrationMatrix();

    // Shat is computed in sdcIterationStep(...) called below
    //SDCTimeGrid::RealMatrix Shat;
    //eulerIntegrationMatrix(grid, Shat);
    //luIntegrationMatrix(grid, Shat);
    //std::cout << "Shat = \n" << Shat << std::endl;

    double tau1=0;

    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::cout << "nvars = " << nvars << std::endl;
    std::cout << "  neq = " <<   neq << std::endl;
    std::cout << "  dof = " <<  size << std::endl;

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

    double const t = eq.time();
    eq.temporalEvaluationRange(t,t+dt);

    double const tau = dt;
    eq.setTau(tau);

    // mesh adaptation loop. Just once if i>0.
    gridManager.setVerbosity(verbosity);
    bool accurate = false;

    bool const iterative = true;       // false: use a direct solver,      true: use an iterative solver
    std::vector<double> rhs(size), sol(size);

    // Evaluate and factorize matrix B(t)-tau*J

    std::vector<State> currSol(n+1,x);
    std::vector<State> lastSol(n+1,x);

    //for (int kk=0; kk<kEnd; kk++)
    int kk=-1;
    do
    {
      kk++;
      std::cout << "sweep: k = " << kk << std::endl;

      eq.time(t);

      CoefficientVectors rU(EvolutionEquation::AnsatzVars::template CoefficientVectorRepresentation<0,neq>::init(ansatzVars));

      //for (int k=0; k< boost::fusion::at_c<0>(rhs1[0].data).size(); k++)
      //std::cout << " size of rU(0) = " << boost::fusion::at_c<0>(rU.data).size() << std::endl;
      //std::cout << " size of rU(1) = " << boost::fusion::at_c<1>(rU.data).size() << std::endl;

      rU = 0;
      std::vector<CoefficientVectors> rUiMat(n+1,rU);
      std::vector<CoefficientVectors> integMatS(n,rU);
      std::vector<CoefficientVectors> dUiMat(n+1,rU);
      CoefficientVectors sHatDu(rU);
      CoefficientVectors currsol(rU);
      CoefficientVectors lastsol(rU);

      CoefficientVectors solution(EvolutionEquation::AnsatzVars::template CoefficientVectorRepresentation<0,neq>::init(ansatzVars));
      solution=0;
      dUiMat[0] = solution;

      for (int ii=0; ii<n+1; ii++)
      {
        if (ii<n) tau1=pts[ii+1]-pts[ii];
        //tau1=pts[ii]-pts[ii-1];
        //eq.time(eq.time()+tau1);
        double tt = eq.time();
        //eq.temporalEvaluationRange(tt,tt+tau1);
        eq.setTau(tau1);

        dx *= 0;
        eq.setSecondRHS(false);
        // righthand-side r of system  [B(t)-tau*J] u_{i+1} = r(u_{i})
        //std::cout << "init RHS: ii = " << ii << ", time = " << eq.time() << std::endl;
        assembler.assemble(Linearization(eq,lastSol[ii],lastSol[ii],dx),Assembler::RHS);

		CoefficientVectors rU(assembler.rhs());
        CoefficientVectors rhs(rU);
        rhs = 0;
        rhs.axpy(1.0/tau1, rU);

        //std::cout << "righthand-side ii = " << ii << std::endl;
        //for (int k=0; k<size; ++k) std::cout << " rhs = " << boost::fusion::at_c<0>(rhs.data)[k] << std::endl;

        rUiMat[ii]=rhs;

        //if (kk>0) eq.time(eq.time()+tau1);
        eq.time(eq.time()+tau1);
      }

      typedef typename Op::field_type field_type;
      typedef Dune::FieldMatrix<field_type,1,1> Field_Matrix;
      typedef Dune::BCRSMatrix<Field_Matrix> BCRS_Matrix;

      // mass matrix M
      dx *= 0;
      eq.setSecondRHS(false);
      eq.setOnlyJacobian(false);
      eq.setOnlyStiffnessMatrix(false);
      eq.setOnlyF_uMatrix(false);
      eq.setOnlyMassMatrix(true);
      assembler.assemble(Linearization(eq,lastSol[0],lastSol[0],dx),Assembler::MATRIX);
      eq.setOnlyMassMatrix(false);
      Op M(assembler);
      BCRS_Matrix  matM(M.template get<BCRS_Matrix>());

      // stiffness matrix A
      dx *= 0;
      eq.setSecondRHS(false);
      eq.setOnlyJacobian(false);
      eq.setOnlyMassMatrix(false);
      eq.setOnlyF_uMatrix(false);
      eq.setOnlyStiffnessMatrix(true);
      assembler.assemble(Linearization(eq,lastSol[0],lastSol[0],dx),Assembler::MATRIX);
      eq.setOnlyStiffnessMatrix(false);
      Op Stiff(assembler);
      BCRS_Matrix  matStiff(Stiff.template get<BCRS_Matrix>());
      //std::cout << "Stiff = \n" << matStiff << std::endl;

      // Reaction matrix f_u for u=Sol[ii-1]
      std::vector<BCRS_Matrix> allMatF_u(n+1,matStiff);
      for (int ii=0; ii<n+1; ii++)
      {
        dx *= 0;
        eq.setSecondRHS(false);
        eq.setOnlyJacobian(false);
        eq.setOnlyMassMatrix(false);
        eq.setOnlyStiffnessMatrix(false);
        eq.setOnlyF_uMatrix(true);
        assembler.assemble(Linearization(eq,lastSol[ii],lastSol[ii],dx),Assembler::MATRIX);
        eq.setOnlyF_uMatrix(false);
        Op F_u(assembler);
        //BCRS_Matrix  matF_u(F_u.template get<BCRS_Matrix>());
        allMatF_u[ii] = F_u.template get<BCRS_Matrix>();
        //std::cout << "ii = " << ii << ", F_u[ii] = \n" << allMatF_u[ii] << std::endl;

      }

      const int blocksize = 1;
      typedef Dune::FieldVector<double,blocksize> BlockType;
      typedef typename Dune::BlockVector<BlockType> B_Vector;

      CoefficientVectors u(rU);
      u = lastSol[0];
      size_t  dofs = ansatzVars.degreesOfFreedom(0,nvars);
      B_Vector uVec(dofs);
      std::vector<double> uStdVec(dofs);
      IstlInterfaceDetail::toVector(u, uStdVec);
      for(int j=0;j<dofs;j++) uVec[j] = uStdVec[j];

      std::vector<B_Vector> rUi(n+1,uVec);
      for (int ii=0; ii<n+1; ii++)
      {
        u = rUiMat[ii];
        IstlInterfaceDetail::toVector(u, uStdVec);
        for(int j=0;j<dofs;j++) uVec[j] = uStdVec[j];
        rUi[ii] = uVec;
      }

      std::vector<B_Vector> solLast(n+1,uVec);
      for (int ii=0; ii<n+1; ii++)
      {
        u = lastSol[ii];
        IstlInterfaceDetail::toVector(u, uStdVec);
        for(int j=0;j<dofs;j++) uVec[j] = uStdVec[j];
        solLast[ii] = uVec;
      }

      B_Vector dxVec(dofs);
      dxVec = 0;

      sdcIterationStep(grid, matM, matStiff, rUi, allMatF_u, solLast, dxVec);

      // sum in the rhs
//      int matCalc=summation(grid,pts,rUiMat,integMatS);

      eq.time(t);

      //currSol <--- solLast  :  after calling sdcIterationStep we have the corrected solution in solLast
      for (int ii=0; ii<n+1; ii++)
      {
        for(int j=0;j<dofs;j++) uStdVec[j] = solLast[ii][j];
        IstlInterfaceDetail::fromVector(uStdVec, u);
        currSol[ii] = u;
      }
      lastSol=currSol;

      eq.time(eq.time()+dt);

      for(int j=0;j<dofs;j++) uStdVec[j] = dxVec[j];
      IstlInterfaceDetail::fromVector(uStdVec, dx);

      // estimate spatial error
      // dx is the defect in the right Lobatto/Radau point
      //if (!tolX.empty() && kk==0)
      //accurate = true;
      if (!tolX.empty() && !accurate)
      {
        State tmp(dx);
        projectHierarchically(ansatzVars,tmp);
        tmp -= dx;

        // perform mesh adaptation
        //accurate = embeddedErrorEstimator(ansatzVars,tmp,x,eq.scaling(),tolX,gridManager,verbosity);
        accurate = embeddedErrorEstimator(ansatzVars,tmp,x,eq.scaling(),tolX,gridManager,1);
        if (!accurate) {
          nnz = assembler.nnz(0,neq,0,nvars,false);
          size = ansatzVars.degreesOfFreedom(0,nvars);
          rhs.resize(size);
          sol.resize(size);
          std::cout << "accurate = " << accurate << ",   dofs after mesh refinement: " << size << std::endl;
        }
      }
    }
    while ( ( kk<(kEnd-1) ) || !accurate );

    u = currSol[n];
    return u;

  }

  State const& step(State const& x, double const dt, std::vector<std::pair<double,double> > const& tolX)
  {
    int const n = 4, kEnd = 4;
    //std::cout << "SDC method: n = " << n << ",  kEnd = " << kEnd << std::endl;

    //LobattoTimeGrid grid(n,eq.time(),eq.time()+dt);
    RadauTimeGrid grid(n,eq.time(),eq.time()+dt);
    std::cout << "SDC method: n = " << n << ",  kEnd = " << kEnd << std::endl;

    Dune::DynamicVector<double> const& pts = grid.points();
//     std::cout << "Lobatto/Radau - Points:\n";
//     for (int j=0; j<=n; j++) {
//       std::cout << pts[j] << std::endl;
//     }

    Dune::DynamicMatrix<double> const& S = grid.integrationMatrix();
//     std::cout << "Integration-matrix S:\n";
//     for (int i=0; i<n; i++)
//     {
//       for (int j=0; j<=n; j++)
//       {
//           std::cout << "S[" << i << "][" << j << "] = " << S[i][j] << "   ";
//       }
//       std::cout << std::endl;
//      }

//     std::cout << "transposed Integration-matrix S:\n";
//     for (int j=0; j<=n; j++)
//     {
//       for (int i=0; i<n; i++)
//       {
//           std::cout << "S'[" << j << "][" << i << "] = " << S[i][j] << "   ";
//       }
//       std::cout << std::endl;
//      }

    SDCTimeGrid::RealMatrix Shat;
    //eulerIntegrationMatrix(grid, Shat);
    luIntegrationMatrix(grid, Shat);
    // std::cout << "Shat = \n" << Shat << std::endl;

//     std::cout << "Shat-matrix:\n";
//     for (int i=0; i<n; i++)
//     {
//       for (int j=0; j<=n; j++)
//       {
//           std::cout << "Shat[" << i << "][" << j << "] = " << Shat[i][j] << "   ";
//       }
//       std::cout << std::endl;
//      }

    double tau1=0;

    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::cout << "nvars = " << nvars << std::endl;
    std::cout << "  neq = " <<   neq << std::endl;
    std::cout << "  dof = " <<  size << std::endl;

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

    double const t = eq.time();
    eq.temporalEvaluationRange(t,t+dt);

    double const tau = dt;
    eq.setTau(tau);

    // mesh adaptation loop. Just once if i>0.
    gridManager.setVerbosity(verbosity);
    bool accurate = false;

    bool const iterative = true;       // false: use a direct solver,      true: use an iterative solver
    std::vector<double> rhs(size), sol(size);

    // Evaluate and factorize matrix B(t)-tau*J

    std::vector<State> currSol(n+1,x);
    std::vector<State> lastSol(n+1,x);

    //for (int kk=0; kk<kEnd; kk++)
    int kk=-1;
    do
    {
      kk++;
      std::cout << "sweep: k = " << kk << std::endl;

      eq.time(t);

      CoefficientVectors rU(EvolutionEquation::AnsatzVars::template CoefficientVectorRepresentation<0,neq>::init(ansatzVars));

      //for (int k=0; k< boost::fusion::at_c<0>(rhs1[0].data).size(); k++)
      //std::cout << " size of rU(0) = " << boost::fusion::at_c<0>(rU.data).size() << std::endl;
      //std::cout << " size of rU(1) = " << boost::fusion::at_c<1>(rU.data).size() << std::endl;

      rU = 0;
      std::vector<CoefficientVectors> rUiMat(n+1,rU);
      std::vector<CoefficientVectors> integMatS(n,rU);
      std::vector<CoefficientVectors> dUiMat(n+1,rU);
      CoefficientVectors sHatDu(rU);
      CoefficientVectors currsol(rU);
      CoefficientVectors lastsol(rU);

      CoefficientVectors solution(EvolutionEquation::AnsatzVars::template CoefficientVectorRepresentation<0,neq>::init(ansatzVars));
      solution=0;
      dUiMat[0] = solution;

      for (int ii=0; ii<n+1; ii++)
      {
        if (ii<n) tau1=pts[ii+1]-pts[ii];
        //tau1=pts[ii]-pts[ii-1];
        //eq.time(eq.time()+tau1);
        double tt = eq.time();
        //eq.temporalEvaluationRange(tt,tt+tau1);
        eq.setTau(tau1);

        dx *= 0;
        eq.setSecondRHS(false);
        // righthand-side r of system  [B(t)-tau*J] u_{i+1} = r(u_{i})
        //std::cout << "init RHS: ii = " << ii << ", time = " << eq.time() << std::endl;
        assembler.assemble(Linearization(eq,lastSol[ii],lastSol[ii],dx),Assembler::RHS);

		CoefficientVectors rU(assembler.rhs());
        CoefficientVectors rhs(rU);
        rhs = 0;
        rhs.axpy(1.0/tau1, rU);

        //std::cout << "righthand-side ii = " << ii << std::endl;
        //for (int k=0; k<size; ++k) std::cout << " rhs = " << boost::fusion::at_c<0>(rhs.data)[k] << std::endl;

        rUiMat[ii]=rhs;

        //if (kk>0) eq.time(eq.time()+tau1);
        eq.time(eq.time()+tau1);
      }

      // sum in the rhs
      int matCalc=summation(grid,pts,rUiMat,integMatS);

      eq.time(t);

      for (int ii=1; ii<n+1; ii++)
      {
        tau1=pts[ii]-pts[ii-1];
        eq.time(eq.time()+tau1);
        double tt = eq.time();
        //eq.temporalEvaluationRange(tt,tt+tau1);

        //std::cout << "ii = " << ii << ": tau = " << tau1 << ", Shat[ii-1][ii] = " << Shat[ii-1][ii] << std::endl;
        tau1 = Shat[ii-1][ii];           // neu !!!
        eq.setTau(tau1);

        // Jacobian r'(u)
        dx *= 0;
        eq.setSecondRHS(false);
        eq.setOnlyJacobian(true);
        assembler.assemble(Linearization(eq,lastSol[ii-1],lastSol[ii-1],dx),Assembler::MATRIX);
        Op Jac(assembler);

        CoefficientVectors du(dUiMat[ii-1]);
        dUiMat[ii-1] = 0;
        Jac.applyscaleadd(+1,du,dUiMat[ii-1]);		// d.h. dUiMat[ii-1] += Jac*du

        // matrix B(t)-tau*J
        dx *= 0;
        eq.setSecondRHS(false);
        eq.setOnlyJacobian(false);
        assembler.assemble(Linearization(eq,lastSol[ii],lastSol[ii],dx),Assembler::MATRIX);
        Op A(assembler);

        // get rhs for linear system

        State nn(currSol[ii-1]);
        currsol = nn;
        lastsol=lastSol[ii];
        eq.setR(currsol,lastsol);

        dx *= 0;
        eq.setSecondRHS(true);
        assembler.assemble(Linearization(eq,currSol[ii-1],currSol[ii-1],dx),Assembler::RHS);

        CoefficientVectors rU(assembler.rhs());
        CoefficientVectors newRHS(rU);
        newRHS = 0;
        newRHS.axpy(1.0/tau1, rU);

        newRHS += integMatS[ii-1];

        // start computation Shat * dU
        sHatDu = 0;
        // j basis function
        for (int j=0; j<ii; j++)
        {
            sHatDu.axpy(Shat[ii-1][j],dUiMat[j]);
        }
        // end computation Shat * dU

        newRHS+= sHatDu;

        // solve linear system
        typedef typename EvolutionEquation::AnsatzVars::template CoefficientVectorRepresentation<0,neq>::type LinearSpaceX;

        if(iterative)
        {
          //solution=0;
          //JacobiPreconditioner<Op> jacobi(A,1.0);
          int fill_lev = 0;
          ILUKPreconditioner<Op> iluk(A,fill_lev,verbosity);
          int iteSteps = 5000;
          double iteEps = 1.0e-12;
          //Dune::BiCGSTABSolver<LinearSpaceX> cg(A,jacobi,iteEps,iteSteps,1);
          Dune::BiCGSTABSolver<LinearSpaceX> cg(A,iluk,iteEps,iteSteps,1);
          Dune::InverseOperatorResult res;
          cg.apply(solution,newRHS,res);
          dx=solution;
        }
        else
        {
            MatrixAsTriplet<double> triplet(nnz);
            triplet = A.template get<MatrixAsTriplet<double> >();

            Factorization<double> *matrix = 0;
            std::cout << "direct type = " << directType << std::endl;
            switch (directType) {
            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 DirectType::MUMPS:
              matrix = new MUMPSFactorization<double>(size,0,triplet.ridx,triplet.cidx,triplet.data);
              break;
            case DirectType::SUPERLU:
              matrix = new SUPERLUFactorization<double>(size,0,triplet.ridx,triplet.cidx,triplet.data);
              break;
            default:
              throw -321;
              break;
            }
            matrix->setVerbose(verbosity);
            //factorizationTime += (double)(timer.elapsed().wall)/1e9;

            // First right hand side (j=0) has been assembled together with matrix.
            //timer.start();
            IstlInterfaceDetail::toVector(newRHS, rhs);
            //newRHS.write(begin(rhs));
            //for (int k=0; k<rhs.size(); ++k) std::cout << "RHS = " << rhs[k] << std::endl;
            matrix->solve(rhs,sol);
            delete matrix;

            for (int k=0; k<sol.size(); ++k) assert(std::isfinite(sol[k]));
            dx.read(sol.begin());
            //for (int k=0; k<sol.size(); ++k) at_c<0>(solution.data)[k] = sol[k];
            solution.read(sol.begin());    //   ???
        }

        dUiMat[ii] = solution;

        L2Norm l2;
        //State defect(nn);
        double nrm2 = l2( boost::fusion::at_c<0>(dx.data) );
        //double nrm2 = l2( dx.data );
        std::cout << " sdc: k = " << kk << ",  ||dx|| = " << nrm2 << std::endl;

        currSol[ii]+=solution;
//        eq.time(eq.time()+tau1);
        //std::cout << " time in ii-loop  at end= " << eq.time() << std::endl;
      }

      lastSol=currSol;

      // estimate spatial error
      // dx is the defect in the right Lobatto/Radau point
      //if (!tolX.empty() && kk==0)
      if (!tolX.empty() && !accurate)
      {
        State tmp(dx);
        projectHierarchically(ansatzVars,tmp);
        tmp -= dx;

        // perform mesh adaptation
        //accurate = embeddedErrorEstimator(ansatzVars,tmp,x,eq.scaling(),tolX,gridManager,verbosity);
        accurate = embeddedErrorEstimator(ansatzVars,tmp,x,eq.scaling(),tolX,gridManager,1);
        if (!accurate) {
          nnz = assembler.nnz(0,neq,0,nvars,false);
          size = ansatzVars.degreesOfFreedom(0,nvars);
          rhs.resize(size);
          sol.resize(size);
          std::cout << "accurate = " << accurate << ",   dofs after mesh refinement: " << size << std::endl;
        }
      }
    }
    while ( ( kk<(kEnd-1) ) || !accurate );

    u = currSol[n];
    return u;

  }

  int summation(LobattoTimeGrid const& grid,
                Dune::DynamicVector<double>  pts,
                std::vector<CoefficientVectors> const& rhs1,
                std::vector<CoefficientVectors>& integMatS )
  {
    Dune::DynamicMatrix<double> const& A = grid.integrationMatrix();

    int n=pts.size()-1;

// 	double sumWeights = 0;
//     for (int i=0; i<n; i++)
//     {
//       for (int j=0; j<=n; j++)
//       {
//           sumWeights += A[i][j];
//           std::cout << "S[" << i << "][" << j << "] = " << A[i][j] << "   ";
//       }
//       std::cout << std::endl;
//      }
//      std::cout << "sum of weights = " << sumWeights << std::endl;

    //  Ensure that integMatS is initialized by Zero
    for (int i=0; i<n; i++) integMatS[i] = 0;

	// j basis function
    for (int j=0; j<=n; j++)
    {
      // i for integration interval [t_i,t_i+1]
      for (int i=0; i<n; i++)
      {
        // k for position x_k
        integMatS[i].axpy(A[i][j],rhs1[j]);
      }
    }
	return true;
  }

  int summation(RadauTimeGrid const& grid,
                Dune::DynamicVector<double>  pts,
                std::vector<CoefficientVectors> const& rhs1,
                std::vector<CoefficientVectors>& integMatS )
  {
    Dune::DynamicMatrix<double> const& A = grid.integrationMatrix();

    int n=pts.size()-1;

// 	double sumWeights = 0;
//     for (int i=0; i<n; i++)
//     {
//       for (int j=0; j<=n; j++)
//       {
//           sumWeights += A[i][j];
//           std::cout << "S[" << i << "][" << j << "] = " << A[i][j] << "   ";
//       }
//       std::cout << std::endl;
//      }
//      std::cout << "sum of weights = " << sumWeights << std::endl;

    //  Ensure that integMatS is initialized by Zero
    for (int i=0; i<n; i++) integMatS[i] = 0;

	// j basis function
    for (int j=0; j<=n; j++)
    {
      // i for integration interval [t_i,t_i+1]
      for (int i=0; i<n; i++)
      {
        // k for position x_k
          integMatS[i].axpy(A[i][j],rhs1[j]);
       }
     }
	return true;
  }

    /**
     * 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
  {
    std::vector<std::pair<double,double> > e(ansatzVars.noOfVariables);
    return e;
  }

  template <class OutStream>
  void reportTime(OutStream& out) const
  {
      out << "sdc time: " << matrixAssemblyTime << "s matrix assembly\n"
          << "            " << rhsAssemblyTime << "s rhs assembly\n"
          << "            " << precAssemblyTime << "s preconditioner assembly\n"
          << "            " << factorizationTime << "s factorization\n"
          << "            " << initSolutionTime << "s init solution\n"
          << "            " << solutionTime << "s solution\n"
          << "            " << estimateTime << "s estimate\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;

  public:
    State u;
    State dx;
    double rhsAssemblyTime, matrixAssemblyTime, precAssemblyTime, factorizationTime, solutionTime;
    double elapsedTimeSinceReset, initSolutionTime, estimateTime;
    DirectType directType;
    PrecondType precondType;
    int verbosity;
  };
} // namespace Kaskade
#endif


/////////////////////////////////////////////////////////////////////////////////////////////
// Copyright (c) 2021 Andreas Milton Maniotis.
//
// Email: andreas.maniotis@gmail.com
//
// 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)
/////////////////////////////////////////////////////////////////////////////////////////////

//#pragma once

#include "../object.hpp"
#include "../logic.hpp"

namespace aml::adt
{
    template< typename Number >
    struct function_argument;

    template<>
    struct function_argument< num<1> >
    {
        template<  typename    Head
                ,  typename... Tail
                >
        static consteval Head&& from_header( Head&& h, Tail&&...t )
        {
            return static_cast<Head&&>(h);
        }
    };

    template<typename Number>
    struct function_argument
    {
        template<  typename    Head
                ,  typename... Tail
                >
        static consteval decltype(auto) from_header( Head&& h, Tail&&... t )
        {
            return function_argument< typename Number::down >::from_header( static_cast<Tail&&>(t)... );
        }
    };

    template<typename N, typename... Args>
    consteval decltype(auto) get_argument(Args&&... args)
    {
        return function_argument<N>::from_header( static_cast<Args&&>(args)... );
    };
}

template<auto n> struct foo {};

#include <iostream>
#include <boost/core/demangle.hpp>
#include <vector>

template<typename T>
struct keeper
{
    template<typename S>
    keeper(S&& t) : t_(std::forward<S>(t) )
    { }

    T t_;
};

template<typename Value>
struct Vector : public std::vector<Value>
{
    Vector(std::initializer_list<double> l )
    : std::vector<double>(l)
    { }

    Vector(Vector const& other)
    :    std::vector<double>(other)
    {
        std::cout << "copy constructing! " << std::endl;
    }

    Vector(Vector && other)
    :    std::vector<double>(std::move(other))
    {
        std::cout << "move constructing! " << std::endl;
    }

};

int main()
{
    foo<  aml::adt::get_argument<aml::num<3> >( 1, 2, 3 ) > f;

    std::cout << boost::core::demangle( typeid(f).name() ) << std::endl;

    keeper< Vector<double> > k1( aml::adt::get_argument<aml::num<2> >( Vector<double>{1, 1.11, 2.22 },
                                                                            Vector<double>{-2, -3.3, -4.44 } ));;

    std::cout << boost::core::demangle( typeid(k1).name() ) << std::endl;
    auto const& v = k1.t_;

    for (auto const& el : v )
        std::cout << el << " : ";
    std::cout << std::endl;
}


/*
    This file contains some auxiliary functions
*/

#include <iostream>
#include <fstream>
#include <string>
#include <boost/multiprecision/gmp.hpp>

#include "./aux.hpp"

#define DEMANGLE_TYPEID_NAME(x) abi::__cxa_demangle(typeid((x)).name(), NULL, NULL, NULL)

/*** Function that reads JSON file with batch auction information and builds appropriate data structures with all the required information **/
void parse_json_file(std::vector<Token> &tokens, std::vector<Order> &orders, std::vector<CP_AMM> &amms, int &_num_tokens, std::map<std::string, int> _idx_tokens, nlohmann::json &json_file)
{
    // TODO: Make input reading safe, i.e., handle all potential exceptions/errors

    // For-loop that reads all TOKEN information

    for (auto &i: json_file["tokens"].items()) {
        boost::multiprecision::mpf_float t;
        if (i.value()["external_price"].is_null())
            t = -1;
        else {
            //std::string tt = std::to_string(i.value()["external_price"]);
            std::string tt = (i.value()["external_price"]).dump();
            t = static_cast<boost::multiprecision::mpf_float>(tt);
        }
        if (i.value()["alias"].is_null())
            tokens.push_back(Token(i.key(), "_NULL", i.value()["decimals"], t));       // TODO: Check if multiple tokens with "null" alias can create any issue
        else
            tokens.push_back(Token(i.key(), i.value()["alias"], i.value()["decimals"], t));
        _idx_tokens[i.key()] = _num_tokens-1;
    }

    // For-loop that reads all ORDERS information
    for (auto &i: json_file["orders"].items()) {
        int t1 = std::stoi(i.key());
        int t2 = _idx_tokens.at(i.value()["sell_token"]);
        int t3 = _idx_tokens.at(i.value()["buy_token"]);
        bool t4 = i.value()["is_sell_order"];
        bool t5 = i.value()["is_liquidity_order"];
        bool t6 = i.value()["allow_partial_fill"];
        std::string tt7 = i.value()["sell_amount"];
        boost::multiprecision::mpz_int t7 = static_cast<boost::multiprecision::mpz_int>(tt7);
        std::string tt8 = i.value()["buy_amount"];
        boost::multiprecision::mpz_int t8 = static_cast<boost::multiprecision::mpz_int>(tt8);
        int t9 = _idx_tokens.at(i.value()["fee"]["token"]);
        std::string tt10 = i.value()["fee"]["amount"];
        boost::multiprecision::mpz_int t10 = static_cast<boost::multiprecision::mpz_int>(tt10);
        int t11 = _idx_tokens.at(i.value()["cost"]["token"]);
        std::string tt12 = i.value()["cost"]["amount"];
        boost::multiprecision::mpz_int t12 = static_cast<boost::multiprecision::mpz_int>(tt12);

        orders.push_back(Order(t1,t2,t3,t4,t5,t6,t7,t8,t9,t10,t11,t12));
    }

    // For-loop that reads all AMM information
    for (auto &i: json_file["amms"].items()) {
        int t1 = std::stoi(i.key());

        // We will only use ConstantProduct AMMs, so this check ensures that we ignore the rest
        if (i.value()["kind"] != "ConstantProduct")
            continue;

        int t2[2];
        boost::multiprecision::mpz_int t3[2];
        int counter = 0;

        for (auto &j: i.value()["reserves"].items()) {
            t2[counter] = _idx_tokens.at(j.key());
            std::string tt3 = j.value();
            t3[counter] = static_cast<boost::multiprecision::mpz_int>(tt3);
            counter++;
        }

        std::string tt4 = i.value()["fee"];
        boost::multiprecision::mpf_float t4 = static_cast<boost::multiprecision::mpf_float>(tt4);
        int t5 = _idx_tokens.at(i.value()["cost"]["token"]);
        std::string tt6 = i.value()["cost"]["amount"];
        boost::multiprecision::mpz_int t6 = static_cast<boost::multiprecision::mpz_int>(tt6);
        bool t7 = i.value()["mandatory"];

        amms.push_back(CP_AMM(t1,t2[0],t3[0],t2[1],t3[1],t4,t5,t6,t7));
    }

    return;
}
/*******************************************************************************************************************************************/


// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://github.com/mCRL2org/mCRL2/blob/master/COPYING
//
// 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)
//
/// \file solve_test.cpp
/// \brief Test for BES solvers.

#define BOOST_TEST_MODULE solve_test
#include "mcrl2/bes/gauss_elimination.h"
#include "mcrl2/bes/parse.h"
#include "mcrl2/bes/small_progress_measures.h"

#include <boost/test/included/unit_test_framework.hpp>

using namespace mcrl2;
using namespace mcrl2::bes;

void run_all_algorithms(std::string const& b, bool expected_outcome)
{
  boolean_equation_system b1;
  std::stringstream from;
  from << "pbes\n" << b << std::endl;
  from >> b1;

  std::clog << "solving the following input bes: \n" << bes::pp(b1) << std::endl;

  BOOST_CHECK_EQUAL(small_progress_measures(b1), expected_outcome);
  BOOST_CHECK_EQUAL(gauss_elimination(b1), expected_outcome);
}

BOOST_AUTO_TEST_CASE(test_simple_nu_mu)
{
  std::string b(
    "mu X1 = X2;\n"
    "nu X2 = X1;\n"
    "init X1;\n"
  );
  run_all_algorithms(b, false);
}

BOOST_AUTO_TEST_CASE(test_simple_mu_nu)
{
  std::string b(
    "nu X1 = X2;\n"
    "mu X2 = X1;\n"
    "init X1;\n"
  );
  run_all_algorithms(b, true);
}

BOOST_AUTO_TEST_CASE(test_simple)
{
  std::string b(
    "mu X1 = X2 && X1; \n"
    "nu X2 = X1 || X3; \n"
    "mu X3 = X1 || X2; \n"
    "                  \n"
    "init X1;          \n"
  );
  run_all_algorithms(b, false);
}


#include <boost/geometry/util/math.hpp>


///////////////////////////////////////////////////////////////////////////////
// null_hypothesis.hpp                                                       //
//                                                                           //
//  Copyright 2010 Erwann Rogard. 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)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_PEARSON_CHISQ_INDEPENDENCE_NULL_HYPOTHESIS_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_PEARSON_CHISQ_INDEPENDENCE_NULL_HYPOTHESIS_HPP_ER_2010
#include <boost/mpl/detail/wrapper.hpp>
#include <boost/mpl/for_each.hpp>
#include <numeric>
#include <boost/lambda/lambda.hpp>
#include <boost/range/begin.hpp>
#include <boost/range/end.hpp>
#include <boost/range/reference.hpp>
#include <boost/type_traits/remove_cv.hpp>
#include <boost/type_traits/remove_reference.hpp>
#include <boost/accumulators/statistics/detail/weighted_count.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/pearson_chisq/common/crtp.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/pearson_chisq/common/null_hypothesis.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/cells/cells_count.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/cells/cells.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/pearson_chisq/independence/aux/lost_df_accumulator.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/pearson_chisq/independence/aux/chi_square_summand.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/pearson_chisq/independence/feature.hpp>

namespace boost {
namespace statistics{
namespace detail{
namespace contingency_table{
namespace pearson_chi_square_statistic{

    template<typename Keys,typename T1>
    struct null_hypothesis<
        pearson_chi_square_statistic::tag::independence_between<Keys,T1>
    > : crtp<
            null_hypothesis<
                pearson_chi_square_statistic::tag::independence_between<Keys,T1>
            >,
            Keys,
            T1
        >
    {
        typedef pearson_chi_square_statistic::tag::independence_between<
            Keys,T1> feature_type;
        typedef null_hypothesis<feature_type> this_type;
        typedef crtp<this_type, Keys, T1> super_type;

        null_hypothesis(){}

        protected:

        template<typename AccSet,typename P>
        T1 statistic_impl(const AccSet& acc, const P& policy)
        {
            namespace ac = boost::accumulators;
            namespace ct = contingency_table;
            namespace ps = pearson_chi_square_statistic;
            namespace aux = ps::independence_between_aux;
            typedef typename AccSet::weight_type weight_;
            typedef typename ct::result_of::extract::cells<
                Keys,AccSet>::type ref_cells_;
            typedef aux::chi_square_summand<T1,Keys,AccSet> summand_;

            ref_cells_ ref_cells = ct::extract::cells<Keys>( acc );
            weight_ n_obs = ac::extract::weighted_count( acc );
            summand_ s(acc, n_obs);
            return std::accumulate(
                boost::begin( ref_cells ),
                boost::end( ref_cells ),
                static_cast<T1>(0),
                boost::lambda::ret<T1>(boost::lambda::_1 + boost::lambda::_2),
                s
            );
        }

        template<typename AccSet,typename P>
        typename AccSet::weight_type
        lost_degrees_of_freedom_impl(const AccSet& acc, const P& policy)
        {
            namespace aux = independence_between_aux;
            namespace ps = pearson_chi_square_statistic;
            typedef boost::mpl::detail::wrapper<boost::mpl::_> op_;
            typedef typename AccSet::weight_type weight_;
            weight_ lost_df = 0;
            boost::mpl::for_each<Keys,op_>(
                aux::make_lost_df_accumulator(acc, lost_df)
            );
            return lost_df;
  	    }

    };

}// pearson_chi_square_statistic
}// contingency_table
}// detail
}// statistics
}// boost

#endif


/*
cd pa	 * Layer.h
 *
 *  Created on: Dec 8, 2016
 *      Author: ken
 */

#ifndef LAYER_LAYER_HPP_
#define LAYER_LAYER_HPP_

#include <armadillo>
#include "optimizer/Optimizer.hpp"

class Layer {
private:

public:
	Layer();
	virtual ~Layer();

	virtual arma::field<arma::Cube<double>> feedForward(const arma::field<arma::Cube<double>>& xs);
	virtual arma::field<arma::Cube<double>> backProp(const arma::field<arma::Cube<double>>& deltas);

};

#endif /* LAYER_LAYER_HPP_ */


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
/*!
 * \file
**/
#ifndef BOOST_SIMD_TOOLBOX_CONSTANT_CONSTANTS_MINF_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_CONSTANT_CONSTANTS_MINF_HPP_INCLUDED

#include <boost/simd/include/functor.hpp>
#include <boost/simd/sdk/meta/float.hpp>
#include <boost/simd/sdk/meta/int_c.hpp>
#include <boost/simd/sdk/meta/double.hpp>
#include <boost/simd/sdk/meta/make_dependent.hpp>
#include <boost/simd/sdk/constant/constant.hpp>
#include <boost/simd/sdk/config.hpp>

/*!
 * \ingroup boost_simd_constant
 * \defgroup boost_simd_constant_minf Minf
 *
 * \par Description
 * Constant Minf
 * \par
 * The value of this constant is type dependant. This means that for different
 * types it does not represent the same mathematical number.
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/minf.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class T,class A0>
 *     meta::call<tag::minf_(A0)>::type
 *     Minf();
 * }
 * \endcode
 *
 *
 * \param T template parameter of Minf
 *
 * \return type T value
 *
 *
**/

namespace boost { namespace simd
{
  namespace tag
  {
    struct Valmin;

    /*!
     * \brief Define the tag Minf of functor Minf
     *        in namespace boost::simd::tag for toolbox boost.simd.constant
    **/
    #ifdef BOOST_SIMD_NO_INFINITIES
    typedef Valmin Minf;
    #else
    struct Minf : ext::pure_constant_<Minf>
    {
      typedef double default_type;
      template<class Target, class Dummy=void>
      struct apply : meta::make_dependent<Valmin, Dummy>::type::template apply<Target,Dummy> {};
    };

    template<class T, class Dummy>
    struct  Minf::apply<boost::dispatch::meta::single_<T>,Dummy>
          : meta::single_<0xFF800000> {};

    template<class T, class Dummy>
    struct  Minf::apply<boost::dispatch::meta::double_<T>,Dummy>
          : meta::double_<0xFFF0000000000000ULL> {};
    #endif
  }

  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Minf, Minf)
} }

#include <boost/simd/toolbox/constant/include/constants/valmin.hpp>
#include <boost/simd/sdk/constant/common.hpp>

#endif


//---------------------------------------------------------------------------//
// Copyright (c) 2020-2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020-2021 Nikita Kaskov <nbering@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_ALGEBRA_RANDOM_ELEMENT_HPP
#define CRYPTO3_ALGEBRA_RANDOM_ELEMENT_HPP

#include <nil/crypto3/algebra/type_traits.hpp>

#include <nil/crypto3/multiprecision/debug_adaptor.hpp>
#include <nil/crypto3/multiprecision/cpp_bin_float.hpp>
#include <nil/crypto3/multiprecision/cpp_int.hpp>

#include <boost/core/ignore_unused.hpp>

#include <boost/random/independent_bits.hpp>
#include <boost/random/discard_block.hpp>
#include <boost/random/xor_combine.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/random_number_generator.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_smallint.hpp>
#include <boost/random/discrete_distribution.hpp>
#include <boost/random/random_device.hpp>

#include <random>

namespace nil {
    namespace crypto3 {
        namespace algebra {

            template<
                typename FieldType,
                typename DistributionType = boost::random::uniform_int_distribution<typename FieldType::modulus_type>,
                typename GeneratorType = boost::random::mt19937>
            typename std::enable_if<is_field<FieldType>::value &&
                                        !(is_extended_field<FieldType>::value),
                                    typename FieldType::value_type>::type
                random_element() {

                using field_type = FieldType;
                using distribution_type = DistributionType;
                using generator_type = GeneratorType;

                distribution_type d(0, field_type::modulus);

                boost::random_device rd;
                // rd.seed(time(0));

                typename field_type::value_type value(d(rd));

                return value;
            }

            template<
                typename FieldType,
                typename DistributionType = boost::random::uniform_int_distribution<typename FieldType::modulus_type>,
                typename GeneratorType = boost::random::mt19937>
            typename std::enable_if<is_field<FieldType>::value
                                        && is_extended_field<FieldType>::value,
                                    typename FieldType::value_type>::type
                random_element() {

                using field_type = FieldType;
                using distribution_type = DistributionType;
                using generator_type = GeneratorType;

                typename field_type::value_type::data_type data;
                const std::size_t data_dimension = field_type::arity / field_type::underlying_field_type::arity;

                for (int n = 0; n < data_dimension; ++n) {
                    data[n] =
                        random_element<typename FieldType::underlying_field_type, distribution_type, generator_type>();
                }

                return typename field_type::value_type(data);
            }

            template<typename CurveGroupType,
                     typename DistributionType = boost::random::uniform_int_distribution<
                         typename CurveGroupType::underlying_field_type::modulus_type>,
                     typename GeneratorType = boost::random::mt19937>
            typename std::enable_if<is_curve_group<CurveGroupType>::value,
                                    typename CurveGroupType::value_type>::type
                random_element() {

                using curve_type = typename CurveGroupType::curve_type;
                using field_type = typename curve_type::scalar_field_type;
                using distribution_type = boost::random::uniform_int_distribution<typename field_type::modulus_type>;
                using generator_type = GeneratorType;

                return random_element<typename curve_type::scalar_field_type, distribution_type, generator_type>() *
                       CurveGroupType::value_type::one();
            }

        }    // namespace algebra
    }        // namespace crypto3
}    // namespace nil
#endif    // CRYPTO3_ALGEBRA_RANDOM_ELEMENT_HPP


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <vector>

#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/mtl/recursion/matrix_recursator.hpp>

using namespace mtl;
using namespace mtl::recursion;
using namespace std;

int main(int argc, char* argv[])
{
    // typedef dense2D<double> matrix_t;
    typedef morton_dense<double,  doppled_32_row_mask> matrix_t;
    const int                                          n= 10;
    matrix_t                                           A1(n, n), C1(n, n);

    // fill matrix, e.g.
    hessian_setup(A1, 1.0);
    hessian_setup(C1, 2.0);

    typedef mat::recursator<matrix_t>               recursator_t;
    recursator_t                                      ra1(A1), rc1(C1);
    std::vector<recursator_t>                         v;
    v.push_back(ra1); v.push_back(rc1);

    // Of course you can also add sub-matrices
    v.push_back(rc1.south_west());

    // Only correct with latest revision (due to subtle problem just fixed)
    for (int i= 0; i < v.size(); i++)
	cout << "Recursator " << i << ": \n" << *v[i].north_west() << "\n";

    // Should also work with older version
    for (int i= 0; i < v.size(); i++)
	cout << "Recursator " << i << ": \n", print_matrix(*v[i].north_west()), cout << "\n";

    return 0;
}


///////////////////////////////////////////////////////////////////////////////
// count_matching.hpp                                                        //
//                                                                           //
//  Copyright 2010 Erwann Rogard. 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)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_CELLS_COUNT_MATCHING_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_CELLS_COUNT_MATCHING_HPP_ER_2010
#include <boost/mpl/assert.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/cells/cells.hpp>
#include <boost/mpl/detail/wrapper.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace contingency_table{

  	template<typename Keys,typename AccSet,typename T>
    std::size_t count_matching(AccSet const& acc,const T& sample){
        namespace ns = contingency_table;
        return ns::extract::cells<Keys>( acc )[ sample ];
    }

    // A: accumulator set
    // S: sample
    // F: unary functor
    template<typename A,typename S,typename F>
    struct fun_count_matcher{

        typedef std::size_t size_;
        typedef void result_type;

        fun_count_matcher(const A& a,const S& s, const F& f)
            :acc( a ), sample( s ), fun( f ){}

        template<typename Keys>
        void operator()(const boost::mpl::detail::wrapper<Keys>& u)const{
            namespace ns = contingency_table;
            this->fun(
                ns::count_matching<Keys>(
                    this->acc,
                    this->sample
                )
            );
        }

        private:
        const A& acc;
        const S& sample;
        F fun;
    };

    template<typename A,typename S,typename F>
    fun_count_matcher<A,S,F>
    make_fun_count_matcher(const A& a,const S& s, const F& f){
        namespace ns = contingency_table;
        typedef ns::fun_count_matcher<A,S,F> result_;
        return result_(a, s, f);
    }

}// contingency_table
}// detail
}// statistics
}// boost

#endif


#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <iomanip>
#include <boost/lexical_cast.hpp>
#include <boost/algorithm/string.hpp>

using std::cout;
using std::endl;
const int NumFlags = 24;

int main()
{
    std::fstream file("readings.txt");

    int badCount = 0;
    std::string badDate;
    int badCountMax = 0;
    while(true)
    {
        std::string line;
        getline(file, line);
        if(!file.good())
            break;

        std::vector<std::string> tokens;
        boost::algorithm::split(tokens, line, boost::is_space());

        if(tokens.size() != NumFlags * 2 + 1)
        {
            cout << "Bad input file." << endl;
            return 0;
        }

        double total = 0.0;
        int accepted = 0;
        for(size_t i = 1; i < tokens.size(); i += 2)
        {
            double val = boost::lexical_cast<double>(tokens[i]);
            int flag = boost::lexical_cast<int>(tokens[i+1]);
            if(flag > 0)
            {
                total += val;
                ++accepted;
                badCount = 0;
            }
            else
            {
                ++badCount;
                if(badCount > badCountMax)
                {
                    badCountMax = badCount;
                    badDate = tokens[0];
                }
            }
        }

        cout << tokens[0];
        cout << "  Reject: " << std::setw(2) << (NumFlags - accepted);
        cout << "  Accept: " << std::setw(2) << accepted;
        cout << "  Average: " << std::setprecision(5) << total / accepted << endl;
    }
    cout << endl;
    cout << "Maximum number of consecutive bad readings is " << badCountMax << endl;
    cout << "Ends on date " << badDate << endl;
}


/*
Copyright 2015 Glen Joseph Fernandes
(glenjofe@gmail.com)

Distributed under the Boost Software License, Version 1.0.
(http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_ALIGN_DETAIL_ALIGN_UP_HPP
#define BOOST_ALIGN_DETAIL_ALIGN_UP_HPP

#include <boost/align/detail/is_alignment.hpp>
#include <boost/assert.hpp>

namespace boost {
namespace alignment {

inline void*
align_up(void* ptr, std::size_t alignment) BOOST_NOEXCEPT
{
    BOOST_ASSERT(detail::is_alignment(alignment));
    return reinterpret_cast<void*>((reinterpret_cast<std::
        size_t>(ptr) + alignment - 1) & ~(alignment - 1));
}

} /* alignment */
} /* boost */

#endif


#ifndef LATTICE_SU2_H
#define LATTICE_SU2_H
#include <complex>
#include <Eigen/Dense>
#include <Eigen/StdVector>

EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix2cd)
typedef Eigen::Matrix2cd SU2mat;

// class that contains U[mu], i.e. 4 x SU(2) matrices
class gauge {
private:
	SU2mat U_[4];
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

	// [mu] operator returns U[mu]
	SU2mat& operator[](int i) { return U_[i]; }
	const SU2mat& operator[](int i) const { return U_[i]; }
};

#endif //LATTICE_SU3_H


#pragma once

#include <polyfem/Problem.hpp>
#include <polyfem/InterpolatedFunction.hpp>
#include <polyfem/RBFInterpolation.hpp>

#include <polyfem/Common.hpp>

#include <vector>
#include <Eigen/Dense>

namespace polyfem
{
	class PointBasedTensorProblem : public Problem
	{
	private:
		class BCValue
		{
		public:
			BCValue()
			{
				Eigen::Matrix<bool, 3, 1> dd;
				dd.setConstant(true);
				init(0, 0, 0, dd);
			}

			Eigen::RowVector3d operator()(const Eigen::RowVector3d &pt) const;

			bool init(const json &data);

			void init(const double x, const double y, const double z, const Eigen::Matrix<bool, 3, 1> &dd)
			{
				this->val << x, y, z;
				dirichlet_dims = dd;
				is_val = true;
			}

			void init(const Eigen::Vector3d &v, const Eigen::Matrix<bool, 3, 1> &dd)
			{
				this->val = v;
				dirichlet_dims = dd;
				is_val = true;
			}

			void init(const Eigen::MatrixXd &pts, const Eigen::MatrixXi &tri, const Eigen::MatrixXd &fun, const int coord, const Eigen::Matrix<bool, 3, 1> &dd)
			{
				tri_func.init(fun, pts, tri);
				is_tri = true;

				coordiante_0 = (coord + 1) % 3;
				coordiante_1 = (coord + 2) % 3;

				dirichlet_dims = dd;

				is_val = false;
			}

			void init(const Eigen::MatrixXd &pts, const Eigen::MatrixXd &fun, const std::string &rbf, const double eps, const int coord, const Eigen::Matrix<bool, 3, 1> &dd)
			{
				rbf_func.init(fun, pts, rbf, eps);
				is_tri = false;

				if (coord >= 0)
				{
					coordiante_0 = (coord + 1) % 3;
					coordiante_1 = (coord + 2) % 3;
				}
				else
				{
					coordiante_0 = -1;
					coordiante_1 = -1;
				}

				dirichlet_dims = dd;

				is_val = false;
			}

			bool is_dirichet_dim(const int d) const { return dirichlet_dims(d); }

		private:
			Eigen::Vector3d val;
			InterpolatedFunction2d tri_func;
			RBFInterpolation rbf_func;
			bool is_val;
			bool is_tri;
			int coordiante_0 = 0;
			int coordiante_1 = 1;
			Eigen::Matrix<bool, 3, 1> dirichlet_dims;
		};

	public:
		PointBasedTensorProblem(const std::string &name);

		void rhs(const AssemblerUtils &assembler, const std::string &formulation, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		bool is_rhs_zero() const override { return abs(rhs_) < 1e-10; }

		void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		void neumann_bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const Eigen::MatrixXd &normals, const double t, Eigen::MatrixXd &val) const override;

		bool has_exact_sol() const override { return false; }
		bool is_scalar() const override { return false; }

		void set_parameters(const json &params) override;

		void add_constant(const int bc_tag, const Eigen::Vector3d &value)
		{
			Eigen::Matrix<bool, 3, 1> dd;
			dd.setConstant(true);
			add_constant(bc_tag, value, dd);
		}

		void add_function(const int bc_tag, const Eigen::MatrixXd &func, const Eigen::MatrixXd &pts, const Eigen::MatrixXi &tri, const int coord)
		{
			Eigen::Matrix<bool, 3, 1> dd;
			dd.setConstant(true);
			add_function(bc_tag, func, pts, tri, coord, dd);
		}

		void add_function(const int bc_tag, const Eigen::MatrixXd &func, const Eigen::MatrixXd &pts, const std::string &rbf, const double eps, const int coord)
		{
			Eigen::Matrix<bool, 3, 1> dd;
			dd.setConstant(true);
			add_function(bc_tag, func, pts, rbf, eps, coord, dd);
		}

		void add_constant(const int bc_tag, const Eigen::Vector3d &value, const Eigen::Matrix<bool, 3, 1> &dd, const bool is_neumann = false);
		void add_function(const int bc_tag, const Eigen::MatrixXd &func, const Eigen::MatrixXd &pts, const Eigen::MatrixXi &tri, const int coord, const Eigen::Matrix<bool, 3, 1> &dd, const bool is_neumann = false);
		void add_function(const int bc_tag, const Eigen::MatrixXd &func, const Eigen::MatrixXd &pts, const std::string &rbf, const double eps, const int coord, const Eigen::Matrix<bool, 3, 1> &dd, const bool is_neumann = false);

		bool is_dimension_dirichet(const int tag, const int dim) const override;
		bool all_dimensions_dirichlet() const override { return all_dimensions_dirichlet_; }

	private:
		bool initialized_ = false;
		bool all_dimensions_dirichlet_ = true;
		double rhs_;
		double scaling_;
		Eigen::Vector3d translation_;
		std::vector<BCValue> bc_;
		std::vector<BCValue> neumann_bc_;
	};
} // namespace polyfem


#include <boost/compute/random.hpp>


#include <PRM.h>
#include <iostream>
#include <Eigen/Dense>
#include <vector>
#include <cstdlib>
#include <ctime>
#include <fstream>

using namespace std;
using namespace Eigen;

double random(double start, double end)
{
    return start+(end-start)*rand()/(RAND_MAX + 1.0);
}

int main(int argc, char *argv[]) {
    uint N = 10;
    if (argc > 1) {
        N = atoi(argv[1]);
    }
    cout << "n = " << N << endl;

    srand(unsigned(time(0)));

    ifstream fid("../test/points.txt");

    int paths_num;
    int fig_num;
    fid >> paths_num >> fig_num;

    vector<Vector3d> sources;
    vector<Vector3d> goals;
    double x,y,z;
    sources.clear();

    int nn = 10;
    for(uint i = 0; i < paths_num; ++i) {
        fid >> x >> y >> z;
        if (i >= nn) continue;
        sources.push_back(Vector3d(x,y,z));
    }
    goals.clear();
    for(uint i = 0; i < paths_num; ++i) {
        fid >> x >> y >> z;
        if (i >= nn) continue;
        goals.push_back(Vector3d(x,y,z));
    }
    fid.close();

    PRM prm(N, sources, goals, Vector3d(0,-50,0), Vector3d(100,50,100));

    // cout << endl;
    // for(uint i = 0; i < N; ++i) {
    //     for(uint j = 0; j < prm.edge[i].size(); ++j) {
    //         cout << prm.vertex[i].transpose() << " " << prm.vertex[prm.edge[i][j]].transpose() << endl;
    //     }
    //     // cout << prm.vertex[i].transpose() << " " << prm.vertex[i].transpose() << endl;
    // }

    vector<int> sources_index;
    vector<int> goals_index;

    for(int i = 0; i < paths_num; ++i) {
        if (i >= nn) continue;
        sources_index.push_back(i);
        // goals_index.push_back(i + paths_num);
        goals_index.push_back(i + nn);
    }
    vector< vector<int>> paths = prm.ComflictBasedSearch(sources_index, goals_index);

    cout << paths_num << endl;

    ofstream fout("output.txt");
    for(uint i = 0; i < paths.size(); ++i) {
        fout << "#" << endl;
        for(auto j = paths[i].begin(); j!=paths[i].end(); ++j) {
            fout << prm.vertex[*j](0) << ',' <<
                    prm.vertex[*j](1) << ',' <<
                    prm.vertex[*j](2) <<
                    endl;
        }
    }
    fout.close();

    return 1;
}


#ifndef mdi_interdataset_hpp
#define mdi_interdataset_hpp

#include <vector>
#include "stats.hpp"

#include <Eigen/Dense>

class interdataset {
  int _nfiles;
  std::vector<int> _phiord;    // was first
  std::vector<int> _weightord; // was second

public:
  interdataset(int nfiles);

  int nfiles() const { return _nfiles; }
  int nphis()  const { return _nfiles*(_nfiles-1)/2; }

  const std::vector<int> &getPhiOrd()    const { return _phiord; }
  const std::vector<int> &getWeightOrd() const { return _weightord; }
};

class mdisampler {
  const interdataset &_inter;

  Eigen::MatrixXf _weight;
  Eigen::VectorXf _dpmass, _phi;
  double _nu;

  Eigen::MatrixXf _prod;
  Eigen::VectorXf _prodsum;

  void updateAllocHistogram();
  void updateColumnProducts();

  Eigen::VectorXd calcProdCoef() const;
  double calcProdPhi(const Eigen::VectorXd &coef) const;

public:
  mdisampler(const interdataset &inter, int nclus)
    : _inter(inter),
      _weight(nclus, inter.nfiles()),
      _dpmass(Eigen::VectorXf::Zero(inter.nfiles())),
      _phi(Eigen::VectorXf::Zero(inter.nphis())),
      _nu(0.0),
      _prod(nclus, (1<<inter.nfiles())-1), _prodsum((1<<inter.nfiles())-1)
  { }

  int nclus()  const { return _weight.rows(); }
  int nfiles() const { return _weight.cols(); }
  int nphis()  const { return _inter.nphis(); }

  void setDpMass(int k, double mass);
  void setWeight(int k, const Eigen::VectorXf &weight);
  void setWeight(const Eigen::MatrixXf &x);
  void setPhi(int k, int l, double phi);
  void setNu(double x);

  // void setWeight(int j, int k, double weight);
  void setDpMass(const Eigen::VectorXf &x);
  void setPhis(const Eigen::VectorXf &x);

  double dpmass(int k)        const { return _dpmass(k); }
  double phi(int phi_kl)      const { return _phi(phi_kl); }
  double phi(int k, int l)    const { return _phi(phiBetween(k, l)); }
  double nu()                 const { return _nu; }
  double weight(int j, int k) const { return _weight(j, k); }

  const Eigen::VectorXf &dpmass()  const { return _dpmass; }
  const Eigen::MatrixXf &weights() const { return _weight; }
  const Eigen::VectorXf &phis()    const { return _phi;    }
  typename Eigen::MatrixXf::ConstColXpr weight(int k) const {
    return _weight.col(k); }

  Eigen::VectorXf phis(int k) const {
    Eigen::VectorXf out(nfiles());

    for (int j = 0; j < nfiles(); j++) {
      if (j < k)
        out[j] = phi(j, k);
      else if (j > k)
        out[j] = phi(k, j);
      else
        out[j] = NAN;
    }

    return out;
  }

  int phiBetween (int k, int l) const {
    assert(k < nfiles() && l < nfiles() && k < l);
    return nfiles()*k - (k+1)*k/2 + l-k - 1;
  }

  void sampleFromPrior();

  double nuConditionalBeta() const;
  double phiConditionalBeta(int k, int l) const;
  double gammaConditionalBeta(int k, int j) const;

  #ifndef NOCUDA

  #endif
};

#endif


#include <k4a/k4a.hpp>

#include <iostream>
#include <fstream>
#include <iomanip>
#include <string>
#include <vector>
#include <array>

#include <Eigen/Dense>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/common/transforms.h>
#include <pcl/io/ply_io.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/bilateral.h>
#include <pcl/filters/fast_bilateral.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>

#include <glm/glm.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtc/type_ptr.hpp>

#include <Open3D/Geometry/PointCloud.h>
#include <Open3d/Registration/ColoredICP.h>
#include <Open3D/Open3D.h>
#include "k4a_grabber.h"

using namespace std;
using namespace boost;
using namespace pcl;
using namespace Eigen;
using namespace glm;

typedef pcl::PointXYZRGB PointType;

template<typename DataType>
Eigen::Matrix<DataType, Eigen::Dynamic, Eigen::Dynamic> load_csv(const std::string & path)
{
	std::ifstream indata;
	indata.open(path);
	std::string line;
	std::vector<DataType> values;
	unsigned int rows = 0;
	while (std::getline(indata, line))
	{
		std::stringstream lineStream(line);
		std::string cell;
		while (std::getline(lineStream, cell, ','))
		{
			values.push_back(std::stod(cell));
		}
		++rows;
	}
	return Eigen::Map<const Eigen::Matrix<typename Eigen::Matrix<DataType, Eigen::Dynamic, Eigen::Dynamic>::Scalar, Eigen::Matrix<DataType, Eigen::Dynamic, Eigen::Dynamic>::RowsAtCompileTime, Eigen::Matrix<DataType, Eigen::Dynamic, Eigen::Dynamic>::ColsAtCompileTime, Eigen::RowMajor>>(values.data(), rows, values.size() / rows);
}

void convertToOpenCDPointCloud(pcl::PointCloud<PointType>::ConstPtr cloud, open3d::geometry::PointCloud &pointCloud)
{
	double x, y, z, r, g, b;
	pointCloud.Clear();

	for (const auto &point : cloud->points)
	{
		x = point.x;
		y = point.y;
		z = point.z;
		r = point.r / 256.0;
		g = point.g / 256.0;
		b = point.b / 256.0;

		pointCloud.points_.push_back(Eigen::Vector3d(x, y, z));
		pointCloud.colors_.push_back(Eigen::Vector3d(r, g, b));
	}
}

Eigen::Matrix4d colorCloudRegistrationOptimization(pcl::PointCloud<PointType>::ConstPtr pcl_source, pcl::PointCloud<PointType>::ConstPtr pcl_target, const Eigen::Matrix4d &init_transformation)
{
	open3d::geometry::PointCloud open3d_source_original, open3d_target_original;
	open3d::geometry::PointCloud open3d_source_current, open3d_target_current;

	convertToOpenCDPointCloud(pcl_source, open3d_source_original);
	convertToOpenCDPointCloud(pcl_target, open3d_target_original);

	open3d::registration::RegistrationResult result;
	Eigen::Matrix4d current_transformation = init_transformation;

	open3d_source_current = open3d_source_original;
	open3d_target_current = open3d_target_original;

	open3d::geometry::PointCloud open3d_source_down;
	open3d::geometry::PointCloud open3d_target_down;

	double radius = 0.01;

	open3d_source_down = *open3d_source_current.VoxelDownSample(radius);
	open3d_target_down = *open3d_target_current.VoxelDownSample(radius);

	open3d_source_down.EstimateNormals(open3d::geometry::KDTreeSearchParamHybrid(radius * 2, 30));
	open3d_target_down.EstimateNormals(open3d::geometry::KDTreeSearchParamHybrid(radius * 2, 30));

	result = open3d::registration::RegistrationColoredICP(open3d_source_down, open3d_target_down,
		radius, current_transformation,
		open3d::registration::ICPConvergenceCriteria(1e-6, 1e-6, 50));

	return result.transformation_;
}

int main(int argc, char **argv)
{
	string frame_sub1_master_file = "frame_sub1_master.csv";
	string frame_sub2_master_file = "frame_sub2_master.csv";
	cout << "Reading csv file" << endl;
	Transform<double, 3, Affine> transformation_sub1_master;
	Transform<double, 3, Affine> transformation_sub2_master;
	transformation_sub1_master.matrix() = load_csv<double>(frame_sub1_master_file);
	transformation_sub2_master.matrix() = load_csv<double>(frame_sub2_master_file);

	const uint32_t deviceCount = k4a::device::get_installed_count();
	cout << deviceCount << " devices connected..." << endl;
	if (deviceCount == 0)
	{
		cout << "no azure kinect devices detected!" << endl;
	}
	if (deviceCount != 3)
	{
		cout << "should connect 3 kinects" << endl;
	}

	// PCL Visualizer
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Point Cloud Viewer"));

	// Point Cloud
	pcl::PointCloud<PointType>::Ptr cloud_master;
	pcl::PointCloud<PointType>::Ptr cloud_sub1;
	pcl::PointCloud<PointType>::Ptr cloud_sub2;

	// Retrieved Point Cloud Callback Function
	boost::mutex mutex_master;
	boost::function<void(const pcl::PointCloud<PointType>::ConstPtr&)> function_master =
		[&cloud_master, &mutex_master](const pcl::PointCloud<PointType>::ConstPtr& ptr)
	{
		boost::mutex::scoped_lock lock(mutex_master);

		/* Point Cloud Processing */

		cloud_master = ptr->makeShared();

	};

	boost::mutex mutex_sub1;
	boost::function<void(const pcl::PointCloud<PointType>::ConstPtr&)> function_sub1 =
		[&cloud_sub1, &mutex_sub1](const pcl::PointCloud<PointType>::ConstPtr& ptr)
	{
		boost::mutex::scoped_lock lock(mutex_sub1);

		/* Point Cloud Processing */

		cloud_sub1 = ptr->makeShared();

	};

	boost::mutex mutex_sub2;
	boost::function<void(const pcl::PointCloud<PointType>::ConstPtr&)> function_sub2 =
		[&cloud_sub2, &mutex_sub2](const pcl::PointCloud<PointType>::ConstPtr& ptr)
	{
		boost::mutex::scoped_lock lock(mutex_sub2);

		/* Point Cloud Processing */

		cloud_sub2 = ptr->makeShared();

	};

	// KinectAzureDKGrabber
	boost::shared_ptr<pcl::Grabber> grabber_sub2 =
		boost::make_shared<pcl::KinectAzureDKGrabber>(2, K4A_DEPTH_MODE_NFOV_UNBINNED, K4A_IMAGE_FORMAT_COLOR_BGRA32, K4A_COLOR_RESOLUTION_720P);
	boost::shared_ptr<pcl::Grabber> grabber_sub1 =
		boost::make_shared<pcl::KinectAzureDKGrabber>(1, K4A_DEPTH_MODE_NFOV_UNBINNED, K4A_IMAGE_FORMAT_COLOR_BGRA32, K4A_COLOR_RESOLUTION_720P);
	boost::shared_ptr<pcl::Grabber> grabber_master =
		boost::make_shared<pcl::KinectAzureDKGrabber>(0, K4A_DEPTH_MODE_NFOV_UNBINNED, K4A_IMAGE_FORMAT_COLOR_BGRA32, K4A_COLOR_RESOLUTION_720P);

	boost::shared_ptr<pcl::KinectAzureDKGrabber> grabber_ = boost::dynamic_pointer_cast<pcl::KinectAzureDKGrabber>(grabber_master);

	// Register Callback Function
	boost::signals2::connection connection_sub1 = grabber_sub1->registerCallback(function_sub1);
	boost::signals2::connection connection_sub2 = grabber_sub2->registerCallback(function_sub2);
	boost::signals2::connection connection_master = grabber_master->registerCallback(function_master);

	// Start Grabber
	grabber_sub2->start();
	grabber_sub1->start();
	grabber_master->start();

	// get the intrinics of master
	k4a::calibration calibration = grabber_->getCalibration();
	k4a_calibration_intrinsic_parameters_t *intrinsics = &calibration.color_camera_calibration.intrinsics.parameters;
	Eigen::Matrix3f intrinsics_eigen;
	intrinsics_eigen <<
		intrinsics->param.fx, 0.0f, intrinsics->param.cx,
		0.0f, intrinsics->param.fy, intrinsics->param.cy,
		0.0f, 0.0f, 1.0f;
	Eigen::Matrix4f extrinsics_eigen = Eigen::Matrix4f::Identity();
	viewer->setCameraParameters(intrinsics_eigen, extrinsics_eigen);

	pcl::PointCloud<PointType>::Ptr cloud(new PointCloud<PointType>);
	float aspect = 0.475f;

	transformation_sub1_master.matrix()(0, 3) *= aspect;
	transformation_sub1_master.matrix()(1, 3) *= aspect;
	transformation_sub1_master.matrix()(2, 3) *= aspect;
	transformation_sub2_master.matrix()(0, 3) *= aspect;
	transformation_sub2_master.matrix()(1, 3) *= aspect;
	transformation_sub2_master.matrix()(2, 3) *= aspect;

	bool refineResult = false;

	Eigen::Matrix4d init_transformation1;
	Eigen::Matrix4d init_transformation2;
	init_transformation1 = transformation_sub1_master.matrix();
	init_transformation2 = transformation_sub2_master.matrix();

	Eigen::Matrix4d refinedTransformation1;
	Eigen::Matrix4d refinedTransformation2;

	while (!viewer->wasStopped())
	{
		// Update Viewer
		viewer->spinOnce();
		boost::mutex::scoped_try_lock lock_master(mutex_master);
		boost::mutex::scoped_try_lock lock_sub1(mutex_sub1);
		boost::mutex::scoped_try_lock lock_sub2(mutex_sub2);

		if (lock_master.owns_lock() && cloud_master &&
			lock_sub1.owns_lock() && cloud_sub1 &&
			lock_sub2.owns_lock() && cloud_sub2)
		{

			if (!refineResult)
			{
				std::cout << "Start Calibration Optimization." << std::endl;
				refinedTransformation1 = colorCloudRegistrationOptimization(cloud_sub1, cloud_master, init_transformation1);
				refinedTransformation2 = colorCloudRegistrationOptimization(cloud_sub2, cloud_master, init_transformation2);
				refineResult = true;
				std::cout << "Calibration optimization process over." << std::endl;
			}

			transformPointCloud(*cloud_sub1, *cloud_sub1, refinedTransformation1, true);
			transformPointCloud(*cloud_sub2, *cloud_sub2, refinedTransformation2, true);

			*cloud = *cloud_sub1 + *cloud_sub2;
			*cloud += *cloud_master;

			// Update Point Cloud
			if (!viewer->updatePointCloud(cloud, "cloud"))
			{
				viewer->addPointCloud(cloud, "cloud");
			}
		}
	}
	// Stop Grabber
	grabber_sub1->stop();
	grabber_sub2->stop();
	grabber_master->stop();

	// Disconnect Callback Function
	if (connection_master.connected() || connection_sub1.connected() || connection_sub2.connected())
	{
		connection_sub1.disconnect();
		connection_sub2.disconnect();
		connection_master.disconnect();
	}

	return 0;
}


#include <limits>
#include <atomic>
#include <string>
#include <stdexcept>
#include <exception>

#include <Eigen/Dense>

#include <ros/ros.h>

#include <tf/transform_listener.h>

#include <std_msgs/Bool.h>
#include <waypoint_manager_msgs/Waypoint.h>

namespace {
    static std::atomic_bool recived_waypoint;
    static float default_goal_radius = 1;
    static float current_goal_radius = default_goal_radius;
    static Eigen::Vector2f goal_position = Eigen::Vector2f::Zero();
}

void waypointCallback(const waypoint_manager_msgs::Waypoint::ConstPtr &msg) {
    try {
        goal_position.x() = msg->pose.position.x;
        goal_position.y() = msg->pose.position.y;

        bool found_goal_radius = false;

        for(unsigned int i = 0; i < msg->properties.size(); i ++) {
            if(msg->properties[i].name == "goal_radius") {
                current_goal_radius = std::stof(msg->properties[i].data);
                found_goal_radius = true;
            }
        }
        if(!found_goal_radius) {
            current_goal_radius = default_goal_radius;
        }
        recived_waypoint.store(true);
    }
    catch(const std::exception &) {
        recived_waypoint.store(false);
        ROS_WARN("Failed parse radius_node");
    }
}

auto main(int argc, char **argv) -> int {
    ros::init(argc, argv, "radius_node");
    ros::NodeHandle nh,
                    private_nh("~");

    std::string goal_topic,
                waypoint_topic,
                is_reached_goal_topic;

    std::string robot_base_frame,
                global_frame;

    float goal_check_frequency,
          wait_no_waypoint_time;

    recived_waypoint.store(false);

    private_nh.param(
        "goal_topic",
        goal_topic,
        std::string("move_base_simple/goal")
    );
    private_nh.param(
        "waypoint",
        waypoint_topic,
        std::string("waypoint")
    );
    private_nh.param(
        "is_reached_goal_topic",
        is_reached_goal_topic,
        std::string("waypoint/is_reached")
    );
    private_nh.param(
        "robot_base_frame",
        robot_base_frame,
        std::string("base_link")
    );
    private_nh.param(
        "global_frame",
        global_frame,
        std::string("map")
    );
    private_nh.param(
        "goal_check_frequency",
        goal_check_frequency,
        static_cast<float>(1.0)
    );
    private_nh.param(
        "wait_no_waypoint_time",
        wait_no_waypoint_time,
        static_cast<float>(5.0)
    );
    private_nh.param(
        "default_goal_radius",
        default_goal_radius,
        static_cast<float>(1.0)
    );
    current_goal_radius = default_goal_radius;

    if(robot_base_frame == global_frame) {
        throw std::runtime_error("Please set different frame name robot_base_frame or global_frame");
    }

    auto loop_rate = ros::Rate(goal_check_frequency);
    auto is_reached_goal_publisher = nh.advertise<std_msgs::Bool>(
        is_reached_goal_topic,
        1,
        true
    );
    auto waypoint_subscriber = nh.subscribe(
        waypoint_topic,
        1,
        waypointCallback
    );

    ROS_INFO("Start radius_node");
    ROS_INFO("robot_base_frame is %s", robot_base_frame.c_str());
    ROS_INFO("global_base_frame is %s", global_frame.c_str());
    ROS_INFO("default_goal_radius is %f", default_goal_radius);

    tf::TransformListener tf_listener;

    while(ros::ok()) {
        Eigen::Vector2f distance_of_goal;

        ros::spinOnce();

        if(!recived_waypoint.load()) {
            ROS_INFO("Waiting waypoint radius_node");
            ros::Duration(wait_no_waypoint_time).sleep();
            continue;
        }

        try {
            tf::StampedTransform tf_transform;
            tf_listener.lookupTransform(
                global_frame,
                robot_base_frame,
                ros::Time(0),
                tf_transform
            );

            distance_of_goal.x() = goal_position.x() - tf_transform.getOrigin().x();
            distance_of_goal.y() = goal_position.y() - tf_transform.getOrigin().y();

            std_msgs::Bool msg;

            if(distance_of_goal.lpNorm<2>() < current_goal_radius) {
                ROS_INFO("Goal reached from goal_event_handler::radius_node");
                msg.data = true;
            }
            else {
                msg.data = false;
            }

            is_reached_goal_publisher.publish(msg);
        }
        catch(const tf::TransformException &e) {
            ROS_WARN("Transform failed %s to %s", global_frame.c_str(), robot_base_frame.c_str());
            ros::Duration(1.0).sleep();
        }
        loop_rate.sleep();
    }
    ROS_INFO("Finish waypoint_server_node");

    return 0;
}


/*
 *            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.
 * 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.
 *
 */

// Third party includes
#include <boost/format.hpp>

// Local VOTCA includes
#include "votca/xtp/adiis.h"
#include "votca/xtp/adiis_costfunction.h"
#include "votca/xtp/bfgs-trm.h"
#include "votca/xtp/logger.h"

namespace votca {
namespace xtp {

Eigen::VectorXd ADIIS::CalcCoeff(const std::vector<Eigen::MatrixXd>& dmathist,
                                 const std::vector<Eigen::MatrixXd>& mathist) {
  success = true;
  Index size = dmathist.size();

  const Eigen::MatrixXd& dmat = dmathist.back();
  const Eigen::MatrixXd& H = mathist.back();
  Eigen::VectorXd DiF = Eigen::VectorXd::Zero(size);
  Eigen::MatrixXd DiFj = Eigen::MatrixXd::Zero(size, size);

  for (Index i = 0; i < size; i++) {
    DiF(i) = ((dmathist[i]) - dmat).cwiseProduct(H).sum();
  }

  for (Index i = 0; i < size; i++) {
    for (Index j = 0; j < size; j++) {
      DiFj(i, j) = ((dmathist[i]) - dmat).cwiseProduct((mathist[j]) - H).sum();
    }
  }

  ADIIS_costfunction a_cost = ADIIS_costfunction(DiF, DiFj);
  BFGSTRM optimizer = BFGSTRM(a_cost);
  Logger log;
  optimizer.setLog(&log);
  optimizer.setNumofIterations(1000);
  optimizer.setTrustRadius(0.01);
  // Starting point: equal weights on all matrices
  Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(size, 1.0 / double(size));
  optimizer.Optimize(coeffs);
  success = optimizer.Success();
  coeffs = optimizer.getParameters().cwiseAbs2();
  double xnorm = coeffs.sum();
  coeffs /= xnorm;

  if (std::abs(coeffs.tail(1).value()) < 0.001) {
    success = false;
  }
  return coeffs;
}

}  // namespace xtp
}  // namespace votca