/*********************************************************************/
/* File:   equilibrium.cc                                            */
/* Author: Joachim Schoeberl                                         */
/* Date:   5. Jul. 2001                                             */
/*********************************************************************/

/* 
   realizations of equilibrium methods
   ref: R. Stenberg (1988) "A Family of Mixed Finite Elements for
   the Elasticity Prolbem", Numer. Math. 53, pp 513-538
*/


#include <fem.hpp>
namespace ngfem
{
  using namespace ngfem;

#ifdef NONE
  class FE_PrismGamma : public NodalFiniteElement
  {
    static ARRAY<IPData*> ipdata;

  public:

    FE_PrismGamma();
    virtual ~FE_PrismGamma();

    virtual int SpatialDim () const { return 3; }
    virtual int GetNDof () const { return 4; }
    virtual int Order () const { return 3; }
    virtual ELEMENT_TYPE ElementType() const { return ET_PRISM; }

    virtual void CalcShape (const IntegrationPoint & ip, 
			    Vector & shape,
			    int comp = 1) const;
    virtual const ARRAY<IPData*> & GetIPData () const { return ipdata; }
  };



  ARRAY<IPData*> FE_PrismGamma::ipdata;

  FE_PrismGamma :: FE_PrismGamma()
  {
    if (!ipdata.Size())
      CalcIPData(ET_PRISM, ipdata);
  }

  FE_PrismGamma :: ~FE_PrismGamma()
  {
    ;
  }

  void FE_PrismGamma :: CalcShape (const IntegrationPoint & ip, 
				   Vector & shape,
				   int comp) const
  {
    shape.SetSize(4);
    double x = ip.Point()[0];
    double y = ip.Point()[1];
    double z = ip.Point()[2];

    shape.Elem(1) = 1;
    shape.Elem(2) = x;
    shape.Elem(3) = y;
    shape.Elem(4) = z;
    /*
      shape.Elem(4) = x*x;
      shape.Elem(5) = x*y;
      shape.Elem(6) = y*y;
    */
  }




  class FE_PrismGamma2 : public NodalFiniteElement
  {
    static ARRAY<IPData*> ipdata;

  public:

    FE_PrismGamma2();
    virtual ~FE_PrismGamma2();

    virtual int SpatialDim () const { return 3; }
    virtual int GetNDof () const { return 3; }
    virtual int Order () const { return 3; }
    virtual ELEMENT_TYPE ElementType() const { return ET_PRISM; }

    virtual void CalcShape (const IntegrationPoint & ip, 
			    Vector & shape,
			    int comp = 1) const;
    virtual const ARRAY<IPData*> & GetIPData () const { return ipdata; }
  };



  ARRAY<IPData*> FE_PrismGamma2::ipdata;

  FE_PrismGamma2 :: FE_PrismGamma2()
  {
    if (!ipdata.Size())
      CalcIPData(ET_PRISM, ipdata);
  }

  FE_PrismGamma2 :: ~FE_PrismGamma2()
  {
    ;
  }

  void FE_PrismGamma2 :: CalcShape (const IntegrationPoint & ip, 
				    Vector & shape,
				    int comp) const
  {
    shape.SetSize(3);
    double x = ip.Point()[0];
    double y = ip.Point()[1];
    double z = ip.Point()[2];
    
    shape.Elem(1) = 1;
    shape.Elem(2) = x;
    shape.Elem(3) = y;
    
    /*
      shape.Elem(1) = 1;
      shape.Elem(2) = x;
      shape.Elem(3) = y;
      shape.Elem(4) = x*x;
      shape.Elem(5) = x*y;
      shape.Elem(6) = y*y;
      shape.Elem(7) = z*1;
      shape.Elem(8) = z*x;
      shape.Elem(9) = z*y;
      shape.Elem(10) = z*x*x;
      shape.Elem(11) = z*x*y;
      shape.Elem(12) = z*y*y;
    */
  }

#endif




  EquilibriumIntegrator :: EquilibriumIntegrator ()
  {
    ;
  }

  EquilibriumIntegrator ::  ~EquilibriumIntegrator ()
  {
    ;
  }

  void EquilibriumIntegrator :: 
  AssembleElementMatrix (const FiniteElement & fel, 
			 const ElementTransformation & eltrans, 
			 FlatMatrix<double> & elmat,
			 LocalHeap & lh) const
  {
    int i, j, k;

    FlatMatrix<> mata, matb, matc;

    ComputeMatrices (fel, eltrans, mata, matb, matc, lh);
  
    (*testout) << "mata = " << endl << mata << endl;
    (*testout) << "matb = " << endl << matb << endl;
    (*testout) << "matc = " << endl << matc << endl;

    int nsigma = mata.Height();
    int nu = matb.Height();

    BitArray internal(nu);
    GetInternalDofs(fel, internal);

    // eliminate primal (flux) variables:
    FlatMatrix<> invmata (nsigma, lh);
    FlatMatrix<> hmatb (matb.Height(), nsigma, lh);

    CalcInverse (mata, invmata);
    (*testout) << "invmata = " << endl << invmata << endl;
    hmatb = matb * invmata;
    matc += hmatb * Trans (matb);

    (*testout) << "Schur-complement = " << endl << matc << endl;
    // eliminate internal dual variables:

    int ni = 0, ne;
    for (i = 0; i < nu; i++)
      if (internal[i]) ni++;
    ne = nu - ni;
  
    FlatMatrix<> matci (ni, lh);
    FlatMatrix<> matcie (ni, ne, lh);
    FlatMatrix<> matce (ne, lh);

    int ii=0, ie=0, ji=0, je=0;
    for (i = 0; i < nu; i++)
      {
	ji = je = 0;
	for (j = 0; j < nu; j++)
	  {
	    if (internal[i] && internal[j])
	      matci(ii,ji) = matc(i,j);
	    else if (internal[i] && !internal[j])
	      matcie(ii,je) = matc(i,j);
	    else if (!internal[i] && !internal[j])
	      matce(ie,je) = matc(i,j);
	  
	    if (internal[j]) ji++; else je++;
	  }
	if (internal[i]) ii++; else ie++;
      }

    (*testout) << "matci = " << matci << endl;

    FlatMatrix<> invmatci (ni, lh);
    FlatMatrix<> hmatcie (ni, ne, lh);

    CalcInverse (matci, invmatci);
    hmatcie = invmatci * matcie;
    matce -= Trans (matcie) * hmatcie;



    elmat.AssignMemory (ne, ne, lh);
    elmat = matce;

    (*testout) << "elmat = " << elmat << endl;
  }


  void EquilibriumIntegrator :: 
  ComputeInternalVariables (const FiniteElement & fel, 
			    const ElementTransformation & eltrans, 
			    const FlatVector<> & uext, 
			    FlatVector<> & sigma, 
			    FlatVector<> & uint,
			    LocalHeap & lh) const
  {
    int i, j, k;

    FlatMatrix<> mata, matb, matc;
    FlatVector<> vecfsigma, vecf;

    ComputeMatrices (fel, eltrans, mata, matb, matc, lh);
    ComputeVectors (fel, eltrans, vecfsigma, vecf, lh);
  
    int nsigma = mata.Height();
    int nu = matb.Height();
    

    //  (*testout) << "nsigma = " << nsigma << " nu = " << nu << endl;
    //  (*testout) << "vecf = " << vecf << endl << ", vecfsigma = " << vecfsigma << endl;

    BitArray internal(nu);
    GetInternalDofs(fel, internal);
    
    // eliminate primal variables:
    FlatMatrix<> invmata (nsigma, lh);
    FlatMatrix<> hmatb (nu, nsigma, lh);
    FlatMatrix<> schur (nu, lh);
  
    CalcInverse (mata, invmata);
    hmatb = matb * invmata;
    matc += hmatb * Trans (matb);
  
    // eliminate internal dual variables:
    int ni = 0, ne;
    for (i = 0; i < nu; i++)
      if (internal[i]) ni++;
    ne = nu - ni;

    FlatMatrix<> matci (ni, lh);
    FlatMatrix<> matcie (ni, ne, lh);
    FlatMatrix<> matce (ne, lh);
    FlatVector<> vecfi (ni, lh), uall(nu, lh);

    int ii=0, ie=0, ji=0, je=0;
    for (i = 0; i < nu; i++)
      {
	if (internal[i]) vecfi(ii) = vecf(i);
	
	ji = je = 0;
	for (j = 0; j < nu; j++)
	  {
	    if (internal[i] && internal[j])
	      matci(ii,ji) = matc(i,j);
	    else if (internal[i] && !internal[j])
	      matcie(ii,je) = matc(i,j);
	    else if (!internal[i] && !internal[j])
	      matce(ie,je) = matc(i,j);

	    if (internal[j]) ji++; else je++;
	  }
	if (internal[i]) ii++; else ie++;
      }

    FlatMatrix<> invmatci (ni, lh);
  
    CalcInverse (matci, invmatci);

    (*testout) << "matci = " << endl << matci << endl;
    (*testout) << "matcie = " << endl << matcie << endl;

    (*testout) << "uext = " << uext << ", vecfi = " << vecfi << endl;

    vecfi -= matcie * uext;
    uint.AssignMemory (ni, lh);
    uint = invmatci * vecfi;

    (*testout) << "uint = " << uint << endl;

    ii=0, ie=0;
    for (i = 0; i < nu; i++)
      {
	if (internal[i])
	  uall(i) = uint(ii);
	else
	  uall(i) = uext(ie);

	if (internal[i]) ii++; else ie++;
      }

    vecfsigma -= Trans (matb) * uall;
    sigma.AssignMemory (nsigma, lh);
    sigma = invmata * vecfsigma;


    //    (*testout) << "uint = " << uint << endl;
    //    (*testout) << "sigma = " << sigma << endl;
    /*
      matcie.MultAdd (1, uext, vecfi);
      uint.SetSize(ni);
      invmatci.Mult (vecfi, uint);

      ii=0, ie=0;
      for (i = 1; i <= nu; i++)
      {
      if (internal.Test(i)) ii++; else ie++;
      
      if (internal.Test(i))
      uall.Elem(i) = -uint.Get(ii);
      else
      uall.Elem(i) = uext.Get(ie);
      }
  
      matb.MultTransAdd (-1, uall, vecfsigma);
      sigma.SetSize (nsigma);
      invmata.Mult (vecfsigma, sigma);
    */
  }


