// Nonlinear_genTerm - A solver for nonlinear problems using FEM
// Copyright (C) 2010-2026 Eric Bechet, Frederic Duboeuf
//
// See the LICENSE file for license information and contributions.
// Please report all bugs and problems to <bechet@cadxfem.org> or <duboeuf@outlook.com>.

#include <cstring>
#include <cstdio>
#include <fstream>

#include "nonLinearSolver.h"
#include "genAlgorithms.h"
#include "genSField.h"
#include "linearSystemCSR.h"
#include "linearSystemPETSc.h"
#include "linearSystemId.h"
#include "genSimpleFunctionExtended.h"

#if defined(HAVE_POST)
#include "PView.h"
#include "PViewData.h"
#endif

NonLinearSolver::~NonLinearSolver()
{
  if ( pAssembler ) delete pAssembler;
  for (int i=0;i<EDomains.size();++i) delete EDomains[i];
}

void NonLinearSolver::CheckProperties()
{
  for (unsigned int i=0; i<Data->Domains.size(); ++i)
  {
    if (Data->Domains[i]->type=="Elastic")
    {
      NLElasticDomain* field = new NLElasticDomain(*(Data->Domains[i]));
      EDomains.push_back (field);
    }
    else if (Data->Domains[i]->type=="IsotropicElastic")
    {
      IsotropicNLElasticDomain* field = new IsotropicNLElasticDomain(*(Data->Domains[i]));
      EDomains.push_back (field);
    }
    else if (Data->Domains[i]->type=="OrthotropicElastic")
    {
      OrthotropicNLElasticDomain* field = new OrthotropicNLElasticDomain(*(Data->Domains[i]));
      EDomains.push_back (field);
    }
    else if (Data->Domains[i]->type=="BitriangularDamage")
    {
      BitriangularDamageDomain* field = new BitriangularDamageDomain(*(Data->Domains[i]));
      EDomains.push_back (field);
    }
  }

  // Combines a boundary condition at a domain
  for (unsigned int j=0; j<Data->BCs.size(); ++j)
  {
    std::vector<genGroupOfElements*> contain;
    for (unsigned int i=0; i<EDomains.size(); ++i)
    {
      genFilterElementGroupVertex filterElt(EDomains[i]->group());
      genGroupOfElements* EF = new genGroupOfElements(Data->BCs[j]->group(), filterElt);
      contain.push_back(EF);
    }
    CombineBCatDomain.push_back(contain);
  }

  for (unsigned int i=0; i<Data->BCs.size(); ++i)
    FinalBCs.push_back(new genBoundaryCondition(*Data->BCs[i]));
  assert(Data->BCs.size() == FinalBCs.size());
}

void NonLinearSolver::readInputFile ( const std::string &fileName )
{
  modelname = fileName;
  std::cout<<"model name : " << modelname << std::endl ;
  Data->readInputFile(fileName);
  CheckProperties();
  printf("--> %d Domains\n", (int)EDomains.size());
  printf("--> %d BCs \n", (int)Data->BCs.size());
}



void NonLinearSolver::BuildDirichlet(double facDirichlet)
{
  for ( unsigned int i = 0; i < Data->BCs.size(); ++i )
  {
    if ((Data->BCs[i]->kind=="Dirichlet")||(Data->BCs[i]->kind==""))
    {
      if (Data->BCs[i]->What=="Displacement")
      {
//        delete Data->BCs[i];
//        Data->BCs[i] = new genBoundaryCondition(*FinalBCs[i]);
        Data->BCs[i]->fscalar = new genSimpleFunctionOperatorProduct<genTensor0<double> > (facDirichlet, FinalBCs[i]->fscalar );
      }
    }
  }
}

void NonLinearSolver::BuildNeumann(double facNeumann)
{
  for ( unsigned int i = 0; i < Data->BCs.size(); ++i )
  {
    if ((Data->BCs[i]->kind=="Neumann")||(Data->BCs[i]->kind==""))
    {
      if (Data->BCs[i]->What=="Force")
      {
//         delete Data->BCs[i];
//         Data->BCs[i] = new genBoundaryCondition(*FinalBCs[i]);
        Data->BCs[i]->fvector = new genSimpleFunctionOperatorProduct<genTensor1<double> > (facNeumann, FinalBCs[i]->fvector );
      }
    }
  }
}


void NonLinearSolver::BuildInitialFunctionSpaces()
{
  FSpaceDisps.resize(EDomains.size());
  dEpsilonDisps.resize(EDomains.size());
  for ( unsigned int i = 0; i < EDomains.size(); ++i )
  {
    NLElasticDomain* E = EDomains[i];

    if ( Data->dim==3 )
      FSpaceDisps[i] = genFSpace<genTensor1<double> >::Handle(new VectorLagrangeFSpace<double>());
    if ( Data->dim==2 )
      FSpaceDisps[i] = genFSpace<genTensor1<double> >::Handle(new VectorLagrangeFSpace<double>(VectorLagrangeFSpace<double>::VECTOR_X, VectorLagrangeFSpace<double>::VECTOR_Y));

    for ( unsigned int j = 0; j < Data->BCs.size(); ++j )
    {
      if (CombineBCatDomain[j][i]->size()!=0)
      if ((Data->BCs[j]->kind=="Dirichlet")||(Data->BCs[j]->kind==""))
      {
        if (Data->BCs[j]->What=="Displacement")
        {
          std::cout <<  "Dirichlet BC Disp" << std::endl;
          genGroupOfElements* EF = CombineBCatDomain[j][i];
          genFilterDofComponent filterDof(Data->BCs[j]->comp1);
          FixNodalDofs(FSpaceDisps[i], EF->begin(), EF->end(), *pAssembler, *(Data->BCs[j]->fscalar), filterDof); // WARNING vérifier dans le cas de noeuds non physiques (SpaceReducer) entre 2 domaines
        }
      }
    }
    // we number the dofs : when a dof is numbered, it cannot be numbered
    // again with another number.
    std::cout << "Domain " << i << " : " << E->group()->size() << " elements"  << std::endl;
    NumberDofs ( FSpaceDisps[i], E->begin(), E->end(),*pAssembler );

    genTerm<genTensor2<double>,1>::Handle G(Gradient(FSpaceDisps[i]));
    genTerm<genTensor2<double>,1>::Handle CG(new genCache<genTensor2<double>,1>(G));
    dEpsilonDisps[i] = (CG+Transpose(CG))*0.5;
  }
  
  /*if ( Data->dim==3 )
    FSpaceDisp = genFSpace<genTensor1<double> >::Handle(new VectorLagrangeFSpace<double>());
  if ( Data->dim==2 )
    FSpaceDisp = genFSpace<genTensor1<double> >::Handle(new VectorLagrangeFSpace<double>(VectorLagrangeFSpace<double>::VECTOR_X, VectorLagrangeFSpace<double>::VECTOR_Y));

  for ( unsigned int i = 0; i < Data->BCs.size(); ++i )
  {
    if ((Data->BCs[i]->kind=="Dirichlet")||(Data->BCs[i]->kind==""))
    {
      if (Data->BCs[i]->What=="Displacement")
      {
        std::cout <<  "Dirichlet BC Disp" << std::endl;
        genFilterDofComponent filter ( Data->BCs[i]->comp1 );
        FixNodalDofs ( FSpaceDisp, Data->BCs[i]->begin(), Data->BCs[i]->end(), *pAssembler, *(Data->BCs[i]->fscalar), filter );
      }
    }
  }

  // we number the dofs : when a dof is numbered, it cannot be numbered
  // again with another number.
  for ( unsigned int i = 0; i < EDomains.size(); ++i )
  {
    std::cout << "Domain " << i << " : " << EDomains[i]->group()->size() << " elements"  << std::endl;
    NumberDofs ( FSpaceDisp, EDomains[i]->begin(), EDomains[i]->end(),*pAssembler );
  }

  genTerm<genTensor2<double>,1>::Handle G(Gradient(FSpaceDisp));
  genTerm<genTensor2<double>,1>::Handle CG(new genCache<genTensor2<double>,1>(G));
  
  dEpsilonDisp = (CG+Transpose(CG))*0.5; // small strain*/
}

void NonLinearSolver::AssembleInitialRHS()
{
  // build the internal force vector
  std::cout << "Internal force" << std::endl;

  GaussQuadrature Integ_Grad ( GaussQuadrature::Grad );
  for ( unsigned int i = 0; i < EDomains.size(); ++i )
  {
    NLElasticDomain* E = EDomains[i];

    genTerm<genTensor0<double>,1>::Handle Fint = E->Force<1>(dEpsilonDisps[i]);
    Assemble ( Fint*(-1), E->begin(), E->end(), Integ_Grad, *pAssembler );
  }
}

void NonLinearSolver::AssembleInitialLHS()
{
  for ( unsigned int i=0; i<EDomains.size(); ++i)
  {
    NLElasticDomain* E = EDomains[i];

    genTerm<genTensor0<double>,2>::Handle Total = E->deltaEnergy<1,1>(dEpsilonDisps[i],dEpsilonDisps[i]); // Possible de sauvegarder la matrice K0 assemblée dans le cas de la méthode de newton modifiée

    GaussQuadrature Integ_Bulk ( GaussQuadrature::GradGrad );
    Assemble ( Total, E->begin(), E->end(), Integ_Bulk, *pAssembler );
  }
}

void NonLinearSolver::BuildInitialLinearSystem()
{
  AssembleInitialRHS();
  AssembleInitialLHS();

  printf ( "nDofs=%d\n",pAssembler->sizeOfR() );
  printf ( "-- done assembling!\n" );

//   if ( Data->solvertype == 1 )
    exportKb(_step,_iter);
}



