// GenFem - A high-level finite element library
// Copyright (C) 2010-2026 Eric Bechet
//
// See the LICENSE file for license information and contributions.
// Please report all bugs and problems to <bechet@cadxfem.org>.
//
// Initial design: Eric Bechet (rev.6919 in gmsh)


#ifndef _GEN_ALGORITHMS_H_
#define _GEN_ALGORITHMS_H_

#include "dofManager.h"
#include "quadratureRules.h"
#include "MVertex.h"
#include "genTerm.h"
#include "savedGenTerm.h"
#include "savedDiffTerm.h"
#include "savedUnknownTerm.h"
#include "genFilters.h"
#include "genSimpleFunction.h"


template<class Iterator, class Assembler, class Scalar> void Assemble(const genTerm<Scalar,2> &term,
                                                                      Iterator itbegin, Iterator itend,
                                                                      QuadratureBase &integrator, Assembler &assembler)
{
  for (Iterator it = itbegin; it != itend; ++it)
  {
    MElement* e = *it;
    Assemble(term,e,integrator,assembler);
  }
}

template<class Assembler,class Scalar> void Assemble(const genTerm<Scalar,2> &term, MElement* e,
                                                     QuadratureBase &integrator, Assembler &assembler)
{
  genMatrix<Scalar> genlocalMatrix;
  std::vector<Dof> R;
  std::vector<Dof> C;
  IntPt* GP;
  int npts = integrator.getIntPoints(e, &GP);
  term.get(e, npts, GP, genlocalMatrix);
  term.getKeys(e, R, 0);
  term.getKeys(e, C, 1);
//   printf("Assemble local matrix of element : %d\n", e->getNum()); genlocalMatrix.print();
//   printf("IntPoints [] = {\n", npts);
//   for (int i=0; i<npts; ++i) printf("{ %+1.12e , %+1.12e , %+1.12e }   w=%+1.12e\n", GP[i].pt[0], GP[i].pt[1], GP[i].pt[2], GP[i].weight);
//   printf("};\n");
//   printf("Keys R [%d] = {\n", (int)R.size());
//   for (int i=0; i<R.size(); ++i) printf("{ %11ld , %6d }\n", R[i].getEntity(), R[i].getType());
//   printf("};\n");
//   printf("Keys C [%d] = {\n", (int)C.size());
//   for (int i=0; i<C.size(); ++i) printf("{ %11ld , %6d }\n", C[i].getEntity(), C[i].getType());
//   printf("};\n");
  fullMatrix<typename Assembler::dataMat> localMatrix((typename Assembler::dataMat*)genlocalMatrix.getDataPtr(),genlocalMatrix.size1(),genlocalMatrix.size2());
  assembler.assemble(R, C, localMatrix);
//    assembler.assemble(C, R, localMatrix.transpose());
}
  
template<class Iterator, class Assembler,class Scalar> void Assemble(const std::shared_ptr<genTerm<Scalar,2> > &term,
                                                                     Iterator itbegin, Iterator itend,
                                                                     QuadratureBase &integrator, Assembler &assembler)
{
  Assemble(*term,itbegin,itend,integrator,assembler);
}

template<class Assembler, class Scalar> void Assemble(const std::shared_ptr<genTerm<Scalar,2> > &term, MElement* e,
                                                      QuadratureBase &integrator, Assembler &assembler)
{
  Assemble(*term,e,integrator,assembler);
}


template<class Iterator, class Assembler,class Scalar> void Assemble(const genTerm<Scalar,1> &term,
                                                                     Iterator itbegin, Iterator itend,
                                                                     QuadratureBase &integrator, Assembler &assembler)
{
  for (Iterator it = itbegin; it != itend; ++it)
  {
    MElement* e = *it;
    Assemble(term,e,integrator,assembler);
  }
}