  EquilibriumForceIntegrator :: 
  EquilibriumForceIntegrator (EquilibriumIntegrator * agequ)
    : LinearFormIntegrator(), gequ(agequ)
  {
    ;
  }

  EquilibriumForceIntegrator :: 
  ~EquilibriumForceIntegrator ()
  {
    delete gequ;
  }

  void EquilibriumForceIntegrator :: 
  AssembleElementVector (const FiniteElement & fel, 
			 const ElementTransformation & eltrans, 
			 FlatVector<double> & elvec,
			 LocalHeap & lh) const
  {
    cout << "gen equ for :: Assel not impl" << endl;
    /*
      int i, j, k;

      FlatMatrix<> mata (lh);
      FlatMatrix<> matb (lh);
      FlatMatrix<> matc (lh);

      FlatVector<> vecfsigma, vecf;

      gequ->ComputeMatrices (eltrans, mata, matb, matc);
      gequ->ComputeVectors (eltrans, vecfsigma, vecf);

      //  (*testout) << "vecfsigma = " << vecfsigma << ", vecf = " << vecf << endl;
  
      int nsigma = mata.Height();
      int nu = matb.Height();


      BitArray internal(nu);
      gequ->GetInternalDofs(fel, internal);
  
      // eliminate primal variables:

      FlatMatrix<> invmata (lh, nsigma);
      FlatMatrix<> hmatb (lh, nu, nsigma);
      FlatMatrix<> schur (lh, nu);
  
      mata.SetSymmetric(1);
      CalcInverse (mata, invmata);
      invmata.SetSymmetric(0);
  
      Mult (matb, invmata, hmatb);
      CalcABt (hmatb, matb, schur);

      matc.Add (1, schur);
  
      // eliminate internal dual variables:

      int ni = 0, ne;
      for (i = 1; i <= nu; i++)
      if (internal.Test(i))
      ni++;
      ne = nu - ni;

      FlatMatrix<> matci (lh, ni);
      FlatMatrix<> matcie (lh, ni, ne);
      FlatMatrix<> matce (lh, ne);
      FlatVector<> vecfi (lh, ni), vecfe(lh, ne), hv(lh, ni);

      int ii=0, ie=0, ji=0, je=0;
      for (i = 1; i <= nu; i++)
      {
      if (internal.Test(i)) ii++; else ie++;
      
      if (internal.Test(i))
      vecfi.Elem(ii) = vecf.Get(i);
      else
      vecfe.Elem(ie) = vecf.Get(i);


      ji = je = 0;
      for (j = 1; j <= nu; j++)
      {
      if (internal.Test(j)) ji++; else je++;

      if (internal.Test(i) && internal.Test(j))
      matci.Elem(ii,ji) = matc.Get(i,j);
      else if (internal.Test(i) && !internal.Test(j))
      matcie.Elem(ii,je) = matc.Get(i,j);
      else if (!internal.Test(i) && !internal.Test(j))
      matce.Elem(ie,je) = matc.Get(i,j);
      }
      }

      FlatMatrix<> invmatci (lh, ni);
  
      matci.SetSymmetric(1);
      CalcInverse (matci, invmatci);
      invmatci.SetSymmetric(0);

  
      invmatci.Mult (vecfi, hv);

      matcie.MultTransAdd (-1, hv, vecfe);


      elvec.SetSize(ne);
      elvec.Set (1, vecfe);
      //  (*testout) << "elvec (new) = " << elvec << endl;
      */
  }








  
  template <int DIM>
  ElasticityEquilibriumIntegrator<DIM> ::   
  ElasticityEquilibriumIntegrator (CoefficientFunction * acoeffe,
				   CoefficientFunction * acoeffnu,
				   CoefficientFunction * afx,
				   CoefficientFunction * afy,
				   CoefficientFunction * afz)
    : EquilibriumIntegrator(), coeffe(acoeffe), coeffnu(acoeffnu), 
      fx(afx), fy(afy), fz(afz)
  {
    ;
  }

  template <int DIM>
  ElasticityEquilibriumIntegrator<DIM> ::  ~ElasticityEquilibriumIntegrator ()
  {
    ;
  }

  template <int DIM>
  void ElasticityEquilibriumIntegrator<DIM> :: 
  GetInternalDofs (const FiniteElement & bfel,
		   BitArray & internal) const
  {
    const TFEL & fel = dynamic_cast<const TFEL&> (bfel);

    const HDivFiniteElement<DIM> & felsigma = fel.GetFE0();
    const NodalFiniteElement & felu    = fel.GetFE1();
    const NodalFiniteElement & felrot  = fel.GetFE2();

    int dim = felu.SpatialDim();
  
    int nrot1 = felrot.GetNDof();
    int nrot = (dim == 2) ? nrot1 : 3*nrot1;
    int nu1 = felu.GetNDof();
    int nu = dim*nu1;

    internal.Clear();
    for (int i = 0; i < nu+nrot; i++)
      internal.Set (i);
  }