void NonLinearSolver::BuildFunctionSpaces()
{
  FSpaceDisps.resize(EDomains.size());
  dEpsilonDisps.resize(EDomains.size());
  dEpsilonNLDisps.resize(EDomains.size());
  for ( unsigned int i = 0; i < EDomains.size(); ++i )
  {
    NLElasticDomain* E = EDomains[i];

    if ( Data->dim==3 )
      FSpaceDisps[i] = genFSpace<genTensor1<double> >::Handle(new VectorLagrangeFSpace<double>());
    if ( Data->dim==2 )
      FSpaceDisps[i] = genFSpace<genTensor1<double> >::Handle(new VectorLagrangeFSpace<double>(VectorLagrangeFSpace<double>::VECTOR_X, VectorLagrangeFSpace<double>::VECTOR_Y));

    for ( unsigned int j = 0; j < Data->BCs.size(); ++j )
    {
      if (CombineBCatDomain[j][i]->size()!=0)
      if ((Data->BCs[j]->kind=="Dirichlet")||(Data->BCs[j]->kind==""))
      {
        if (Data->BCs[j]->What=="Displacement")
        {
          std::cout <<  "Dirichlet BC Disp" << std::endl;
          genGroupOfElements* EF = CombineBCatDomain[j][i];
          genFilterDofComponent filterDof(Data->BCs[j]->comp1);
          FixNodalDofs ( FSpaceDisps[i], EF->begin(), EF->end(), *pAssembler, *(Data->BCs[j]->fscalar), filterDof ); // WARNING vérifier dans le cas de noeuds non physiques (SpaceReducer) entre 2 domaines
        }
      }
    }
    // we number the dofs : when a dof is numbered, it cannot be numbered
    // again with another number.
    std::cout << "Domain " << i << " : " << E->group()->size() << " elements"  << std::endl;
    NumberDofs ( FSpaceDisps[i], E->begin(), E->end(),*pAssembler );
    
    genTerm<genTensor2<double>,1>::Handle G(Gradient(FSpaceDisps[i]));
    genTerm<genTensor2<double>,1>::Handle CG(new genCache<genTensor2<double>,1>(G));
    #ifndef LARGE_STRAIN
    dEpsilonDisps[i] = (CG+Transpose(CG))*0.5; // small strain
    #else
    std::shared_ptr<SavedGenTerm<genTensor2<double>,0> > lastG;
    savedGenTermManager.getSavedGenTerm("Grad",lastG,_step,0,i);
    genTerm<genTensor2<double>,0>::Handle lastG2=lastG;
    dEpsilonDisps[i] = (CG+Transpose(CG) + Compose<ContrProdOp>(Transpose(lastG2),CG) + Compose<ContrProdOp>(Transpose(CG),lastG2) )*0.5; // large strain
    dEpsilonNLDisps[i] = ( Compose<ContrProdOp>(Transpose(CG),CG) + Compose<ContrProdOp>(Transpose(CG),CG) )*0.5;
    #endif
  }

  
  
/*  if ( Data->dim==3 )
    FSpaceDisp = genFSpace<genTensor1<double> >::Handle(new VectorLagrangeFSpace<double>());
  if ( Data->dim==2 )
    FSpaceDisp = genFSpace<genTensor1<double> >::Handle(new VectorLagrangeFSpace<double>(VectorLagrangeFSpace<double>::VECTOR_X, VectorLagrangeFSpace<double>::VECTOR_Y));

  for ( unsigned int i = 0; i < Data->BCs.size(); ++i )
  {
    if ((Data->BCs[i]->kind=="Dirichlet")||(Data->BCs[i]->kind==""))
    {
      if (Data->BCs[i]->What=="Displacement")
      {
        std::cout <<  "Dirichlet BC Disp" << std::endl;
        genFilterDofComponent filter ( Data->BCs[i]->comp1 );
        FixNodalDofs ( FSpaceDisp, Data->BCs[i]->begin(), Data->BCs[i]->end(), *pAssembler, *(Data->BCs[i]->fscalar), filter );
      }
    }
  }

  // we number the dofs : when a dof is numbered, it cannot be numbered
  // again with another number.
  for ( unsigned int i = 0; i < EDomains.size(); ++i )
  {
    std::cout << "Domain " << i << " : " << EDomains[i]->group()->size() << " elements"  << std::endl;
    NumberDofs ( FSpaceDisp, EDomains[i]->begin(), EDomains[i]->end(),*pAssembler );
  }

  genTerm<genTensor2<double>,1>::Handle G(Gradient(FSpaceDisp));
  genTerm<genTensor2<double>,1>::Handle CG(new genCache<genTensor2<double>,1>(G));
#ifndef LARGE_STRAIN
  dEpsilonDisp = (CG+Transpose(CG))*0.5; // small strain
#else
  std::shared_ptr<SavedGenTerm<genTensor2<double>,0> > lastG;
  savedGenTermManager.getSavedGenTerm("Grad",lastG,_step);
  genTerm<genTensor2<double>,0>::Handle lastG2=lastG;
  dEpsilonDisp = (CG+Transpose(CG) + Compose<ContrProdOp>(Transpose(lastG2),CG) + Compose<ContrProdOp>(Transpose(CG),lastG2) )*0.5; // large strain
  dEpsilonNLDisp = ( Compose<ContrProdOp>(Transpose(CG),CG) + Compose<ContrProdOp>(Transpose(CG),CG) )*0.5;
#endif*/
}

void NonLinearSolver::AssembleRHS()
{
  GaussQuadrature Integ_Boundary ( GaussQuadrature::Val );
  GaussQuadrature Integ_Grad ( GaussQuadrature::Grad );

  for ( unsigned int i = 0; i < EDomains.size(); ++i )
  {
    NLElasticDomain* E = EDomains[i];

    // build the external force vector
    for ( unsigned int j = 0; j < Data->BCs.size(); ++j )
    {
      if (CombineBCatDomain[j][i]->size()!=0)
      if ((Data->BCs[j]->kind=="Neumann")||(Data->BCs[j]->kind==""))
      {
        if (Data->BCs[j]->What=="Force")
        {
          std::cout << "Neumann BC Force" << std::endl;
          genTerm<genTensor1<double> ,0>::Handle Func(new FunctionField<genTensor1<double> >(*(Data->BCs[j]->fvector)));
          genTerm<genTensor0<double>,1>::Handle loadterm = Compose<FullContrProdOp>(FSpaceDisps[i],Func);
          genGroupOfElements* EF = CombineBCatDomain[j][i];
          Assemble ( loadterm, EF->begin(), EF->end(), Integ_Boundary, *pAssembler );
        }
      }
    }
    // build the internal force vector
    std::cout << "Internal force" << std::endl;

    genTerm<genTensor0<double>,1>::Handle Fint = E->Force<1>(dEpsilonDisps[i]);
    Assemble ( Fint*(-1), E->begin(), E->end(), Integ_Grad, *pAssembler );
  }

  
  /*GaussQuadrature Integ_Boundary ( GaussQuadrature::Val );

  // build the external force vector
  for ( unsigned int i = 0; i < Data->BCs.size(); ++i )
  {
    if ((Data->BCs[i]->kind=="Neumann")||(Data->BCs[i]->kind==""))
    {
      if (Data->BCs[i]->What=="Force")
      {
        std::cout << "Neumann BC Force" << std::endl;
        genTerm<genTensor1<double> ,0>::Handle Func(new FunctionField<genTensor1<double> >(*(Data->BCs[i]->fvector)));
        genTerm<double,1>::Handle loadterm = Compose<FullContrProdOp>(FSpaceDisp,Func);
        Assemble ( loadterm , Data->BCs[i]->begin(), Data->BCs[i]->end(), Integ_Boundary, *pAssembler );
      }
    }
  }
  // build the internal force vector
  std::cout << "Internal force" << std::endl;

  GaussQuadrature Integ_Grad ( GaussQuadrature::Grad );
  for ( unsigned int i = 0; i < EDomains.size(); ++i )
  {
    NLElasticDomain* E = EDomains[i];

//     std::shared_ptr<SavedGenTerm<genTensor2<double>,0> > lastStress;
//     savedGenTermManager.getSavedGenTerm("Stress",lastStress,_step,0,i);
//     genTerm<double,1>::Handle Fint = Compose<FullContrProdOp>(Transpose(dEpsilonDisp), lastStress);
    genTerm<double,1>::Handle Fint = E->Force<1>(dEpsilonDisp);

    Assemble ( Fint*(-1), EDomains[i]->begin(), EDomains[i]->end(), Integ_Grad, *pAssembler );
  }*/
}

void NonLinearSolver::AssembleLHS()
{
#ifndef LARGE_STRAIN
  for ( unsigned int i=0; i<EDomains.size(); ++i) // small strain
  {
    NLElasticDomain* E = EDomains[i];

    genTerm<genTensor0<double>,2>::Handle Total = E->deltaEnergy<1,1>(dEpsilonDisps[i],dEpsilonDisps[i]); // Possible de sauvegarder la matrice K0 assemblée dans le cas de la méthode de newton modifiée

    GaussQuadrature Integ_Bulk ( GaussQuadrature::GradGrad );
    Assemble ( Total, E->begin(), E->end(), Integ_Bulk, *pAssembler );
  }
#else
  for ( unsigned int i = 0; i < EDomains.size(); ++i ) // Green-Lagrange // large strain
  {
    NLElasticDomain* E = EDomains[i];
    
    genTerm<genTensor0<double>,2>::Handle Kl = E->deltaEnergy<1,1>(dEpsilonDisps[i],dEpsilonDisps[i]); // Possible de sauvegarder la matrice K0 assemblée dans le cas de la méthode de newton modifiée

//     std::shared_ptr<SavedGenTerm<genTensor2<double>,0> > lastStress;
//     savedGenTermManager.getSavedGenTerm("Stress",lastStress,_step,0,i);
//     genTerm<double,2>::Handle Knl = Compose<FullContrProdOp>(Transpose(dEpsilonNLDisp), lastStress);
    genTerm<double,2>::Handle Knl = E->Force<2>(dEpsilonNLDisps[i]);

    GaussQuadrature Integ_Bulk ( GaussQuadrature::GradGrad );
    Assemble ( Kl + Knl ,E->begin(),E->end(),Integ_Bulk,*pAssembler );
  }
#endif
}

void NonLinearSolver::BuildLinearSystem()
{
  AssembleRHS();
  AssembleLHS();

  printf ( "nDofs=%d\n",pAssembler->sizeOfR() );
  printf ( "-- done assembling!\n" );

//   if ( Data->solvertype == 1 )
    exportKb(_step,_iter);
}



void NonLinearSolver::InitField()
{
  savedGenTermManager.defineMaxSteps(2);
//   savedGenTermManager.defineMaxSteps(Data->User.Integers["nb_step"]);

  for ( unsigned int i = 0; i < EDomains.size(); ++i )
  {
    NLElasticDomain* E = EDomains[i];

    // Initialise displacement
    //   AssembleInitDisp();
    std::shared_ptr<SavedUnknownTerm<genTensor1<double> > > InitD(new SavedUnknownTerm<genTensor1<double> >(FSpaceDisps[i]));
    SaveUnknown(*pAssembler,E->begin(), E->end(),*InitD);
    savedGenTermManager.addSavedGenTerm("Disp",InitD,0,0,i);
    printData(InitD,"Disp",i);

    // Initialise displacement increment
    std::shared_ptr<SavedUnknownTerm<genTensor1<double> > > InitDIncr(new SavedUnknownTerm<genTensor1<double> >(FSpaceDisps[i]));
    *InitDIncr = *InitD;
    genTerm<genTensor0<double>,1>::Handle DIncr = InitDIncr*0.;
    SaveModifUnknown(*DIncr, E->begin(), E->end(), *InitDIncr);
    savedGenTermManager.addSavedGenTerm("DispIncr",InitDIncr,0,0,i);
    printData(InitDIncr,"DispIncr",i);

    // Initialise displacement increment on a step
    std::shared_ptr<SavedUnknownTerm<genTensor1<double> > > InitDIncrS(new SavedUnknownTerm<genTensor1<double> >(FSpaceDisps[i]));
    *InitDIncrS = *InitDIncr;
    savedGenTermManager.addSavedGenTerm("DispIncrStep",InitDIncrS,0,0,i);
    printData(InitDIncrS,"DispIncrStep",i);

    // Initialise gradient
    genTensor2<double> t(0.);
    ConstantField<genTensor2<double> > tZero ( t );
    std::shared_ptr<SavedGenTerm<genTensor2<double>,0> > InitG(new SavedGenTerm<genTensor2<double>,0>());
    GaussQuadrature Integ_Grad ( GaussQuadrature::Grad );
    SavePtGauss(tZero,E->begin(), E->end(),Integ_Grad,*InitG);
    savedGenTermManager.addSavedGenTerm("Grad",InitG,0,0,i);
    printData(InitG,"Grad",i);

    // Initialise strain
    std::shared_ptr<SavedGenTerm<genTensor2<double>,0> > InitStrain(new SavedGenTerm<genTensor2<double>,0>());
    SavePtGauss(tZero,E->begin(), E->end(),Integ_Grad,*InitStrain);
    savedGenTermManager.addSavedGenTerm("Strain",InitStrain,0,0,i);
    printData(InitStrain,"Strain",i);
  }
}

