// SpaFEDTe, a Template based C++ library for creating 
// Discontinuous Finite Element Spaces,
// Copyright (C) 2012 Lorenzo Alessio Botti

/* This library is free software; you can redistribute it and/or */
/* modify it under the terms of the GNU Lesser General Public */ 
/* License as published by the Free Software Foundation either */ 
/* version 3.0 of the License, or (at your option) any later version. */

/* This software is distributed in the hope that it will be useful, */
/* but WITHOUT ANY WARRANTY; without even the implied warranty of */
/* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU */
/* Lesser General Public License for more details. */

/* You should have received a copy of the GNU Lesser General Public */
/* License along with this software; if not, a copy of the full */
/* GNU Lesser General Public License can be found at */
/* http://www.gnu.org/licenses/. */

#ifndef __orthonormal_physical_space_finite_element_h__
#define __orthonormal_physical_space_finite_element_h__

#include "FiniteElementBasis.hpp"
#include "EnumElemType.hpp"
#include "JacobiPoly.hpp"
#include "ReferenceFiniteElement.hpp"
#include "BasisFunction.hpp"
#include "CollectiveBasisFunction.hpp"
#include "MappingBasisFunction.hpp"
#include "CollectiveMappingFunction.hpp"
#include "Quadrature.hpp"
#include "PhysicalSpaceFiniteElement.hpp"
#include "PhysicalSpaceScaledRotatedFiniteElement.hpp"
#include "OrthoNormalPhysicalSpaceFiniteElementBase.hpp"
#include "LagrangianFiniteElementBasis.hpp"
#include "MonomialFiniteElementBasis.hpp"
#include "DimensionBasedPolynomial.hpp"
#include "FEManager.hpp"

template<class AuxFESpaceT>
class OrthoNormalPhysicalSpaceFiniteElement : public OrthoNormalPhysicalSpaceFiniteElementBase<AuxFESpaceT>
{
public:
  typedef AuxFESpaceT AuxFESpaceType;
  typedef Quadrature::PointVector PointVector;
  typedef Eigen::Matrix<double,2,Eigen::Dynamic,Eigen::RowMajor> OrthoMatrixType;
  typedef LagrangianFiniteElementBasis<MonomialFiniteElementBasis<DimBasedPoly> > LagrangianBasisFunction;

  OrthoNormalPhysicalSpaceFiniteElement() 
    {
      ;
    }
 
  ~OrthoNormalPhysicalSpaceFiniteElement() {}
  
  template <unsigned int Dim, unsigned int DIm, 
	    template <unsigned int, unsigned int> class MappingBasisFunctionType,
	    template <typename> class MappingFunctionType, 
	    class TopoMOABType> 
  static void initFE(BasisFunction<DIm>& elemBasisFunction, const unsigned int elemBorder,
                     BasisFunction<DIm>& neighborBasisFunction, const unsigned int neighborBorder,
                     MappingFunctionType<MappingBasisFunctionType<Dim,DIm> >& sideMappingFunction, const unsigned int m_order,
                     Quadrature& quadrature, const unsigned int q_order,
	             ElemGeometryInfo<MappingFunctionType<MappingBasisFunctionType<DIm,DIm> > >& elemGeoInfo,
                     TopoMOABType& feTOmoab, 
		     const OrthoMatrixType& elemOrtCoeff, const OrthoMatrixType& neighborOrtCoeff)
  {
    AuxFESpaceType::initFE(elemBasisFunction, elemBorder, neighborBasisFunction, neighborBorder, sideMappingFunction, m_order, quadrature, q_order, elemGeoInfo, feTOmoab);

    if (elemBasisFunction.computeShape())
      mgs1_Shape(elemBasisFunction, elemOrtCoeff);
    if (elemBasisFunction.computeShapeDeriv())
      mgs1_shapeDeriv(elemBasisFunction, elemOrtCoeff);

    if (neighborBasisFunction.computeShape())
      mgs1_Shape(neighborBasisFunction, neighborOrtCoeff);
    if (neighborBasisFunction.computeShapeDeriv())
      mgs1_shapeDeriv(neighborBasisFunction, neighborOrtCoeff);
  }
  
