// 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 __physical_space_scaled_rotated_finite_element_h__
#define __physical_space_scaled_rotated_finite_element_h__

#include "BasisFunction.hpp"
#include "ElemGeometryInfo.hpp"
#include "FiniteElementBasis.hpp"
#include "FiniteElementMapping.hpp"
#include "MappingBasisFunction.hpp"
#include "ItemMappingFunction.hpp"
#include "Quadrature.hpp"
#include "FiniteElementQuadrature.hpp"
#include "Eigen/Eigenvalues"
#include "ReferenceFiniteElement.hpp"
#include "Mesh.hpp"

template<class MappingT, class BasisT, class QuadratureT>
class PhysicalSpaceScaledRotatedFiniteElement 
{
public:
  typedef FiniteElementMapping<MappingT> FiniteElementMappingType;
  typedef FiniteElementBasis<BasisT> FiniteElementBasisType;
  typedef FiniteElementQuadrature<QuadratureT> FiniteElementQuadratureType;
  typedef SpaMesh::Connectivity::TopoPointVector TopoPointVector;

  PhysicalSpaceScaledRotatedFiniteElement() 
    {
       ;
    }
  
  ~PhysicalSpaceScaledRotatedFiniteElement() {}
  
public:
  template <unsigned int Dim, unsigned int DIm,
	    template <unsigned int, unsigned int> class MappingBasisFunctionType,
	    class TopologyMOABType>
    static void initFE(BasisFunction<DIm>& elemBasisFunction, const unsigned int elemBorder,
                       BasisFunction<DIm>& neighborBasisFunction, const unsigned int neighborBorder,
                       ItemMappingFunction<MappingBasisFunctionType<Dim,DIm> >& sideMappingFunction, const unsigned int m_order,
                       Quadrature& quadrature, const unsigned int q_order,
                       ElemGeometryInfo<ItemMappingFunction<MappingBasisFunctionType<DIm,DIm> > >& elemGeoInfo,
 	               const TopologyMOABType& feTOmoab)
  {
    const TopoPointVector& elem_coord = feTOmoab.getElemCoord();
    const TopoPointVector& neighbor_coord = feTOmoab.getNeighborCoord();
    const TopoPointVector& side_coord = feTOmoab.getSideCoord();

    const ElemType elemType = feTOmoab.getElemType();
    const ElemType neighborType = feTOmoab.getNeighborType();
    const ElemType sideType = feTOmoab.getSideType();
    
    FiniteElementQuadratureType::init(quadrature, sideType, q_order);
    FiniteElementMappingType::init(sideMappingFunction.getMappingFunction(), quadrature, sideType, side_coord, m_order);
    typename Quadrature::PointVector auxQuadrature;

    computeCentroidAndScaling(neighbor_coord, neighborType, m_order, elemGeoInfo);
    computePhysicalQuadrature(sideMappingFunction, elemGeoInfo, auxQuadrature);
    FiniteElementBasisType::reInit(neighborBasisFunction, auxQuadrature, neighborType, neighborBorder);
    computeBasisFunction(neighborBasisFunction, elemGeoInfo);

    computeCentroidAndScaling(elem_coord, elemType, m_order, elemGeoInfo);
    computePhysicalQuadrature(sideMappingFunction, elemGeoInfo, auxQuadrature);
    FiniteElementBasisType::reInit(elemBasisFunction, auxQuadrature, elemType, elemBorder);
    computeBasisFunction(elemBasisFunction, elemGeoInfo);
  }
  
  template <unsigned int Dim, unsigned int DIm,
	    template <unsigned int, unsigned int> class MappingBasisFunctionType,
	    class TopologyMOABType>
    static void initFE(BasisFunction<DIm>& elemBasisFunction, const unsigned int elemBorder,
                       ItemMappingFunction<MappingBasisFunctionType<Dim,DIm> >& sideMappingFunction, const unsigned int m_order,
                       Quadrature& quadrature, const unsigned int q_order,
                       ElemGeometryInfo<ItemMappingFunction<MappingBasisFunctionType<DIm,DIm> > >& elemGeoInfo,
		       const TopologyMOABType& feTOmoab)
  {
    const TopoPointVector& elem_coord = feTOmoab.getElemCoord();
    const TopoPointVector& side_coord = feTOmoab.getSideCoord();

    const ElemType elemType = feTOmoab.getElemType();
    const ElemType sideType = feTOmoab.getSideType();
  
    computeCentroidAndScaling(elem_coord, elemType, m_order, elemGeoInfo);

    FiniteElementQuadratureType::init(quadrature, sideType, q_order);
    FiniteElementMappingType::init(sideMappingFunction.getMappingFunction(), quadrature, sideType, side_coord, m_order);
    
    typename Quadrature::PointVector auxQuadrature;
    computePhysicalQuadrature(sideMappingFunction, elemGeoInfo, auxQuadrature);
    
    FiniteElementBasisType::reInit(elemBasisFunction, auxQuadrature, elemType, elemBorder);
    
    computeBasisFunction(elemBasisFunction, elemGeoInfo);
  }
  