void NonLinearSolver::CopyField()
{
  for ( unsigned int i = 0; i < EDomains.size(); ++i )
  {
    NLElasticDomain* E = EDomains[i];

    // Copy displacement Increment dui
    std::shared_ptr<SavedUnknownTerm<genTensor1<double> > > OldDIncr;
    if(i==0) savedGenTermManager.getSavedGenTerm("DispIncr",OldDIncr,_step-1,0,i);
    else savedGenTermManager.getSavedGenTerm("DispIncr",OldDIncr,_step,1,i);
    savedGenTermManager.addSavedGenTerm("DispIncr",OldDIncr,_step,0,i);

    // Copy displacement increment on a step
    std::shared_ptr<SavedUnknownTerm<genTensor1<double> > > SaveDIncrS(new SavedUnknownTerm<genTensor1<double> >(FSpaceDisps[i]));
    *SaveDIncrS = *OldDIncr;
    genTerm<genTensor0<double>,1>::Handle DIncrS = SaveDIncrS*0.;
    SaveModifUnknown(*DIncrS,E->begin(), E->end(),*SaveDIncrS);
    savedGenTermManager.addSavedGenTerm("DispIncrStep",SaveDIncrS,_step,0,i);

    // Copy displacement : u(t+dt,0) = u(t)
    std::shared_ptr<SavedUnknownTerm<genTensor1<double> > > OldD;
    if(i==0) savedGenTermManager.getSavedGenTerm("Disp",OldD,_step-1,0,i);
    else savedGenTermManager.getSavedGenTerm("Disp",OldD,_step,1,i);
    savedGenTermManager.addSavedGenTerm("Disp",OldD,_step,0,i);

    // Copy gradient : Grad(t+dt,0) = Grad(t)
    std::shared_ptr<SavedGenTerm<genTensor2<double>,0> > OldG;
    if(i==0) savedGenTermManager.getSavedGenTerm("Grad",OldG,_step-1,0,i);
    else savedGenTermManager.getSavedGenTerm("Grad",OldG,_step,1,i);
    savedGenTermManager.addSavedGenTerm("Grad",OldG,_step,0,i);

    // Copy strain : Strain(t+dt,0) = Strain(t)
    std::shared_ptr<SavedGenTerm<genTensor2<double>,0> > OldStrain;
    if(i==0) savedGenTermManager.getSavedGenTerm("Strain",OldStrain,_step-1,0,i);
    else savedGenTermManager.getSavedGenTerm("Strain",OldStrain,_step,1,i);
    savedGenTermManager.addSavedGenTerm("Strain",OldStrain,_step,0,i);

    // Copy stress : Stress(t+dt,0) = Stress(t)
    E->copyHistory();
  }


/*  // Copy displacement Increment dui
  std::shared_ptr<SavedUnknownTerm<genTensor1<double> > > OldDIncr;
  savedGenTermManager.getSavedGenTerm("DispIncr",OldDIncr,_step-1);
  savedGenTermManager.addSavedGenTerm("DispIncr",OldDIncr,_step);


  // Copy displacement increment on a step
  std::shared_ptr<SavedUnknownTerm<genTensor1<double> > > SaveDIncrS(new SavedUnknownTerm<genTensor1<double> >(FSpaceDisp));
  *SaveDIncrS = *OldDIncr;
  for ( unsigned int i = 0; i < EDomains.size(); ++i )
  {
    genTerm<double,1>::Handle DIncrS = (SaveDIncrS)*0.;
    SaveModifUnknown(*DIncrS,EDomains[i]->begin(), EDomains[i]->end(),*SaveDIncrS);
  }
  savedGenTermManager.addSavedGenTerm("DispIncrStep",SaveDIncrS,_step);


  // Copy displacement : u(t+dt,0) = u(t)
  std::shared_ptr<SavedUnknownTerm<genTensor1<double> > > OldD;
  savedGenTermManager.getSavedGenTerm("Disp",OldD,_step-1);
  savedGenTermManager.addSavedGenTerm("Disp",OldD,_step);


  // Copy gradient : Grad(t+dt,0) = Grad(t)
  std::shared_ptr<SavedGenTerm<genTensor2<double>,0> > OldG;
  savedGenTermManager.getSavedGenTerm("Grad",OldG,_step-1);
  savedGenTermManager.addSavedGenTerm("Grad",OldG,_step);


  // Copy stress : Strain(t+dt,0) = Strain(t)
  std::shared_ptr<SavedGenTerm<genTensor2<double>,0> > OldStrain;
  savedGenTermManager.getSavedGenTerm("Strain",OldStrain,_step-1);
  savedGenTermManager.addSavedGenTerm("Strain",OldStrain,_step);


  // Copy stress : Stress(t+dt,0) = Stress(t)
  for ( unsigned int i = 0; i < EDomains.size(); ++i )
  {
    EDomains[i]->copyHistory();
  }*/
}

void NonLinearSolver::SaveField()
{
  for ( unsigned int i = 0; i < EDomains.size(); ++i )
  {
    NLElasticDomain* E = EDomains[i];
    
    // Save strain (nonlinear)
    std::shared_ptr<SavedGenTerm<genTensor2<double>,0> > OldG;
    savedGenTermManager.getSavedGenTerm("Grad",OldG,_step,0,i);
    genTerm<genTensor2<double>,0>::Handle OldG2 = OldG;
    diffTerm<genTensor1<double> ,0>::Handle Field ( new genSField<genTensor1<double> >(pAssembler, FSpaceDisps[i]) );
    genTerm<genTensor2<double>,0>::Handle G = Gradient(Field);
    #ifndef LARGE_STRAIN
    genTerm<genTensor2<double>,0>::Handle StrainIncr = (G+Transpose(G))*0.5;  // small strain
    #else
    genTerm<genTensor2<double>,0>::Handle Bl = ( G+Transpose(G) + Compose<ContrProdOp>(Transpose(OldG2),G) + Compose<ContrProdOp>(Transpose(G),OldG2) )*0.5; // Green-Lagrange // large strain
    genTerm<genTensor2<double>,0>::Handle Bnl = ( Compose<ContrProdOp>(Transpose(G),G) )*0.5; // Green-Lagrange // large strain
    genTerm<genTensor2<double>,0>::Handle StrainIncr = Bl + Bnl; // Green-Lagrange // large strain
    #endif
    std::shared_ptr<SavedGenTerm<genTensor2<double>,0> > OldStrain;
    savedGenTermManager.getSavedGenTerm("Strain",OldStrain,_step,0,i);
    genTerm<genTensor2<double>,0>::Handle NewStrain = OldStrain + StrainIncr;
    std::shared_ptr<SavedGenTerm<genTensor2<double>,0> > SaveStrain(new SavedGenTerm<genTensor2<double>,0>());
    savedGenTermManager.addSavedGenTerm("Strain",SaveStrain,_step,0,i);
    GaussQuadrature Integ_Grad ( GaussQuadrature::Grad );
    SavePtGauss(*NewStrain,E->begin(), E->end(),Integ_Grad,*SaveStrain);
    printData(SaveStrain,"Strain",i);


    // Save stress (nonlinear)
    E->saveHistory(StrainIncr,SaveStrain);


    // Save displacement Increment dui
    std::shared_ptr<SavedUnknownTerm<genTensor1<double> > > SaveDIncr(new SavedUnknownTerm<genTensor1<double> >(FSpaceDisps[i]));
    SaveUnknown(*pAssembler,E->begin(), E->end(),*SaveDIncr);
    savedGenTermManager.addSavedGenTerm("DispIncr",SaveDIncr,_step,0,i);
    printData(SaveDIncr,"DispIncr",i);


    // Save displacement Increment on a step dui
    std::shared_ptr<SavedUnknownTerm<genTensor1<double> > > OldDIncrS;
    savedGenTermManager.getSavedGenTerm("DispIncrStep",OldDIncrS,_step,0,i);
    std::shared_ptr<SavedUnknownTerm<genTensor1<double> > > SaveDIncrS(new SavedUnknownTerm<genTensor1<double> >(FSpaceDisps[i]));
    *SaveDIncrS=*OldDIncrS;
    genTerm<genTensor0<double>,1>::Handle DIncrS = OldDIncrS+SaveDIncr;
    SaveModifUnknown(*DIncrS,E->begin(), E->end(),*SaveDIncrS);
    savedGenTermManager.addSavedGenTerm("DispIncrStep",SaveDIncrS,_step,0,i);
    printData(SaveDIncrS,"DispIncrStep",i);


    // Save displacement ui+1 = ui + dui
    std::shared_ptr<SavedUnknownTerm<genTensor1<double> > > OldD;
    savedGenTermManager.getSavedGenTerm("Disp",OldD,_step,0,i);
    printData(OldD,"OldD",i);

    SavedUnknownAddToSolution(*OldD,*pAssembler);

    std::shared_ptr<SavedUnknownTerm<genTensor1<double> > > SaveD(new SavedUnknownTerm<genTensor1<double> >(FSpaceDisps[i]));
    SaveUnknown(*pAssembler, E->begin(), E->end(), *SaveD);
    savedGenTermManager.addSavedGenTerm("Disp",SaveD,_step,0,i);
    printData(SaveD,"Disp",i);


    // Save gradient Grad_ui+1 (linear)
    diffTerm<genTensor1<double> ,0>::Handle NewField ( new genSField<genTensor1<double> >(pAssembler, FSpaceDisps[i]) ); // u(t+dt,i+1) = u(t+dt,i) + du
    genTerm<genTensor2<double>,0>::Handle NewG = Gradient(NewField);
    std::shared_ptr<SavedGenTerm<genTensor2<double>,0> > SaveG(new SavedGenTerm<genTensor2<double>,0>());
    SavePtGauss(*NewG,E->begin(), E->end(),Integ_Grad,*SaveG);
    savedGenTermManager.addSavedGenTerm("Grad",SaveG,_step,0,i);
    printData(SaveG,"Grad",i);
  }
}