  template <unsigned int Dim, unsigned int DIm, 
	    template <unsigned int, unsigned int> class MappingBasisFunctionType,
	    template <typename> class MappingFunctionType, 
	    template <typename> class MappingFunctionTypeGeo, 
	    class TopoMOABType> 
  static void initFE(BasisFunction<DIm>& elemBasisFunction, const unsigned int elemBorder,
                     MappingFunctionType<MappingBasisFunctionType<Dim,DIm> >& sideMappingFunction, const unsigned int m_order,
                     Quadrature& quadrature, const unsigned int q_order,
	             ElemGeometryInfo<MappingFunctionTypeGeo<MappingBasisFunctionType<DIm,DIm> > >& elemGeoInfo,
                     TopoMOABType& feTOmoab, const OrthoMatrixType& ortCoeff)
  {
    AuxFESpaceType::initFE(elemBasisFunction, elemBorder, sideMappingFunction, m_order, quadrature, q_order, elemGeoInfo, feTOmoab);
    
    if (elemBasisFunction.computeShape())
      mgs1_Shape(elemBasisFunction, ortCoeff);
    if (elemBasisFunction.computeShapeDeriv())
      mgs1_shapeDeriv(elemBasisFunction, ortCoeff);
  }
  
  template <unsigned int FaceDim, unsigned int ElemDim, 
	    template <unsigned int, unsigned int> class MappingBasisFunctionType,
	    template <typename> class MappingFunctionType, 
	    class TopoMOABType> 
  static void initFE(BasisFunction<FaceDim>& elemBasisFunction, const unsigned int elemBorder,
                     MappingFunctionType<MappingBasisFunctionType<FaceDim,ElemDim> >& sideMappingFunction, const unsigned int m_order,
                     Quadrature& quadrature, const unsigned int q_order,
	             ElemGeometryInfo<MappingFunctionType<MappingBasisFunctionType<FaceDim,ElemDim> > >& elemGeoInfo,
                     TopoMOABType& feTOmoab, const OrthoMatrixType& ortCoeff)
  {
    AuxFESpaceType::initFE(elemBasisFunction, elemBorder, sideMappingFunction, m_order, quadrature, q_order, elemGeoInfo, feTOmoab);
    
    if (elemBasisFunction.computeShape())
      mgs1_Shape(elemBasisFunction, ortCoeff);
    if (elemBasisFunction.computeShapeDeriv())
      mgs1_shapeDeriv(elemBasisFunction, ortCoeff);
  }
  
  template <unsigned int Dim, 
	    template <unsigned int, unsigned int> class MappingBasisFunctionType,
	    template <typename> class MappingFunctionType, 
	    class TopoMOABType> 
  static void initFE(BasisFunction<Dim>& basisFunction, const unsigned int b_order,
                     MappingFunctionType<MappingBasisFunctionType<Dim,Dim> >& mappingFunction, const unsigned int m_order,
                     Quadrature& quadrature, const unsigned int q_order,
	             ElemGeometryInfo<MappingFunctionType<MappingBasisFunctionType<Dim,Dim> > >& elemGeoInfo,
                     TopoMOABType& feTOmoab,
		     const OrthoMatrixType& ortCoeff, const unsigned int additionalQorder = 0)
  {
    AuxFESpaceType::initFE(basisFunction, b_order, mappingFunction, m_order, quadrature, q_order+additionalQorder, elemGeoInfo, feTOmoab);

    if (basisFunction.computeShape())
      mgs1_Shape(basisFunction, ortCoeff);
    if (basisFunction.computeShapeDeriv())
      mgs1_shapeDeriv(basisFunction, ortCoeff);
  }
  
  template <unsigned int Dim, unsigned int DIm,
	    template <unsigned int, unsigned int> class MappingBasisFunctionType,
	    template <typename> class MappingFunctionType,
	    template <typename> class MappingFunctionTypeGeo>
  static void reInitFE(BasisFunction<Dim>& basisFunction, const unsigned int b_order,
                       const MappingFunctionType<MappingBasisFunctionType<Dim,DIm> >& mappingFunction,
	               const ElemGeometryInfo<MappingFunctionTypeGeo<MappingBasisFunctionType<Dim,DIm> > >& elemGeoInfo,
		       const OrthoMatrixType& ortCoeff)
  {
    AuxFESpaceType::reInitFE(basisFunction, b_order, mappingFunction, elemGeoInfo);

    if (basisFunction.computeShape())
      mgs1_Shape(basisFunction, ortCoeff);
    if (basisFunction.computeShapeDeriv())
      mgs1_shapeDeriv(basisFunction, ortCoeff);
  }
  
