// elastic_genTerm_Cutmesh_Xfem - A linear solver for elastic problems using X-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>.

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

#include "elasticCutmeshXfemSolver.h"
//#include "SpaceReducerForStableMultipliers.h"
#include "spaceReducer.h"
#include "genAlgorithms.h"
#include "genSField.h"


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


ElasticCutmeshXfemSolver::~ElasticCutmeshXfemSolver()
{
}

void ElasticCutmeshXfemSolver::CheckProperties()
{
  ElasticSolver::CheckProperties();

  double eqfac = Data->User.Scalars["ScaleElastic"];
  std::map<std::pair<int,int>,int> mapDimTagPos;

  for (unsigned int i = 0; i < Data->BCs.size(); ++i)
  {
    if (Data->BCs[i]->kind=="Lagrange")
    {
      if (Data->BCs[i]->What=="Displacement")
      {
        std::map<std::pair<int,int>,int>::iterator it;
        std::pair<int,int> id(Data->BCs[i]->dim,Data->BCs[i]->tag);
        it=mapDimTagPos.find(id);
        if(it==mapDimTagPos.end())
        {
          StableLagrangeMultiplierDomain* field = new StableLagrangeMultiplierDomain(Data->BCs[i], eqfac);
          mapDimTagPos[id] = StableLagMultDomains.size();
          StableLagMultDomains.push_back(field);
        }
        else
        {
          StableLagMultDomains[it->second]->addComponent(Data->BCs[i]);
        }
      }
    }
  }
}

void ElasticCutmeshXfemSolver::readInputFile ( const std::string &fileName )
{
  ElasticSolver::readInputFile(fileName);
}


void ElasticCutmeshXfemSolver::CreateFunctionSpaces()
{
  ElasticSolver::CreateFunctionSpaces();
  FSpaceStableLagMult.resize(StableLagMultDomains.size());
  for ( unsigned int i=0 ; i < FSpaceStableLagMult.size(); ++i)
  {
    VectorLagrangeFSpace<double>::Along vectLag[3];
    vectLag[0] = VectorLagrangeFSpace<double>::VECTOR_X;
    vectLag[1] = VectorLagrangeFSpace<double>::VECTOR_Y;
    vectLag[2] = VectorLagrangeFSpace<double>::VECTOR_Z;

    std::vector<int> comps(StableLagMultDomains[i]->comps.begin(), StableLagMultDomains[i]->comps.end());
    printf("StableLagMultDomains[%d] : %d components\n", i, (int)comps.size());

    switch (comps.size())
    {
      case 1 :
        FSpaceStableLagMult[i] = genFSpace<genTensor1<double> >::Handle(new VectorLagrangeFSpace<double>(vectLag[comps[0]]));
        break;
      case 2 :
        FSpaceStableLagMult[i] = genFSpace<genTensor1<double> >::Handle(new VectorLagrangeFSpace<double>(vectLag[comps[0]], vectLag[comps[1]]));
        break;
      case 3 :
        FSpaceStableLagMult[i] = genFSpace<genTensor1<double> >::Handle(new VectorLagrangeFSpace<double>());
        break;
      default :
        Msg::Error("StableLagrangeMultipliersSpace size error");
    }
    std::cout << "StableLagMult Field : iField=" << FSpaceStableLagMult[i]->getIncidentSpaceTag() << std::endl;
  }

  for ( unsigned int i = 0; i < Data->BCs.size(); ++i)
  {
    if (Data->BCs[i]->kind=="Lagrange_GaetanMethod")
    {
//       FSpaceLagMult = genFSpace<genTensor0<double> >::Handle(new ScalarLagrangeFSpace<genTensor0<double> >()); // here we take parent vertex for dof...
//       FSpaceLagMult = genFSpace<double>::Handle(new ScalarLagrangeFSpaceOfElement(Data->tag+1)); // here level set vertices ?
    }
  }

  /*

      // fill mean hanging nodes

  if (_ListHangingNodes)
  {
      std::map<int,std::vector <int> >::iterator ith;
      ith = _ListHangingNodes->begin();
      int compt = 1;
      while (ith!=_ListHangingNodes->end())
      {
          float fac;
          fac = 1.0/(ith->second).size();  // mean hanging nodes
          for (int j=0;j<_dim;j++)
          {
              DofAffineConstraint<double> constraint;
              int type = Dof::createTypeWithTwoInts(j, _tag);
              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;
              pAssembler->setLinearConstraint (hgnd, constraint);
          }
          ith++;
          compt++;
      }
  }
  */
}