template<class Assembler,class Scalar> void Assemble(const genTerm<Scalar,1> &term,  MElement* e,
                                                     QuadratureBase &integrator, Assembler &assembler)
{
  genVector<Scalar> genlocalVector;
  std::vector<Dof> R;
  IntPt* GP;
  int npts = integrator.getIntPoints(e, &GP);
  term.get(e, npts, GP, genlocalVector);
  term.getKeys(e, R);
  fullVector<typename Assembler::dataMat> localVector((typename Assembler::dataMat*) genlocalVector.getDataPtr(),genlocalVector.size());
  assembler.assemble(R, localVector);
}


template<class Iterator, class Assembler,class Scalar> void Assemble(const std::shared_ptr<genTerm<Scalar,1> > &term,
                                                                     Iterator itbegin, Iterator itend,
                                                                     QuadratureBase &integrator, Assembler &assembler)
{
  Assemble(*term,itbegin,itend,integrator,assembler);
}

template<class Assembler,class Scalar> void Assemble(const std::shared_ptr<genTerm<Scalar,1> > &term, MElement* e,
                                                     QuadratureBase &integrator, Assembler &assembler)
{
  Assemble(*term,e,integrator,assembler);
}



template<class Iterator, class dataMat, class Scalar> void Assemble(const genTerm<Scalar,0> &term,
                                                                    Iterator itbegin, Iterator itend,
                                                                    QuadratureBase &integrator, dataMat &val)
{
  for (Iterator it = itbegin; it != itend; ++it)
  {
    MElement* e = *it;
    Assemble(term,e,integrator,val);
  }
}

template<class dataMat,class Scalar> void Assemble(const genTerm<Scalar,0> &term, MElement* e,
                                                   QuadratureBase &integrator, dataMat &val)
{
  genScalar<Scalar> localval;
  IntPt* GP;
  int npts = integrator.getIntPoints(e, &GP);
  term.get(e, npts, GP, localval);
  val += localval()();
}

template<class Iterator, class dataMat,class Scalar> void Assemble(const std::shared_ptr<genTerm<Scalar,0> > &term,
                                                                   Iterator itbegin, Iterator itend,
                                                                   QuadratureBase &integrator, dataMat &val)
{
  Assemble(*term,itbegin,itend,integrator,val);
}

template<class dataMat, class Scalar> void Assemble(const std::shared_ptr<genTerm<Scalar,0> > &term, MElement* e,
                                                    QuadratureBase &integrator, dataMat &val)
{
  Assemble(*term,e,integrator,val);
}



template<class Assembler> void FixDofs(Assembler &assembler, std::vector<Dof> &dofs,
                                       const std::vector<typename Assembler::dataVec> &vals)
{
  int nbff = dofs.size();
  for (int i = 0; i < nbff; ++i){
    assembler.fixDof(dofs[i], vals[i]);
  }
}


