// 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.8804 in gmsh)


#include "genMiscTerms.h"
#include "genTermCompose.h"

//--------------------------------------------------------------------------
// ScalarTermBase
//--------------------------------------------------------------------------



//--------------------------------------------------------------------------
// LinearTermBase
//--------------------------------------------------------------------------



//--------------------------------------------------------------------------
// BilinearTermBase
//--------------------------------------------------------------------------


void StabilizedLagrangeMultiplierTerm::get(MElement* ele, int npts, IntPt* GP, std::vector<ContainerValType> &vvals) const
{
  std::vector<genVector<genTensor1<double> > > vvals1(npts);
  std::vector<genVector<genTensor0<double> > > vvals2(npts);
  BilinearTerm<genTensor1<double>,genTensor0<double> >::space1->get(ele,npts,GP,vvals1);
  BilinearTerm<genTensor1<double>,genTensor0<double> >::space2->get(ele,npts,GP,vvals2);
  genScalar<genTensor1<double> > fsd(_d);
  for (int i=0; i<npts; ++i)
  {
    genVector<genTensor0<double> > temp;
    
    Compose<FullContrProdOp<genTensor1<double>, genTensor1<double> > >(vvals1[i],fsd,temp);
//    ComposeOp<FullContrProdOp>(vvals1[i],_d,temp);
    Compose<TensProdOp<genTensor0<double>,genTensor0<double> > >(temp,vvals2[i],vvals[i]);
//    TensProdCompose(temp,vvals2[i],vvals[i]);
    
  }
}

void StabilizedLagrangeMultiplierTerm::get(MElement* ele, int npts, IntPt* GP, ContainerValType &vals) const
{
  int nbFF1 = BilinearTerm<genTensor1<double> , genTensor0<double> >::space1->getNumKeys(ele); //nbVertices*nbcomp of parent
  int nbFF2 = BilinearTerm<genTensor1<double> , genTensor0<double> >::space2->getNumKeys(ele); //nbVertices of boundary
  double jac[3][3];
  vals.resize(nbFF1, nbFF2);
  vals.setAll(0.);
  for(int i = 0; i < npts; ++i)
  {
    double u = GP[i].pt[0]; double v = GP[i].pt[1]; double w = GP[i].pt[2];
    const double weight = GP[i].weight; const double detJ = ele->getJacobian(u, v, w, jac);
    genFSpace<genTensor1<double> >::ContainerValType Vals;
    genFSpace<genTensor0<double> >::ContainerValType ValsT;
    BilinearTerm<genTensor1<double> ,genTensor0<double> >::space1->f(ele, u, v, w, Vals);
    BilinearTerm<genTensor1<double> ,genTensor0<double> >::space2->f(ele, u, v, w, ValsT);
    for(int j = 0; j < nbFF1; ++j)
    {
      for(int k = 0; k < nbFF2; ++k)
      {
        vals(j, k) += dot(Vals(j), _d) * ValsT(k) * weight * detJ;
      }
    }
  }
}


void DispLagrangeMultiplierTerm::get(MElement* ele, int npts, IntPt* GP, std::vector<ContainerValType> &vvals) const
{
  std::vector<genVector<genTensor1<double> > > vvals1(npts);
  std::vector<genVector<genTensor1<double> > > vvals2(npts);
  BilinearTerm<genTensor1<double> ,genTensor1<double> >::space1->get(ele,npts,GP,vvals1);
  BilinearTerm<genTensor1<double> ,genTensor1<double> >::space2->get(ele,npts,GP,vvals2);

  for (int k=0;k<npts;++k)
  {
//    Prod(vvals1[k],genScalar<double>(_eqfac),vvals1[k]);
    Compose<ContrProdOp<genTensor1<double>, genTensor0<double> > >(vvals1[k],genScalar<genTensor0<double> >(_eqfac),vvals1[k]);
    //    FullContrProdCompose(vvals1[k],vvals2[k],vvals[k]);
    Compose<FullContrProdOp<genTensor1<double>,genTensor1<double> > >(vvals1[k],vvals2[k],vvals[k]);
  }
}