void ElasticCutmeshXfemSolver::BuildFunctionSpaces()
{
  ElasticSolver::BuildFunctionSpaces();
  
  for ( unsigned int i = 0; i < FSpaceStableLagMult.size(); ++i)
  {
#if 0
    std::vector<int> comps(StableLagMultDomains[i]->comps.begin(), StableLagMultDomains[i]->comps.end());
    SpaceReducerForStableMultipliers* spacereducer = new SpaceReducerForStableMultipliers(StableLagMultDomains[i]->group(), Data->tag+2+i);
    spacereducer->BuildLinearConstraints(comps, pAssembler);
#else
    SpaceReducer spaceReducer(StableLagMultDomains[i]->group());
    spaceReducer.BuildLinearConstraints(FSpaceStableLagMult[i], pAssembler);
#endif
  }
 
  // we number the LagrangeMultipliers dofs
  for ( unsigned int i = 0; i < FSpaceStableLagMult.size(); ++i)
  {
    NumberDofs ( FSpaceStableLagMult[i], StableLagMultDomains[i]->begin(), StableLagMultDomains[i]->end(), *pAssembler );
  }
  for (unsigned int i = 0; i<Data->BCs.size(); ++i)
  {
    if (Data->BCs[i]->kind=="Lagrange_GaetanMethod") // Gaetan method
;//      NumberDofs ( FSpaceLagMult, Data->BCs[i]->begin(), Data->BCs[i]->end(), *pAssembler );
  }
}

void ElasticCutmeshXfemSolver::AssembleRHS()
{
  ElasticSolver::AssembleRHS();
  double volume = 0;
  genTerm<genTensor0<double> ,0>::Handle Volume(new ConstantField<genTensor0<double> >(1.0));

  GaussQuadrature Integ_Boundary ( GaussQuadrature::Val );
  for ( unsigned int i = 0; i < StableLagMultDomains.size(); ++i)
  {
    StableLagrangeMultiplierDomain* S = StableLagMultDomains[i];
    for (int j=0; j<S->comps.size(); ++j) std::cout << "Lagrange BC Disp" << std::endl;
//     genTerm<genTensor1<double> ,0>::Handle Func(new FunctionField<genTensor1<double> >(*(S->f));
//     genTerm<double,1>::Handle loadterm = Compose<FullContrProdOp>(FSpaceStableLagMult,Func);
    genTerm<genTensor0<double> ,1>::Handle loadterm = S->Lterm<1>(FSpaceStableLagMult[i]);
    Assemble ( loadterm, S->begin(), S->end(), Integ_Boundary, *pAssembler );
    volume = 0;
    Assemble (Volume, S->begin(), S->end(), Integ_Boundary, volume);
    printf("Support(%dD,%d) = %f [L^%d]\n", S->dim, S->tag, volume, S->dim);
  }

  for (unsigned int i=0; i<Data->BCs.size(); ++i)
  {
/*    if (Data->BCs[i]->kind=="Lagrange_GaetanMethod")
    {
      std::cout << "Dirichlet BC Disp" << std::endl;
      genTerm<genTensor0<double>,0>::Handle Func(new FunctionField<genTensor0<double> >(*(Data->BCs[i]->fscalar)));
      genTerm<genTensor0<double>,1>::Handle loadterm = Compose<FullContrProdOp>(FSpaceLagMult,Func);
      Assemble ( loadterm , Data->BCs[i]->begin(), Data->BCs[i]->end(), Integ_Boundary, *pAssembler );
      volume=0;
      Assemble ( Volume, Data->BCs[i]->begin(), Data->BCs[i]->end(), Integ_Boundary,volume);
      printf("Support(%dD,%d) = %f [L^%d]\n", Data->BCs[i]->dim, Data->BCs[i]->tag, volume, Data->BCs[i]->dim);
    }*/
  }
}