  template <unsigned int FaceDim, unsigned int ElemDim,
	    template <unsigned int, unsigned int> class MappingBasisFunctionType,
	    class TopologyMOABType>
    static void initFE(BasisFunction<FaceDim>& faceBasisFunction, const unsigned int faceBorder,
                       ItemMappingFunction<MappingBasisFunctionType<FaceDim,ElemDim> >& sideMappingFunction, const unsigned int m_order,
                       Quadrature& quadrature, const unsigned int q_order,
                       ElemGeometryInfo<ItemMappingFunction<MappingBasisFunctionType<FaceDim,ElemDim> > >& faceGeoInfo,
		       const TopologyMOABType& feTOmoab)
  {
    const TopoPointVector& side_coord = feTOmoab.getSideCoord();
    const TopoPointVector& face_coord = feTOmoab.getFaceCoord();
    const ElemType faceType = feTOmoab.getSideType();
    
    computeCentroidAndScaling(side_coord, faceType, m_order, faceGeoInfo);

    FiniteElementQuadratureType::init(quadrature, faceType, q_order);
    FiniteElementMappingType::init(sideMappingFunction.getMappingFunction(), quadrature, faceType, side_coord, m_order);

    typename Quadrature::PointVector _auxQuadrature;
    _auxQuadrature.resize(quadrature.nbPoint(), Eigen::NoChange);
    faceGeoInfo.getMaxDistanceVertex() = face_coord.row(0).segment(0,2); 
    computePhysicalQuadrature(sideMappingFunction, faceGeoInfo, _auxQuadrature);
    
    FiniteElementBasisType::reInit(faceBasisFunction, _auxQuadrature, faceType, faceBorder);
    
    computeBasisFunction(faceBasisFunction, faceGeoInfo);
  }
  
  template <unsigned int Dim,
	    template <unsigned int, unsigned int> class MappingBasisFunctionType,
	    class TopologyMOABType>
    static void initFE(BasisFunction<Dim>& basisFunction, const unsigned int b_order,
                       ItemMappingFunction<MappingBasisFunctionType<Dim,Dim> >& mappingFunction, const unsigned int m_order,
                       Quadrature& quadrature, const unsigned int q_order,
                       ElemGeometryInfo<ItemMappingFunction<MappingBasisFunctionType<Dim,Dim> > >& elemGeoInfo,
		       const TopologyMOABType& feTOmoab, const unsigned int additionalQorder = 0)
  {
    const TopoPointVector& elem_coord = feTOmoab.getElemCoord();
    const ElemType elemType = feTOmoab.getElemType();

    computeCentroidAndScaling(elem_coord, elemType, m_order, elemGeoInfo);
    
    FiniteElementQuadratureType::init(quadrature, elemType, q_order+additionalQorder);
    FiniteElementMappingType::init(mappingFunction.getMappingFunction(), quadrature, elemType, elem_coord, m_order);
    typename Quadrature::PointVector auxQuadrature;
    computePhysicalQuadrature(mappingFunction, elemGeoInfo, auxQuadrature);

    FiniteElementBasisType::reInit(basisFunction, auxQuadrature, elemType, b_order);
    
    computeBasisFunction(basisFunction, elemGeoInfo);
  }
  