  template <int DIM>
  void ElasticityEquilibriumIntegrator<DIM> :: 
  ComputeMatrices (const FiniteElement & bfel, 
		   const ElementTransformation & eltrans, 
		   FlatMatrix<> & mata,
		   FlatMatrix<> & matb,
		   FlatMatrix<> & matc,
		   LocalHeap & lh) const
  {
    const TFEL & fel = dynamic_cast<const TFEL&> (bfel);

    const HDivFiniteElement<DIM> & felsigma = fel.GetFE0();
    const NodalFiniteElement & felu    = fel.GetFE1();
    const NodalFiniteElement & felrot  = fel.GetFE2();


    int dim = DIM;
    
    int i, j, k, l, ii, jj;
    double det, fac;
    
    int nsigma1 = felsigma.GetNDof();
    int nsigma = dim*nsigma1;
    int nrot1 = felrot.GetNDof();
    int nrot = (dim == 2) ? nrot1 : 3*nrot1;
    int nu1 = felu.GetNDof();
    int nu = dim*nu1;
    int nuf;

    
    switch (eltrans.GetElementType())
      {
      case ET_TRIG:
	nuf = 2*6; break;
      case ET_TET:
	nuf = 3*12; break;
      case ET_PRISM:
	nuf = 3*18; break;
      }
    
    mata.AssignMemory (nsigma, nsigma, lh);
    matb.AssignMemory (nu+nrot+nuf, nsigma, lh);
    matc.AssignMemory (nu+nrot+nuf, nu+nrot+nuf, lh);
    
    FlatMatrix<> matb1(nu, nsigma, lh);
    FlatMatrix<> matb2(nrot, nsigma, lh);
    FlatMatrix<> matb3(nuf, nsigma, lh);

    
    FlatMatrix<> bmatsigma1 (dim, nsigma1, lh);
    FlatMatrix<> bmatsigma (dim*dim, nsigma, lh);
    FlatMatrix<> bmatsigmans ((dim==2) ? 1 : 3, nsigma, lh);
    FlatMatrix<> bmatsigmatrace (1, nsigma, lh);
    FlatMatrix<> bmatdivsigma1 (1, nsigma1, lh);
    FlatMatrix<> bmatdivsigma (dim, nsigma, lh);
    FlatMatrix<> bmatu (dim, nu, lh);
    FlatMatrix<> bmatu1 (1, nu1, lh);
    FlatMatrix<> bmatrot1 (1, nrot1, lh);
    FlatMatrix<> bmatrot (3, nrot, lh);
    FlatMatrix<> bmatgradu (dim, nu1, lh);
    FlatMatrix<> bmatepsu (dim*dim, nu+nrot+nuf, lh);
    FlatMatrix<> dbmatepsu (dim*dim, nu+nrot+nuf, lh);
    FlatMatrix<> bmatepsu2 (dim*dim, nu, lh);
    FlatMatrix<> bmatasgradugamma ((dim==2) ? 1 : 3, nu+nrot+nuf, lh);
    

    FlatMatrix<> matb1scal (nu1, nsigma1, lh);
    FlatMatrix<> matascal (nsigma1, lh);
    
    FlatMatrix<> dbmatsigma (dim*dim, nsigma, lh);
    FlatMatrix<> dmatsigma (dim*dim, lh);
    FlatMatrix<> dmateps (dim*dim, lh);


    int order = 2 * felsigma.Order();
    const IntegrationRule & ir = 
      GetIntegrationRules().SelectIntegrationRule (felsigma.ElementType(), order);


    mata = 0;
    matb = 0;
    matc = 0;

    matb1 = 0;
    matb2 = 0;
    matb3 = 0;

    matb1scal = 0;
    matascal = 0;


    void * heapp = lh.GetPointer();
    
    // alpha = 0.2, beta = 0.3
    double stabalpha = 0.5;  // 0..non stab, --->1 max stab
    double stabbeta = 1;

    for (i = 0; i < ir.GetNIP(); i++)
      {
	lh.CleanUp (heapp);
      
	SpecificIntegrationPoint<DIM,DIM> sip (ir[i], eltrans, lh);
	const IntegrationPoint & ip = sip.IP();

	det = fabs (sip.GetJacobiDet());
	fac = det * ip.Weight();


	// Piola for sigma, dim components
	bmatsigma1 = (1/det) * (sip.GetJacobian() * Trans (felsigma.GetShape(sip.IP(), lh)));
	
	// natural for rot:
	bmatrot1 = Trans (felrot.GetShape(sip.IP(), lh));
	
	if (dim == 3)
	  {
	    bmatrot = 0;
	    for (k = 0; k < 3; k++)
	      for (j = 0; j < nrot1; j++)
		bmatrot(k, dim*j+k) = bmatrot1 (0, j);
	  }

	bmatsigma = 0;
	for (j = 0; j < dim; j++)
	  for (k = 0; k < nsigma1; k++)
	    for (l = 0; l < dim; l++)
	      bmatsigma(dim*j + l, dim*k + l) = bmatsigma1 (j, k);
	
	for (j = 0; j < dim; j++)
	  for (k = 0; k < nsigma1; k++)
	    bmatsigmatrace(0, dim*k+j) = bmatsigma1(j,k);


	if (dim == 2)
	  {
	    for (k = 0; k < nsigma; k++)
	      bmatsigmans(0, k) = bmatsigma(1, k) - bmatsigma(2, k);
	  }
	else
	  {
	    for (k = 0; k < nsigma; k++)
	      {
		bmatsigmans(0, k) = bmatsigma(1, k) - bmatsigma(3, k);
		bmatsigmans(1, k) = bmatsigma(2, k) - bmatsigma(6, k);
		bmatsigmans(2, k) = bmatsigma(5, k) - bmatsigma(7, k);
	      }
	  }
	
	bmatdivsigma1 = (1/det) * Trans (felsigma.GetDivShape (sip.IP(), lh));

	bmatdivsigma = 0;
	for (k = 0; k < nsigma1; k++)
	  for (l = 0; l < dim; l++)
	    bmatdivsigma(l, dim*k + l) = bmatdivsigma1 (0, k);
	
	
	bmatu1 = Trans (felu.GetShape (ip, lh));

	bmatu = 0;
	for (k = 0; k < dim; k++)
	  for (j = 0; j < nu1; j++)
	    bmatu(k, dim*j+k) = bmatu1(0,j);


	
	// compliance:
	double cnu = Evaluate (*coeffnu, sip);
	double ce = Evaluate (*coeffe, sip);
	
	/*
	double h;
	if (DIM == 2)
	  h = sqrt (sqr (sip.GetJacobian()(0,0)) +
		    sqr (sip.GetJacobian()(0,1)));
	else
	  h = sqrt (sqr (sip.GetJacobian()(0,0)) +
		    sqr (sip.GetJacobian()(0,1)) +
		    sqr (sip.GetJacobian()(0,2)));
	*/	  
	
	matascal += ((1-stabalpha) * (1+cnu)/ce * fac) *
	  (Trans (bmatsigma1) * bmatsigma1);

	mata +=  (-(1-stabalpha) * cnu/ce * fac) *
	  Trans (bmatsigmatrace) * bmatsigmatrace;



	dmatsigma = 0;
	for (k = 0; k < dim*dim; k++)
	  dmatsigma (k,k) = (1+cnu)/ce;
	for (j = 0; j < dim*dim; j+=(dim+1))
	  for (k = 0; k < dim*dim; k+=(dim+1))
	    dmatsigma(j,k) -= cnu/ce;
	
	
      
	if (dim == 2)
	  matb2 += fac * Trans (bmatrot1) * bmatsigmans;
	else
	  matb2 += fac * Trans (bmatrot) * bmatsigmans;


	matb1scal += fac * (Trans (bmatu1) * bmatdivsigma1);


	bmatgradu = Trans (sip.GetJacobianInverse ()) * Trans (felu.GetDShape(ip, lh));


	// stab term  - beta || as (C^-1 sigma - \nabla u + gamma) ||
	bmatasgradugamma = 0;
	if (dim == 2)
	  {
	    for (k = 0; k < nu1; k++)
	      {
		bmatasgradugamma(0, 2*k  ) =  0.5 * bmatgradu(1, k);
		bmatasgradugamma(0, 2*k+1) = -0.5 * bmatgradu(0, k);
	      }
	    for (k = 0; k < nrot1; k++)
	      bmatasgradugamma(0, nu+k) = bmatrot1(0, k);
	  }
	else
	  {
	    for (k = 0; k < nu1; k++)
	      {
		bmatasgradugamma (0, dim*k  ) =  0.5 * bmatgradu(1, k);
		bmatasgradugamma (0, dim*k+1) = -0.5 * bmatgradu(0, k);
		
		bmatasgradugamma (1, dim*k  ) =  0.5 * bmatgradu(2, k);
		bmatasgradugamma (1, dim*k+2) = -0.5 * bmatgradu(0, k);
		
		bmatasgradugamma (2, dim*k+1) =  0.5 * bmatgradu(2, k);
		bmatasgradugamma (2, dim*k+2) = -0.5 * bmatgradu(1, k);
	      }
	    for (k = 0; k < nrot1; k++)
	      {
		bmatasgradugamma(0, nu+k*3  ) = bmatrot1(0,k);
		bmatasgradugamma(1, nu+k*3+1) = bmatrot1(0,k);
		bmatasgradugamma(2, nu+k*3+2) = bmatrot1(0,k);
	      }
	    
	    
#ifdef NONE
	    if (felsigma->ElementType() == ET_PRISM)
	      {
		double h = 
		  sqrt (sqr (sip.GetJacobian().Get(1,1)) +
			sqr (sip.GetJacobian().Get(1,2)));
		double t = fabs (sip.GetJacobian().Get(3,3));
		for (k = 1; k <= bmatasgradugamma.Width(); k++)
		  {
		    //	      bmatasgradugamma.Elem(1,k) *= 200;
		    bmatasgradugamma.Elem(2,k) *= t/(h+t);
		    bmatasgradugamma.Elem(3,k) *= t/(h+t);
		  }
		
		for (k = 1; k <= bmatsigmans.Width(); k++)
		  {
		    bmatsigmans.Elem(2,k) *= t/(h+t);
		    bmatsigmans.Elem(3,k) *= t/(h+t);
		  }
	      }
#endif
	  }

	/*
	mata -= (stabbeta * fac) * Trans (bmatsigmans) * bmatsigmans;
	matb -= (stabbeta * fac) * Trans (bmatasgradugamma) * bmatsigmans;
	*/
	matc += (stabbeta * fac) * Trans (bmatasgradugamma) * bmatasgradugamma;




	// stab term -alpha || C^-1 sigma - eps(u) ||_C^2
	bmatepsu = 0;
	for (j = 0; j < nu1; j++)
	  for (k = 0; k < dim; k++)
	    for (l = 0; l < dim; l++)
	      {
		int p1 = k*dim+l;
		int p2 = l*dim+k;
		bmatepsu(p1, dim*j+k) += 0.5 * bmatgradu(l,j);
		bmatepsu(p1, dim*j+l) += 0.5 * bmatgradu(k,j);
	      }

	// correct for nu = 0, e = 1 ????
	
	CalcInverse (dmatsigma, dmateps);
	
	dbmatepsu = dmateps * bmatepsu;
	matc += (stabalpha * fac) * (Trans (bmatepsu) * dbmatepsu);

	for (k = 0; k < nu; k++)
	  for (j = 0; j < dim*dim; j++)
	    bmatepsu2(j,k) = bmatepsu(j,k);

	matb1 += (stabalpha * fac) *  (Trans (bmatepsu2) * bmatsigma);
      }





    for (j = 0; j < dim; j++)
      for (k = 0; k < nsigma1; k++)
	for (l = 0; l < nsigma1; l++)
	  mata(dim*k+j, dim*l+j) += matascal(k,l);
    
    for (j = 0; j < dim; j++)
      for (k = 0; k < nu1; k++)
	for (l = 0; l < nsigma1; l++)
	  matb1(dim*k+j, dim*l+j) += matb1scal(k,l);

    
    matb3 = 0;
    for (i = 0; i < nuf; i++)
      matb3(i,i) = 1;
    
    /*
    Matrix<> invmatascal(6);
    CalcInverse (matascal, invmatascal);
    */
    
    for (i = 0; i < nu; i++)
      for (j = 0; j < nsigma; j++)
	matb(i, j) += matb1(i,j);
    for (i = 0; i < nrot; i++)
      for (j = 0; j < nsigma; j++)
	matb(i+nu, j) += matb2(i,j);
    for (i = 0; i < nuf; i++)
      for (j = 0; j < nsigma; j++)
	matb(i+nu+nrot, j) += matb3(i,j);
  }