// checks if the linear constraint "cons" is not recursively defined.
// if it is the case, replace it with sth equivalent by pivoting
template<class Assembler> void CheckLC(Assembler &assembler, DofAffineConstraint<typename Assembler::dataVec> &cons, Dof &to_replace)
{
  std::cout << "willing to set Dof[" << to_replace.getType() << ":" << to_replace.getEntity()  << "] = ";
  for (int k=0;k<cons.linear.size();++k)
  {
    std::cout << "Dof[" << cons.linear[k].first.getType() << ":" << cons.linear[k].first.getEntity()  << "] * " << cons.linear[k].second << " +" ; 
  }
  std::cout << " " << cons.shift << std::endl;
  std::vector<double> coefloop;
  std::vector<DofAffineConstraint<typename Assembler::dataVec> > dac;
  std::vector<double > dacrat;
  DofAffineConstraint<double> copycons;
  for (int ffi=0; ffi<cons.linear.size(); ++ffi)
  {
    DofAffineConstraint<typename Assembler::dataVec> cons2;
    bool ok=true;
    if (assembler.getLinearConstraint(cons.linear[ffi].first,cons2))
    {
      std::cout << "potential loop in LC " << std::endl;
      std::cout << "Dof[" << cons.linear[ffi].first.getType() << ":" << cons.linear[ffi].first.getEntity()  << "] = ";
      for (int k=0;k<cons2.linear.size();++k)
      {
        std::cout << "Dof[" << cons2.linear[k].first.getType() << ":" << cons2.linear[k].first.getEntity()  << "] * " << cons2.linear[k].second << " +" ;
        if (cons2.linear[k].first == to_replace)
        {
          coefloop.push_back(cons2.linear[k].second);
          typename std::vector<std::pair<Dof,typename Assembler::dataVec> >::iterator itk(&cons2.linear[k]);
          cons2.linear.erase(itk);k--;
          ok=false;
        }
      }
      std::cout << " " << cons2.shift << std::endl;
      if (!ok)
      { 
        dac.push_back(cons2);
        dacrat.push_back(cons.linear[ffi].second);
      }
    }
    if (ok)
    {
      copycons.linear.push_back(cons.linear[ffi]);
    }
  }
  copycons.shift=cons.shift;
  double divisor=1.0;
  assert(coefloop.size()==dac.size());
  for (int l=0;l<dac.size();++l)
  {
    divisor-=coefloop[l]*dacrat[l];
    for (int m=0;m<dac[l].linear.size();++m)
      copycons.linear.push_back(std::pair<Dof,typename Assembler::dataVec>(dac[l].linear[m].first,dac[l].linear[m].second*coefloop[l]));
      copycons.shift+=dac[l].shift*coefloop[l];
  }
  assert(divisor!=0.0);
  for (int m=0;m<copycons.linear.size();++m)
    copycons.linear[m].second/=divisor;
  copycons.shift/=divisor;
  cons=copycons;
  std::cout << "instead set Dof[" << to_replace.getType() << ":" << to_replace.getEntity()  << "] = ";
  for (int k=0;k<cons.linear.size();++k)
  {
    std::cout << "Dof[" << cons.linear[k].first.getType() << ":" << cons.linear[k].first.getEntity()  << "] * " << cons.linear[k].second << " +" ; 
  }
  std::cout << " " << cons.shift << std::endl;
}


// That function takes care of previous LC existing in the dofManager.
template<class T,class Assembler> void FixNodalDofsLC(genFSpace<T> &space, MElement* e,
                                                      Assembler &assembler,
                                                      genSimpleFunction<typename TensorialTraits<T>::ScalarType > &fct,
                                                      genSimpleFunction<T> &orientation,
                                                      const genFilterDof &filter)
{
  std::vector<Dof> R;
  space.getKeys(e, R);
  int nv = e->getNumVertices();
  std::vector<MVertex*> tabV;
  tabV.reserve(nv);
  for (int i = 0; i < nv; ++i)
    tabV.push_back(e->getVertex(i));

  for (int i = 0; i < nv; ++i)
  {
    typename TensorialTraits<T>::ScalarType val=fct(tabV[i]->x(), tabV[i]->y(), tabV[i]->z()),max(0.);
    int maxi=-1;
    double xyz[3]={tabV[i]->x(), tabV[i]->y(), tabV[i]->z()};
    T ori=orientation(xyz[0],xyz[1],xyz[2]);
    double uvw[3];
    e->xyz2uvw(xyz,uvw);
    typename genFSpace<T>::ContainerValType FFVals;
    space.f(e, uvw[0],uvw[1],uvw[2], FFVals);
    typename ContainerTraits<typename TensorialTraits<T>::ScalarType,1>::Container coeffs(FFVals.size());
    for (int ffi=0;ffi<FFVals.size();++ffi)
    {
      if (filter(R[ffi]))
      {
        FullContrProdOp<T,T>()(FFVals(ffi),ori,coeffs(ffi));
        if (max< fabs(coeffs(ffi))) 
        {
          DofAffineConstraint<double> cons2;
          if (!assembler.getLinearConstraint(R[ffi],cons2)) // we don't want to replace an actual LC, see if we can choose another value
          {
            max=fabs(coeffs(ffi));maxi=ffi;
          }
        }
      }
    }
    if (maxi!=-1)
    {
      DofAffineConstraint<double> cons;
      for (int ffi=0; ffi<R.size(); ++ffi)
      {
        if ((ffi!=maxi)&&filter(R[ffi])) // that one ought to be replaced
        {
          double rat=-coeffs(ffi)/coeffs(maxi);
          if (fabs(rat)>1e-13) // avoid very weak relative LC that may fill up the linear system for nothing
          {
            cons.linear.push_back(std::pair<Dof,double>(R[ffi],rat));
          }
        }
      }
      cons.shift = val;
      CheckLC(assembler,cons,R[maxi]); // avoid recursive LC

      DofAffineConstraint<double> cons2;
      if (assembler.getLinearConstraint(R[maxi],cons2))
      {
        std::cout << "conflict in LC " << std::endl;
      }
      else
      {
        assembler.setLinearConstraint(R[maxi],cons);
      }
    }
  }
}