  template <unsigned int Dim,
	    template <unsigned int, unsigned int> class MappingBasisFunctionType,
	    class TopologyMOABType>
    static void initFE(BasisFunction<Dim>& basisFunction, const unsigned int b_order,
                       ItemMappingFunction<MappingBasisFunctionType<Dim,Dim> >& mappingFunction, const unsigned int m_order,
                       Quadrature& quadrature, const unsigned int q_order,
		       const TopologyMOABType& feTOmoab, const unsigned int additionalQorder = 0)
  {
    ElemGeometryInfo<MappingBasisFunctionType<Dim,Dim> > elemGeoInfo;
    const TopoPointVector& elem_coord = feTOmoab.getElemCoord();
    const ElemType elemType = feTOmoab.getElemType();
 
    computeCentroidAndScaling(elem_coord, elemType, m_order, elemGeoInfo);
    
    FiniteElementQuadratureType::init(quadrature, elemType, q_order+additionalQorder);
    FiniteElementMappingType::init(mappingFunction.getMappingFunction(), quadrature, elemType, elem_coord, m_order);
    typename Quadrature::PointVector auxQuadrature;
    computePhysicalQuadrature(mappingFunction, elemGeoInfo, auxQuadrature);

    FiniteElementBasisType::reInit(basisFunction, auxQuadrature, elemType, b_order);
    
    computeBasisFunction(basisFunction, elemGeoInfo);
  }
  
  template <unsigned int Dim, unsigned int DIm,
	    template <unsigned int, unsigned int> class MappingBasisFunctionType,
	    class TopologyMOABType>
    static double sizeFE(TopologyMOABType& feTOmoab, ItemMappingFunction<MappingBasisFunctionType<Dim,DIm> >& sizeMapping, Quadrature& sizeQuadrature)
  {
    const TopoPointVector& elem_coord = feTOmoab.getElemCoord();
    const ElemType elemType = feTOmoab.getElemType();

    const unsigned int q_order = ReferenceFiniteElement::getGeoOrder(feTOmoab.getElemType());
    FiniteElementQuadratureType::init(sizeQuadrature, elemType, q_order);
    FiniteElementMappingType::init(sizeMapping.getMappingFunction(), sizeQuadrature, elemType, elem_coord, q_order);
    return sizeMapping.getJacobianXweight().sum();
  }
  
  template <unsigned int Dim,
	    template <unsigned int, unsigned int> class MappingBasisFunctionType,
	    class TopologyMOABType>
    static void initFENodalPoints(BasisFunction<Dim>& basisFunction, const unsigned int b_order,
	                          ItemMappingFunction<MappingBasisFunctionType<Dim,Dim> >& mappingFunction, const unsigned int m_order,
                                  ElemGeometryInfo<ItemMappingFunction<MappingBasisFunctionType<Dim,Dim> > >& elemGeoInfo,
                                  const TopologyMOABType& feTOmoab,
			          const Quadrature::PointVector& referenceSpaceNodes)
  {
    const ElemType elemType = feTOmoab.getElemType();
    const TopoPointVector& elem_coord = feTOmoab.getElemCoord();
    computeCentroidAndScaling(elem_coord, elemType, m_order, elemGeoInfo);

    FiniteElementMappingType::computePhysicalSpacePoints(mappingFunction.getMappingFunction(),referenceSpaceNodes,elemType,elem_coord,m_order);
    typename Quadrature::PointVector auxQuadrature;
    computePhysicalQuadrature(mappingFunction, elemGeoInfo, auxQuadrature);

    FiniteElementBasisType::reInit(basisFunction, auxQuadrature, elemType, b_order);
    computeBasisFunction(basisFunction, elemGeoInfo);
  }
  
  template <unsigned int Dim, unsigned int DIm, 
	    template <unsigned int, unsigned int> class MappingBasisFunctionType,
	    class TopologyMOABType>
    static void initFENodalPoints(BasisFunction<DIm>& basisFunction, const unsigned int b_order,
	                          ItemMappingFunction<MappingBasisFunctionType<Dim,DIm> >& sideMappingFunction, const unsigned int m_order,
                                  ElemGeometryInfo<ItemMappingFunction<MappingBasisFunctionType<DIm,DIm> > >& elemGeoInfo,
                                  const TopologyMOABType& feTOmoab,
			          const Quadrature::PointVector& referenceSpaceNodes)
  {
    const ElemType elemType = feTOmoab.getElemType();
    const ElemType sideType = feTOmoab.getSideType();
    const TopoPointVector& elem_coord = feTOmoab.getElemCoord();
    const TopoPointVector& side_coord = feTOmoab.getSideCoord();

    computeCentroidAndScaling(elem_coord, elemType, m_order, elemGeoInfo);

    FiniteElementMappingType::computePhysicalSpacePointsWithMap(sideMappingFunction.getMappingFunction(),referenceSpaceNodes,sideType,side_coord,m_order);
    typename Quadrature::PointVector auxQuadrature;
    computePhysicalQuadrature(sideMappingFunction, elemGeoInfo, auxQuadrature);

    FiniteElementBasisType::reInit(basisFunction, auxQuadrature, elemType, b_order);
    computeBasisFunction(basisFunction, elemGeoInfo);
  }
  