  template <unsigned int Dim, unsigned int DIm,
	    template <unsigned int, unsigned int> class MappingBasisFunctionType,
	    template <typename> class MappingFunctionType,
	    template <typename> class MappingFunctionTypeGeo>
  static void reInitFEonFace(BasisFunction<Dim>& basisFunction, const unsigned int b_order,
                             const MappingFunctionType<MappingBasisFunctionType<DIm,Dim> >& mappingFunction,
	                     const ElemGeometryInfo<MappingFunctionTypeGeo<MappingBasisFunctionType<Dim,Dim> > >& elemGeoInfo,
		             const OrthoMatrixType& ortCoeff)
  {
    AuxFESpaceType::reInitFEonFace(basisFunction, b_order, mappingFunction, elemGeoInfo);

    if (basisFunction.computeShape())
      mgs1_Shape(basisFunction, ortCoeff);
    if (basisFunction.computeShapeDeriv())
      mgs1_shapeDeriv(basisFunction, ortCoeff);
  }
  
  template <unsigned int Dim, 
	    template <unsigned int, unsigned int> class MappingBasisFunctionType,
	    template <typename> class MappingFunctionType, 
	    class TopoMOABType> 
  static void initFE(BasisFunction<Dim>& basisFunction, const unsigned int b_order,
                     MappingFunctionType<MappingBasisFunctionType<Dim,Dim> >& mappingFunction, const unsigned int m_order,
                     Quadrature& quadrature, const unsigned int q_order,
                     TopoMOABType& feTOmoab, const unsigned int additionalQorder = 0)
  {
    FEManager<BasisFunction<Dim>, MappingFunctionType<MappingBasisFunctionType<Dim,Dim> > > auxFEManager;
    ElemGeometryInfo<MappingFunctionType<MappingBasisFunctionType<Dim,Dim> > > elemGeoInfo;
    OrthoMatrixType ortCoeff;
    initAuxFESpace(b_order, m_order, q_order, auxFEManager, elemGeoInfo, feTOmoab, ortCoeff);

    AuxFESpaceType::initFE(basisFunction, b_order, mappingFunction, m_order, quadrature, q_order+additionalQorder, elemGeoInfo, feTOmoab);
    if (basisFunction.computeShape())
      mgs1_Shape(basisFunction, ortCoeff);
    if (basisFunction.computeShapeDeriv())
      mgs1_shapeDeriv(basisFunction, ortCoeff);
  }
  
  template <unsigned int Dim, unsigned int DIm, 
	    template <unsigned int, unsigned int> class MappingBasisFunctionType,
	    template <typename> class MappingFunctionType, 
	    class TopoMOABType> 
  static void initFENodalPoints(BasisFunction<DIm>& basisFunction, const unsigned int b_order,
                                MappingFunctionType<MappingBasisFunctionType<Dim,DIm> >& mappingFunction, const unsigned int m_order,
	                        ElemGeometryInfo<MappingFunctionType<MappingBasisFunctionType<DIm,DIm> > >& elemGeoInfo,
                                TopoMOABType& feTOmoab,
                                Quadrature::PointVector& referenceSpaceNodes,
				const OrthoMatrixType& ortCoeff)
  {
    AuxFESpaceType::initFENodalPoints(basisFunction, b_order, mappingFunction, m_order, elemGeoInfo, feTOmoab, referenceSpaceNodes);
    mgs1_Shape(basisFunction, ortCoeff);
    if (basisFunction.computeShapeDeriv())
      mgs1_shapeDeriv(basisFunction, ortCoeff);
  }
  
  template <unsigned int Dim, 
	    template <unsigned int, unsigned int> class MappingBasisFunctionType,
	    template <typename> class MappingFunctionType, 
	    class TopoMOABType> 
  static void initFENodalPoints(BasisFunction<Dim>& basisFunction, const unsigned int b_order,
                                MappingFunctionType<MappingBasisFunctionType<Dim,Dim> >& mappingFunction, const unsigned int m_order,
                                TopoMOABType& feTOmoab,
                                Quadrature::PointVector& referenceSpaceNodes)
  {
    FEManager<BasisFunction<Dim>, MappingFunctionType<MappingBasisFunctionType<Dim,Dim> > > auxFEManager;
    ElemGeometryInfo<MappingFunctionType<MappingBasisFunctionType<Dim,Dim> > > elemGeoInfo;
    OrthoMatrixType ortCoeff;
    initAuxFESpace(b_order, m_order, b_order*2, auxFEManager, elemGeoInfo, feTOmoab, ortCoeff);

    AuxFESpaceType::initFENodalPoints(basisFunction, b_order, mappingFunction, m_order, elemGeoInfo, feTOmoab, referenceSpaceNodes);
    mgs1_Shape(basisFunction, ortCoeff);
    if (basisFunction.computeShapeDeriv())
      mgs1_shapeDeriv(basisFunction, ortCoeff);
  }
  