  template<int DIM>
  void ElasticityEquilibriumIntegrator<DIM> :: 
  ComputeVectors (const FiniteElement & fel, 
		  const ElementTransformation & eltrans, 
		  FlatVector<> & vecfsigma,
		  FlatVector<> & vecf,
		  LocalHeap & lh) const
  {
    cout << "elast equil, comp vecs" << endl;
    /*
      int dim = eltrans.GetElement().SpatialDim();

      HDivFiniteElement * felsigma;
      FiniteElement * felu, *felrot;

      GetElement (eltrans.GetElement().ElementType(),
      felsigma, felu, felrot);

      int i, j, k, l, ii, jj;
      double det;

      int nu1 = felu->GetNDof();
      int nu = dim*nu1;
      int nsigma1 = felsigma -> GetNDof();
      int nsigma = dim*nsigma1;
      int nrot1 = felrot->GetNDof();
      int nrot = (dim == 2) ? nrot1 : 3*nrot1;
      int nuf;

      switch (eltrans.GetElement().ElementType())
      {
      case ET_TRIG:
      nuf = 2*6; break;
      case ET_TET:
      nuf = 3*12; break;
      case ET_PRISM:
      nuf = 3*18; break;
      }



      vecf.SetSize(nu+nrot+nuf);
      vecfsigma.SetSize(nsigma);
      vecf.SetScalar (0);
      vecfsigma.SetScalar (0);

      FlatVector<> vecfu (nu);

      FlatMatrix<> bmatu (lh, dim, nu);

      int order = 2;
      const IntegrationRule & ir = 
      GetIntegrationRules().SelectIntegrationRule (eltrans.GetElement().ElementType(), order);

      vecfu.SetScalar (0);

      void * heapp;
      if (lh) heapp = lh->GetPointer();

      for (i = 1; i <= ir.GetNIP(); i++)
      {
      if (lh) lh -> CleanUp (heapp);

      const IntegrationPoint & ip = ir.GetIP(i);
      
      SpecificIntegrationPoint sip (ir[i], eltrans, lh);
      det = fabs (sip.GetJacobiDet());

      const FlatVector<> & vecu = felu->GetShape (ip);
      bmatu.SetScalar(0);
      for (k = 1; k <= dim; k++)
      for (j = 1; j <= nu1; j++)
      bmatu.Elem(k, dim*(j-1)+k) = vecu.Get(j);
      
      
      FlatVector<> valf(dim);
      valf.Elem(1) = fx->Evaluate (sip);
      valf.Elem(2) = fy->Evaluate (sip);
      if (dim == 3)
      {
      valf.Elem(3) = fz->Evaluate (sip);
      }

      FlatVector<> partvecf(vecfu.Size());
      bmatu.MultTrans (valf, partvecf);
      vecfu.Add (det * ip.Weight(), partvecf);
      }

      vecf.AddPart (1, 1, vecfu);
    */
  }


  template <int DIM>
  void ElasticityEquilibriumIntegrator<DIM> :: 
  ComputePointValues (const FiniteElement & bfel,
		      const ElementTransformation & eltrans, 
		      const IntegrationPoint & ip,
		      const FlatVector<> & sigma, 
		      const FlatVector<> & uint,
		      FlatVector<> & psigma, 
		      FlatVector<> & pu, 
		      LocalHeap & lh) const
  {
    FlatVector<> prot;
    int dim = DIM;

    const TFEL & fel = dynamic_cast<const TFEL&> (bfel);

    const HDivFiniteElement<DIM> & felsigma = fel.GetFE0();
    const NodalFiniteElement & felu    = fel.GetFE1();
    const NodalFiniteElement & felrot  = fel.GetFE2();
  
    int i, j, k, l, ii, jj;
    double det;
    
    int nsigma1 = felsigma.GetNDof();
    int nsigma = dim*nsigma1;
    int nu1 = felu.GetNDof();
    int nu = dim*nu1;
    int nrot1 = felrot.GetNDof();
    int nrot = (dim == 2) ? nrot1 : 3*nrot1;
    
    psigma.AssignMemory (dim*dim, lh);
    pu.AssignMemory (dim, lh);
    prot.AssignMemory ((dim==2)? 1 : 3, lh);
    psigma = 0;
    pu = 0;
    prot = 0;

    SpecificIntegrationPoint<DIM,DIM> sip(ip, eltrans, lh);
    
    FlatMatrix<> bmatsigma1 (dim, nsigma1, lh);
    FlatMatrix<> bmatsigma (dim*dim, nsigma, lh);
    FlatMatrix<> bmatdivsigma1 (1, nsigma1, lh);
    FlatMatrix<> bmatdivsigma (dim, nsigma, lh);
    FlatMatrix<> bmatu (dim, nu+nrot, lh);
    FlatMatrix<> bmatrot ((dim==2)? 1 : 3, nu+nrot, lh);
    
    void * heapp = lh.GetPointer();
    
    det = fabs (sip.GetJacobiDet());
    
    // Piola for sigma, dim components
    bmatsigma1 = (1/det) * (sip.GetJacobian() * Trans (felsigma.GetShape(sip.IP(), lh)));    

    bmatsigma = 0;
    for (j = 0; j < dim; j++)
      for (k = 0; k < nsigma1; k++)
	for (l = 0; l < dim; l++)
	  bmatsigma(dim*j+l, dim*k+l) = bmatsigma1(j, k);


    bmatdivsigma1 = (1/det) * Trans (felsigma.GetDivShape (sip.IP(), lh));
    bmatdivsigma = 0;
    for (j = 0; j < dim; j++)
      for (k = 0; k < nsigma1; k++)
	bmatdivsigma(j, dim*k+j) = bmatdivsigma1(0, k);



    const FlatVector<> & vecu = felu.GetShape (sip.IP(), lh);
    bmatu = 0;
    for (k = 0; k < dim; k++)
      for (j = 0; j < nu1; j++)
	bmatu(k, dim*j+k) = vecu(j);

    

    const FlatVector<> & vecrot = felrot.GetShape (sip.IP(), lh);
    bmatrot = 0;
    for (k = 0; k < 1; k++)
      for (j = 0; j < nrot1; j++)
	bmatrot(k, nu+1*j+k) = vecrot(j);


    psigma = bmatsigma * sigma;
    pu = bmatu * uint;
    prot = bmatrot * uint;
  }



  template<int DIM>
  void ElasticityEquilibriumIntegrator<DIM> :: 
  GetElement (ELEMENT_TYPE eltype,
	      HDivFiniteElement<DIM> *& felsigma,
	      FiniteElement *& felu,
	      FiniteElement *& felgamma) const
  {
    cout << "Get El not impl" << endl;
    /*
      static FE_RTTrig0plus rttrig0p;
      static FE_BDMTrig1 bdmtrig1;
      static FE_BDFMTrig2 bdfmtrig2;

      static FE_BDMTet1 bdmtet1;
      static FE_BDFMTet2 bdfmtet2;
      static FE_BDMPrism1p bdmprism1;
      static FE_BDFMPrism2 bdfmprism2;

      static FE_Trig0 trig0;
      static FE_Trig1 trig1;
      static FE_Trig2 trig2;

      static FE_Prism1 prism1;
      static FE_Prism2 prism2;
      static FE_Prism2aniso prism2a;
      static FE_Prism3aniso prism3a;
      static FE_PrismGamma prismgamma;

      static FE_Tet1 tet1;
      static FE_Tet2 tet2;


      switch (eltype)
      {
      case ET_TRIG:
      // felsigma = &bdfmtrig2;
      felsigma = &bdmtrig1;
      felu = &trig2;
      felgamma = &trig1;
      break;
      case ET_TET:
      //      felsigma = &bdfmtet2;
      felsigma = &bdmtet1;
      felu = &tet2;
      felgamma = &tet1;
      break;
      case ET_PRISM:
      //    felsigma = &bdfmprism2;
      felsigma = &bdmprism1;
      felu = &prism3a;
      felgamma = &prism2a;
      break;
      default:
      cout << "element " << eltype << " not implemented" << endl;
      }
    */ 
  }





  template class ElasticityEquilibriumIntegrator<2>;
  template class ElasticityEquilibriumIntegrator<3>;





  // ************************************ ElasticityEquilibriumIntegratorStab ************************ */



  ElasticityEquilibriumIntegratorStab ::   
  ElasticityEquilibriumIntegratorStab (CoefficientFunction * acoeffe,
				       CoefficientFunction * acoeffnu,
				       CoefficientFunction * afx,
				       CoefficientFunction * afy,
				       CoefficientFunction * afz)
    : EquilibriumIntegrator(), coeffe(acoeffe), coeffnu(acoeffnu), fx(afx), fy(afy), fz(afz)
  {
    ;
  }

  ElasticityEquilibriumIntegratorStab ::  ~ElasticityEquilibriumIntegratorStab ()
  {
    ;
  }