template<class T,class Iterator, class Assembler> void FixNodalDofsLC(genFSpace<T> &space,
                                                                      Iterator itbegin, Iterator itend,
                                                                      Assembler &assembler,
                                                                      genSimpleFunction<typename TensorialTraits<T>::ScalarType> &fct,
                                                                      genSimpleFunction<T> &orientation,
                                                                      const genFilterDof &filter)
{
  for (Iterator it = itbegin; it != itend; ++it)
    FixNodalDofsLC(space, *it, assembler, fct, orientation, filter);
}

template<class T,class Iterator, class Assembler> void FixNodalDofsLC(genFSpace<T> &space,
                                                                      Iterator itbegin, Iterator itend,
                                                                      genSimpleFunction<T> &orientation,
                                                                      Assembler &assembler)
{
  genFilterDofTrivial filter;
  genSimpleFunctionConstant<typename TensorialTraits<T>::ScalarType> fct(0.);
  FixNodalDofsLC(space, itbegin, itend, assembler, fct, orientation, filter);
}



template<class T,class Assembler> void FixNodalDofsLC(const std::shared_ptr<genFSpace<T> > &space, MElement* e,
                                                      Assembler &assembler,
                                                      genSimpleFunction<typename TensorialTraits<T>::ScalarType> &fct,
                                                      genSimpleFunction<T> &orientation,
                                                      const genFilterDof &filter)
{
  FixNodalDofsLC(*space,e,assembler,fct,orientation,filter);
}

template<class T,class Iterator, class Assembler> void FixNodalDofsLC(const std::shared_ptr<genFSpace<T> > &space,
                                                                      Iterator itbegin, Iterator itend,
                                                                      Assembler &assembler,
                                                                      genSimpleFunction<typename TensorialTraits<T>::ScalarType> &fct,
                                                                      genSimpleFunction<T> &orientation,
                                                                      const genFilterDof &filter)
{
  FixNodalDofsLC(*space,itbegin,itend,assembler,fct,orientation,filter);
}


template<class T,class Assembler> void FixNodalDofs(genFSpace<T> &space, MElement* e,
                                                    Assembler &assembler,
                                                    genSimpleFunction<typename TensorialTraits<T>::ScalarType > &fct,
                                                    const genFilterDof &filter)
{
  std::vector<Dof> R;
  space.getKeys(e, R);
  int nv = e->getNumVertices();
  std::vector<MVertex*> tabV;
  tabV.reserve(nv);
  for (int i = 0; i < nv; ++i)
    tabV.push_back(e->getVertex(i));

  for (std::vector<Dof>::iterator itd = R.begin(); itd != R.end(); ++itd)
  {
    Dof key = *itd;
    if (filter(key))
      for (int i = 0;i < nv; ++i)
        if (tabV[i]->getNum() == key.getEntity())
        {
          assembler.fixDof(key, fct(tabV[i]->x(), tabV[i]->y(), tabV[i]->z())());
          break;
        }
  }
}

template<class T,class Iterator, class Assembler> void FixNodalDofs(genFSpace<T> &space,
                                                                    Iterator itbegin, Iterator itend,
                                                                    Assembler &assembler,
                                                                    genSimpleFunction<typename TensorialTraits<T>::ScalarType> &fct,
                                                                    const genFilterDof &filter)
{
  for (Iterator it = itbegin; it != itend; ++it)
    FixNodalDofs(space, *it, assembler, fct, filter);
}