  template <unsigned int Dim,
	    template <unsigned int, unsigned int> class MappingBasisFunctionType,
	    class TopologyMOABType>
    static void initFENodalPoints(BasisFunction<Dim>& basisFunction, const unsigned int b_order,
	                          ItemMappingFunction<MappingBasisFunctionType<Dim,Dim> >& mappingFunction, const unsigned int m_order,
                                  const TopologyMOABType& feTOmoab,
			          const Quadrature::PointVector& referenceSpaceNodes)
  {
    ElemGeometryInfo<ItemMappingFunction< MappingBasisFunctionType<Dim,Dim> > > elemGeoInfo;
    const ElemType elemType = feTOmoab.getElemType();
    const TopoPointVector& elem_coord = feTOmoab.getElemCoord();
    computeCentroidAndScaling(elem_coord, elemType, m_order, elemGeoInfo);

    FiniteElementMappingType::computePhysicalSpacePoints(mappingFunction.getMappingFunction(),referenceSpaceNodes,elemType,elem_coord,m_order);
    typename Quadrature::PointVector auxQuadrature;
    computePhysicalQuadrature(mappingFunction, elemGeoInfo, auxQuadrature);

    FiniteElementBasisType::reInit(basisFunction, auxQuadrature, elemType, b_order);
    computeBasisFunction(basisFunction, elemGeoInfo);
  }
  
  bool is_hierarchical ()
  {
    return FiniteElementBasisType::is_hierarchical();
  }
 
private:
  template< template <unsigned int, unsigned int> class MappingBasisFunctionType>
  static void computeCentroidAndScaling(const TopoPointVector& elem_coord, 
                                        const ElemType& elemType, 
        				const unsigned int m_order,
                                        ElemGeometryInfo<ItemMappingFunction<MappingBasisFunctionType<3,3> > >& elemGeoInfo)
  {
    Quadrature& _sizeQuadrature = elemGeoInfo.getQuadrature();
    MappingBasisFunctionType<3,3>& localMapping3D = elemGeoInfo.getMapping();
    Eigen::Matrix<double,3,1>& centroid = elemGeoInfo.getCentroid();
    Eigen::Matrix<double,3,1>& max_distance = elemGeoInfo.getMaxDistance();
    Eigen::Matrix<double,3,3>& rotation = elemGeoInfo.getRotation();
    double& size = elemGeoInfo.getSize();
    
    FiniteElementQuadratureType::init(_sizeQuadrature, elemType, 2*m_order);
    FiniteElementMappingType::init(localMapping3D, _sizeQuadrature, elemType, elem_coord, m_order);
    
    size = localMapping3D.getJacobianXweight().sum();
    for (unsigned int d=0; d<3; d++)
      centroid(d) = localMapping3D.getQpointX().col(d).dot(localMapping3D.getJacobianXweight());
    centroid /= size;

    Quadrature::PointVector& sizeQpoints = _sizeQuadrature.getQpoint();
    Quadrature::PointVector sizeQpointsScaled;
    sizeQpointsScaled.resize(_sizeQuadrature.nbPoint(), Eigen::NoChange);
    for (unsigned int d = 0; d < 3; d++)
      sizeQpointsScaled.col(d).noalias() = ((localMapping3D.getQpointX().col(d)).array() - centroid(d)).matrix();
    for(unsigned int j = 0; j < 3; j++)
      for(unsigned int i = j; i < 3; i++)
        rotation(i,j) = sizeQpointsScaled.col(i).cwiseProduct(sizeQpointsScaled.col(j)).dot(localMapping3D.getJacobianXweight());

    if (!is_diagonal(rotation))
    {
      Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double,3,3> > es;
      es.compute(rotation);
      rotation = es.eigenvectors();
      right_hand(rotation);
    }
    
    max_distance.setZero();
    for (unsigned int p = 0; p < elem_coord.rows(); p++)
      for (unsigned int j = 0; j < 3; j++)
      {
        double distance = 0;
        for (unsigned int d = 0; d < 3; d++)
          distance += (centroid(d) - elem_coord(p,d)) * rotation(d,j); 
        max_distance(j) = std::max(std::abs(distance), max_distance(j));
      }
  }
  