  void ElasticityEquilibriumIntegratorStab :: 
  GetInternalDofs (const FiniteElement & fel,
		   BitArray & internal) const
  {
    cout << "last equ, int dofs not impl" << endl;
    /*
      int dim = eltrans.GetElement().SpatialDim();

      HDivFiniteElement * felsigma;
      FiniteElement * felu, *felrot;

      GetElement (eltrans.GetElement().ElementType(),
      felsigma, felu, felrot);
  
      int i, j, k, l, ii, jj;
      double det;

      int nu1 = felu->GetNDof();
      int nu = dim*nu1;

      internal.Clear();
      for (i = 1; i <= nu; i++)
      internal.Set (i);
    */
  }

  void ElasticityEquilibriumIntegratorStab :: 
  ComputeMatrices (const FiniteElement & fel, 
		   const ElementTransformation & eltrans, 
		   FlatMatrix<> & mata,
		   FlatMatrix<> & matb,
		   FlatMatrix<> & matc,
		   LocalHeap & lh) const
  {
    cout << "elast, comp mat not impl" << endl;
    /*
      int dim = eltrans.GetElement().SpatialDim();

      HDivFiniteElement * felsigma;
      FiniteElement * felu, *felrot;

      GetElement (eltrans.GetElement().ElementType(),
      felsigma, felu, felrot);
  
      int i, j, k, l, ii, jj;
      double det;

      int dimns = (dim == 2) ? 1 : 3;
      int nsigma1 = felsigma->GetNDof();
      int nsigma = dim*nsigma1;
      int nu1 = felu->GetNDof();
      int nu = dim*nu1;
      int nrot1 = felrot->GetNDof();
      int nrot = dimns*nrot1;
      int nuf;

      switch (eltrans.GetElement().ElementType())
      {
      case ET_TRIG:
      nuf = 2*6; break;
      case ET_TET:
      nuf = 3*12; break;
      case ET_PRISM:
      nuf = 3*18; break;
      }
  
      mata.SetSize(nsigma);
      matb.SetSize (nu+nuf, nsigma);
      matc.SetSize (nu+nuf);


      FlatMatrix<> matb1(lh, nu, nsigma);
      FlatMatrix<> matb2stab(lh, nrot, nsigma);
      FlatMatrix<> matb3(lh, nuf, nsigma);
      FlatMatrix<> matc1(lh, nu);  
      FlatMatrix<> matc2stab(lh, nrot);  

      FlatMatrix<> bmatsigma1 (lh, dim, nsigma1);
      FlatMatrix<> bmatsigma (lh, dim*dim, nsigma);
      FlatMatrix<> bmatsigmaas (lh, dimns, nsigma);
      FlatMatrix<> bmatsigmaasstab (lh, dimns, nsigma);
      FlatMatrix<> bmatsigmatrace (lh, 1, nsigma);
      FlatMatrix<> bmatdivsigma1 (lh, 1, nsigma1);
      FlatMatrix<> bmatdivsigma (lh, dim, nsigma);
      FlatMatrix<> bmatu (lh, dim, nu);
      FlatMatrix<> bmatu1 (lh, 1, nu1);
      FlatMatrix<> bmatgradu1 (lh, dim, nu1);
      FlatMatrix<> bmatgradu (lh, dim*dim, nu);
      FlatMatrix<> bmatepsu (lh, dim*dim, nu);
      FlatMatrix<> dbmatepsu (lh, dim*dim, nu);
      FlatMatrix<> bmatgraduas (lh, dimns, nu);
      FlatMatrix<> bmatrot (lh, dimns, nrot);
      FlatMatrix<> dbmatrot (lh, dimns, nrot);


  
      FlatMatrix<> dbmatsigma (lh, dim*dim, nsigma);
      FlatMatrix<> dmatsigma (lh, dim*dim);
      FlatMatrix<> dmateps (lh, dim*dim);
      FlatMatrix<> dmatrot (lh, dimns);

      FlatMatrix<> partmata (lh, nsigma);
      FlatMatrix<> partmatc (lh, nu+nuf);
      FlatMatrix<> partmatb (lh, nu+nuf, nsigma);
      FlatMatrix<> partmatb1 (lh, nu, nsigma);
      FlatMatrix<> partmatb2stab (lh, nrot, nsigma);
      FlatMatrix<> partmatc1 (lh, nu);
      FlatMatrix<> partmatc2stab (lh, nrot);


      int order = 2; // 2 * (felsigma->Order());
      if (felsigma->ElementType() == ET_PRISM)
      order = 6;

      const IntegrationRule & ir = 
      GetIntegrationRules().SelectIntegrationRule (felsigma->ElementType(), order);

      mata.SetScalar (0);
      matb.SetScalar (0);
      matc.SetScalar (0);

      matb1.SetScalar (0);
      matb3.SetScalar (0);
      matc1.SetScalar (0);
      matb2stab.SetScalar (0);
      matc2stab.SetScalar (0);

      void * heapp;
      if (lh) heapp = lh->GetPointer();

      double stabalpha = 0.5;  // 0..non stab, --->1 max stab
      double stabbeta = 0.2;

      for (i = 1; i <= ir.GetNIP(); i++)
      {
      if (lh) lh->CleanUp (heapp);

      const IntegrationPoint & ip = ir.GetIP(i);
      
      SpecificIntegrationPoint sip (ir[i], eltrans, lh);
      det = fabs (sip.GetJacobiDet());

      // Piola for sigma, dim components
      const FlatMatrix<> & sigref = felsigma->GetShapes (sip.IP());
      Mult (sip.GetJacobian(), sigref, bmatsigma1);
      bmatsigma1 *= 1/det;

      bmatsigma.SetScalar(0);
      for (j = 1; j <= dim; j++)
      for (k = 1; k <= nsigma1; k++)
      for (l = 1; l <= dim; l++)
      bmatsigma.Elem(dim*(j-1) + l, dim*(k-1) + l)
      = bmatsigma1.Get (j, k);

      for (j = 1; j <= dim; j++)
      for (k = 1; k <= nsigma1; k++)
      bmatsigmatrace.Elem(1, dim*(k-1)+j) = bmatsigma1.Get(j,k);

      if (dim == 2)
      {
      for (k = 1; k <= nsigma; k++)
      bmatsigmaas.Elem(1, k) = bmatsigma.Get(2, k) - bmatsigma.Get(3, k);
      }
      else
      {
      for (k = 1; k <= nsigma; k++)
      {
      bmatsigmaas.Elem(1, k) = bmatsigma.Get(2, k) - bmatsigma.Get(4, k);
      bmatsigmaas.Elem(2, k) = bmatsigma.Get(3, k) - bmatsigma.Get(7, k);
      bmatsigmaas.Elem(3, k) = bmatsigma.Get(6, k) - bmatsigma.Get(8, k);
      }
      }

      const FlatVector<> & divsigref = felsigma->GetDivShape (sip.IP());
      for (k = 1; k <= nsigma1; k++)
      bmatdivsigma1.Elem(1,k) = divsigref.Get(k);
      bmatdivsigma1 *= 1/fabs (sip.GetJacobiDet());

      bmatdivsigma.SetScalar(0);
      for (k = 1; k <= nsigma1; k++)
      for (l = 1; l <= dim; l++)
      bmatdivsigma.Elem(l, dim*(k-1) + l) = bmatdivsigma1.Get (1, k);


      const FlatVector<> & vecu = felu->GetShape (ip);
      bmatu.SetScalar(0);
      for (k = 1; k <= dim; k++)
      for (j = 1; j <= nu1; j++)
      bmatu.Elem(k, dim*(j-1)+k) = vecu.Get(j);
      for (j = 1; j <= nu1; j++)
      bmatu1.Elem(1, j) = vecu.Get(j);

      // compliance:
      double cnu = coeffnu->Evaluate (sip);
      double ce = coeffe->Evaluate (sip);
      cnu = 0.2; // .2;


      dmatsigma.SetSize (dim*dim);
      dmatsigma.SetScalar (0);
      for (k = 1; k <= dim*dim; k++)
      dmatsigma.Elem (k,k) = (1+cnu)/ce;
      for (j = 1; j <= dim*dim; j+=(dim+1))
      for (k = 1; k <= dim*dim; k+=(dim+1))
      dmatsigma.Elem(j,k) -= cnu/ce;


      Mult (dmatsigma, bmatsigma, dbmatsigma);
      CalcAtB (bmatsigma, dbmatsigma, partmata);
      
      mata.Add ((1-stabalpha) * det * ip.Weight(), partmata);

      double h = 
      sqrt (sqr (sip.GetJacobian().Get(1,1)) +
      sqr (sip.GetJacobian().Get(1,2)));



      if (felsigma->ElementType() == ET_PRISM)
      {
      double h = 
      sqrt (sqr (sip.GetJacobian().Get(1,1)) +
      sqr (sip.GetJacobian().Get(1,2)));
      double t = fabs (sip.GetJacobian().Get(3,3));


      const FlatVector<> & vecrot = felrot->GetShape (ip);
      bmatrot.SetScalar(0);
      for (k = 1; k <= dimns; k++)
      for (j = 1; j <= nrot1; j++)
      bmatrot.Elem(k, dimns*(j-1)+k) = vecrot.Get(j);


      dmatrot.SetScalar (0);
      dmatrot.Elem(1,1) = 1;
      dmatrot.Elem(2,2) = 
      dmatrot.Elem(3,3) = t/(h+t);


      Mult (dmatrot, bmatrot, dbmatrot);
      CalcAtB (dbmatrot, bmatrot, partmatc2stab);
      matc2stab.Add (det * ip.Weight(), partmatc2stab);      

      CalcAtB (bmatrot, bmatsigmaas, partmatb2stab);
      matb2stab.Add (det * ip.Weight(), partmatb2stab);      
      }

      CalcAtA (bmatsigmaas, partmata);
      mata.Add (1 / stabbeta * det * ip.Weight(), partmata);      

      CalcAtA (bmatdivsigma, partmata);
      mata.Add (h*h * det * ip.Weight(), partmata);      

      CalcAtB (sip.GetJacobianInverse (), felu->GetDShape(ip), bmatgradu1);

      bmatgradu.SetScalar(0);
      for (j = 1; j <= dim; j++)
      for (k = 1; k <= nu1; k++)
      for (l = 1; l <= dim; l++)
      bmatgradu.Elem(dim*(j-1) + l, dim*(k-1) + l)
      = bmatgradu1.Get (j, k);


      if (dim == 2)
      {
      for (k = 1; k <= nu; k++)
      bmatgraduas.Elem(1, k) = 0.5 * (bmatgradu.Get(2, k) - bmatgradu.Get(3, k));
      }
      else
      {
      for (k = 1; k <= nu; k++)
      {
      bmatgraduas.Elem(1, k) = 0.5 * (bmatgradu.Get(2, k) - bmatgradu.Get(4, k));
      bmatgraduas.Elem(2, k) = 0.5 * (bmatgradu.Get(3, k) - bmatgradu.Get(7, k));
      bmatgraduas.Elem(3, k) = 0.5 * (bmatgradu.Get(6, k) - bmatgradu.Get(8, k));
      }
      }

      bmatepsu.SetScalar (0);
      for (j = 1; j <= nu; j++)
      for (k = 1; k <= dim; k++)
      for (l = 1; l <= dim; l++)
      {
      int p1 = (k-1)*dim+l;
      int p2 = (l-1)*dim+k;
      bmatepsu.Elem(p1, j) = 0.5 * (bmatgradu.Elem(p1,j) + bmatgradu.Elem(p2,j)); 
      }





      CalcAtB (bmatu, bmatdivsigma, partmatb1);
      matb1.Add (det * ip.Weight(), partmatb1);

      CalcAtB (bmatepsu, bmatsigma, partmatb1);
      matb1.Add (stabalpha * det * ip.Weight(), partmatb1);
      
      CalcAtB (bmatgraduas, bmatsigmaas, partmatb1);
      matb1.Add (det * ip.Weight(), partmatb1);



      // 	  if (felsigma->ElementType() == ET_PRISM)
      // 	    {
      // 	      double h = 
      // 		sqrt (sqr (sip.GetJacobian().Get(1,1)) +
      // 		      sqr (sip.GetJacobian().Get(1,2)));
      // 	      double t = fabs (sip.GetJacobian().Get(3,3));
      // 	      for (k = 1; k <= bmatasgradugamma.Width(); k++)
      // 		{
      // 		  //	      bmatasgradugamma.Elem(1,k) *= 200;
      // 		  bmatasgradugamma.Elem(2,k) *= t/(h+t);
      // 		  bmatasgradugamma.Elem(3,k) *= t/(h+t);
      // 		}

      // 	      for (k = 1; k <= bmatsigmaas.Width(); k++)
      // 		{
      // 		  bmatsigmaas.Elem(2,k) *= t/(h+t);
      // 		  bmatsigmaas.Elem(3,k) *= t/(h+t);
      // 		}
      // 	    }


      // stab term -alpha || C^-1 sigma - eps(u) ||_C^2

      CalcInverse (dmatsigma, dmateps);

      Mult (dmateps, bmatepsu, dbmatepsu);
      CalcAtB (bmatepsu, dbmatepsu, partmatc1);
      matc1.Add (stabalpha * det * ip.Weight(), partmatc1);
      }


      if (felsigma->ElementType() == ET_PRISM && 0)
      {
      CalcInverse (matc2stab, partmatc2stab);
      Mult (partmatc2stab, matb2stab, partmatb2stab);
      CalcAtB (matb2stab, partmatb2stab, partmata);
      mata.Add (1, partmata);
      }




      if (dim == 2)
      {
      matb3.SetSize(12, nsigma);
      matb3.SetScalar (0);
      for (j = 0; j < 2; j++)
      for (i = 1; i <= 3; i++)
      {
      double sign2 = (i==2) ? -0.5 : 0.5;
      matb3.Elem(4*i-3+j, 2*i-1+j ) = sign2;
      matb3.Elem(4*i-3+j, 2*i+5+j ) = -sign2;
      matb3.Elem(4*i-1+j, 2*i-1+j ) = sign2;
      matb3.Elem(4*i-1+j, 2*i+5+j ) = sign2;
      }
      }
      else
      {
      matb3.SetSize(nuf, nsigma);
      matb3.SetScalar (0);
      for (i = 1; i <= nuf; i++)
      matb3.Elem(i,i) = 1;
      }


      for (i = 1; i <= nu; i++)
      for (j = 1; j <= nsigma; j++)
      matb.Elem (i, j) += matb1.Get(i,j);
      for (i = 1; i <= nuf; i++)
      for (j = 1; j <= nsigma; j++)
      matb.Elem (i+nu, j) += matb3.Get(i,j);

      for (i = 1; i <= nu; i++)
      for (j = 1; j <= nu; j++)
      matc.Elem (i, j) += matc1.Get(i,j);
    */

    /*
      FlatVector<> lami(matc.Height());
      FlatMatrix<> vecs(matc.Height());
      vecs.SetScalar (0);
      lami.SetScalar (0);
      matc.EigenSystem (lami, vecs);
      (*testout) << "matc ev = " << endl << lami << endl;
    */

    /*
      for (i = 1; i <= matc.Height(); i++)
      matc.Elem(i,i) += 1e-25;
    */
    /*
      (*testout) << "mata = " << endl << mata << endl;
      (*testout) << "matb = " << endl << matb << endl;
      (*testout) << "matc = " << endl << matc << endl;
    */
  }