void ElasticCutmeshXfemSolver::AssembleLHS()
{
  ElasticSolver::AssembleLHS();

  for ( unsigned int i = 0; i < StableLagMultDomains.size(); ++i)
  {
    StableLagrangeMultiplierDomain* S = StableLagMultDomains[i];
    genTerm<genTensor0<double>,2>::Handle CrossLagTerm1 = S->CrossLagTerm<1,1>(FSpaceDisp,FSpaceStableLagMult[i]);
    genTerm<genTensor0<double>,2>::Handle CrossLagTerm2 = S->CrossLagTerm<1,1>(FSpaceStableLagMult[i],FSpaceDisp);
    genTerm<genTensor0<double>,2>::Handle SelfLagTerm = S->SelfLagTerm<1,1>(FSpaceStableLagMult[i],FSpaceStableLagMult[i]);

    GaussQuadrature Integ_LagrangeMult ( GaussQuadrature::ValVal );
    Assemble ( CrossLagTerm1, S->begin(), S->end(), Integ_LagrangeMult, *pAssembler );
    Assemble ( CrossLagTerm2, S->begin(), S->end(), Integ_LagrangeMult, *pAssembler );
    Assemble (  SelfLagTerm , S->begin(), S->end(), Integ_LagrangeMult, *pAssembler );
  }
  
  for (unsigned int i = 0; i<Data->BCs.size(); ++i)
  {
 /*   if (Data->BCs[i]->kind=="Lagrange_GaetanMethod") // Gaetan method
    {

      const genSimpleFunction<genTensor1<double> > &fvector = *(Data->BCs[i]->fvector);
      const genSimpleFunction<genTensor0<double> > &fscalar = *(Data->BCs[i]->fscalar);
      genTensor1<double> v = fvector(0.,0.,0.);
      genTensor0<double> tau = fscalar(0.,0.,0.);
      genTerm<genTensor1<double> ,0>::Handle d(new ConstantField<genTensor1<double> >(v)); // genTerm<genTensor1<double> ,0>::Handle d(new FunctionField<genTensor1<double> >(*(Data->BCs[i]->fvector)));

      genTerm<genTensor1<double> ,1>::Handle G = Gradient(FSpaceLagMult);
      genTerm<genTensor1<double> ,1>::Handle GC(new genCache<genTensor1<double> ,1>(G));

//       genTerm<genTensor0<double>,2>::Handle CrossLagTerm1 = Compose<TensProdOp>(Compose<FullContrProdOp>(FSpaceDisp,d),FSpaceLagMult);
      genTerm<genTensor0<double>,2>::Handle CrossLagTerm1 = Compose<TensProdOp>(Compose<FullContrProdOp>(FSpaceDisp,d),FSpaceLagMult);
      genTerm<genTensor0<double>,2>::Handle CrossLagTerm2 = Compose<TensProdOp>(FSpaceLagMult,Compose<FullContrProdOp>(FSpaceDisp,d));
      genTerm<genTensor0<double>,2>::Handle LapTerm = Compose<FullContrProdOp>(GC,GC)*tau;

      GaussQuadrature Integ_LagrangeMult ( GaussQuadrature::ValVal );
      GaussQuadrature Integ_Laplace ( GaussQuadrature::GradGrad );
      Assemble ( CrossLagTerm1, Data->BCs[i]->begin(), Data->BCs[i]->end(), Integ_LagrangeMult, *pAssembler );
      Assemble ( CrossLagTerm2, Data->BCs[i]->begin(), Data->BCs[i]->end(), Integ_LagrangeMult, *pAssembler );
      Assemble ( LapTerm, Data->BCs[i]->begin(), Data->BCs[i]->end(), Integ_Laplace, *pAssembler );
    }*/
  }
}

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

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

  for ( unsigned int i = 0; i < FSpaceStableLagMult.size(); ++i)
  {
    SpaceReducerForStableMultipliers* spacereducer = new SpaceReducerForStableMultipliers(StableLagMultSupports[i]->g, Data->tag+2+i);
    spacereducer->BuildLinearConstraints(StableLagMultSupports[i]->comp, pAssembler2);
  }

  // we number the LagrangeMultipliers dofs
  for ( unsigned int i = 0; i < FSpaceStableLagMult.size(); ++i)
  {
    NumberDofs ( *FSpaceStableLagMult[i], StableLagMultSupports[i]->g()->begin(), StableLagMultSupports[i]->g()->end(), *pAssembler );
  }
  

  for ( unsigned int i = 0; i < StableLagMultSupports.size(); ++i)
  {
    StableLagrangeMultiplierDomain* S = StableLagMultSupports[i];
    diffTerm<genTensor1<double> ,0>::Handle Field ( new genSField<genTensor1<double> >(pAssembler, FSpaceDisp) );
    diffTerm<genTensor1<double> ,0>::Handle LagField ( new genSField<genTensor1<double> >(pAssembler, FSpaceStableLagMult[i]) );
    genTerm<double,1>::Handle CrossLagTerm2 = S->CrossLagTerm<1,0>(FSpaceStableLagMult[i],Field);
    genTerm<double,1>::Handle SelfLagTerm = S->SelfLagTerm<1,0>(FSpaceStableLagMult[i],LagField);

    GaussQuadrature Integ_LagrangeMult ( GaussQuadrature::ValVal );
    Assemble ( *CrossLagTerm2, S->g()->begin(), S->g()->end(), Integ_LagrangeMult, *pAssembler2 );
    Assemble ( * SelfLagTerm , S->g()->begin(), S->g()->end(), Integ_LagrangeMult, *pAssembler2 );
  }

  GaussQuadrature Integ_Boundary ( GaussQuadrature::Val );
  for ( unsigned int i = 0; i < Data->BCs.size(); i++ )
  {
    if (Data->BCs[i]->What==genBoundaryCondition::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]->g()->begin(), Data->BCs[i]->g()->end(), Integ_Boundary, *pAssembler2 );
    }
  }
  for ( unsigned int i = 0; i < StableLagMultSupports.size(); ++i)
  {
    StableLagrangeMultiplierDomain* S = StableLagMultSupports[i];
    genTerm<double,1>::Handle loadterm = S->Lterm<1>(FSpaceStableLagMult[i])*(-1.);
    Assemble ( *loadterm, S->g()->begin(), S->g()->end(), Integ_Boundary, *pAssembler2 );
  }

  pAssembler2->systemSolve();

  for ( unsigned int i = 0; i < StableLagMultSupports.size(); ++i )
  {
    SavedUnknownTerm<genTensor1<double> > saveResidual(FSpaceStableLagMult[i]);
    SaveUnknown(*pAssembler2,StableLagMultSupports[i]->g()->begin(), StableLagMultSupports[i]->g()->end(),saveResidual);
    saveResidual.printfile("saveLagResidual.txt");
  }


  delete buf;
  delete pAssembler2;*/
}