  template< template <unsigned int, unsigned int> class MappingBasisFunctionType>
  static void computeCentroidAndScaling(const TopoPointVector& elem_coord, 
                                        const ElemType& elemType, 
        				const unsigned int m_order,
                                        ElemGeometryInfo<ItemMappingFunction<MappingBasisFunctionType<1,2> > >& elemGeoInfo)
  {
    Quadrature& _sizeQuadrature = elemGeoInfo.getQuadrature();
    MappingBasisFunctionType<1,2>& localMapping2D = elemGeoInfo.getMapping();
    Eigen::Matrix<double,2,1>& centroid = elemGeoInfo.getCentroid();
    Eigen::Matrix<double,1,1>& max_distance = elemGeoInfo.getMaxDistance();
    Eigen::Matrix<double,1,1>& rotation = elemGeoInfo.getRotation();
    double& size = elemGeoInfo.getSize();
    
    FiniteElementQuadratureType::init(_sizeQuadrature, elemType, 2*m_order);
    FiniteElementMappingType::init(localMapping2D, _sizeQuadrature, elemType, elem_coord, m_order);
    
    size = localMapping2D.getJacobianXweight().sum();
    for (unsigned int d=0; d<2; d++)
      centroid(d) = localMapping2D.getQpointX().col(d).dot(localMapping2D.getJacobianXweight());
    centroid /= size;
    
    rotation(0,0) = 1.;
    
    typename MappingBasisFunctionType<1,2>::CoordVector distanceVec;
    distanceVec.setZero();
    for (unsigned int p = 0; p < elem_coord.rows(); p++)
      for (unsigned int d = 0; d < 2; d++)
      {
        double distance = std::abs(centroid(d) - elem_coord(p,d)); 
        distanceVec(d) = std::max(distance, distanceVec(d));
      }
    max_distance(0) = distanceVec.squaredNorm();
  }
  
  template< template <unsigned int, unsigned int> class MappingBasisFunctionType>
  static void computeCentroidAndScaling(const TopoPointVector& elem_coord, 
                                        const ElemType& elemType, 
        				const unsigned int m_order,
                                        ElemGeometryInfo<ItemMappingFunction<MappingBasisFunctionType<2,2> > >& elemGeoInfo)
  {
    Quadrature& _sizeQuadrature = elemGeoInfo.getQuadrature();
    MappingBasisFunctionType<2,2>& localMapping2D = elemGeoInfo.getMapping();
    Eigen::Matrix<double,2,1>& centroid = elemGeoInfo.getCentroid();
    Eigen::Matrix<double,2,1>& max_distance = elemGeoInfo.getMaxDistance();
    Eigen::Matrix<double,2,2>& rotation = elemGeoInfo.getRotation();
    double& size = elemGeoInfo.getSize();
    
    FiniteElementQuadratureType::init(_sizeQuadrature, elemType, 2*m_order);
    FiniteElementMappingType::init(localMapping2D, _sizeQuadrature, elemType, elem_coord, m_order);
    
    size = localMapping2D.getJacobianXweight().sum();
    for (unsigned int d=0; d<2; d++)
      centroid(d) = localMapping2D.getQpointX().col(d).dot(localMapping2D.getJacobianXweight());
    centroid /= size;

    Quadrature::PointVector& sizeQpoints = _sizeQuadrature.getQpoint();
    Quadrature::PointVector sizeQpointsScaled;
    sizeQpointsScaled.resize(_sizeQuadrature.nbPoint(), Eigen::NoChange);
    for (unsigned int d = 0; d < 2; d++)
      sizeQpointsScaled.col(d).noalias() = ((localMapping2D.getQpointX().col(d)).array() - centroid(d)).matrix();
    for(unsigned int j = 0; j < 2; j++)
      for(unsigned int i = j; i < 2; i++)
        rotation(i,j) = sizeQpointsScaled.col(i).cwiseProduct(sizeQpointsScaled.col(j)).dot(localMapping2D.getJacobianXweight());

    if (!is_diagonal(rotation))
    {
      Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double,2,2> > es;
      es.compute(rotation);
      rotation = es.eigenvectors();
      right_hand(rotation);
    }
    
    max_distance.setZero();
    for (unsigned int p = 0; p < elem_coord.rows(); p++)
      for (unsigned int j = 0; j < 2; j++)
      {
        double distance = 0;
        for (unsigned int d = 0; d < 2; d++)
          distance += (centroid(d) - elem_coord(p,d)) * rotation(d,j); 
        max_distance(j) = std::max(std::abs(distance), max_distance(j));
      }
  }
  