  void ElasticityEquilibriumIntegratorStab :: 
  ComputeVectors (const FiniteElement & fel, 
		  const ElementTransformation & eltrans, 
		  FlatVector<> & vecfsigma,
		  FlatVector<> & vecf,
		  LocalHeap & lh) const
  {
    cout << "comp vecs not impl" << endl;
    /*
      int dim = eltrans.GetElement().SpatialDim();

      HDivFiniteElement * felsigma;
      FiniteElement * felu, *felrot;

      GetElement (eltrans.GetElement().ElementType(),
      felsigma, felu, felrot);

      int i, j, k, l, ii, jj;
      double det;

      int nu1 = felu->GetNDof();
      int nu = dim*nu1;
      int nsigma1 = felsigma -> GetNDof();
      int nsigma = dim*nsigma1;
      int nuf;

      switch (eltrans.GetElement().ElementType())
      {
      case ET_TRIG:
      nuf = 2*6; break;
      case ET_TET:
      nuf = 3*12; break;
      case ET_PRISM:
      nuf = 3*18; break;
      }



      vecf.SetSize(nu+nuf);
      vecfsigma.SetSize(nsigma);
      vecf.SetScalar (0);
      vecfsigma.SetScalar (0);

      FlatVector<> vecfu (nu);

      FlatMatrix<> bmatu (lh, dim, nu);

      int order = 2;
      const IntegrationRule & ir = 
      GetIntegrationRules().SelectIntegrationRule (eltrans.GetElement().ElementType(), order);

      vecfu.SetScalar (0);

      void * heapp;
      if (lh) heapp = lh->GetPointer();

      for (i = 1; i <= ir.GetNIP(); i++)
      {
      if (lh) lh -> CleanUp (heapp);

      const IntegrationPoint & ip = ir.GetIP(i);
      
      SpecificIntegrationPoint sip (ir[i], eltrans, lh);
      det = fabs (sip.GetJacobiDet());

      const FlatVector<> & vecu = felu->GetShape (ip);
      bmatu.SetScalar(0);
      for (k = 1; k <= dim; k++)
      for (j = 1; j <= nu1; j++)
      bmatu.Elem(k, dim*(j-1)+k) = vecu.Get(j);
      
      
      FlatVector<> valf(dim);
      valf.Elem(1) = fx->Evaluate (sip);
      valf.Elem(2) = fy->Evaluate (sip);
      if (dim == 3)
      {
      valf.Elem(3) = fz->Evaluate (sip);
      }

      FlatVector<> partvecf(vecfu.Size());
      bmatu.MultTrans (valf, partvecf);
      vecfu.Add (det * ip.Weight(), partvecf);
      }

      vecf.AddPart (1, 1, vecfu);
    */
  }