void NonLinearSolver::AssembleInitDisp()
{
  /*linearSystemId<double>* buf= new linearSystemId<double>;
  dofManager<double>* pAssembler2 = new dofManager<double> ( buf );

  for ( unsigned int i = 0; i < Data->BCs.size(); ++i )
  {
    if ((Data->BCs[i]->kind=="Dirichlet")||(Data->BCs[i]->kind==""))
    {
      if (Data->BCs[i]->What=="Displacement")
      {
        genFilterDofComponent filter ( Data->BCs[i]->comp1 );
        genSimpleFunctionConstant<double> Func(0);
        FixNodalDofs ( FSpaceDisp, Data->BCs[i]->begin(), Data->BCs[i]->end(), *pAssembler2, Func, filter );
      }
    }
  }

  // we number the dofs : when a dof is numbered, it cannot be numbered
  // again with another number.
  for ( unsigned int i = 0; i < EDomains.size(); ++i )
  {
    NumberDofs ( FSpaceDisp, EDomains[i]->begin(), EDomains[i]->end(),*pAssembler2 );
  }

  bool noNeumann = true;
  GaussQuadrature Integ_Boundary ( GaussQuadrature::Val );
  for ( unsigned int i = 0; i < Data->BCs.size(); ++i )
  {
    if ((Data->BCs[i]->kind=="Neumann")||(Data->BCs[i]->kind==""))
    {
      if (Data->BCs[i]->What=="Force")
      {
        noNeumann = true;
        genTerm<genTensor1<double> ,0>::Handle Func(new FunctionField<genTensor1<double> >( genSimpleFunctionConstant<genTensor1<double> >(0.) ) );
        genTerm<double,1>::Handle loadterm = Compose<FullContrProdOp>(FSpaceDisp,Func);
        Assemble ( loadterm , Data->BCs[i]->begin(), Data->BCs[i]->end(), Integ_Boundary, *pAssembler2 );
      }
    }
  }
  if (noNeumann)
  {
    genTerm<genTensor1<double> ,0>::Handle Func(new FunctionField<genTensor1<double> >( genSimpleFunctionConstant<genTensor1<double> >(0.) ) );
    genTerm<double,1>::Handle loadterm = Compose<FullContrProdOp>(FSpaceDisp,Func);
    Assemble ( loadterm, EDomains[0]->begin(), EDomains[0]->end(), Integ_Boundary, *pAssembler2 );
  }
    
  pAssembler2->systemSolve();

// -_Initial_Disp_-----------------------------------------------
  std::shared_ptr<SavedUnknownTerm<genTensor1<double> > > saveInitD(new SavedUnknownTerm<genTensor1<double> >(FSpaceDisp));
  for ( unsigned int i = 0; i < EDomains.size(); ++i )
  {
    SaveUnknown(*pAssembler2, EDomains[i]->begin(), EDomains[i]->end(), *saveInitD);
  }
  savedGenTermManager.addSavedGenTerm("Disp",saveInitD);
//   InitD.printfile("data_InitD");

  delete buf;
  delete pAssembler2;*/
}

void NonLinearSolver::AssembleResidual()
{
  /*linearSystemId<double>* buf= new linearSystemId<double>;
  dofManager<double>* pAssembler2 = new dofManager<double> ( buf );

  for ( unsigned int i = 0; i < Data->BCs.size(); ++i )
  {
    if ((Data->BCs[i]->kind=="Dirichlet")||(Data->BCs[i]->kind==""))
    {
      if (Data->BCs[i]->What=="Displacement")
      {
        genFilterDofComponent filter ( Data->BCs[i]->comp1 );
        FixNodalDofs ( FSpaceDisp, Data->BCs[i]->begin(), Data->BCs[i]->end(), *pAssembler2, *(Data->BCs[i]->fscalar), filter );
      }
    }
  }

  // we number the dofs : when a dof is numbered, it cannot be numbered
  // again with another number.
  for ( unsigned int i=0; i<EDomains.size(); ++i )
  {
    NumberDofs ( FSpaceDisp, EDomains[i]->begin(), EDomains[i]->end(),*pAssembler2 );
  }

  genTerm<genTensor2<double>,1>::Handle G = Gradient(FSpaceDisp);
  std::shared_ptr<SavedGenTerm<genTensor2<double>,0> > lastG;
  savedGenTermManager.getSavedGenTerm("Grad",lastG,_step);
  genTerm<genTensor2<double>,0>::Handle lastG2 = lastG;
#ifndef LARGE_STRAIN
  genTerm<genTensor2<double>,1>::Handle Bl = ( G+Transpose(G) )*0.5;
#else
  genTerm<genTensor2<double>,1>::Handle Bl = ( G+Transpose(G) + Compose<ContrProdOp>(Transpose(lastG2),G) + Compose<ContrProdOp>(Transpose(G),lastG2) )*0.5;
#endif
  GaussQuadrature Integ_Grad ( GaussQuadrature::Grad );
  for ( unsigned int i=0; i<EDomains.size(); ++i )
  {
    NLElasticDomain* E = EDomains[i];
//     std::shared_ptr<SavedGenTerm<genTensor2<double>,0> > lastStress;
//     savedGenTermManager.getSavedGenTerm("Stress",lastStress,_step,0,i);
//     genTerm<double,1>::Handle Fint = Compose<FullContrProdOp>(Transpose(Bl), lastStress);
    genTerm<double,1>::Handle Fint = E->Force<1>(Bl);

    Assemble ( Fint ,E->begin(),E->end(),Integ_Grad,*pAssembler2 );
  }

  GaussQuadrature Integ_Boundary ( GaussQuadrature::Val );
  for ( unsigned int i = 0; i < Data->BCs.size(); ++i )
  {
    if ((Data->BCs[i]->kind=="Neumann")||(Data->BCs[i]->kind==""))
    {
      if (Data->BCs[i]->What=="Force")
      {
        genTerm<genTensor1<double> ,0>::Handle Func(new FunctionField<genTensor1<double> >(*(Data->BCs[i]->fvector)));
        genTerm<double,1>::Handle loadterm = Compose<FullContrProdOp>(FSpaceDisp,Func)*-1;
        Assemble ( loadterm , Data->BCs[i]->begin(), Data->BCs[i]->end(), Integ_Boundary, *pAssembler2 );
      }
    }
  }

  pAssembler2->systemSolve();
  
// -_Save_Residual_-----------------------------------------------
  std::shared_ptr<SavedUnknownTerm<genTensor1<double> > > saveResidual(new SavedUnknownTerm<genTensor1<double> >(FSpaceDisp) );
  for ( unsigned int i=0; i<EDomains.size(); ++i )
  {
    SaveUnknown(*pAssembler2,EDomains[i]->begin(), EDomains[i]->end(),*saveResidual);
//      SaveDof(*pAssembler2,EDomains[i]->begin(), EDomains[i]->end(),saveResidual);
  }
  savedGenTermManager.addSavedGenTerm("Residual",saveResidual);
  saveResidual->printfile("data_saveResidual.txt");

  delete buf;
  delete pAssembler2;*/
}

void NonLinearSolver::AssembleEnergy()
{
  /*linearSystemId<double>* buf= new linearSystemId<double>;
  dofManager<double>* pAssembler2 = new dofManager<double> ( buf );

  for ( unsigned int i = 0; i < Data->BCs.size(); ++i )
  {
    if ((Data->BCs[i]->kind=="Dirichlet")||(Data->BCs[i]->kind==""))
    {
      if (Data->BCs[i]->What=="Displacement")
      {
        genFilterDofComponent filter ( Data->BCs[i]->comp1 );
        FixNodalDofs ( FSpaceDisp, Data->BCs[i]->begin(), Data->BCs[i]->end(), *pAssembler2, *(Data->BCs[i]->fscalar), filter );
      }
    }
  }

  // we number the dofs : when a dof is numbered, it cannot be numbered
  // again with another number.
  for ( unsigned int i=0; i<EDomains.size(); ++i )
  {
    NumberDofs ( FSpaceDisp, EDomains[i]->begin(), EDomains[i]->end(),*pAssembler2 );
  }

  genTerm<genTensor2<double>,1>::Handle G = Gradient(FSpaceDisp);
  std::shared_ptr<SavedGenTerm<genTensor2<double>,0> > lastG;
  savedGenTermManager.getSavedGenTerm("Grad",lastG,_step);
  genTerm<genTensor2<double>,0>::Handle lastG2 = lastG;
#ifndef LARGE_STRAIN
  genTerm<genTensor2<double>,1>::Handle Bl = ( G+Transpose(G) )*0.5;
#else
  genTerm<genTensor2<double>,1>::Handle Bl = ( G+Transpose(G) + Compose<ContrProdOp>(Transpose(lastG2),G) + Compose<ContrProdOp>(Transpose(G),lastG2) )*0.5;
#endif
  GaussQuadrature Integ_Grad ( GaussQuadrature::Grad );
  for ( unsigned int i=0; i<EDomains.size(); ++i )
  {
    NLElasticDomain* E = EDomains[i];
    
//     std::shared_ptr<SavedGenTerm<genTensor2<double>,0> > lastStress;
//     savedGenTermManager.getSavedGenTerm("Stress",lastStress,_step,0,i);
//     genTerm<double,1>::Handle Fint = Compose<FullContrProdOp>(Transpose(Bl), lastStress);
    genTerm<double,1>::Handle Fint = E->Force<1>(Bl);
    
    Assemble ( Fint ,EDomains[i]->begin(),EDomains[i]->end(),Integ_Grad,*pAssembler2 );
  }

  GaussQuadrature Integ_Boundary ( GaussQuadrature::Val );
  for ( unsigned int i = 0; i < Data->BCs.size(); ++i )
  {
    if ((Data->BCs[i]->kind=="Neumann")||(Data->BCs[i]->kind==""))
    {
      if (Data->BCs[i]->What=="Force")
      {
        genTerm<genTensor1<double> ,0>::Handle Func(new FunctionField<genTensor1<double> >(*(Data->BCs[i]->fvector)));
        genTerm<double,1>::Handle loadterm = Compose<FullContrProdOp>(FSpaceDisp,Func)*-1;
        Assemble ( loadterm , Data->BCs[i]->begin(), Data->BCs[i]->end(), Integ_Boundary, *pAssembler2 );
      }
    }
  }
  
  pAssembler2->systemSolve();
  
// -_Save_Residual_----------------------------------------------- WARNING Residual is a SavedUnknownTerm<genTensor1<double> >
  std::shared_ptr<SavedUnknownTerm<genTensor1<double> > > Residual(new SavedUnknownTerm<genTensor1<double> >(FSpaceDisp));
  for ( unsigned int i=0; i<EDomains.size(); ++i )
  {
    SaveUnknown(*pAssembler2,EDomains[i]->begin(), EDomains[i]->end(),*Residual);
  }
  genTerm<double,1>::Handle Residual2 = Residual;

// -_Load_DispIncrement_-----------------------------------------------
  std::shared_ptr<SavedUnknownTerm<genTensor1<double> > > DispIncr;
  savedGenTermManager.getSavedGenTerm("DispIncr",DispIncr,_step);
  genTerm<double,1>::Handle DispIncr2 = DispIncr;

// -_Save_Energy_-----------------------------------------------
  std::shared_ptr<SavedUnknownTerm<genTensor1<double> > > saveEnergy(new SavedUnknownTerm<genTensor1<double> >(FSpaceDisp));
  *saveEnergy=*Residual;
  for ( unsigned int i=0; i<EDomains.size(); ++i )
  {
    genTerm<double,1>::Handle Energy = Apply<FullContrProdOp>(Transpose(DispIncr2),Residual2);
    SaveModifUnknown(*Energy,EDomains[i]->begin(), EDomains[i]->end(),*saveEnergy);
  }
  savedGenTermManager.addSavedGenTerm("Energy",saveEnergy);
  saveEnergy->printfile("data_Energy.txt");

  delete buf;
  delete pAssembler2;*/
}




void NonLinearSolver::InitCriteria()
{
  std::shared_ptr<double> normDisp(new double(0.));
  savedGenTermManager.addSavedGenTerm("NormDisp",normDisp,_step);
  bool containDirichlet = false;
  for (int i=0; i<Data->BCs.size(); ++i)
  {
    if (!containDirichlet)
    {
      if ((Data->BCs[i]->kind=="Dirichlet")||(Data->BCs[i]->kind==""))
      {
        if (Data->BCs[i]->What=="Displacement")
        {
          NormDispIncr(*normDisp);
          containDirichlet = true;
        }
      }
    }
  }
  
  std::shared_ptr<double> normForce(new double(0.));
  savedGenTermManager.addSavedGenTerm("NormFext",normForce,_step);
  bool containNeumann = false;
  for (int i=0; i<Data->BCs.size(); ++i)
  {
    if (!containNeumann)
    {
      if ((Data->BCs[i]->kind=="Neumann")||(Data->BCs[i]->kind==""))
      {
        if (Data->BCs[i]->What=="Force")
        {
          NormResidualForce(*normForce);
          containNeumann = true;
        }
      }
    }
  }

  if (*normForce==0) *normForce = (*normDisp)*Data->User.Scalars["ScaleElastic"];
}

