#ifndef __lagrangian_finite_element_basis_h__
#define __lagrangian_finite_element_basis_h__

#include "FiniteElementBasis.hpp"
#include "FiniteElementMappingBasis.hpp"
#include "EnumElemType.hpp"
#include "JacobiPoly.hpp"
#include "ReferenceFiniteElement.hpp"
#include "BasisFunction.hpp"
#include "LagrangeNodalMappingFunction.hpp"
#include "Tuple.hpp"
#include "Quadrature.hpp"
#include "Eigen/Core"
#include "Eigen/Dense"
#include "Eigen/SVD"

template<class ToolBasisFunctionT>
class LagrangianFiniteElementBasis
{
public:
  typedef FiniteElementBasis<ToolBasisFunctionT> ToolBasisFunctionType;
  typedef Quadrature::PointVector PointVector;

  LagrangianFiniteElementBasis() 
    {
      ;
    }
  
  ~LagrangianFiniteElementBasis() {}
  
public:
  template<unsigned int Dim> void initBasisFunction(BasisFunction<Dim>& basisFunction, BasisFunction<Dim>& toolBasisFunction)
  {
    typename BasisFunction<Dim>::ShapeMatrix& shapeMatrix = basisFunction.getShapeFunction();
    typename BasisFunction<Dim>::ShapeMatrix& toolShapeMatrix = toolBasisFunction.getShapeFunction();
    this->pseudoInvertVandermondeMatrix(shapeMatrix);
    //toolShapeMatrix.conservativeResize(_vandermondeInvMatrix.cols(),Eigen::NoChange);
    shapeMatrix.noalias() = _vandermondeInvMatrix * toolShapeMatrix;
  }
  
  template<unsigned int Dim> void initBasisFunctionDeriv(BasisFunction<Dim>& basisFunction, BasisFunction<Dim>& toolBasisFunction)
  {
    typename BasisFunction<Dim>::ShapeDerivMatrix& shapeDerivMatrix = basisFunction.getShapeFunctionDeriv();
    typename BasisFunction<Dim>::ShapeDerivMatrix& toolShapeDerivMatrix = toolBasisFunction.getShapeFunctionDeriv();
    typename BasisFunction<Dim>::ShapeMatrix& shapeMatrix = basisFunction.getShapeFunction();
    this->pseudoInvertVandermondeMatrix(shapeMatrix);
    for (unsigned int d = 0; d < Dim; d++)
      toolShapeDerivMatrix(d).noalias() = _vandermondeInvMatrix * shapeDerivMatrix(d);
  }

  template<unsigned int Dim, unsigned int DIm> static void initBasisFunction(LagrangeNodalMappingFunction<Dim, DIm>& mappingFunction, const PointVector& points)
  {
    const ElemType itemType = mappingFunction.getItemType();
    const unsigned int b_order = mappingFunction.getOrder();
    BasisFunction<Dim>& tool_basisFunction = mappingFunction.getVandermondeBasisFunction();
    Eigen::MatrixXd& vandermondeInvertedMat = mappingFunction.getVandermondeInvMat(); 
    // get nodes and compute vandermonde matrix, check if we really need to init in case of reInit 
    if (itemType != mappingFunction.getVandermondeItemType() || b_order != mappingFunction.getVandermondeOrder())
    {
      Quadrature::PointVector nodesLocation;
      computeNodesLocation(b_order, itemType, nodesLocation); 
      tool_basisFunction.doComputeShape(true);
      tool_basisFunction.doComputeShapeDeriv(false);
      ToolBasisFunctionType::reInit(tool_basisFunction, nodesLocation, itemType, b_order);
      invertVandermondeMatrix(tool_basisFunction.getShapeFunction(), nodesLocation, vandermondeInvertedMat);
      mappingFunction.getVandermondeItemType() = itemType;
      mappingFunction.getVandermondeOrder() = b_order;
    }
    // initialize tool basis on points
    if (!mappingFunction.computeShape())
      tool_basisFunction.doComputeShape(false);
    if (mappingFunction.computeShapeDeriv())
      tool_basisFunction.doComputeShapeDeriv(true);
    ToolBasisFunctionType::reInit(tool_basisFunction, points, itemType, b_order);
    // compute lagrange basis on points
    if (mappingFunction.computeShape())
    {
      typename BasisFunction<Dim>::ShapeMatrix& shapeMatrix = mappingFunction.getShapeFunction();
      typename BasisFunction<Dim>::ShapeMatrix& toolShapeMatrix = tool_basisFunction.getShapeFunction();
      shapeMatrix.noalias() = vandermondeInvertedMat * toolShapeMatrix;
    }
    if (mappingFunction.computeShapeDeriv())
    {
      typename BasisFunction<Dim>::ShapeDerivMatrix& shapeDerivMatrix = mappingFunction.getShapeFunctionDeriv();
      typename BasisFunction<Dim>::ShapeDerivMatrix& toolShapeDerivMatrix = tool_basisFunction.getShapeFunctionDeriv();
      for (unsigned int d = 0; d < Dim; d++)
        shapeDerivMatrix(d).noalias() = vandermondeInvertedMat * toolShapeDerivMatrix(d);
    }
  }
  