  void ElasticityEquilibriumIntegratorStab :: 
  ComputePointValues (const FiniteElement & fel,
		      const ElementTransformation & eltrans, 
		      const IntegrationPoint & ip,
		      const FlatVector<> & sigma, const FlatVector<> & uint,
		      FlatVector<> & psigma, FlatVector<> & pu, 
		      LocalHeap & lh) const
  {
    cout << "comp pvals not impl" << endl;
    /*
      int dim = eltrans.GetElement().SpatialDim();

      HDivFiniteElement * felsigma;
      FiniteElement * felrot, *felu;
  
      GetElement (eltrans.GetElement().ElementType(),
      felsigma, felu, felrot);

      int i, j, k, l, ii, jj;
      double det;

      int nsigma1 = felsigma->GetNDof();
      int nsigma = dim*nsigma1;
      int nu1 = felu->GetNDof();
      int nu = dim*nu1;

      psigma.SetSize(dim*dim);
      pu.SetSize(dim);
      psigma.SetScalar (0);
      pu.SetScalar (0);
  
  
      FlatMatrix<> bmatsigma1 (lh, dim, nsigma1);
      FlatMatrix<> bmatsigma (lh, dim*dim, nsigma);
      FlatMatrix<> bmatdivsigma1 (lh, 1, nsigma1);
      FlatMatrix<> bmatdivsigma (lh, dim, nsigma);
      FlatMatrix<> bmatu (lh, dim, nu);

      void * heapp;
      if (lh) heapp = lh->GetPointer();
  
      det = fabs (sip.GetJacobiDet());
  
      // Piola for sigma, dim components
      const FlatMatrix<> & sigref = felsigma->GetShapes (sip.IP());
      Mult (sip.GetJacobian(), sigref, bmatsigma1);
      bmatsigma1 *= 1/fabs(sip.GetJacobiDet());
  
      bmatsigma.SetScalar(0);
      for (j = 1; j <= dim; j++)
      for (k = 1; k <= nsigma1; k++)
      for (l = 1; l <= dim; l++)
      bmatsigma.Elem(dim*(j-1) + l, dim*(k-1) + l)
      = bmatsigma1.Get (j, k);

      const FlatVector<> & divsigref = felsigma->GetDivShape (sip.IP());
      for (k = 1; k <= nsigma1; k++)
      bmatdivsigma1.Elem(1,k) = divsigref.Get(k);
      bmatdivsigma1 *= 1/fabs (sip.GetJacobiDet());
      bmatdivsigma.SetScalar(0);
      for (j = 1; j <= dim; j++)
      for (k = 1; k <= nsigma1; k++)
      bmatdivsigma.Elem(j, dim*(k-1) + j)
      = bmatdivsigma1.Get (1, k);



      const FlatVector<> & vecu = felu->GetShape (sip.IP());
      bmatu.SetScalar(0);
      for (k = 1; k <= dim; k++)
      for (j = 1; j <= nu1; j++)
      bmatu.Elem(k, dim*(j-1)+k) = vecu.Get(j);

      bmatsigma.Mult (sigma, psigma);
      // bmatdivsigma.Mult (sigma, pu);
      bmatu.Mult (uint, pu);

      prot.SetScalar (0);
    */
  }




  void ElasticityEquilibriumIntegratorStab :: 
  GetElement (ELEMENT_TYPE eltype,
	      HDivFiniteElement<2> *& felsigma,
	      FiniteElement *& felu,
	      FiniteElement *& felgamma) const
  {
    cout << "get el not impl" << endl;
    /*
      static FE_RTTrig0plus rttrig0p;
      static FE_BDMTrig1 bdmtrig1;
      static FE_BDFMTrig2 bdfmtrig2;

      static FE_BDMTet1 bdmtet1;
      static FE_BDFMTet2 bdfmtet2;
      static FE_BDMPrism1p bdmprism1;
      static FE_BDFMPrism2 bdfmprism2;

      static FE_Trig0 trig0;
      static FE_Trig1 trig1;
      static FE_Trig2 trig2;

      static FE_Prism0 prism0;
      static FE_Prism1 prism1;
      static FE_Prism2 prism2;
      static FE_Prism2aniso prism2a;
      static FE_Prism3aniso prism3a;
      static FE_PrismGamma prismgamma;
      static FE_PrismGamma2 prismgamma2;

      static FE_Tet1 tet1;
      static FE_Tet2 tet2;


      switch (eltype)
      {
      case ET_TRIG:
      // felsigma = &bdfmtrig2;
      felsigma = &bdmtrig1;
      felu = &trig2;
      felgamma = &trig1;
      break;
      case ET_TET:
      //      felsigma = &bdfmtet2;
      felsigma = &bdmtet1;
      felu = &tet2;
      felgamma = &tet1;
      break;
      case ET_PRISM:
      //    felsigma = &bdfmprism2;
      felsigma = &bdmprism1;
      felu = &prism3a;
      felgamma = &prismgamma;
      break;
      default:
      cout << "element " << eltype << " not implemented" << endl;
      }
    */ 
  }


















  
  EquilibriumSourceIntegrator :: 
  EquilibriumSourceIntegrator (EquilibriumIntegrator * agequ)
    : LinearFormIntegrator(), gequ(agequ)
  {
    ;
  }
  
  EquilibriumSourceIntegrator :: 
  ~EquilibriumSourceIntegrator ()
  {
    delete gequ;
  }

  void EquilibriumSourceIntegrator :: 
  AssembleElementVector (const FiniteElement & fel, 
			 const ElementTransformation & eltrans, 
			 FlatVector<double> & elvec,
			 LocalHeap & lh) const
  {
    int i, j, k;
    
    FlatMatrix<> mata, matb, matc;
    FlatVector<> vecfsigma, vecf;

    gequ->ComputeMatrices (fel, eltrans, mata, matb, matc, lh);
    gequ->ComputeVectors (fel, eltrans, vecfsigma, vecf, lh);

    //  (*testout) << "vecfsigma = " << vecfsigma << ", vecf = " << vecf << endl;
    
    int nsigma = mata.Height();
    int nu = matb.Height();


    BitArray internal(nu);
    gequ->GetInternalDofs(fel, internal);
  
    // eliminate primal variables:
    
    FlatMatrix<> invmata (nsigma, nsigma, lh);
    FlatMatrix<> hmatb (nu, nsigma, lh);
    FlatMatrix<> schur (nu,nu,lh);
    
    CalcInverse (mata, invmata);

    hmatb = matb * invmata;
    matc += hmatb * Trans (matb);
  
    // eliminate internal dual variables:
    int ni = 0, ne;
    for (i = 0; i < nu; i++)
      if (internal[i])
	ni++;
    ne = nu - ni;

    FlatMatrix<> matci (ni, lh);
    FlatMatrix<> matcie (ni, ne, lh);
    FlatMatrix<> matce (ne, lh);
    FlatVector<> vecfi (ni, lh), vecfe(ne, lh), hv(ni, lh);
    
    int ii=0, ie=0, ji=0, je=0;
    for (i = 0; i < nu; i++)
      {
	if (internal[i])
	  vecfi(ii) = vecf(i);
	else
	  vecfe(ie) = vecf(i);

	ji = je = 0;
	for (j = 0; j < nu; j++)
	  {
	    if (internal[i] && internal[j])
	      matci(ii,ji) = matc(i,j);
	    else if (internal[i] && !internal[j])
	      matcie(ii,je) = matc(i,j);
	    else if (!internal[i] && !internal[j])
	      matce(ie,je) = matc(i,j);
	    if (internal[j]) ji++; else je++;
	  }

	if (internal[i]) ii++;
	else ie++;
      }
    
    (*testout) << "matci = " << endl << matci << endl;
    (*testout) << "matce = " << endl << matce << endl;
    (*testout) << "matcie = " << endl << matcie << endl;

    FlatMatrix<> invmatci (ni, lh);
    CalcInverse (matci, invmatci);
  
    (*testout) << "invci = " << endl << invmatci << endl;

    hv = invmatci * vecfi;
    vecfe -= Trans (matcie) * hv;

    elvec.AssignMemory (ne, lh);
    elvec = vecfe;
    (*testout) << "elvec (new) = " << elvec << endl;
  }






  ScalarEquilibriumIntegrator ::   
  ScalarEquilibriumIntegrator (CoefficientFunction * alam,
			       CoefficientFunction * arho,
			       CoefficientFunction * af)
    : EquilibriumIntegrator(), lam(alam), rho(arho), f(af)
  {
    ;
  }

  ScalarEquilibriumIntegrator ::  ~ScalarEquilibriumIntegrator ()
  {
    ;
  }

  void ScalarEquilibriumIntegrator :: 
  GetInternalDofs (const FiniteElement & fel,
		   BitArray & internal) const
  {
    int dim = fel.SpatialDim();

    const FiniteElement & felu = 
      dynamic_cast<const CompositeFiniteElement<HDivHighOrderFiniteElement<2>,L2HighOrderFiniteElement>&> (fel) . GetFE1 ();
  
    int i, j, k, l, ii, jj;
    double det;

    int nu = felu.GetNDof();

    internal.Clear();
    for (i = 0; i < nu; i++)
      internal.Set (i);
  }