  template <unsigned int Dim, 
	    template <unsigned int, unsigned int> class MappingBasisFunctionType,
	    template <typename> class MappingFunctionType, 
	    class TopoMOABType> 
    static void initFE_ReducedQuadrature(BasisFunction<Dim>& basisFunction, const unsigned int b_order,
                                         MappingFunctionType<MappingBasisFunctionType<Dim,Dim> >& mappingFunction, const unsigned int m_order,
                                         Quadrature& quadrature, const unsigned int q_order,
	                                 FEManager<BasisFunction<Dim>, MappingFunctionType<MappingBasisFunctionType<Dim,Dim> > >& auxFEManager,
	                                 ElemGeometryInfo<MappingFunctionType<MappingBasisFunctionType<Dim,Dim> > >& elemGeoInfo,
                                         TopoMOABType& feTOmoab, OrthoMatrixType& ortCoeff)
  {
    typename Quadrature::PointVector auxQuadrature;
    AuxFESpaceType::computeQuadrature(mappingFunction, m_order, quadrature, q_order, elemGeoInfo, feTOmoab, auxQuadrature);
    initAuxFESpace(q_order, m_order, q_order, auxFEManager, elemGeoInfo, feTOmoab, ortCoeff);
    computeQuadratureWeights(basisFunction, q_order, mappingFunction, m_order, elemGeoInfo, feTOmoab, auxQuadrature, ortCoeff);
    initFE_RQ(basisFunction, b_order, elemGeoInfo, feTOmoab, auxQuadrature, ortCoeff);
  }
  
  template <unsigned int Dim, 
	    template <unsigned int, unsigned int> class MappingBasisFunctionType,
	    template <typename> class MappingFunctionType, 
	    class TopoMOABType> 
    static void computeLebesgueConstants(BasisFunction<Dim>& basisFunction, const unsigned int b_order,
                                         MappingFunctionType<MappingBasisFunctionType<Dim,Dim> >& mappingFunction, const unsigned int m_order,
                                         MappingFunctionType<MappingBasisFunctionType<Dim,Dim> >& auxMapFunction,
				         Quadrature& quadrature, const unsigned int q_order,
	                                 TopoMOABType& feTOmoab,
				         typename Quadrature::PointVector& auxQuadrature,
				         const OrthoMatrixType& ortCoeff,
				         double& leb, unsigned int& nNewPoints, unsigned int& nOldPoints, 
				         double& minW, double& maxW)
  {
    unsigned int myBorder = 2*b_order;
    const ElemType elemType = feTOmoab.getElemType();
    bool computeShapeDeriv = basisFunction.computeShapeDeriv();
    basisFunction.doComputeShapeDeriv(false);
    AuxFESpaceType::FiniteElementBasisType::reInit(basisFunction, auxQuadrature, elemType, myBorder);
    
    ElemGeometryInfo<MappingFunctionType<MappingBasisFunctionType<Dim,Dim> > > elemGeoInfo;
    AuxFESpaceType::initQuadrature(mappingFunction, m_order, quadrature, q_order, elemGeoInfo, feTOmoab, auxQuadrature);
    BasisFunction<Dim> toolBasisFunction; 
    toolBasisFunction.doComputeShapeDeriv(false);
    LagrangianBasisFunction lagrangianBasisFunction;
    LagrangianBasisFunction::ToolBasisFunctionType::reInit(toolBasisFunction, auxQuadrature, elemType, myBorder);
    mgs1_Shape(toolBasisFunction, ortCoeff);
    typename BasisFunction<Dim>::ShapeMatrix& toolShapeMatrix = toolBasisFunction.getShapeFunction();
    mgs1_Shape(basisFunction, ortCoeff);
    lagrangianBasisFunction.initBasisFunction(basisFunction, toolBasisFunction);
    typename BasisFunction<Dim>::ShapeMatrix& shapeMatrix = basisFunction.getShapeFunction();
    typename MappingBasisFunction<Dim,Dim>::JacVector& jacXweight = mappingFunction.getJacobianXweight();
    typename Quadrature::RealVector weights(shapeMatrix.rows());
    for (unsigned int i = 0; i < shapeMatrix.rows(); i++)
      weights(i)=shapeMatrix.row(i).dot(jacXweight);
    jacXweight = weights;
    mappingFunction.getQpointX() = auxMapFunction.getQpointX(); 
    minW = weights.minCoeff();
    maxW = weights.maxCoeff();

    Eigen::VectorXd allLeb(shapeMatrix.cols());
    for (unsigned int i = 0; i < shapeMatrix.cols(); i++)
      allLeb(i) = shapeMatrix.col(i).array().abs().sum();
    leb=allLeb.maxCoeff();

    typename MappingBasisFunction<Dim,Dim>::PointVector& physical_quadrature = mappingFunction.getQpointX();
//    for (unsigned int i = 0; i < physical_quadrature.rows(); i++)
//      std::cout<<physical_quadrature(i,0)<<", "<<physical_quadrature(i,1)<<", "<<physical_quadrature(i,2)<<", "<<jacXweight(i)<<std::endl;
    nNewPoints = physical_quadrature.rows();
    nOldPoints = shapeMatrix.cols();
  }
  