  template< template <unsigned int, unsigned int> class MappingBasisFunctionType>
  static void computeCentroidAndScaling(const TopoPointVector& elem_coord, 
                                        const ElemType& elemType, 
        				const unsigned int m_order,
                                        ElemGeometryInfo<ItemMappingFunction<MappingBasisFunctionType<1,1> > >& elemGeoInfo)
  {
    Quadrature& _sizeQuadrature = elemGeoInfo.getQuadrature();
    MappingBasisFunction<1,1>& localMapping1D = elemGeoInfo.getMapping();
    Eigen::Matrix<double,1,1>& centroid = elemGeoInfo.getCentroid();
    Eigen::Matrix<double,1,1>& max_distance = elemGeoInfo.getMaxDistance();
    Eigen::Matrix<double,1,1>& rotation = elemGeoInfo.getRotation();
    double& size = elemGeoInfo.getSize();
    
    FiniteElementQuadratureType::init(_sizeQuadrature, elemType, 2*m_order);
    FiniteElementMappingType::init(localMapping1D, _sizeQuadrature, elemType, elem_coord, m_order);
    
    size = localMapping1D.getJacobianXweight().sum();
    centroid(0) = localMapping1D.getQpointX().col(0).dot(localMapping1D.getJacobianXweight());
    centroid /= size;

    rotation(0,0) = 1.;
    
    max_distance.setZero();
    for (unsigned int p = 0; p < elem_coord.rows(); p++)
    {
      double distance = (centroid(0) - elem_coord(p,0)); 
      max_distance(0) = std::max(std::abs(distance), max_distance(0));
    }
  }

  template<unsigned int DIm, class ElemGeometryInfoType> 
    static void computeBasisFunction(BasisFunction<DIm>& basisFunction, 
	                             const ElemGeometryInfoType& elemGeoInfo)
  {
    if(basisFunction.computeShapeDeriv())
    {
      const typename ElemGeometryInfoType::DistanceVector& max_distance = elemGeoInfo.getMaxDistance();
      const typename ElemGeometryInfoType::RotationMatrix& rotation = elemGeoInfo.getRotation();
      unsigned int firstIndex, lastIndex;
      typename BasisFunction<DIm>::ShapeDerivMatrix& shapeDerivMatrix = basisFunction.getShapeFunctionDeriv();
      Eigen::Matrix<double, DIm, 1> ishapeDerivBkp;
      for (unsigned int qp = 0; qp < shapeDerivMatrix(0).cols(); qp++)
        for (unsigned int i = 0; i < shapeDerivMatrix(0).rows(); i++)
	{
          for (unsigned int d = 0; d < DIm; d++)
	    ishapeDerivBkp(d) = shapeDerivMatrix(d)(i,qp)/max_distance(d);
          for (unsigned int d = 0; d < DIm; d++)
            shapeDerivMatrix(d)(i,qp) = ishapeDerivBkp.dot(rotation.col(d));
	}
    }
  }

  static void right_hand(Eigen::Matrix<double,3,3>& a)
  {
    if ((a(1,1)*a(2,2)-a(2,1)*a(1,2))+(a(2,1)*a(0,2)-a(0,1)*a(2,2))+(a(0,1)*a(1,2)-a(1,1)*a(0,2)) < 0.)
      a.col(0) *= -1.;
  }