void DispLagrangeMultiplierTerm::get(MElement* ele, int npts, IntPt* GP, ContainerValType &vals) const
{
//  BilinearTermBase<double>::get(ele,npts,GP,vals);
  
  int nbFF1 = BilinearTerm<genTensor1<double> , genTensor1<double> >::space1->getNumKeys(ele); //nbVertices*nbcomp of parent
  int nbFF2 = BilinearTerm<genTensor1<double> , genTensor1<double> >::space2->getNumKeys(ele); //nbVertices of boundary
  double jac[3][3];
  vals.resize(nbFF1, nbFF2);
  vals.setAll(0.);
  for(int i = 0; i < npts; ++i)
  {
    double u = GP[i].pt[0]; double v = GP[i].pt[1]; double w = GP[i].pt[2];
    const double weight = GP[i].weight; const double detJ = ele->getJacobian(u, v, w, jac);
    genFSpace<genTensor1<double> >::ContainerValType Vals;
    genFSpace<genTensor1<double> >::ContainerValType ValsT;
    BilinearTerm<genTensor1<double> ,genTensor1<double> >::space1->f(ele, u, v, w, Vals);
    BilinearTerm<genTensor1<double> ,genTensor1<double> >::space2->f(ele, u, v, w, ValsT);
    for(int j = 0; j < nbFF1; ++j)
    {
      for(int k = 0; k < nbFF2; ++k)
      {
        vals(j, k) += _eqfac * dot(Vals(j), ValsT(k)) * weight * detJ;
      }
    }
  }
}


//--------------------------------------------------------------------------
// ElasticTerm
//--------------------------------------------------------------------------


template <> void ElasticTerm<0,0>::get(MElement* ele, int npts, IntPt* GP, std::vector<ContainerValType> &m) const
{
  if(ele->getParent()) ele = ele->getParent();
  fullMatrix<double> Hs((double*)H.getDataPtr(),6,6);
  if(sym)
  {
    std::vector<diffTerm<genTensor1<double> ,0>::ContainerGradType> vGrads(npts); 
    space1->getgradf(ele, npts, GP, vGrads);
    fullMatrix<double> B(6, 1);
    fullMatrix<double> BTH(1, 6);
    fullMatrix<double> BT(1, 6);
    for(int i = 0; i < npts; ++i)
    {
      BT(0, 0) = B(0, 0) = vGrads[i]()(0, 0); 
      BT(0, 1) = B(1, 0) = vGrads[i]()(1, 1); 
      BT(0, 2) = B(2, 0) = vGrads[i]()(2, 2);  
      BT(0, 3) = B(3, 0) = vGrads[i]()(0, 1) + vGrads[i]()(1, 0);
      BT(0, 4) = B(4, 0) = vGrads[i]()(1, 2) + vGrads[i]()(2, 1);
      BT(0, 5) = B(5, 0) = vGrads[i]()(0, 2) + vGrads[i]()(2, 0);
      BTH.setAll(0.);
      BTH.gemm(BT, Hs);
      fullMatrix<double> BTHB(1,1);
      BTHB.setAll(0.);
      BTHB.gemm(BTH, B);
      m[i]=ContainerValType(BTHB(0,0));
    }
  }
  else
  {
    double jac[3][3];
    std::vector<diffTerm<genTensor1<double> ,0>::ContainerGradType> vGrads(npts);
    std::vector<diffTerm<genTensor1<double> ,0>::ContainerGradType> vGradsT(npts);
    space1->getgradf(ele, npts, GP, vGrads);
    space2->getgradf(ele, npts, GP, vGradsT);

    fullMatrix<double> B(6, 1);
    fullMatrix<double> BTH(1, 6);
    fullMatrix<double> BT(1, 6);
    for(int i = 0; i < npts; ++i)
    {
      BT(0, 0) = vGrads[i]()(0, 0);
      BT(0, 1) = vGrads[i]()(1, 1);
      BT(0, 2) = vGrads[i]()(2, 2);
      BT(0, 3) = vGrads[i]()(0, 1) + vGrads[i]()(1, 0);
      BT(0, 4) = vGrads[i]()(1, 2) + vGrads[i]()(2, 1);
      BT(0, 5) = vGrads[i]()(0, 2) + vGrads[i]()(2, 0);
      B(0, 0) = vGradsT[i]()(0, 0);
      B(1, 0) = vGradsT[i]()(1, 1);
      B(2, 0) = vGradsT[i]()(2, 2);
      B(3, 0) = vGradsT[i]()(0, 1) + vGradsT[i]()(1, 0);
      B(4, 0) = vGradsT[i]()(1, 2) + vGradsT[i]()(2, 1);
      B(5, 0) = vGradsT[i]()(0, 2) + vGradsT[i]()(2, 0);
      BTH.setAll(0.);
      BTH.gemm(BT, Hs);
      fullMatrix<double> BTHB(1,1);
      BTHB.setAll(0.);
      BTHB.gemm(BTH, B);
      m[i]=ContainerValType(BTHB(0,0));
    }
  }
}