  template <class AuxFEManagerType, class ElemGeometryInfoType, class TopoMOABType> 
    static void initAuxFESpace (const unsigned int border, const unsigned int morder, const unsigned int qorder, 
	                        AuxFEManagerType& auxFEManager, 
				ElemGeometryInfoType& elemGeoInfo, 
				TopoMOABType& feTOmoab, 
				OrthoMatrixType& ortCoeff)
    
  {
    auxFEManager.basisFunction.doComputeShapeDeriv(false);
    AuxFESpaceType::initFE(auxFEManager.basisFunction, border, auxFEManager.mappingFunction, morder, auxFEManager.quadrature, qorder, elemGeoInfo, feTOmoab);
    mgs0(auxFEManager.basisFunction, auxFEManager.mappingFunction, ortCoeff);
  }

  template <unsigned int Dim, 
	    template <unsigned int, unsigned int> class MappingBasisFunctionType,
	    template <typename> class MappingFunctionType, 
	    class TopoMOABType> 
  static void initFECoeff(BasisFunction<Dim>& basisFunction, const unsigned int b_order,
                          MappingFunctionType<MappingBasisFunctionType<Dim,Dim> >& mappingFunction, const unsigned int m_order,
                          Quadrature& quadrature, const unsigned int q_order,
	                  ElemGeometryInfo<MappingFunctionType<MappingBasisFunctionType<Dim,Dim> > >& elemGeoInfo,
		          TopoMOABType& feTOmoab,
                          OrthoMatrixType& elemOrtCoeff) 
  {
    AuxFESpaceType::initFE(basisFunction, b_order, mappingFunction, m_order, quadrature, q_order, elemGeoInfo, feTOmoab);
    mgs0(basisFunction, mappingFunction, elemOrtCoeff);
  }

  template <unsigned int Dim, 
	    template <unsigned int, unsigned int> class MappingBasisFunctionType,
	    template <typename> class MappingFunctionType, 
	    class TopoMOABType> 
    static void computeQuadratureWeights(BasisFunction<Dim>& basisFunction, const unsigned int q_order,
                                         MappingFunctionType<MappingBasisFunctionType<Dim,Dim> >& mappingFunction, const unsigned int m_order,
	                                 const ElemGeometryInfo<MappingFunctionType<MappingBasisFunctionType<Dim,Dim> > >& elemGeoInfo,
	                                 TopoMOABType& feTOmoab, 
				         const typename Quadrature::PointVector& auxQuadrature,
				         const OrthoMatrixType& ortCoeff)
  {
    bool computeShapeDeriv = basisFunction.computeShapeDeriv();
    basisFunction.doComputeShapeDeriv(false);
    AuxFESpaceType::FiniteElementBasisType::reInit(basisFunction, auxQuadrature, AGGLOELEM, q_order);
    mgs1_Shape(basisFunction, ortCoeff);
    typename BasisFunction<Dim>::ShapeMatrix& shapeMatrix = basisFunction.getShapeFunction();
    Eigen::VectorXd rhs = Eigen::VectorXd::Zero(shapeMatrix.rows());
    const double& area = elemGeoInfo.getSize();
    rhs(0) = sqrt(area);
    typename MappingBasisFunction<Dim,Dim>::JacVector& jacXweight = mappingFunction.getJacobianXweight();
    jacXweight.noalias() = shapeMatrix.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(rhs);
    if (computeShapeDeriv)
      basisFunction.doComputeShapeDeriv(true);
  }
  