template<class T,class Iterator, class Assembler> void FixNodalDofs(genFSpace<T> &space,
                                                                    Iterator itbegin, Iterator itend,
                                                                    Assembler &assembler)
{
  genFilterDofTrivial filter;
  genSimpleFunctionConstant<typename TensorialTraits<T>::ScalarType> fct(0.);
  FixNodalDofs(space, itbegin, itend, assembler, fct, filter);
}



template<class T,class Assembler> void FixNodalDofs(const std::shared_ptr<genFSpace<T> > &space, MElement* e,
                                                    Assembler &assembler,
                                                    genSimpleFunction<typename TensorialTraits<T>::ScalarType> &fct,
                                                    const genFilterDof &filter)
{
  FixNodalDofs(*space,e,assembler,fct,filter);
}

template<class T,class Iterator, class Assembler> void FixNodalDofs(const std::shared_ptr<genFSpace<T> > &space,
                                                                    Iterator itbegin, Iterator itend,
                                                                    Assembler &assembler,
                                                                    genSimpleFunction<typename TensorialTraits<T>::ScalarType> &fct,
                                                                    const genFilterDof &filter)
{
  FixNodalDofs(*space,itbegin,itend,assembler,fct,filter);
}

template<class T,class Iterator, class Assembler> void FixNodalDofs(const std::shared_ptr<genFSpace<T> > &space,
                                                                    Iterator itbegin, Iterator itend,
                                                                    Assembler &assembler)
{
  FixNodalDofs(*space,itbegin,itend,assembler);
}


template<class Assembler> void NumberDofs(genTermBase<1> &space,
                                          MElement* e,
                                          Assembler &assembler)
{
  std::vector<Dof> R;
  space.getKeys(e, R);
  int nbdofs = R.size();
  for (int i = 0; i < nbdofs; ++i) assembler.numberDof(R[i]);
}


template<class Iterator, class Assembler> void NumberDofs(genTermBase<1> &space,
                                                          Iterator itbegin, Iterator itend,
                                                          Assembler &assembler)
{
  for (Iterator it = itbegin; it != itend; ++it)
    NumberDofs<Assembler>(space,*it,assembler);
}

template<class Iterator, class Assembler> void NumberDofs(genTermBase<1> &space1,genTermBase<1> &space2,
                                                          Iterator itbegin, Iterator itend,
                                                          Assembler &assembler)
{
  for (Iterator it = itbegin; it != itend; ++it)
  {
    NumberDofs<Assembler>(space1,*it,assembler);
    NumberDofs<Assembler>(space2,*it,assembler);
  }
}


template<class Assembler> void NumberDofs(const std::shared_ptr<genTermBase<1> > &space,
                                          MElement* e,
                                          Assembler &assembler)
{
  NumberDofs(*space,e,assembler);
}


template<class Iterator, class Assembler> void NumberDofs(const std::shared_ptr<genTermBase<1> > &space,
                                                          Iterator itbegin, Iterator itend,
                                                          Assembler &assembler)
{
  NumberDofs(*space,itbegin,itend,assembler);
}

template<class Iterator, class Assembler> void NumberDofs(const std::shared_ptr<genTermBase<1> > &space1, const std::shared_ptr<genTermBase<1> > &space2,
                                                          Iterator itbegin, Iterator itend,
                                                          Assembler &assembler)
{
  NumberDofs(*space1,*space2,itbegin,itend,assembler);
}




//--------------------------------------------------------------------------
// Algorithms for periodic boundary conditions
//--------------------------------------------------------------------------