  static void right_hand(Eigen::Matrix<double,2,2>& a)
  {
    if ((a(0,0)*a(1,1)-a(0,1)*a(1,0)) < 0.)
      a.col(0) *= -1.;
  }

  static bool is_diagonal(Eigen::Matrix<double,2,2>& a)
  {
    if (std::abs(a.squaredNorm()-a.diagonal().squaredNorm()) > 1.e-3)
      return false;
//    for(unsigned int j = 0; j < 2; j++)
//      for(unsigned int i = j; i < 2; i++)
//        if (i!=j && a(i,j)>1.e-5)
//	{
//	  return false; 
//	}
    a = Eigen::Matrix2d::Identity();
    return true;
  }
  
  static bool is_diagonal(Eigen::Matrix<double,3,3>& a)
  {
    if (std::abs(a.squaredNorm()-a.diagonal().squaredNorm()) > 1.e-3)
      return false;
//    for(unsigned int j = 0; j < 3; j++)
//      for(unsigned int i = j; i < 3; i++)
//        if (i!=j && a(i,j)>1.e-5)
//	{
//	  return false; 
//	}
    a = Eigen::Matrix3d::Identity();
    return true;
  }
  
  template <unsigned int Dim, unsigned int DIm,
            template <unsigned int, unsigned int> class MappingBasisFunctionType>
    static void computePhysicalQuadrature(ItemMappingFunction<MappingBasisFunctionType<Dim,DIm> >& mappingFunction, 
                                          const ElemGeometryInfo<ItemMappingFunction<MappingBasisFunctionType<DIm,DIm> > >& elemGeoInfo, 
        				  typename Quadrature::PointVector& auxQuadrature)
  {
    const typename ElemGeometryInfo<ItemMappingFunction<MappingBasisFunctionType<DIm,DIm> > >::CoordVector& centroid = elemGeoInfo.getCentroid();
    const typename ElemGeometryInfo<ItemMappingFunction<MappingBasisFunctionType<DIm,DIm> > >::DistanceVector& max_distance = elemGeoInfo.getMaxDistance();
    const typename ElemGeometryInfo<ItemMappingFunction<MappingBasisFunctionType<DIm,DIm> > >::RotationMatrix& rotation = elemGeoInfo.getRotation();
    const typename MappingBasisFunction<Dim,DIm>::PointVector& physical_quadrature = mappingFunction.getQpointX();
    auxQuadrature.noalias() = Quadrature::PointVector::Zero(physical_quadrature.rows(),3); 
    for (unsigned int j = 0; j < DIm; j++)
      for (unsigned int d = 0; d < DIm; d++)
        auxQuadrature.col(j).noalias() += (physical_quadrature.col(d).array() - centroid(d)).matrix() * (rotation(d,j)/max_distance(j)); 

    if (Dim == 0 && DIm == 1)
    {
      typename MappingBasisFunction<Dim,DIm>::NormalVector& normalVec = mappingFunction.getMappingFunction().getNormalVec();
      normalVec(0)(0) = physical_quadrature(0,0) - centroid(0);
      normalVec(0) /= normalVec(0).norm();
    }
  }
  
  template <template <unsigned int, unsigned int> class MappingBasisFunctionType>
    static void computePhysicalQuadrature(const ItemMappingFunction<MappingBasisFunctionType<1,2> >& mappingFunction,
                                          const ElemGeometryInfo<ItemMappingFunction<MappingBasisFunctionType<1,2> > >& elemGeoInfo, 
        				  typename Quadrature::PointVector& auxQuadrature)
  {
    const typename MappingBasisFunctionType<1,2>::CoordVector& centroid = elemGeoInfo.getCentroid();
    const typename MappingBasisFunctionType<1,2>::DistanceVector& max_distance = elemGeoInfo.getMaxDistance();
    const typename MappingBasisFunctionType<1,2>::PointVector& physical_quadrature = mappingFunction.getQpointX();
    const Eigen::Matrix<double,2,1>& firstVertex = elemGeoInfo.getMaxDistanceVertex(); 
    for (unsigned int qp = 0; qp < auxQuadrature.rows(); qp ++)
    {
      auxQuadrature(qp,0)=((firstVertex-centroid).dot(physical_quadrature.row(qp).segment(0,2)-centroid.transpose()))/max_distance(0);
      auxQuadrature(qp,1) = 0.;
    }
  }
};

#endif