void NonLinearSolver::MaxCriterion(const genSField<genTensor1<double> > &SField, double &retnorm)
{
  double max=0;
  for ( unsigned int i=0; i<EDomains.size(); ++i )
  {
    genGroupOfElements::elementContainer::const_iterator it;
    for ( it = EDomains[i]->begin(); it != EDomains[i]->end(); ++it )
    {
      MElement* e=*it;
      MElement* ep=e->getParent(); // cut
      if ( !ep )
      {
        for ( int j = 0; j < e->getNumVertices(); ++j )
        {
          std::vector<genScalar<genTensor1<double> > > val(1);
          SPoint3 p ( e->getVertex ( j )->x(),e->getVertex ( j )->y(),e->getVertex ( j )->z() );
          double pnt[3];
          pnt[0]=p.x();
          pnt[1]=p.y();
          pnt[2]=p.z();
          double uvw[3];
          e->xyz2uvw ( pnt,uvw );
          IntPt pt;
          pt.pt[0]=uvw[0];
          pt.pt[1]=uvw[1];
          pt.pt[2]=uvw[2];
          pt.weight=1.0;
          SField.get(e,1,&pt,val);
          retnorm = norm( val[0]() );
          if (retnorm > max) max = retnorm;
        }
      }
      else
      {
        // take all the parts
        for ( int i = 0; i < e->getNumChildren(); i ++ )
        {
          MElement* ec;
          ec = e->getChild ( i );  // get part
          std::vector<MVertex*> vertices;
          vertices.clear();
          for ( int j = 0; j < ec->getNumVertices(); ++j )
          {
            std::vector<genScalar<genTensor1<double> > > val(1);
            SPoint3 p ( ec->getVertex ( j )->x() , ec->getVertex ( j )->y(), ec->getVertex ( j )->z() );
            double pnt[3];
            pnt[0]=p.x();
            pnt[1]=p.y();
            pnt[2]=p.z();
            double uvw[3];
            // the parent owned the field
            ep->xyz2uvw ( pnt,uvw );
            std::vector<double> vec ( 3 );
            IntPt pt;
            pt.pt[0]=uvw[0];
            pt.pt[1]=uvw[1];
            pt.pt[2]=uvw[2];
            pt.weight=1.0;
            SField.get(e,1,&pt,val);
            retnorm = norm( val[0]() );
            if (retnorm > max) max = retnorm;
          }
        }
      }
    }
  }
  retnorm = max;
}

bool NonLinearSolver::ValidateCriterion(const genSField<genTensor1<double> > &SField, genSupport &Domain, const double &criterion, double &retnorm)
{
  double max=0;
  for ( genGroupOfElements::elementContainer::const_iterator it = Domain.begin(); it != Domain.end(); ++it )
  {
    MElement* e=*it;
    MElement* ep=e->getParent(); // cut
    if ( !ep )
    {
      for ( int j = 0; j < e->getNumVertices(); ++j )
      {
        std::vector<genScalar<genTensor1<double> > > val(1);
        SPoint3 p ( e->getVertex ( j )->x(),e->getVertex ( j )->y(),e->getVertex ( j )->z() );
        double pnt[3];
        pnt[0]=p.x();
        pnt[1]=p.y();
        pnt[2]=p.z();
        double uvw[3];
        e->xyz2uvw ( pnt,uvw );
        IntPt pt;
        pt.pt[0]=uvw[0];
        pt.pt[1]=uvw[1];
        pt.pt[2]=uvw[2];
        pt.weight=1.0;
        SField.get(e,1,&pt,val);
        retnorm = norm( val[0]() );
        if (retnorm > criterion ) return false;
        if (retnorm > max) max = retnorm;
      }
    }
    else
    {
      // take all the parts
      for ( int i = 0; i < e->getNumChildren(); i ++ )
      {
        MElement* ec;
        ec = e->getChild ( i );  // get part
        std::vector<MVertex*> vertices;
        vertices.clear();
        for ( int j = 0; j < ec->getNumVertices(); ++j )
        {
          std::vector<genScalar<genTensor1<double> > > val(1);
          SPoint3 p ( ec->getVertex ( j )->x() , ec->getVertex ( j )->y(), ec->getVertex ( j )->z() );
          double pnt[3];
          pnt[0]=p.x();
          pnt[1]=p.y();
          pnt[2]=p.z();
          double uvw[3];
          // the parent owned the field
          ep->xyz2uvw ( pnt,uvw );
          std::vector<double> vec ( 3 );
          IntPt pt;
          pt.pt[0]=uvw[0];
          pt.pt[1]=uvw[1];
          pt.pt[2]=uvw[2];
          pt.weight=1.0;
          SField.get(e,1,&pt,val);
          retnorm = norm( val[0]() );
          if (retnorm > criterion) return false;
          if (retnorm > max) max = retnorm;
        }
      }
    }
  }
  retnorm = max;
  return true;
}

bool NonLinearSolver::ValidateCriterion(const dofManager<double>* assembler, const double &criterion, double &retnorm)
{
  double max=0;
  double val;
  std::string sysname = "A";
//   std::cout<<"sizeOfR : "<<pAssembler->sizeOfR()<<std::endl;
  for ( unsigned int i = 0; i<pAssembler->sizeOfR(); ++i )
  {
    pAssembler->getLinearSystem(sysname)->getFromSolution(i, val);
//     std::cout<<" x("<<i<<") = "<<val<<std::endl;
    retnorm = fabs(val);
    if (retnorm > max) max = retnorm;
  }
  retnorm = max;
  if (retnorm > criterion ) return false;
  else return true;
}

// Relative Norm Displacement Increment Test
void NonLinearSolver::NormDispIncr(double &normDisp)
{
  linearSystemId<double>* buf= new linearSystemId<double>;
  dofManager<double>* pAssembler2 = new dofManager<double> ( buf );

  for ( unsigned int i=0; i<EDomains.size(); ++i )
  {
    for ( unsigned int j = 0; j < Data->BCs.size(); ++j )
    {
      if (CombineBCatDomain[j][i]->size()!=0)
      if ((Data->BCs[j]->kind=="Dirichlet")||(Data->BCs[j]->kind==""))
      {
        if (Data->BCs[j]->What=="Displacement")
        {
          genGroupOfElements* EF = CombineBCatDomain[j][i];
          genFilterDofComponent filterDof ( Data->BCs[j]->comp1 );
          FixNodalDofs ( FSpaceDisps[i], EF->begin(), EF->end(), *pAssembler2, *(Data->BCs[j]->fscalar), filterDof );
        }
      }
    }
  }
  
  for ( unsigned int i=0; i<EDomains.size(); ++i )
  {
    NumberDofs ( FSpaceDisps[i], EDomains[i]->begin(), EDomains[i]->end(),*pAssembler2 );
  }

  for ( unsigned int i=0; i<EDomains.size(); ++i )
  {
//     std::shared_ptr<SavedUnknownTerm<genTensor1<double> > > lastDIncr;
//     savedGenTermManager.getSavedGenTerm("DispIncr",lastDIncr,_step,0,i);

    std::shared_ptr<SavedUnknownTerm<genTensor1<double> > > lastDIncrS;
    savedGenTermManager.getSavedGenTerm("DispIncrStep",lastDIncrS,_step,0,i);
    
    std::string linSystName("A");
    linearSystem<double>* linearSyst = pAssembler2->getLinearSystem(linSystName);
//   SavedUnknownAddToRightHandSide(*lastDIncr,*pAssembler2);
    SavedUnknownAddToRightHandSide(*lastDIncrS,*pAssembler2);
  }

  pAssembler2->systemSolve();

// -_Critere_DispIncr_-----------------------------------------------
  double max_normDisp = 0;
  for ( unsigned int i=0; i<EDomains.size(); ++i )
  {
    genSField<genTensor1<double> > Disp(pAssembler2,FSpaceDisps[i]);
    MaxCriterion(Disp, normDisp);
    if(normDisp>max_normDisp) max_normDisp = normDisp;
  }
  normDisp = max_normDisp;

  delete buf;
  delete pAssembler2;

  /*linearSystemId<double>* buf= new linearSystemId<double>;
  dofManager<double>* pAssembler2 = new dofManager<double> ( buf );

  for ( unsigned int i = 0; i < Data->BCs.size(); ++i )
  {
    if ((Data->BCs[i]->kind=="Dirichlet")||(Data->BCs[i]->kind==""))
    {
      if (Data->BCs[i]->What=="Displacement")
      {
        genFilterDofComponent filter ( Data->BCs[i]->comp1 );
        FixNodalDofs ( FSpaceDisp, Data->BCs[i]->begin(), Data->BCs[i]->end(), *pAssembler2, *(Data->BCs[i]->fscalar), filter );
      }
    }
  }

  // we number the dofs : when a dof is numbered, it cannot be numbered
  // again with another number.
  for ( unsigned int i=0; i<EDomains.size(); ++i )
  {
    NumberDofs ( FSpaceDisp, EDomains[i]->begin(), EDomains[i]->end(),*pAssembler2 );
  }

//   std::shared_ptr<SavedUnknownTerm<genTensor1<double> > > lastDIncr;
//   savedGenTermManager.getSavedGenTerm("DispIncr",lastDIncr,_step);

  std::shared_ptr<SavedUnknownTerm<genTensor1<double> > > lastDIncrS;
  savedGenTermManager.getSavedGenTerm("DispIncrStep",lastDIncrS,_step);

  std::string linSystName("A");
  linearSystem<double>* linearSyst = pAssembler2->getLinearSystem(linSystName);
//   SavedUnknownAddToRightHandSide(*lastDIncr,*pAssembler2);
  SavedUnknownAddToRightHandSide(*lastDIncrS,*pAssembler2);
  
  pAssembler2->systemSolve();
  
// -_Critere_DispIncr_-----------------------------------------------
  genSField<genTensor1<double> > Disp(pAssembler2,FSpaceDisp);
  MaxCriterion(Disp, normDisp);

  delete buf;
  delete pAssembler2;*/
}