  void ScalarEquilibriumIntegrator :: 
  ComputeMatrices (const FiniteElement & fel, 
		   const ElementTransformation & eltrans, 
		   FlatMatrix<> & mata,
		   FlatMatrix<> & matb,
		   FlatMatrix<> & matc,
		   LocalHeap & lh) const
  {
    const HDivHighOrderFiniteElement<2> & felsigma = 
      dynamic_cast<const CompositeFiniteElement<HDivHighOrderFiniteElement<2>,L2HighOrderFiniteElement>&> (fel) . GetFE0 ();
    
    const NodalFiniteElement & felu = 
      dynamic_cast<const CompositeFiniteElement<HDivHighOrderFiniteElement<2>,L2HighOrderFiniteElement>&> (fel) . GetFE1 ();
  
    int i, j, k, l;
    double det; 
    
    int nsigma = felsigma.GetNDof();
    int nu = felu.GetNDof();
    int nuf;

    switch (felsigma.ElementType())
      {
      case ET_TRIG:
	nuf = 3*(felsigma.Order()+1); break;
      }
  
    mata.AssignMemory (nsigma, nsigma, lh);
    matb.AssignMemory (nu+nuf, nsigma, lh);
    matc.AssignMemory (nu+nuf, nu+nuf, lh);

    FlatMatrix<> shapesigma(2, nsigma, lh);
    FlatMatrix<> divsigma (1, nsigma, lh);
    FlatMatrix<> shapeu (1, nu, lh);

    FlatMatrix<> matb1 (nu, nsigma, &matb(0,0));
    FlatMatrix<> matb2 (nuf, nsigma, &matb(nu,0));
    FlatMatrix<> matc1 (nu, nu, lh);

    const IntegrationRule & ir = 
      GetIntegrationRules().SelectIntegrationRule (felsigma.ElementType(), 
						   2*felsigma.Order());

    mata = 0;
    matb = 0;
    matc = 0;
    matc1 = 0;


    for (i = 0; i < ir.GetNIP(); i++)
      {
	SpecificIntegrationPoint<2,2> sip(ir[i], eltrans, lh);
	
	shapesigma = (1.0/sip.GetJacobiDet())
	  * (sip.GetJacobian() * Trans (felsigma.GetShape(sip.IP(), lh)));

	divsigma = 1.0/sip.GetJacobiDet() * 
	  Trans (felsigma.GetDivShape(sip.IP(), lh));

	shapeu = Trans (felu.GetShape (sip.IP(), lh));

	mata += fabs (sip.GetJacobiDet()) * sip.IP().Weight() / Evaluate (*lam, sip) *
	  (Trans (shapesigma) * shapesigma);

	matb1 += fabs (sip.GetJacobiDet()) * sip.IP().Weight() *
	  (Trans (shapeu) * divsigma);

	matc1 += fabs (sip.GetJacobiDet()) * sip.IP().Weight() * Evaluate (*rho, sip) *
	  (Trans (shapeu) * shapeu);
      }

    for (i = 0; i < nu; i++)
      for (j = 0; j < nu; j++)
	matc(i,j) = matc1(i,j);


    
    int ii = 0, jj = 3;
    for (i = 0; i < 3; i++)
      {
	int sign = felsigma.EdgeOrientation (i);
	  
	matb2(ii++, i) = sign;
	for (int j = 0; j < felsigma.Order(); j++)
	  matb2(ii++, jj++) = sign;
      }
  }



  void ScalarEquilibriumIntegrator :: 
  ComputeVectors (const FiniteElement & fel, 
		  const ElementTransformation & eltrans, 
		  FlatVector<> & vecfsigma,
		  FlatVector<> & vecf,
		  LocalHeap & lh) const
  {
    const HDivFiniteElement<2> & felsigma = 
      dynamic_cast<const CompositeFiniteElement<HDivHighOrderFiniteElement<2>,L2HighOrderFiniteElement>&> (fel) . GetFE0 ();
    
    const NodalFiniteElement & felu = 
      dynamic_cast<const CompositeFiniteElement<HDivHighOrderFiniteElement<2>,L2HighOrderFiniteElement>&> (fel) . GetFE1 ();
  
    int i, j, k, l;
    double det; 
    
    int nsigma = felsigma.GetNDof();
    int nu = felu.GetNDof();
    int nuf;

    switch (felsigma.ElementType())
      {
      case ET_TRIG:
	nuf = 3*(felsigma.Order()+1); break;
      }
  
    vecfsigma.AssignMemory (nsigma, lh);
    vecf.AssignMemory (nu+nuf, lh);

    FlatMatrix<> shapesigma(2, nsigma, lh);
    FlatMatrix<> divsigma (1, nsigma, lh);
    FlatMatrix<> shapeu (1, nu, lh);


    FlatVector<> vecf1 (nu, &vecf(0));

    const IntegrationRule & ir = 
      GetIntegrationRules().SelectIntegrationRule (felsigma.ElementType(), 
						   2*felsigma.Order());

    vecf = 0;
    vecfsigma = 0;

    for (int i = 0; i < ir.GetNIP(); i++)
      {
	SpecificIntegrationPoint<2,2> 
	  sip(ir[i], eltrans, lh);
	
	shapeu = Trans (felu.GetShape (sip.IP(), lh));

	vecf1 += fabs (sip.GetJacobiDet()) * sip.IP().Weight() * Evaluate (*f, sip) *
	  felu.GetShape (sip.IP(), lh);
      }

    (*testout) << "vecf = " << endl << vecf << endl;
  }


  void ScalarEquilibriumIntegrator :: 
  ComputePointValues (const FiniteElement & fel,
		      const ElementTransformation & eltrans, 
		      const IntegrationPoint & ip,
		      const FlatVector<> & sigma, const FlatVector<> & uint,
		      FlatVector<> & psigma, FlatVector<> & pu,
		      LocalHeap & lh) const
  {
    const HDivFiniteElement<2> & felsigma = 
      dynamic_cast<const CompositeFiniteElement<HDivHighOrderFiniteElement<2>,L2HighOrderFiniteElement>&> (fel) . GetFE0 ();
    
    const NodalFiniteElement & felu = 
      dynamic_cast<const CompositeFiniteElement<HDivHighOrderFiniteElement<2>,L2HighOrderFiniteElement>&> (fel) . GetFE1 ();
  
    
    int nsigma = felsigma.GetNDof();
    int nu = felu.GetNDof();

    psigma.AssignMemory (2, lh);
    pu.AssignMemory (1, lh);


    SpecificIntegrationPoint<2,2> sip(ip, eltrans, lh);


    pu(0) = InnerProduct (felu.GetShape (ip, lh), uint);
    Vec<2> hv = Trans (felsigma.GetShape (ip, lh)) * sigma;
    psigma = (1.0/sip.GetJacobiDet()) * (sip.GetJacobian() * hv);
  }




  void ScalarEquilibriumIntegrator :: 
  GetElement (ELEMENT_TYPE eltype,
	      HDivFiniteElement<2> *& felsigma,
	      FiniteElement *& felu) const
  {
    cout << "get el not impl" << endl;
    /*
      static FE_RTTrig0plus rttrig0p;
      static FE_BDMTrig1 bdmtrig1;
      static FE_BDFMTrig2 bdfmtrig2;

      static FE_BDMTet1 bdmtet1;
      static FE_BDFMTet2 bdfmtet2;
      static FE_BDMPrism1p bdmprism1;
      static FE_BDFMPrism2 bdfmprism2;

      static FE_Trig0 trig0;
      static FE_Trig1 trig1;
      static FE_Trig2 trig2;

      static FE_Prism0 prism0;
      static FE_Prism1 prism1;
      static FE_Prism2 prism2;
      static FE_Prism2aniso prism2a;
      static FE_Prism3aniso prism3a;
      static FE_PrismGamma prismgamma;
      static FE_PrismGamma2 prismgamma2;

      static FE_Tet1 tet1;
      static FE_Tet2 tet2;


      switch (eltype)
      {
      case ET_TRIG:
      // felsigma = &bdfmtrig2;
      felsigma = &bdmtrig1;
      felu = &trig2;
      felgamma = &trig1;
      break;
      case ET_TET:
      //      felsigma = &bdfmtet2;
      felsigma = &bdmtet1;
      felu = &tet2;
      felgamma = &tet1;
      break;
      case ET_PRISM:
      //    felsigma = &bdfmprism2;
      felsigma = &bdmprism1;
      felu = &prism3a;
      felgamma = &prismgamma;
      break;
      default:
      cout << "element " << eltype << " not implemented" << endl;
      }
    */ 
  }
















  // standard integratos:

  namespace {
    class InitEqu
    { 
    public: 
      InitEqu ();
    };
    
    InitEqu::InitEqu()
    {
      GetIntegrators().AddBFIntegrator ("equilibrium", 2, 2,
					ElasticityEquilibriumIntegrator<2>::Create);
      GetIntegrators().AddBFIntegrator ("equilibrium", 3, 2,
					ElasticityEquilibriumIntegrator<3>::Create);

      GetIntegrators().AddBFIntegrator ("hybridscalar", 2, 3,
					ScalarEquilibriumIntegrator::Create);
      GetIntegrators().AddLFIntegrator ("hybridsource", 2, 3,
					EquilibriumSourceIntegrator::Create);

    }
    InitEqu initequ;
    
  }



  
}