#include "MPoint.h"
template<class T, class Assembler> void LinearCombinationNodalDofs(genFSpace<T> &space, MVertex* v_neg, MVertex* v_pos,
                                                                   Assembler &assembler,
                                                                   genSimpleFunction<genTensor2<typename Assembler::dataVec> > &fct)
{
  genTensor1<double> v(v_pos->x()-v_neg->x(), v_pos->y()-v_neg->y(), v_pos->z()-v_neg->z());
  genTensor1<double> val = fct(v.x(),v.y(),v.z()) * v;

  MPoint pt_pos(v_pos);     // modifier fspace afin  d'avoir directement la possibiliter de passer un vertex ?
  MPoint pt_neg(v_neg);     // modifier pour passer directement deux iterateurs ?
  std::vector<Dof> k_pos, k_neg;
  space.getKeys(&pt_pos,k_pos);
  space.getKeys(&pt_neg,k_neg);

  int ndofs=k_pos.size();
  DofAffineConstraint<double> cons;
  for (int i=0; i<ndofs; ++i)
  {
    cons.linear.push_back(std::pair<Dof,double>(k_neg[i],1.0));
    cons.shift = val(i);
    assembler.setLinearConstraint(k_pos[i],cons);
    cons.linear.clear();
  }
}

template<class T, class Iterator1, class Iterator2, class Assembler> void LinearCombinationNodalDofs(genFSpace<T> &space,
                                                                                                     Iterator1 itbegin_neg, Iterator1 itend_neg,
                                                                                                     Iterator2 itbegin_pos, Iterator2 itend_pos,
                                                                                                     Assembler &assembler,
                                                                                                     genSimpleFunction<genTensor2<typename Assembler::dataVec> > &fct)
{
  Iterator1 it_neg = itbegin_neg;
  Iterator2 it_pos = itbegin_pos;
  for (; (it_neg != itend_neg) && (it_pos != itend_pos) ; ++it_neg, ++it_pos)
    LinearCombinationNodalDofs(space, *it_neg, *it_pos, assembler, fct);
}

template<class T,class Assembler> void LinearCombinationNodalDofs(const std::shared_ptr<genFSpace<T> > &space, MVertex* v_neg, MVertex* v_pos,
                                                                  Assembler &assembler,
                                                                  genSimpleFunction<genTensor2<typename Assembler::dataVec> > &fct)
{
  LinearCombinationNodalDofs(*space,v_neg,v_pos,assembler,fct);
}

template<class T, class Iterator1, class Iterator2, class Assembler> void LinearCombinationNodalDofs(const std::shared_ptr<genFSpace<T> > &space,
                                                                                                     Iterator1 itbegin_neg, Iterator1 itend_neg,
                                                                                                     Iterator2 itbegin_pos, Iterator2 itend_pos,
                                                                                                     Assembler &assembler,
                                                                                                     genSimpleFunction<genTensor2<typename Assembler::dataVec> > &fct)
{
  LinearCombinationNodalDofs(*space,itbegin_neg,itend_neg,itbegin_pos,itend_pos,assembler,fct);
}


template<class Iterator> void SelectPosVertexFromMapping(Iterator itbegin_neg, Iterator itend_neg,
                                                         Iterator itbegin_pos, Iterator itend_pos,
                                                         genSimpleFunction<genTensor1<double> > &mapping,
                                                         std::vector<MVertex*> &v)
{
  for (Iterator it=itbegin_neg; it!=itend_neg; ++it)
  {
    MVertex* v_neg = *it;
    genTensor1<double> m_tmp = mapping(v_neg->x(),v_neg->y(),v_neg->z());
    MVertex vm_tmp (m_tmp.x(), m_tmp.y(), m_tmp.z());
    MVertex* v_pos = FindNearestVertex(&vm_tmp, itbegin_pos, itend_pos);
    v.push_back( v_pos );
  }
}


template<class Iterator> MVertex* FindNearestVertex(MVertex* v, Iterator itbegin, Iterator itend)
{
  MVertex* v_min = *itbegin;
  double d_min = v_min->distance(v);
  for (Iterator it=itbegin; it!=itend; ++it)
  {
    MVertex* v_tmp = *it;
    double d_tmp = v_tmp->distance(v);
    if (d_tmp<d_min)
    {
      d_min = d_tmp;
      v_min = v_tmp;
    }
  }
  return v_min;
}


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