bool NonLinearSolver::DispIncrCriterion(const double &normDisp, double &normDispIncr)
{
  linearSystemId<double>* buf= new linearSystemId<double>;
  dofManager<double>* pAssembler2 = new dofManager<double> ( buf );

  for ( unsigned int i = 0; i < EDomains.size(); ++i )
  {
    for ( unsigned int j = 0; j < Data->BCs.size(); ++j )
    {
      if (CombineBCatDomain[j][i]->size()!=0)
      if ((Data->BCs[j]->kind=="Dirichlet")||(Data->BCs[j]->kind==""))
      {
        if (Data->BCs[j]->What=="Displacement")
        {
          genGroupOfElements* EF = CombineBCatDomain[j][i];
          genSimpleFunctionConstant<genTensor0<double> > fzero (0.);
          genFilterDofComponent filterDof ( Data->BCs[j]->comp1 );
          FixNodalDofs ( FSpaceDisps[i], EF->begin(), EF->end(), *pAssembler2, fzero, filterDof );
        }
      }
    }
  }

  for ( unsigned int i = 0; i < EDomains.size(); ++i )
  {
    NumberDofs ( FSpaceDisps[i], EDomains[i]->begin(), EDomains[i]->end(),*pAssembler2);
  }

  for ( unsigned int i = 0; i < EDomains.size(); ++i )
  {
    std::shared_ptr<SavedUnknownTerm<genTensor1<double> > > lastDIncr;
    savedGenTermManager.getSavedGenTerm("DispIncr",lastDIncr,_step,0,i);

    std::string linSystName("A");
    linearSystem<double>* linearSyst = pAssembler2->getLinearSystem(linSystName);
    SavedUnknownAddToRightHandSide(*lastDIncr,*pAssembler2);
  }

  pAssembler2->systemSolve();

// -_Critere_DispIncr_-----------------------------------------------
  double criterion = Data->User.Scalars["tolerance"]*normDisp;
  bool convergeance = true;
  double max_normDispIncr = 0;
  for ( unsigned int i=0; i<EDomains.size(); ++i )
  {
    genSField<genTensor1<double> > DispIncr2(pAssembler2,FSpaceDisps[i]);
    bool conv = ValidateCriterion(DispIncr2, *EDomains[i], criterion, normDispIncr);
//  bool convergeance = ValidateCriterion(pAssembler2, criterion, normDispIncr);

    if(normDispIncr>max_normDispIncr) max_normDispIncr = normDispIncr;
    if(!conv) convergeance = conv;
  }
  normDispIncr = max_normDispIncr;

//  delete pAssembler2;
//  delete buf;

  return convergeance;


  /*linearSystemId<double>* buf= new linearSystemId<double>;
  dofManager<double>* pAssembler2 = new dofManager<double> ( buf );

  for ( unsigned int i = 0; i < Data->BCs.size(); ++i )
  {
    if ((Data->BCs[i]->kind=="Dirichlet")||(Data->BCs[i]->kind==""))
    {
      if (Data->BCs[i]->What=="Displacement")
      {
        genFilterDofComponent filter ( Data->BCs[i]->comp1 );
        genSimpleFunctionConstant<double> fzero (0.);
        FixNodalDofs ( FSpaceDisp, Data->BCs[i]->begin(), Data->BCs[i]->end(), *pAssembler2, fzero, filter );
      }
    }
  }

  // we number the dofs : when a dof is numbered, it cannot be numbered
  // again with another number.
  for ( unsigned int i = 0; i < EDomains.size(); ++i )
  {
    NumberDofs ( FSpaceDisp, EDomains[i]->begin(), EDomains[i]->end(),*pAssembler2 );
  }

  std::shared_ptr<SavedUnknownTerm<genTensor1<double> > > lastDIncr;
  savedGenTermManager.getSavedGenTerm("DispIncr",lastDIncr,_step);

  std::string linSystName("A");
  linearSystem<double>* linearSyst = pAssembler2->getLinearSystem(linSystName);
  SavedUnknownAddToRightHandSide(*lastDIncr,*pAssembler2);

  pAssembler2->systemSolve();

// -_Critere_DispIncr_-----------------------------------------------
  double criterion = Data->User.Scalars["tolerance"]*normDisp;

  genSField<genTensor1<double> > DispIncr2(pAssembler2,FSpaceDisp);
  bool convergeance = ValidateCriterion(DispIncr2, criterion, normDispIncr);
//  bool convergeance = ValidateCriterion(pAssembler2, criterion, normDispIncr);

  delete buf;
  delete pAssembler2;

  return convergeance;*/
}

// Relative Norm Unbalance Test
void NonLinearSolver::NormResidualForce(double &normForce)
{
  linearSystemId<double>* buf= new linearSystemId<double>;
  dofManager<double>* pAssembler2 = new dofManager<double> ( buf );

  for ( unsigned int i = 0; i < EDomains.size(); ++i )
  {
    NLElasticDomain* E = EDomains[i];
    for ( unsigned int j = 0; j < Data->BCs.size(); ++j )
    {
      if (CombineBCatDomain[j][i]->size()!=0)
      if ((Data->BCs[j]->kind=="Dirichlet")||(Data->BCs[j]->kind==""))
      {
        if (Data->BCs[j]->What=="Displacement")
        {
          genGroupOfElements* EF = CombineBCatDomain[j][i];
          genFilterDofComponent filterDof ( Data->BCs[j]->comp1 );
          FixNodalDofs ( FSpaceDisps[i], EF->begin(), EF->end(), *pAssembler2, *(Data->BCs[j]->fscalar), filterDof );
        }
      }
    }
  }

  for ( unsigned int i = 0; i < EDomains.size(); ++i )
  {
    NumberDofs ( FSpaceDisps[i], EDomains[i]->begin(), EDomains[i]->end(),*pAssembler2);
  }

  for ( unsigned int i = 0; i < EDomains.size(); ++i )
  {
    NLElasticDomain* E = EDomains[i];

    genTerm<genTensor2<double>,1>::Handle G = Gradient(FSpaceDisps[i]);
    #ifndef LARGE_STRAIN
    genTerm<genTensor2<double>,1>::Handle Bl = ( G+Transpose(G) )*0.5;
    #else
    std::shared_ptr<SavedGenTerm<genTensor2<double>,0> > lastG;
    savedGenTermManager.getSavedGenTerm("Grad",lastG,_step,0,i);
    genTerm<genTensor2<double>,0>::Handle lastG2 = lastG;
    genTerm<genTensor2<double>,1>::Handle Bl = ( G+Transpose(G) + Compose<ContrProdOp>(Transpose(lastG2),G) + Compose<ContrProdOp>(Transpose(G),lastG2) )*0.5;
    #endif

    GaussQuadrature Integ_Bulk ( GaussQuadrature::GradGrad );
//     std::shared_ptr<SavedGenTerm<genTensor2<double>,0> > lastStress;
//     savedGenTermManager.getSavedGenTerm("Stress",lastStress,_step,0,i);
//     genTerm<double,1>::Handle Fint = Compose<FullContrProdOp>(Transpose(Bl),lastStress);
     genTerm<genTensor0<double>,1>::Handle Fint = E->Force<1>(Bl);
    Assemble ( Fint ,E->begin(),E->end(),Integ_Bulk,*pAssembler2 );

    GaussQuadrature Integ_Boundary ( GaussQuadrature::Val );
    for ( unsigned int j = 0; j < Data->BCs.size(); ++j )
    {
      if (CombineBCatDomain[j][i]->size()!=0)
      if ((Data->BCs[j]->kind=="Neumann")||(Data->BCs[j]->kind==""))
      {
        if (Data->BCs[j]->What=="Force")
        {
          genTerm<genTensor1<double> ,0>::Handle Func(new FunctionField<genTensor1<double> >(*(Data->BCs[j]->fvector)));
          genTerm<genTensor0<double>,1>::Handle loadterm = Compose<FullContrProdOp>(FSpaceDisps[i],Func)*-1.0;
          genGroupOfElements* EF = CombineBCatDomain[j][i];
          Assemble ( loadterm , EF->begin(), EF->end(), Integ_Boundary, *pAssembler2 );
        }
      }
    }
  }
  
  pAssembler2->systemSolve();

// -_Critere_Residual_-----------------------------------------------
  double max_normForce = 0;
  for ( unsigned int i=0; i<EDomains.size(); ++i )
  {
    genSField<genTensor1<double> > Fext(pAssembler2,FSpaceDisps[i]);
    MaxCriterion(Fext, normForce);
    if(normForce>max_normForce) max_normForce = normForce;
  }
  normForce = max_normForce;

  delete buf;
  delete pAssembler2;

  /*linearSystemId<double>* buf= new linearSystemId<double>;
  dofManager<double>* pAssembler2 = new dofManager<double> ( buf );

  for ( unsigned int i = 0; i < Data->BCs.size(); ++i )
  {
    if ((Data->BCs[i]->kind=="Dirichlet")||(Data->BCs[i]->kind==""))
    {
      if (Data->BCs[i]->What=="Displacement")
      {
        genFilterDofComponent filter ( Data->BCs[i]->comp1 );
        FixNodalDofs ( FSpaceDisp, Data->BCs[i]->begin(), Data->BCs[i]->end(), *pAssembler2, *(Data->BCs[i]->fscalar), filter );
      }
    }
  }
  
  for ( unsigned int i = 0; i < EDomains.size(); ++i )
  {
    NumberDofs ( FSpaceDisp, EDomains[i]->begin(), EDomains[i]->end(),*pAssembler2 );
  }

  std::shared_ptr<SavedGenTerm<genTensor2<double>,0> > lastG;
  savedGenTermManager.getSavedGenTerm("Grad",lastG,_step);
  genTerm<genTensor2<double>,0>::Handle lastG2 = lastG;
  genTerm<genTensor2<double>,1>::Handle G = Gradient(FSpaceDisp);
#ifndef LARGE_STRAIN
  genTerm<genTensor2<double>,1>::Handle Bl = ( G+Transpose(G) )*0.5;
#else
  genTerm<genTensor2<double>,1>::Handle Bl = ( G+Transpose(G) + Compose<ContrProdOp>(Transpose(lastG2),G) + Compose<ContrProdOp>(Transpose(G),lastG2) )*0.5;
#endif
  GaussQuadrature Integ_Bulk ( GaussQuadrature::GradGrad );
  for ( unsigned int i=0; i<EDomains.size(); ++i )
  {
    NLElasticDomain* E = EDomains[i];

//     std::shared_ptr<SavedGenTerm<genTensor2<double>,0> > lastStress;
//     savedGenTermManager.getSavedGenTerm("Stress",lastStress,_step,0,i);
//     genTerm<double,1>::Handle Fint = Compose<FullContrProdOp>(Transpose(Bl),lastStress);
    genTerm<double,1>::Handle Fint = E->Force<1>(Bl);

    Assemble ( Fint ,E->begin(),E->end(),Integ_Bulk,*pAssembler2 );
  }

  GaussQuadrature Integ_Boundary ( GaussQuadrature::Val );
  for ( unsigned int i = 0; i < Data->BCs.size(); ++i )
  {
    if ((Data->BCs[i]->kind=="Neumann")||(Data->BCs[i]->kind==""))
    {
      if (Data->BCs[i]->What=="Force")
      {
        genTerm<genTensor1<double> ,0>::Handle Func(new FunctionField<genTensor1<double> >(*(Data->BCs[i]->fvector)));
        genTerm<double,1>::Handle loadterm = Compose<FullContrProdOp>(FSpaceDisp,Func)*-1;
        Assemble ( loadterm , Data->BCs[i]->begin(), Data->BCs[i]->end(), Integ_Boundary, *pAssembler2 );
      }
    }
  }

  pAssembler2->systemSolve();
  
// -_Critere_Residual_-----------------------------------------------
  genSField<genTensor1<double> > Fext(pAssembler2,FSpaceDisp);
  MaxCriterion(Fext, normForce);

  delete buf;
  delete pAssembler2;*/
}