template <> void ElasticTerm<1,1>::get(MElement* ele, int npts, IntPt* GP, std::vector<ContainerValType> &m) const
{
  if(ele->getParent()) ele = ele->getParent();
  fullMatrix<double> Hs((double*)H.getDataPtr(),6,6);
  if(sym)
  {
    int nbFF = space1->getNumKeys(ele);
    std::vector<diffTerm<genTensor1<double> ,1>::ContainerGradType> vGrads(npts);
    space1->getgradf(ele, npts, GP, vGrads);

    fullMatrix<double> B(6, nbFF);
    fullMatrix<double> BTH(nbFF, 6);
    fullMatrix<double> BT(nbFF, 6);
    fullMatrix<double> MM(nbFF, nbFF);
    for(int i = 0; i < npts; ++i)
    {
      for(int j = 0; j < nbFF; ++j)
      {

        BT(j, 0) = B(0, j) = vGrads[i](j)(0, 0);
        BT(j, 1) = B(1, j) = vGrads[i](j)(1, 1);
        BT(j, 2) = B(2, j) = vGrads[i](j)(2, 2);
        BT(j, 3) = B(3, j) = vGrads[i](j)(0, 1) + vGrads[i](j)(1, 0);
        BT(j, 4) = B(4, j) = vGrads[i](j)(1, 2) + vGrads[i](j)(2, 1);
        BT(j, 5) = B(5, j) = vGrads[i](j)(0, 2) + vGrads[i](j)(2, 0);
      }
      BTH.setAll(0.);
      BTH.gemm(BT, Hs);
      m[i].resize(nbFF, nbFF);
//      m[i].setAll(0.);
      MM.gemm(BTH, B);
      for(int ii = 0; ii < nbFF; ++ii)
        for(int jj = 0; jj < nbFF; ++jj)
          m[i](ii,jj)=MM(ii,jj);
    }
  }
  else
  {
    int nbFF1 = space1->getNumKeys(ele);
    int nbFF2 = space2->getNumKeys(ele);
    std::vector<diffTerm<genTensor1<double> ,1>::ContainerGradType> vGrads(npts);
    std::vector<diffTerm<genTensor1<double> ,1>::ContainerGradType> vGradsT(npts);
    space1->getgradf(ele, npts, GP, vGrads);
    space2->getgradf(ele, npts, GP, vGradsT);

    fullMatrix<double> B(6, nbFF2);
    fullMatrix<double> BTH(nbFF1, 6);
    fullMatrix<double> BT(nbFF1, 6);
    fullMatrix<double> MM(nbFF1, nbFF2);
    for(int i = 0; i < npts; ++i)
    {
      for(int j = 0; j < nbFF1; ++j)
      {
        BT(j, 0) = vGrads[i](j)(0, 0); 
        BT(j, 1) = vGrads[i](j)(1, 1);
        BT(j, 2) = vGrads[i](j)(2, 2);
        BT(j, 3) = vGrads[i](j)(0, 1) + vGrads[i](j)(1, 0);
        BT(j, 4) = vGrads[i](j)(1, 2) + vGrads[i](j)(2, 1);
        BT(j, 5) = vGrads[i](j)(0, 2) + vGrads[i](j)(2, 0);
      }
      for(int j = 0; j < nbFF2; ++j)
      {
        B(0, j) = vGradsT[i](j)(0, 0);
        B(1, j) = vGradsT[i](j)(1, 1);
        B(2, j) = vGradsT[i](j)(2, 2);
        B(3, j) = vGradsT[i](j)(0, 1) + vGradsT[i](j)(1, 0);
        B(4, j) = vGradsT[i](j)(1, 2) + vGradsT[i](j)(2, 1);
        B(5, j) = vGradsT[i](j)(0, 2) + vGradsT[i](j)(2, 0);
      }
      BTH.setAll(0.);
      BTH.gemm(BT, Hs);
      MM.gemm(BTH, B);
      m[i].resize(nbFF1, nbFF2);
      for(int ii = 0; ii < nbFF1; ++ii)
        for(int jj = 0; jj < nbFF2; ++jj)
          m[i](ii,jj)=MM(ii,jj);      
    }
  }
}