  template <unsigned int Dim, class ElemGeometryInfoType, class TopoMOABType> 
    static void initFE_RQ(BasisFunction<Dim>& basisFunction, const unsigned int b_order,
	                  const ElemGeometryInfoType& elemGeoInfo,
                          TopoMOABType& feTOmoab,
		          const typename Quadrature::PointVector& auxQuadrature,
		          const OrthoMatrixType& ortCoeff)
  {
    AuxFESpaceType::initFESRQuadPoints(basisFunction, b_order, elemGeoInfo, auxQuadrature);
    mgs1_Shape(basisFunction, ortCoeff);
    if (basisFunction.computeShapeDeriv())
      mgs1_shapeDeriv(basisFunction, ortCoeff);
  }
  
  template<unsigned int Dim> static void mgs1_Shape(BasisFunction<Dim>& basisFunction, const OrthoMatrixType& ortCoeff)
  {
    typename BasisFunction<Dim>::ShapeMatrix& shapeMatrixRef = basisFunction.getShapeFunction();
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> shapeMatrix = shapeMatrixRef;
    for (unsigned int i = 0; i < shapeMatrix.rows(); i++)
    {
      for (unsigned int n = 0; n < ortCoeff.rows(); n++)
      {
        for (unsigned int j = 0; j < i; j++)
	{
          shapeMatrix.row(i).noalias() -= shapeMatrix.row(j) * ortCoeff(n,j+((i+1)*(i))/2);
	}
        shapeMatrix.row(i) /= ortCoeff(n,i+((i+1)*(i))/2);
      }
    }
    shapeMatrixRef.noalias() = shapeMatrix;
  }

private:
  
  template<unsigned int Dim, unsigned int DIm,
	    template <unsigned int, unsigned int> class MappingBasisFunctionType,
	    template <typename> class MappingFunctionType>
    static void mgs0(BasisFunction<Dim>& basisFunction, 
	             const MappingFunctionType<MappingBasisFunctionType<Dim,DIm> >& mappingFunction, 
		     OrthoMatrixType& ortCoeff)
  {
    typename BasisFunction<Dim>::ShapeMatrix& shapeMatrixRef = basisFunction.getShapeFunction();
    const Eigen::Matrix<double, Eigen::Dynamic, 1>& jacXweight = mappingFunction.getJacobianXweight();
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> shapeMatrix = shapeMatrixRef;

    const unsigned int size = shapeMatrix.rows();
    const unsigned int sizeC = (size*size + size)/2;
    const unsigned int nOrto = ortCoeff.rows();
    ortCoeff.resize(Eigen::NoChange,sizeC);
    for (unsigned int i = 0; i < shapeMatrix.rows(); i++)
    {
      for (unsigned int n = 0; n < nOrto; n++)
      {
        for (unsigned int j = 0; j < i; j++)
        {
          const unsigned int index = j+((i+1)*(i))/2;
          ortCoeff(n,index) = shapeMatrix.row(i).cwiseProduct(shapeMatrix.row(j)).dot(jacXweight);
          shapeMatrix.row(i).noalias() -= shapeMatrix.row(j) * ortCoeff(n,index);
        }
        const unsigned int index = i+((i+1)*(i))/2;
        ortCoeff(n,index) = shapeMatrix.row(i).cwiseProduct(shapeMatrix.row(i)).dot(jacXweight);
        ortCoeff(n,index) = sqrt(ortCoeff(n,index));
        shapeMatrix.row(i) /= ortCoeff(n,index);
      }
    }
  }
  
  template<unsigned int Dim> static void mgs1_shapeDeriv(BasisFunction<Dim>& basisFunction, const OrthoMatrixType& ortCoeff)
  {
    typename BasisFunction<Dim>::ShapeDerivMatrix& shapeDerivMatrixRef = basisFunction.getShapeFunctionDeriv();
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> shapeDerivMatrix;

    for (unsigned int k = 0; k < Dim; k++)
    {
      shapeDerivMatrix.noalias() = shapeDerivMatrixRef(k);
      for (unsigned int i = 0; i < shapeDerivMatrix.rows(); i++)
      {
        for (int n = 0; n < ortCoeff.rows(); n++)
        {
          for (unsigned int j = 0; j < i; j++)
	  {
            shapeDerivMatrix.row(i).noalias() -= shapeDerivMatrix.row(j) * ortCoeff(n,j+((i+1)*(i))/2);
	  }
          shapeDerivMatrix.row(i) /= ortCoeff(n,i+((i+1)*(i))/2);
        }
      }
      shapeDerivMatrixRef(k).noalias() = shapeDerivMatrix;
    }
  }

  
};

#endif 