template<class T,class Assembler> void SavedUnknownAddToRightHandSide(const SavedUnknownTerm<T> &term,
                                                                      Assembler &assembler,
                                                                      std::string LinSystName="A")
{
  int nbDof = term.size();
  linearSystem<typename Assembler::dataMat>* linearSyst = assembler.getLinearSystem(LinSystName);
  if (!linearSyst->isAllocated()) linearSyst->allocate(nbDof);
  
  typename std::map<Dof, typename SavedUnknownTerm<T>::ValType >::const_iterator it;
  for (it = term.begin(); it != term.end(); ++it)
  {
    Dof D = it->first;
    typename SavedUnknownTerm<T>::ValType val = it->second;
    assert( assembler.isAnUnknown(D) );
    int DofNum = assembler.getDofNumber(D);
    linearSyst->addToRightHandSide(DofNum, val);
  }
}

template<class T,class Assembler> void SavedUnknownAddToSolution(const SavedUnknownTerm<T> &term,
                                                                 Assembler &assembler,
                                                                 std::string LinSystName="A")
{
  linearSystem<typename Assembler::dataMat>* linearSyst = assembler.getLinearSystem(LinSystName);
  
  typename std::map<Dof, typename SavedUnknownTerm<T>::ValType >::const_iterator it;
  for (it = term.begin(); it != term.end(); ++it)
  {
    Dof D = it->first;
    typename SavedUnknownTerm<T>::ValType val = it->second;
    assert( assembler.isAnUnknown(D) );
    int DofNum = assembler.getDofNumber(D);
    linearSyst->addToSolution(DofNum, val);
  }
}

template<class Iterator,class T, class Scalar> void SaveModifUnknown(const genTerm<Scalar,1> &term,
                                                                     Iterator itbegin, Iterator itend,
                                                                     SavedUnknownTerm<T> &save)
{
  for (Iterator it = itbegin; it != itend; ++it)
  {
    MElement* e = *it;
    IntPt* GP;
    std::vector<typename SavedUnknownTerm<T>::ContainerValType> vvals(1);
    term.get(e, 1, GP, vvals);
    int nbUnknown = vvals[0].size();

    std::vector<Dof> D;
    save.getKeys(e,D);                   //WARNING le terme save doit posseder le meme espace fonctionnel que term
    int nbdofs = D.size();

    std::vector<Dof> UD;
    for (int i = 0; i < nbdofs; ++i)
    {
      if( save.isAnUnknown(D[i]) )       //WARNING le terme save doit deja contenir la map des inconnues
        UD.push_back(D[i]);
    }
    save.set(e, nbUnknown, UD, vvals[0]);//WARNING le terme term ne doit pas contenir le terme save
  }
}


template<class Iterator,class Assembler,class T> void SaveUnknown(const Assembler &assembler,
                                                                  Iterator itbegin, Iterator itend,
                                                                  SavedUnknownTerm<T> &save)
{
  for (Iterator it = itbegin; it != itend; ++it)
  {
    MElement* e = *it;

    std::vector<Dof> D;
    save.getKeys(e,D);
    int nbdofs = D.size();

    std::vector<Dof> UD;
    typename ContainerTraits<genTensor0<double>,1>::Container UVals;
    UVals.resize(nbdofs);
    int nbUnknown = 0;
    for (int i = 0; i < nbdofs; ++i)
    {
      if( assembler.getAnUnknown(D[i], UVals(nbUnknown)()) )
      {
        UD.push_back(D[i]);
        ++nbUnknown;
      }
    }
    UVals.resize(nbUnknown,false);
    save.set(e,nbUnknown,UD,UVals);
  }
}


template<class Iterator,class Assembler,class genTerm> void SaveDof(const Assembler &assembler,
                                                                    Iterator itbegin, Iterator itend,
                                                                    genTerm &save)
{
  for (Iterator it = itbegin; it != itend; ++it)
  {
    MElement* e = *it;

    std::vector<Dof> D;
    save.getKeys(e,D);
    int nbdofs = D.size();

    typename ContainerTraits<genTensor0<double>,1>::Container DMVals;
    DMVals.resize(nbdofs);
    for (int i = 0; i < nbdofs; ++i)
    {
      assembler.getDofValue(D[i], DMVals(i)());
    }
    save.set(e,nbdofs,D,DMVals);
  }
}