bool NonLinearSolver::ResidualForceCriterion(const double &normFext, double &normResidual)
{
  linearSystemId<double>* buf= new linearSystemId<double>;
  dofManager<double>* pAssembler2 = new dofManager<double> ( buf );

  for ( unsigned int i=0; i<EDomains.size(); ++i )
  {
    NLElasticDomain* E = EDomains[i];

    for ( unsigned int j = 0; j < Data->BCs.size(); ++j )
    {
      if (CombineBCatDomain[j][i]->size()!=0)
      if ((Data->BCs[j]->kind=="Dirichlet")||(Data->BCs[j]->kind==""))
      {
        if (Data->BCs[j]->What=="Displacement")
        {
          genGroupOfElements* EF = CombineBCatDomain[j][i];
          genFilterDofComponent filterDof ( Data->BCs[j]->comp1 );
          FixNodalDofs ( FSpaceDisps[i], EF->begin(), EF->end(), *pAssembler2, *(Data->BCs[j]->fscalar), filterDof );
        }
      }
    }
  }

  for ( unsigned int i=0; i<EDomains.size(); ++i )
  {
    NumberDofs ( FSpaceDisps[i], EDomains[i]->begin(), EDomains[i]->end(),*pAssembler2 );
  }

  for ( unsigned int i=0; i<EDomains.size(); ++i )
  {
    NLElasticDomain* E = EDomains[i];

    std::shared_ptr<SavedGenTerm<genTensor2<double>,0> > lastG;
    savedGenTermManager.getSavedGenTerm("Grad",lastG,_step,0,i);
    genTerm<genTensor2<double>,0>::Handle lastG2 = lastG;
    genTerm<genTensor2<double>,1>::Handle G = Gradient(FSpaceDisps[i]);
  #ifndef LARGE_STRAIN
    genTerm<genTensor2<double>,1>::Handle Bl = ( G+Transpose(G) )*0.5;
  #else
    genTerm<genTensor2<double>,1>::Handle Bl = ( G+Transpose(G) + Compose<ContrProdOp>(Transpose(lastG2),G) + Compose<ContrProdOp>(Transpose(G),lastG2) )*0.5;
  #endif

    GaussQuadrature Integ_Bulk ( GaussQuadrature::GradGrad );
//     std::shared_ptr<SavedGenTerm<genTensor2<double>,0> > lastStress;
//     savedGenTermManager.getSavedGenTerm("Stress",lastStress,_step,0,i);
//     genTerm<double,1>::Handle Fint = Compose<FullContrProdOp>(Transpose(Bl),lastStress);
    genTerm<genTensor0<double>,1>::Handle Fint = E->Force<1>(Bl);
    Assemble ( Fint ,E->begin(),E->end(),Integ_Bulk,*pAssembler2 );

    GaussQuadrature Integ_Boundary ( GaussQuadrature::Val );
    for ( unsigned int j = 0; j < Data->BCs.size(); ++j )
    {
      if (CombineBCatDomain[j][i]->size()!=0)
      if ((Data->BCs[j]->kind=="Neumann")||(Data->BCs[j]->kind==""))
      {
        if (Data->BCs[j]->What=="Force")
        {
          genTerm<genTensor1<double> ,0>::Handle Func(new FunctionField<genTensor1<double> >(*(Data->BCs[j]->fvector)));
          genTerm<genTensor0<double>,1>::Handle loadterm = Compose<FullContrProdOp>(FSpaceDisps[i],Func)*-1;
          genGroupOfElements* EF = CombineBCatDomain[j][i];
          Assemble ( loadterm , EF->begin(), EF->end(), Integ_Boundary, *pAssembler2 );
        }
      }
    }
  }
  
  pAssembler2->systemSolve();

// -_Critere_Residual_-----------------------------------------------
  double criterion = Data->User.Scalars["tolerance"]*normFext;
  bool convergeance = true;
  double max_normResidual = 0;
  for ( unsigned int i=0; i<EDomains.size(); ++i )
  {
    genSField<genTensor1<double> > Residual(pAssembler2,FSpaceDisps[i]);
    bool conv = ValidateCriterion(Residual, *EDomains[i], criterion, normResidual);
//  bool convergeance = ValidateCriterion(pAssembler2, criterion, normResidual);

    if(normResidual>max_normResidual) max_normResidual = normResidual;
    if(!conv) convergeance = conv;
  }
  normResidual = max_normResidual;

//   delete pAssembler2;
//   delete buf;

  return convergeance;

  /*  linearSystemId<double>* buf= new linearSystemId<double>;
  dofManager<double>* pAssembler2 = new dofManager<double> ( buf );

  for ( unsigned int i = 0; i < Data->BCs.size(); ++i )
  {
    if ((Data->BCs[i]->kind=="Dirichlet")||(Data->BCs[i]->kind==""))
    {
      if (Data->BCs[i]->What=="Displacement")
      {
        genFilterDofComponent filter ( Data->BCs[i]->comp1 );
        FixNodalDofs ( FSpaceDisp, Data->BCs[i]->begin(), Data->BCs[i]->end(), *pAssembler2, *(Data->BCs[i]->fscalar), filter );
      }
    }
  }

  // we number the dofs : when a dof is numbered, it cannot be numbered
  // again with another number.
  for ( unsigned int i=0; i<EDomains.size(); ++i )
  {
    NumberDofs ( FSpaceDisp, EDomains[i]->begin(), EDomains[i]->end(),*pAssembler2 );
  }

  std::shared_ptr<SavedGenTerm<genTensor2<double>,0> > lastG;
  savedGenTermManager.getSavedGenTerm("Grad",lastG,_step);
  genTerm<genTensor2<double>,0>::Handle lastG2 = lastG;
  genTerm<genTensor2<double>,1>::Handle G = Gradient(FSpaceDisp);
#ifndef LARGE_STRAIN
  genTerm<genTensor2<double>,1>::Handle Bl = ( G+Transpose(G) )*0.5;
#else
  genTerm<genTensor2<double>,1>::Handle Bl = ( G+Transpose(G) + Compose<ContrProdOp>(Transpose(lastG2),G) + Compose<ContrProdOp>(Transpose(G),lastG2) )*0.5;
#endif
  GaussQuadrature Integ_Bulk ( GaussQuadrature::GradGrad );
  for ( unsigned int i=0; i<EDomains.size(); ++i )
  {
    NLElasticDomain* E = EDomains[i];

//     std::shared_ptr<SavedGenTerm<genTensor2<double>,0> > lastStress;
//     savedGenTermManager.getSavedGenTerm("Stress",lastStress,_step,0,i);
//     genTerm<double,1>::Handle Fint = Compose<FullContrProdOp>(Transpose(Bl),lastStress);
    genTerm<double,1>::Handle Fint = E->Force<1>(Bl);

    Assemble ( Fint ,E->begin(),E->end(),Integ_Bulk,*pAssembler2 );
  }

  GaussQuadrature Integ_Boundary ( GaussQuadrature::Val );
  for ( unsigned int i = 0; i < Data->BCs.size(); ++i )
  {
    if ((Data->BCs[i]->kind=="Neumann")||(Data->BCs[i]->kind==""))
    {
      if (Data->BCs[i]->What=="Force")
      {
        genTerm<genTensor1<double> ,0>::Handle Func(new FunctionField<genTensor1<double> >(*(Data->BCs[i]->fvector)));
        genTerm<double,1>::Handle loadterm = Compose<FullContrProdOp>(FSpaceDisp,Func)*-1;
        Assemble ( loadterm , Data->BCs[i]->begin(), Data->BCs[i]->end(), Integ_Boundary, *pAssembler2 );
      }
    }
  }
  
  pAssembler2->systemSolve();

// -_Critere_Residual_-----------------------------------------------
  double criterion = Data->User.Scalars["tolerance"]*normFext;
  genSField<genTensor1<double> > Residual(pAssembler2,FSpaceDisp);
  bool convergeance = ValidateCriterion(Residual, criterion, normResidual);
//  bool convergeance = ValidateCriterion(pAssembler2, criterion, normResidual);

  delete buf;
  delete pAssembler2;

  return convergeance;*/
}

bool NonLinearSolver::StoppingCriteria()
{
  bool convergeance = true;

  std::shared_ptr<double> normForce;
  savedGenTermManager.getSavedGenTerm("NormFext",normForce,_step);
  if (*normForce!=0)
  {
    double normResidualForce;
    convergeance = ResidualForceCriterion(*normForce, normResidualForce);
    printf("--normForceTolerance %12.5E \n",(*normForce)*Data->User.Scalars["tolerance"]);
    printf("--ResidualForce %12.5E \n",normResidualForce);
    if (!convergeance) return convergeance;
  }

//   std::shared_ptr<double> normDisp;
//   savedGenTermManager.getSavedGenTerm("NormDisp",normDisp,_step);
//   if (*normDisp!=0)
//   {
//     double normDispIncr;
//     convergeance = DispIncrCriterion(*normDisp, normDispIncr);
//     printf("--normDispTolerance %12.5E \n",(*normDisp)*Data->User.Scalars["tolerance"]);
//     printf("--DisplIncrement %12.5E \n",normDispIncr);
//     if (!convergeance) return convergeance;
//   }

/*
  double* normEnergy;
  _savedGenTermManager.getSavedGenTerm("NormEnergy",normEnergy,_step);
  if (*normEnergy!=0)
  {
    double normEnergyIncr;
    convergeance = EnergyIncrCriterion(*normEnergy,normEnergyIncr);
    printf("--normEnergyTolerance %12.5E \n",(*normEnergy)*_tolerance);
    printf("--EnergyIncrement %12.5E \n",normEnergyIncr);
    if (!convergeance) return convergeance;
  }
*/
  return convergeance;
}





