// 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_finite_element_h__
#define __physical_space_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 "Mesh.hpp"
#include "MeshFEDataAccessor.hpp"

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

  PhysicalSpaceFiniteElement() 
    {
    }
  
  ~PhysicalSpaceFiniteElement() {}
  
public:
  template <unsigned int Dim, unsigned int DIm, 
	    template <unsigned int, unsigned int> class MappingBasisFunctionType>
    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 MeshFEDataAccessor& 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;
    _auxQuadrature.resize(quadrature.nbPoint(), Eigen::NoChange);
    
    computeCentroidAndScaling(elem_coord, elemGeoInfo);
    computeScaledQuadrature(sideMappingFunction, elemGeoInfo, _auxQuadrature);
    FiniteElementBasisType::reInit(elemBasisFunction, _auxQuadrature, elemType, elemBorder);
    computeBasisFunction(elemBasisFunction, elemGeoInfo);

    computeCentroidAndScaling(neighbor_coord, elemGeoInfo);
    computeScaledQuadrature(sideMappingFunction, elemGeoInfo, _auxQuadrature);
    FiniteElementBasisType::reInit(neighborBasisFunction, _auxQuadrature, neighborType, neighborBorder);
    computeBasisFunction(neighborBasisFunction, elemGeoInfo);
  }
  
  template <unsigned int Dim, unsigned int DIm,
	    template <unsigned int, unsigned int> class MappingBasisFunctionType>
    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 MeshFEDataAccessor& 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, elemGeoInfo);
  
    FiniteElementQuadratureType::init(quadrature, sideType, q_order);
    FiniteElementMappingType::init(sideMappingFunction.getMappingFunction(), quadrature, sideType, side_coord, m_order);

    typename Quadrature::PointVector _auxQuadrature;
    _auxQuadrature.resize(quadrature.nbPoint(), Eigen::NoChange);
    computeScaledQuadrature(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>
    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 MeshFEDataAccessor& feTOmoab)
  {
    const TopoPointVector& side_coord = feTOmoab.getSideCoord();
    const TopoPointVector& face_coord = feTOmoab.getFaceCoord();
    const ElemType faceType = feTOmoab.getSideType();
    
    computeCentroidAndScaling(side_coord, faceGeoInfo);
 
    //avoid the next two lines
    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); 
    computeScaledQuadrature(sideMappingFunction, faceGeoInfo, _auxQuadrature);
    
    FiniteElementBasisType::reInit(faceBasisFunction, _auxQuadrature, faceType, faceBorder);
    
    computeBasisFunction(faceBasisFunction, faceGeoInfo);
  }
  
  template <unsigned int Dim,
	    template <unsigned int, unsigned int> class MappingBasisFunctionType>
    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 MeshFEDataAccessor& feTOmoab, const unsigned int additionalQorder = 0)
  {
    const TopoPointVector& elem_coord = feTOmoab.getElemCoord();
    const ElemType elemType = feTOmoab.getElemType();
    
    computeCentroidAndScaling(elem_coord, elemGeoInfo);
  
    FiniteElementQuadratureType::init(quadrature, elemType, q_order+additionalQorder);
    FiniteElementMappingType::init(mappingFunction.getMappingFunction(), quadrature, elemType, elem_coord, m_order);
    typename Quadrature::PointVector _auxQuadrature;
    _auxQuadrature.resize(quadrature.nbPoint(), Eigen::NoChange);
    computeScaledQuadrature(mappingFunction, elemGeoInfo, _auxQuadrature);

    FiniteElementBasisType::reInit(basisFunction, _auxQuadrature, elemType, b_order);
    
    computeBasisFunction(basisFunction, elemGeoInfo);
  }
  
  template <unsigned int Dim,
	    template <unsigned int, unsigned int> class MappingBasisFunctionType>
    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 MeshFEDataAccessor& 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, elemGeoInfo);
  
    FiniteElementQuadratureType::init(quadrature, elemType, q_order+additionalQorder);
    FiniteElementMappingType::init(mappingFunction.getMappingFunction(), quadrature, elemType, elem_coord, m_order);
    typename Quadrature::PointVector _auxQuadrature;
    _auxQuadrature.resize(quadrature.nbPoint(), Eigen::NoChange);
    computeScaledQuadrature(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>
    static double sizeFE(MeshFEDataAccessor& 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.getMappingFunction().getJacobianXweight().sum();
  }

  template <unsigned int Dim,
	    template <unsigned int, unsigned int> class MappingBasisFunctionType>
    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 MeshFEDataAccessor& feTOmoab,
			          const Quadrature::PointVector& referenceSpaceNodes)
  {
    const ElemType elemType = feTOmoab.getElemType();
    const TopoPointVector& elem_coord = feTOmoab.getElemCoord();
    computeCentroidAndScaling(elem_coord, elemGeoInfo);

    FiniteElementMappingType::computePhysicalSpacePoints(mappingFunction.getMappingFunction(),referenceSpaceNodes,elemType,elem_coord,m_order);
    typename Quadrature::PointVector _auxQuadrature;
    _auxQuadrature.resize(referenceSpaceNodes.rows(), Eigen::NoChange);
    computeScaledQuadrature(mappingFunction, elemGeoInfo, _auxQuadrature);

    FiniteElementBasisType::reInit(basisFunction, _auxQuadrature, elemType, b_order);
    computeBasisFunction(basisFunction, elemGeoInfo);
  }
  
  static bool is_hierarchical ()
  {
    return FiniteElementBasisType::is_hierarchical();
  }
  
  template <unsigned int Dim,
	    template <unsigned int, unsigned int> class MappingBasisFunctionType>
    static void initFENodalPoints(BasisFunction<Dim>& basisFunction, const unsigned int b_order,
	                          ItemMappingFunction<MappingBasisFunctionType<Dim,Dim> >& mappingFunction, const unsigned int m_order,
                                  const MeshFEDataAccessor& 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, elemGeoInfo);

    FiniteElementMappingType::computePhysicalSpacePoints(mappingFunction.getMappingFunction(),referenceSpaceNodes,elemType,elem_coord,m_order);
    typename Quadrature::PointVector _auxQuadrature;
    _auxQuadrature.resize(referenceSpaceNodes.rows(), Eigen::NoChange);
    computeScaledQuadrature(mappingFunction, elemGeoInfo, _auxQuadrature);

    FiniteElementBasisType::reInit(basisFunction, _auxQuadrature, elemType, b_order);
    computeBasisFunction(basisFunction, elemGeoInfo);
  }