template<class Iterator,class GenT> void SaveVertex(const GenT &term,
                                                    Iterator itbegin, Iterator itend,
                                                    QuadratureBase &integrator,
                                                    SavedGenTerm<typename GenT::ValType, GenT::Nsp> &save)
{
  for (Iterator it = itbegin; it != itend; ++it)
  {
    MElement* e = *it;
    int nv = e->getNumVertices();
    std::vector<MVertex*> verts;
    verts.reserve(nv);
    for (int i = 0; i < nv; ++i)
      verts.push_back(e->getVertex(i));
    IntPt pts[nv];
    for(int i=0; i<nv; ++i)
    {
      pts[i].pt[0]=verts[i]->x();
      pts[i].pt[1]=verts[i]->y();
      pts[i].pt[2]=verts[i]->z();
      pts[i].weight=1.;
    }
    std::vector<typename GenT::ContainerValType> vvals(nv);
    term.get(e, nv, &pts, vvals);
    save.set(e, nv, &pts, vvals);
  }
}

template<class Iterator,class GenT> void SavePtGauss(const GenT &term,
                                                     Iterator itbegin, Iterator itend,
                                                     QuadratureBase &integrator,
                                                     SavedGenTerm<typename GenT::ValType, GenT::Nsp> &save)
{
  for (Iterator it = itbegin; it != itend; ++it)
  {
    MElement* e = *it;
    IntPt* GP;
    int npts = integrator.getIntPoints(e, &GP);
    std::vector<typename GenT::ContainerValType> vvals(npts);
    term.get(e, npts, GP, vvals);
    save.set(e, npts, GP, vvals);
  }
}

template<class Iterator,class GenT> void SaveElt(const GenT &term,
                                                 Iterator itbegin, Iterator itend,
                                                 QuadratureBase &integrator,
                                                 SavedGenTerm<typename GenT::ValType, GenT::Nsp> &save)
{
  for (Iterator it = itbegin; it != itend; ++it)
  {
    MElement* e = *it;
    IntPt* GP;
    int npts = integrator.getIntPoints(e, &GP);
    typename GenT::ContainerValType vals;
    term.get(e, npts, GP, vals);
    std::vector<typename GenT::ContainerValType> vvals(1,vals);
    save.set(e, npts, GP, vvals);
  }
}

template<class T> void SaveConstant(const ConstantField<T> &term, ConstantField<T> &save)
{
  MElement* e;
  IntPt* GP;
  std::vector<typename ConstantField<T>::ContainerValType> vvals(1);
  term.get(e, 1, GP, vvals);
  save.set(e, 1, GP, vvals);
}


//// Mean HangingNodes
//template<class Assembler> void FillHangingNodes(genFSpaceBase &space, std::map<int,std::vector <int> > &HangingNodes, Assembler &assembler, int &field, int &dim)
//{
//  std::map<int, std::vector <int> >::iterator ith;
//  ith = HangingNodes.begin();
//  int compt = 1;
//  while (ith != HangingNodes.end()){
//    float fac;
//    fac = 1.0 / (ith->second).size();
//    for (int j = 0; j < dim; j++){
//      DofAffineConstraint<double> constraint;
//      int type = Dof::createTypeWithTwoInts(j, field);
//      Dof hgnd(ith->first, type);
//      for (int i = 0; i < (ith->second).size(); ++i){
//          Dof key((ith->second)[i], type);
//          std::pair<Dof, double > linDof(key, fac);
//          constraint.linear.push_back(linDof);
//      }
//      constraint.shift = 0;
//      assembler.setLinearConstraint (hgnd, constraint);
//    }
//    ith++;
//    compt++;
//  }
//}

#endif// _GENALGORITHMS_H_