  template<unsigned int Dim> static void initBasisFunction(BasisFunction<Dim>& mappingFunction, const PointVector& points)
  {
    const ElemType itemType = mappingFunction.getItemType();
    const unsigned int b_order = mappingFunction.getOrder();
    BasisFunction<Dim> tool_basisFunction;
    Eigen::MatrixXd vandermondeInvertedMat;
    Quadrature::PointVector nodesLocation;
    computeNodesLocation(b_order, itemType, nodesLocation); 
    tool_basisFunction.doComputeShape(true);
    tool_basisFunction.doComputeShapeDeriv(false);
    ToolBasisFunctionType::reInit(tool_basisFunction, nodesLocation, itemType, b_order);
    invertVandermondeMatrix(tool_basisFunction.getShapeFunction(), nodesLocation, vandermondeInvertedMat);
    // initialize tool basis on points
    if (!mappingFunction.computeShape())
      tool_basisFunction.doComputeShape(false);
    if (mappingFunction.computeShapeDeriv())
      tool_basisFunction.doComputeShapeDeriv(true);
    ToolBasisFunctionType::reInit(tool_basisFunction, points, itemType, b_order);
    // compute lagrange basis on points
    if (mappingFunction.computeShape())
    {
      typename BasisFunction<Dim>::ShapeMatrix& shapeMatrix = mappingFunction.getShapeFunction();
      typename BasisFunction<Dim>::ShapeMatrix& toolShapeMatrix = tool_basisFunction.getShapeFunction();
      shapeMatrix.noalias() = vandermondeInvertedMat * toolShapeMatrix;
    }
    if (mappingFunction.computeShapeDeriv())
    {
      typename BasisFunction<Dim>::ShapeDerivMatrix& shapeDerivMatrix = mappingFunction.getShapeFunctionDeriv();
      typename BasisFunction<Dim>::ShapeDerivMatrix& toolShapeDerivMatrix = tool_basisFunction.getShapeFunctionDeriv();
      for (unsigned int d = 0; d < Dim; d++)
        shapeDerivMatrix(d).noalias() = vandermondeInvertedMat * toolShapeDerivMatrix(d);
    }
  }
  
  static unsigned int dofsNumber(const unsigned int Dim, const unsigned int order, const ElemType itemType)
  {
    if (itemType == INVALID_ELEM)
    {
      std::cout<<"Error in ExplicitLagrangianFiniteElementBasis, dofs number required without specifing itemType"<<std::endl; 
      std::exit(1);
    }
    return nbShapeFunction(order, itemType);
  }
  
  static unsigned int nbShapeFunction(const unsigned int order, const ElemType itemType);
  
  static bool is_hierarchical();

private:
  static void computeNodesLocation(const unsigned int bOrder, const ElemType itemType, Quadrature::PointVector& nodesLocation)
  {
    ReferenceFiniteElement::getVerticesN(itemType, nodesLocation);
  }
  
  static void invertVandermondeMatrix(const Eigen::MatrixXd& shapeMatrix, const Quadrature::PointVector& nodesLocation, Eigen::MatrixXd& vandInvMatrix)
  {
    // vandermonde inversion
    unsigned int size = nodesLocation.rows();
    assert(shapeMatrix.rows() == size);
    Eigen::MatrixXd rhsMatrix = Eigen::MatrixXd::Identity(size,size);
    vandInvMatrix = shapeMatrix.colPivHouseholderQr().solve(rhsMatrix);
  }

  void pseudoInvertVandermondeMatrix(Eigen::MatrixXd& shapeMatrix)
  {
    // vandermonde pseudo inversion
    unsigned int cols = shapeMatrix.cols();
    unsigned int rows = shapeMatrix.rows();
    // conservative resize above needed
//    shapeMatrix.conservativeResize(cols,cols);
//    Eigen::MatrixXd rhsMatrix = Eigen::MatrixXd::Identity(cols,cols);
//    _vandermondeInvMatrix = shapeMatrix.fullPivHouseholderQr().solve(rhsMatrix);
// 
    //overdermitato
    // conservative resize above needed
//    Eigen::MatrixXd rhsMatrix = Eigen::MatrixXd::Identity(rows,rows);
//    _vandermondeInvMatrix.resize(cols,cols);
//    Eigen::JacobiSVD<Eigen::MatrixXd> svd(shapeMatrix, Eigen::ComputeThinU | Eigen::ComputeThinV);
//    std::cout<<"minchia"<<std::endl;
//    for (unsigned int i = 0; i < cols; i++)
//      _vandermondeInvMatrix.col(i) = svd.solve(rhsMatrix.col(i));
//    std::cout<<"minchia 2"<<std::endl;

//    //overdermitato
//    // conservative resize above not needed
//    Eigen::MatrixXd rhsMatrix = Eigen::MatrixXd::Identity(rows,rows);
//    _vandermondeInvMatrix.resize(cols,rows);
//    Eigen::JacobiSVD<Eigen::MatrixXd> svd(shapeMatrix, Eigen::ComputeThinU | Eigen::ComputeThinV);
//    for (unsigned int i = 0; i < rows; i++)
//      _vandermondeInvMatrix.col(i) = svd.solve(rhsMatrix.col(i));

    //sottodermitato
    // conservative resize above not needed
    Eigen::MatrixXd rhsMatrix = Eigen::MatrixXd::Identity(rows,rows);
    _vandermondeInvMatrix.resize(cols,rows);
    Eigen::JacobiSVD<Eigen::MatrixXd,Eigen::HouseholderQRPreconditioner> svd(shapeMatrix, Eigen::ComputeThinU | Eigen::ComputeThinV);
    for (unsigned int i = 0; i < rows; i++)
      _vandermondeInvMatrix.col(i) = svd.solve(rhsMatrix.col(i));
  }
  
private:
  Eigen::MatrixXd _vandermondeInvMatrix;
};

#endif 