private:
  template<unsigned int Dim,
	   template <unsigned int, unsigned int> class MappingBasisFunctionType>
    static void computeCentroidAndScaling(const TopoPointVector& coord, ElemGeometryInfo<ItemMappingFunction<MappingBasisFunctionType<Dim,Dim> > >& elemGeoInfo)
  {
    typename MappingBasisFunctionType<Dim,Dim>::CoordVector& centroid = elemGeoInfo.getCentroid();
    typename MappingBasisFunctionType<Dim,Dim>::DistanceVector& max_distance = elemGeoInfo.getMaxDistance();
    
    centroid.setZero();
    for (unsigned int n=0; n<coord.rows(); n++)
      for (unsigned int d=0; d<Dim; d++)
        centroid(d) += coord(n,d);
    centroid /= static_cast<double>(coord.rows());

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

  template <template <unsigned int, unsigned int> class MappingBasisFunctionType>
    static void computeCentroidAndScaling(const TopoPointVector& coord, ElemGeometryInfo<ItemMappingFunction<MappingBasisFunctionType<1,2> > >& elemGeoInfo)
  {
    typename MappingBasisFunctionType<1,2>::CoordVector& centroid = elemGeoInfo.getCentroid();
    typename MappingBasisFunctionType<1,2>::DistanceVector& max_distance = elemGeoInfo.getMaxDistance();
    
    centroid.setZero();
    for (unsigned int n=0; n<coord.rows(); n++)
      for (unsigned int d=0; d<2; d++)
        centroid(d) += coord(n,d);
    centroid /= static_cast<double>(coord.rows());

    typename MappingBasisFunctionType<1,2>::CoordVector distanceVec;
    distanceVec.setZero();
    for (unsigned int p = 0; p < coord.rows(); p++)
      for (unsigned int d = 0; d < 2; d++)
      {
        double distance = std::abs(centroid(d) - coord(p,d)); 
        distanceVec(d) = std::max(distance, distanceVec(d));
      }
    max_distance(0) = distanceVec.squaredNorm();
  }
  
  template <unsigned int FaceDim, unsigned int ElemDIm,
	    template <unsigned int, unsigned int> class MappingBasisFunctionType>
    static void computeScaledQuadrature(const ItemMappingFunction<MappingBasisFunctionType<FaceDim,ElemDIm> >& mappingFunction,
                                        const ElemGeometryInfo<ItemMappingFunction<MappingBasisFunctionType<ElemDIm,ElemDIm> > >& elemGeoInfo, 
					typename Quadrature::PointVector& auxQuadrature)
  {
    const typename MappingBasisFunctionType<ElemDIm,ElemDIm>::CoordVector& centroid = elemGeoInfo.getCentroid();
    const typename MappingBasisFunctionType<ElemDIm,ElemDIm>::DistanceVector& max_distance = elemGeoInfo.getMaxDistance();
    const typename MappingBasisFunctionType<FaceDim,ElemDIm>::PointVector& physical_quadrature = mappingFunction.getQpointX();
    for (unsigned int qp = 0; qp < auxQuadrature.rows(); qp ++)
      for (unsigned int d = 0; d < ElemDIm; d++)
        auxQuadrature(qp,d) = (physical_quadrature(qp,d) - centroid(d))/max_distance(d); 
  }
  
  template <template <unsigned int, unsigned int> class MappingBasisFunctionType>
    static void computeScaledQuadrature(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.;
//      if ((firstVertex.transpose()-physical_quadrature.row(qp).segment(0,2)).norm() >= (firstVertex - centroid).norm())
//        sign=1.;
//      else
//	sign=-1.;
//      max_distance(0) = max_distance.norm();
//      max_distance(1) = 0.;
//      auxQuadrature(qp,0) = sign * (physical_quadrature.block(qp,0,1,2).transpose() - centroid).norm()/max_distance(0); 
//      auxQuadrature(qp,1) = 0.;
    }
  }

  template<unsigned int ElemDIm, class ElemGeometryInfoType> static void computeBasisFunction(BasisFunction<ElemDIm>& basisFunction, 
                                                                                              const ElemGeometryInfoType& elemGeoInfo)
  {
    if(basisFunction.computeShapeDeriv())
    {
      const typename ElemGeometryInfoType::DistanceVector& max_distance = elemGeoInfo.getMaxDistance();
      unsigned int firstIndex, lastIndex;
      typename BasisFunction<ElemDIm>::ShapeDerivMatrix& shapeDerivMatrix = basisFunction.getShapeFunctionDeriv();
      for (unsigned int k = 0; k < ElemDIm; k++)
        for (unsigned int i = 0; i < shapeDerivMatrix(0).rows(); i++)
          for (unsigned int qp = 0; qp < shapeDerivMatrix(0).cols(); qp++)
            shapeDerivMatrix(k)(i,qp) /= max_distance(k);
    }
  }
};

#endif