/*PView* ElasticCutmeshXfemSolver::buildStableLagrangeMultiplierView ( const std::string &postFileName )
{
  std::cout <<  "build Stable Lagrange Multiplier View"<< std::endl;

  // store the StableLagMult values <vertex,value>
  std::map<int, std::vector<double> > data;
  PView* pv = NULL;

  // for new elements
  std::vector<MVertex*> vecVertices;
  std::vector <MElement*> vecElements;
  std::map<int, std::vector<MElement*> > Cut_Elements;

  // copy of _pModel
  GModel* pModelPost = new GModel ( *_pModel );
  GModel::setCurrent ( pModelPost );

  // tolerance for discontinuity point calcul
  double eps = 1e-6;

  // entity associate to cut elements
  std::map<MElement*, GEntity*> ElementCutEntity;
  typedef std::set<GFace*, GEntityLessThan>::iterator fiter;
  for ( fiter it = pModelPost->firstFace(); it != pModelPost->lastFace(); ++it )
    for ( unsigned int i = 0; i < ( *it )->polygons.size(); i++ )
      ElementCutEntity[ ( *it )->polygons[i]] = ( *it );

  // REGION
  typedef std::set<GRegion*, GEntityLessThan>::iterator riter;
  for ( riter it = pModelPost->firstRegion(); it != pModelPost->lastRegion(); ++it )
    for ( unsigned int i = 0; i < ( *it )->polyhedra.size(); i++ )
      ElementCutEntity[ ( *it )->polyhedra[i]] = ( *it );


  for ( unsigned int i = 0; i < FSpaceStableLagMult.size(); ++i )
  {
    diffTerm<genTensor1<double> ,1>::Handle FS(FSpaceStableLagMult[i],NoDelete());
    genSField<genTensor1<double> > Field(pAssembler, FS);
    groupOfElements::elementContainer::const_iterator it;

    for ( it = _StableLagrangeMultiplierSupports[i].g()->begin(); it != _StableLagrangeMultiplierSupports[i].g()->end(); ++it )
    {
      MElement* e=*it;
      for ( int j = 0; j < e->getNumVertices(); ++j )
      {
        MVertex* v = e->getVertex(j);
        SPoint3 p ( v->x(), v->y(), v->z() );
        double pnt[3];
        pnt[0]=p.x();
        pnt[1]=p.y();
        pnt[2]=p.z();
        double uvw[3];
        e->xyz2uvw ( pnt,uvw );
        genTensor1<double> val;
        IntPt pt;
        pt.pt[0]=uvw[0];
        pt.pt[1]=uvw[1];
        pt.pt[2]=uvw[2];
        pt.weight=1.0;
        Field.get(e,1,&pt,val);
        std::vector<double> vec ( 3 );
        vec[0]=val(0);
        vec[1]=val(1);
        vec[2]=val(2);
        data[ e->getVertex(j)->getNum() ]=vec;
        // printf("Vertex index: %d, force[ %lf %lf %lf ]\n", e->getVertex ( j )->getNum(), vec[0], vec[1], vec[2]);
        // La valeur pour un vertex dans deux elt distincts est identique
      }
    }

    if (pv == NULL)
      pv = new PView (postFileName, "NodeData", _pModel, data, i);
    else
      pv->addStep(_pModel, data, i, 3);

    int temp=0;
    std::vector<double> vec(3,0);
    std::map<int, std::vector<double> >::iterator itdata;
    for( itdata = data.begin(); itdata != data.end(); ++itdata)
    {
      vec[0] += (*itdata).second[0];
      vec[1] += (*itdata).second[1];
      vec[2] += (*itdata).second[2];
      temp++;
    }

    printf("nb de lag: %d\n", temp);
    double eqfac = ESupports[0]->scale;
    vec[0] *= eqfac/(double)temp;
    vec[1] *= eqfac/(double)temp;
    vec[2] *= eqfac/(double)temp;

    printf("LS n°%d : { %lf %lf %lf }\n ", i, vec[0], vec[1], vec[2] );
  }

  std::map < int, std::map<int, std::string > > physical; // empty

  // store new elements and vertices
  pModelPost->storeChain ( _dim, Cut_Elements, physical );

  return pv;
}*/