void NonLinearSolver::solve()
{
  linearSystem<double>* lsys=NULL;
  if ( Data->solvertype == 2 )
  {
#if defined(HAVE_TAUCS)
    lsys = new linearSystemCSRTaucs<double>;
#else
    printf ( "Taucs is not installed : Gmm is chosen to solve\n" );
    linearSystemCSRGmm<double>* buf= new linearSystemCSRGmm<double>;
    buf->setNoisy ( 2 );
    lsys=buf;
    Data->solvertype = 1;
#endif
  }
  else if ( Data->solvertype == 3 )
  {
#if defined(HAVE_PETSC)
    lsys = new linearSystemPETSc<double>;
#else
    printf ( "Petsc is not installed : Gmm is chosen to solve\n" );
    linearSystemCSRGmm<double>* buf= new linearSystemCSRGmm<double>;
    buf->setNoisy ( 2 );
    lsys=buf;
    Data->solvertype = 1;
#endif

  }
  else if ( Data->solvertype == 1 )
  {
    linearSystemCSRGmm<double>* buf= new linearSystemCSRGmm<double>;
    buf->setNoisy ( 2 );
    lsys=buf;
    Data->solvertype = 1;
  }

  if ( pAssembler ) delete pAssembler;
  pAssembler = new dofManager<double> ( lsys );

  printf ( "-- start solving\n" );
  // Parameters
  if (Data->User.Integers["nb_step"] == 0)
  {
    Data->User.Integers["keep_initial_stiffness"] = 0;
    Data->User.Scalars["final_time"] = 1.;
    Data->User.Integers["nb_step"] = 2;
    Data->User.Integers["max_iter"] = 20;
    Data->User.Integers["control"] = 0; // 0=Load, 1=Dislacement, 2=Arc_length WARNING temporaire
    Data->User.Scalars["tolerance"] = 0.0001;
  }

  if (Data->User.Vectors["load_steps"].size() == 0)
    for (int i=0; i<=Data->User.Integers["nb_step"]; ++i)
    {
      Data->User.Vectors["load_steps"].push_back(i*(Data->User.Scalars["final_time"]/(double)Data->User.Integers["nb_step"]));;
    }

  assert(Data->User.Vectors["load_steps"].size() == Data->User.Integers["nb_step"]+1);

  //Initial step
  _step=0;_iter=0;
  BuildDirichlet(0.);
  BuildNeumann(0.);
  BuildInitialFunctionSpaces();
  BuildInitialLinearSystem();
  pAssembler->systemSolve();
  exportx(_step,_iter);
  InitField();

  for (_step=1; _step<=Data->User.Integers["nb_step"]; ++_step)
  {
    printf("---- Step %d \n",_step);
    
    //Initial iteration
    _iter=0;
    
    double facDirichlet = Data->User.Vectors["load_steps"][_step]-Data->User.Vectors["load_steps"][_step-1];
    double facNeumann = Data->User.Vectors["load_steps"][_step];
    if (Data->User.Integers["control"] == 0)
    {
      BuildDirichlet(0.);
      BuildNeumann(facNeumann);
    }
    else if (Data->User.Integers["control"] == 1)
    {
      BuildDirichlet(facDirichlet);
      BuildNeumann(0.);
    }
    else if (Data->User.Integers["control"] == 2)
    {// change with arc length method
      BuildDirichlet(facDirichlet);
      BuildNeumann(facNeumann);
    }

    CopyField();

    bool convergeance = false;
    //Next iterations
    for (_iter=1; _iter<=Data->User.Integers["max_iter"]; ++_iter)
    {
      printf("------ Iteration %d \n",_iter);
      
      pAssembler->systemClear();
      if(_iter==1 && Data->User.Integers["control"] == 1) BuildDirichlet(facDirichlet);
      else BuildDirichlet(0.);
      BuildFunctionSpaces();
      BuildLinearSystem();
      
      if(_iter==1) InitCriteria();
      
      pAssembler->systemSolve();
      exportx(_step,_iter);
      SaveField();
      AssembleResidual();
      AssembleEnergy();

      exportKb(_step,_iter);
      exportx(_step,_iter);


      convergeance = StoppingCriteria();
      if (convergeance) break;
    }
//     _savedGenTermManager.print();
    if (!convergeance)
    {
      printf ( "------ Unconverged Newton iteration step %d\n", _step);
      return;
    }

    if(_iter!=1 && Data->User.Integers["control"] == 1) BuildDirichlet(facNeumann); // BuildDirichlet(fac/_step);
    if(_iter!=1 && Data->User.Integers["control"] == 1) BuildFunctionSpaces();

    // Post - traitement
    postprocessing(_step);

    printf ( "---- done step %d/%d ---------------------------- \n", _step, Data->User.Integers["nb_step"] );
  }
//  _savedGenTermManager.print();

  _step = Data->User.Integers["nb_step"];

  printf ( "-- done solving! \n" );

  double Volume=0;
  double elasticEnerg=0;
  double totalEnerg=0;
  GaussQuadrature Integ_Vol ( GaussQuadrature::ValVal );
  GaussQuadrature Integ_Bulk ( GaussQuadrature::GradGrad );
  for ( unsigned int i = 0; i < EDomains.size(); ++i )
  {
    NLElasticDomain* E = EDomains[i];

    // Volume
    genTerm<genTensor0<double>,0>::Handle One( new ConstantField<genTensor0<double> >(1.0) );
    Assemble ( One, E->begin(), E->end(), Integ_Vol, Volume );
    
    // Elastic Energy
    diffTerm<genTensor1<double> ,0>::Handle Field ( new genSField<genTensor1<double> >(pAssembler, FSpaceDisps[i]) );
//     diffTerm<genTensor1<double> ,0>::Handle Field ( new genSField<genTensor1<double> >(pAssembler, FSpaceDisp) );
    genTerm<genTensor2<double>,0>::Handle G = Gradient(Field);
    genTerm<genTensor2<double>,0>::Handle B = (G+Transpose(G))*0.5;
    genTerm<genTensor2<double>,0>::Handle CB ( new genCache<genTensor2<double>,0>(B) ); // use cache
    genTerm<genTensor0<double>,0>::Handle Energy = E->Force<0>(CB);
    Assemble ( Energy, E->begin(), E->end(), Integ_Bulk, elasticEnerg);

    // Total Energy
    // ...
  }
  printf ( "initial volume=%f\n", Volume );
  printf ( "elastic energy=%f\n", elasticEnerg*0.5 );
  //printf ( "L + NL elastic energy=%f\n",totalEnerg*0.5);
  

//  -_Test_Manager_---------------------------------------
//     SavedDiffTerm<double>* saveDof;
//     _savedGenTermManager.addSavedGenTerm("Dof",saveDof,0,0);
//     SavedDiffTerm<genTensor1<double> > saveDof(FSpaceDisp);
//     for ( unsigned int i = 0; i < EDomains.size(); ++i )
//     {
//       SaveDof(*pAssembler,EDomains[i]->begin(), EDomains[i]->end(),saveDof);
//     }
//     saveDof.print("saveDof");
      
//    _savedGenTermManager.print();
//  ----------------------------------------------------

}



void NonLinearSolver::exportKb()
{
  FILE* f;
  double valeur;
  std::string sysname = "A";
  
  if (Data->solvertype == 1)
  {
    f = fopen ( "K.txt", "w" );
    for ( int i = 0 ; i < pAssembler->sizeOfR() ; i ++ )
    {
      for ( int j = 0 ; j < pAssembler->sizeOfR() ; j ++ )
      {
        pAssembler->getLinearSystem ( sysname )->getFromMatrix ( i,j, valeur );
        fprintf ( f,"%+e ",valeur );
      }
      fprintf ( f,"\n" );
    }
    fclose ( f );
  }

  f = fopen ( "b.txt", "w" );
  for ( int i = 0 ; i < pAssembler->sizeOfR() ; i ++ )
  {
    pAssembler->getLinearSystem ( sysname )->getFromRightHandSide ( i,valeur );
    fprintf ( f,"%+e\n",valeur ) ;
  }
  fclose ( f );
}

void NonLinearSolver::exportKb(int step,int iter)
{
  if (step==0 & iter==0)
  {
    exportKb();
    return;
  }


  FILE* f;
  double valeur;
  std::string sysname = "A";

  if (Data->solvertype == 1)
  {
    f = fopen ( "K.txt", "a" );
    fprintf ( f,"------------------------------\n Step %d, Iter %d  :\n", step, iter );
    for ( int i = 0 ; i < pAssembler->sizeOfR() ; i ++ )
    {
      for ( int j = 0 ; j < pAssembler->sizeOfR() ; j ++ )
      {
        pAssembler->getLinearSystem ( sysname )->getFromMatrix ( i,j, valeur );
        fprintf ( f,"%+e ",valeur ) ;
      }
      fprintf ( f,"\n" );
    }
    fclose ( f );
  }

  f = fopen ( "b.txt", "a" );
  fprintf ( f,"------------------------------\n Step %d, Iter %d  :\n", step, iter );
  for ( int i = 0 ; i < pAssembler->sizeOfR() ; i ++ )
  {
    pAssembler->getLinearSystem ( sysname )->getFromRightHandSide ( i,valeur );
    fprintf ( f,"%+e\n",valeur ) ;
  }
  fclose ( f );

}

void NonLinearSolver::exportx()
{
  FILE* f = fopen ( "x.txt", "w" );
  double valeur;
  std::string sysname = "A";
  for ( int i = 0 ; i < pAssembler->sizeOfR() ; i ++ )
  {
    pAssembler->getLinearSystem ( sysname )->getFromSolution ( i,valeur );
    fprintf ( f,"%+e\n",valeur ) ;
  }

  fclose ( f );

}

void NonLinearSolver::exportx(int step,int iter)
{
  if (step==0 & iter==0)
  {
    exportx();
    return;
  }

  FILE* f = fopen ( "x.txt", "a" );
  fprintf ( f,"------------------------------\n Step %d, Iter %d  :\n", step, iter );
  double valeur;
  std::string sysname = "A";
  for ( int i = 0 ; i < pAssembler->sizeOfR() ; i ++ )
  {
    pAssembler->getLinearSystem ( sysname )->getFromSolution ( i,valeur );
    fprintf ( f,"%+e\n",valeur ) ;
  }

  fclose ( f );

}

void NonLinearSolver::postprocessing(int step)
{
  std::ostringstream temp;
  temp << "displacement " << step ;
  PView* pv = buildDisplacementView( temp.str() );
  temp.str(""); temp << "disp_" << step << ".msh";
  pv->getData()->writeMSH(temp.str(), 3);
  delete pv;
}


PView* NonLinearSolver::buildDisplacementView ( const std::string &postFileName )
{
  std::vector<genTerm<genTensor1<double> ,0>::ConstHandle> Displacement;
  for ( unsigned int i = 0; i < EDomains.size(); ++i )
  {
    diffTerm<genTensor1<double> ,0>::Handle Field(new genSField<genTensor1<double> >( pAssembler, FSpaceDisps[i] ));
    genTerm<genTensor1<double> ,0>::Handle Disp = Field;
    Displacement.push_back(Disp);
  }

  return buildViewNodal(Displacement,EDomains,postFileName);
}

PView* NonLinearSolver::buildElasticEnergyView ( const std::string &postFileName )
{
  std::vector<genTerm<genTensor0<double>,0>::ConstHandle> Energy;
  for ( unsigned int i = 0; i < EDomains.size(); ++i )
  {
    NLElasticDomain* E = EDomains[i];

    diffTerm<genTensor1<double> ,0>::Handle Field(new genSField<genTensor1<double> >( pAssembler, FSpaceDisps[i] ));
    genTerm<genTensor2<double>,0>::Handle G = Gradient(Field);
    genTerm<genTensor2<double>,0>::Handle CG(new genCache<genTensor2<double>,0>(G));
    genTerm<genTensor2<double>,0>::Handle B = (G+Transpose(G))*0.5;
    genTerm<genTensor2<double>,0>::Handle CB(new genCache<genTensor2<double>,0>(B));
  
    Energy.push_back(E->deltaEnergy<0,0>(CB,CB));
  }

  return buildViewElement(Energy, EDomains, postFileName);
}

PView* NonLinearSolver::buildStressView ( const std::string &postFileName )
{
  std::vector<genTerm<genTensor2<double>,0>::ConstHandle> Stresses;
  for( unsigned int i = 0; i < EDomains.size(); i++ )
  {
    NLElasticDomain* E = EDomains[i];

  diffTerm<genTensor1<double>,0>::Handle Field(new genSField<genTensor1<double> >( pAssembler, FSpaceDisps[i] ));
  genTerm<genTensor2<double>,0>::Handle G = Gradient(Field);
  genTerm<genTensor2<double>,0>::Handle CG(new genCache<genTensor2<double>,0>(G));
  genTerm<genTensor2<double>,0>::Handle B = (G+Transpose(G))*0.5;
  genTerm<genTensor2<double>,0>::Handle CB(new genCache<genTensor2<double>,0>(B));

    Stresses.push_back(E->Stress());
  }

  return buildViewElementNode(Stresses,EDomains,postFileName);
//   return buildViewNodal(Stresses,EDomains,postFileName);
}

PView* NonLinearSolver::buildStrainView ( const std::string &postFileName )
{
  std::vector<genTerm<genTensor2<double>,0>::ConstHandle> Strain;
  for( unsigned int i = 0; i < EDomains.size(); i++ )
  {
    std::shared_ptr<SavedGenTerm<genTensor2<double>,0> > SavedStrain;
    savedGenTermManager.getSavedGenTerm("Strain",SavedStrain,_step,0,i);

    Strain.push_back(SavedStrain);
  }

  return buildViewElementNode(Strain,EDomains,postFileName);
//   return buildViewNodal(Strain,EDomains,postFileName);
}