#include "Eigen/Dense"
#include "SpaCN.hpp"
#include "EntityTypesMesh.hpp"
#include <iostream>
#include <iomanip>
#include <string>
#include <vector>
#include <array>
#include <memory>

#ifndef __spa_mesh_hpp__
#define __spa_mesh_hpp__

namespace SpaMesh
{
  namespace Connectivity
  {
    template <unsigned int dim>
    struct MeshTypes 
    {
    };
    
    template<>
    struct MeshTypes<0> 
    {
      enum class Types {
        node = 0,
        NumberOfElemTypesMesh = 1
      };
    };
    
    template<>
    struct MeshTypes<1> 
    {
      enum class Types {
        line  = 0,
        NumberOfElemTypesMesh = 1
      };
    };
    
    template<>
    struct MeshTypes<2> 
    {
      enum class Types {
        tri   = 0,
        quad  = 1,
        NumberOfElemTypesMesh = 2
      };
    };
    
    template<>
    struct MeshTypes<3> 
    {
      enum class Types {
        tet   = 0,
        pyram = 1,
        prism = 2,
        hex   = 3,
        NumberOfElemTypesMesh = 4
      };
    };
  
    //Elem -> Node connectivity
    using ConnArray = const std::tuple<std::array<int,static_cast<std::size_t>(MeshTypes<0>::Types::NumberOfElemTypesMesh)>,
                                       std::array<int,static_cast<std::size_t>(MeshTypes<1>::Types::NumberOfElemTypesMesh)>,
  	                               std::array<int,static_cast<std::size_t>(MeshTypes<2>::Types::NumberOfElemTypesMesh)>,
  				       std::array<int,static_cast<std::size_t>(MeshTypes<3>::Types::NumberOfElemTypesMesh)> >;
    static ConnArray elemTypesNodeNumber({1}, //node
                                         {2}, //line
        		                 {3, //tri
                                          4}, // quad
        		                 {4, //tet
        		                  5,//pyram
        		                  6, //prism
                                          8});  //hex
    const size_t maxNodesNumber = 8;
    typedef Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor, maxNodesNumber, Eigen::Dynamic> ElemNodesConnMatrixDynamic;
    
    typedef Eigen::Matrix<int,    Eigen::Dynamic, 1, Eigen::ColMajor, Connectivity::maxNodesNumber, 1> TopoIndexVector;
    typedef Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor, SpaCN::MAX_NODES_PER_ELEMENT, 3> TopoPointVector;
    typedef std::vector<TopoIndexVector> ElemNodesConnVectorDynamic;
    
    using NodesArrayAllTypes = const std::array<int,ElemTypesMesh::NumberOfElemTypesMesh>;
    static NodesArrayAllTypes elemTypesNodesNumberAll = {{1, //node
                                                          2, //line
        		                                  3, //tri
                                                          4, //quad
        		                                  4, //tet
        		                                  5,//pyram
        		                                  6, //prism
                                                          8}}; //hex
    //Elem -> Side connectivity
    using SidesArray = const std::tuple<std::array<int,static_cast<std::size_t>(MeshTypes<0>::Types::NumberOfElemTypesMesh)>,
                                        std::array<int,static_cast<std::size_t>(MeshTypes<1>::Types::NumberOfElemTypesMesh)>,
  	                                std::array<int,static_cast<std::size_t>(MeshTypes<2>::Types::NumberOfElemTypesMesh)>,
  				        std::array<int,static_cast<std::size_t>(MeshTypes<3>::Types::NumberOfElemTypesMesh)> >;
    static SidesArray elemTypesSideNumber({0}, //node
                                          {2}, //line
        		                  {3, //tri
                                           4}, //quad
  				          {4, //tet
        		                   5,//pyram
        		                   5, //prism
                                           6});  //hex
    const size_t maxSidesNumber = 6;
    typedef Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor, maxSidesNumber, Eigen::Dynamic> ElemSidesConnMatrixDynamic;
    typedef Eigen::Matrix<int, Eigen::Dynamic, 1,              Eigen::ColMajor, maxSidesNumber, 1>              ElemConnVector;
    typedef ElemSidesConnMatrixDynamic::InnerIterator NeighborIterator;
    
    using SidesArrayAllTypes = const std::array<int,ElemTypesMesh::NumberOfElemTypesMesh>;
    static SidesArrayAllTypes elemTypesSideNumberAll = {{0, //node
                                                         2, //line
        		                                 3, //tri
                                                         4, //quad
        		                                 4, //tet
        		                                 5,//pyram
        		                                 5, //prism
                                                         6}}; //hex
    
    //Side -> Elem connectivity
    typedef Eigen::Matrix<int, 2, Eigen::Dynamic, Eigen::ColMajor> SideElemsConnMatrix;
    typedef Eigen::Matrix<int, 2, 1, Eigen::ColMajor> NeighborsPair;
    typedef std::vector<NeighborsPair> SideElemsConnVector;
  
    template <unsigned int dim> 
    struct dimMinusOne
    {
      static void dimMinusOneTypes(int& dimMinusOneSize)
      {
        dimMinusOneSize += static_cast<int>(MeshTypes<dim-1>::Types::NumberOfElemTypesMesh);
        dimMinusOne<dim-1>::dimMinusOneTypes(dimMinusOneSize);
      }
    };
  
    template<>
    struct dimMinusOne<0>
    {
      static void dimMinusOneTypes(int& dimMinusOneSize)
      {
      }
    };
    
    static const unsigned int dimensionFromType(const ElemTypesMesh elemType) 
    {
      switch (elemType)
      {
        case 0: 
  	return 0;
  	break;
        case 1:
  	return 1;
  	break;
        case 2:
        case 3:
  	return 2;
  	break;
        case 4:
        case 5:
        case 6:
        case 7:
  	return 3;
  	break;
        default:
  	std::cerr<<"Dimension from type required for type "<<elemType<<std::endl;
  	std::abort();
  	break;
      }
    }
  
  };//namespace Connectivity
  
  template<unsigned int dim>
  class DimMeshBase
  {
    public:
    using ElemNodesConnectivity = std::array<Connectivity::ElemNodesConnMatrixDynamic, static_cast<std::size_t>(Connectivity::MeshTypes<dim>::Types::NumberOfElemTypesMesh)>;
    using PreviousTypesSize = std::array<int, static_cast<std::size_t>(Connectivity::MeshTypes<dim>::Types::NumberOfElemTypesMesh)+1>;
    
    DimMeshBase()
    {
      for (unsigned int i = 0; i < std::get<dim>(Connectivity::elemTypesNodeNumber).size(); i++)
      {
        elemNodesConnectivity[i].resize(std::get<dim>(Connectivity::elemTypesNodeNumber)[i],0);
      }
    }

    const int dimMeshElemType(const ElemTypesMesh elemType) const 
    {
      int dimMinusOneTypesSize = 0;
      Connectivity::dimMinusOne<dim>::dimMinusOneTypes(dimMinusOneTypesSize);
      return elemType - dimMinusOneTypesSize;
    }
    
    const int elemTypeFromDimType(const int typeDimIndex) const 
    {
      int dimMinusOneTypesSize = 0;
      Connectivity::dimMinusOne<dim>::dimMinusOneTypes(dimMinusOneTypesSize);
      return typeDimIndex + dimMinusOneTypesSize;
    }
  
    const PreviousTypesSize& computePreviousTypesSize() 
    {
      previousTypesSize[0] = 0;
      for (unsigned int type = 1; type <= elemNodesConnectivity.size(); type++)
      {
        previousTypesSize[type] = previousTypesSize[type-1] + elemNodesConnectivity[type-1].cols();
      }
      return previousTypesSize;
    }
    
    const unsigned int typeFromLID (const int lid, int & lidType) const
    {
      unsigned int type = 0;
      while (lid - previousTypesSize[type+1] >= 0) 
	type++;
      lidType = lid - previousTypesSize[type];
      return type;
    }
    
    const unsigned int typeFromLID (const int lid) const
    {
      unsigned int type = 0;
      while (lid - previousTypesSize[type+1] >= 0) 
	type++;
      return type;
    }
    
    //Elem Nodes Connectivity
    void resizeElemNodesConn(const typename Connectivity::MeshTypes<dim>::Types elemType, const std::size_t numberOfTypeElements)
    {
      this->elemNodesConnectivity[static_cast<size_t>(elemType)].resize(Eigen::NoChange,numberOfTypeElements);
    }
  
    void resizeElemNodesConnConservative(const typename Connectivity::MeshTypes<dim>::Types elemType, const std::size_t numberOfTypeElements, std::size_t& cols)
    {
      cols = this->elemNodesConnectivity[static_cast<size_t>(elemType)].cols();
      if (cols > 0)
        this->elemNodesConnectivity[static_cast<size_t>(elemType)].conservativeResize(Eigen::NoChange,cols+numberOfTypeElements);
      else
        this->elemNodesConnectivity[static_cast<size_t>(elemType)].resize(Eigen::NoChange,numberOfTypeElements);
    }
  
    Connectivity::ElemNodesConnMatrixDynamic& getElemNodesConnForType(const typename Connectivity::MeshTypes<dim>::Types elemType)
    {
      return elemNodesConnectivity[static_cast<size_t>(elemType)];
    }
    const Connectivity::ElemNodesConnMatrixDynamic& getElemNodesConnForType(const typename Connectivity::MeshTypes<dim>::Types elemType) const 
    {
      return elemNodesConnectivity[static_cast<size_t>(elemType)];
    }
    
    ElemNodesConnectivity elemNodesConnectivity;
    PreviousTypesSize previousTypesSize;
  };

  template<unsigned int dim>
  class ElemMesh : public DimMeshBase<dim>
  {
    public:
    using ElemSidesConnectivity = std::array<Connectivity::ElemSidesConnMatrixDynamic, static_cast<std::size_t>(Connectivity::MeshTypes<dim>::Types::NumberOfElemTypesMesh)>;
  
    ElemMesh()
    {
      for (unsigned int i = 0; i < std::get<dim>(Connectivity::elemTypesSideNumber).size(); i++)
      {
        elemSidesConnectivity[i].resize(std::get<dim>(Connectivity::elemTypesSideNumber)[i],0);
        elemElemsConnectivity[i].resize(std::get<dim>(Connectivity::elemTypesSideNumber)[i],0);
      }
    }
  
    //Elem Sides Connectivity
    void resizeElemSidesConn(const typename Connectivity::MeshTypes<dim>::Types elemType, const std::size_t numberOfTypeElements)
    {
      this->elemSidesConnectivity[static_cast<size_t>(elemType)].resize(Eigen::NoChange,numberOfTypeElements);
    }
  
    void resizeElemSidesConnConservative(const typename Connectivity::MeshTypes<dim>::Types elemType, const std::size_t numberOfTypeElements, std::size_t& cols)
    {
      cols = this->elemSidesConnectivity[static_cast<size_t>(elemType)].cols();
      if (cols > 0)
        this->elemSidesConnectivity[static_cast<size_t>(elemType)].conservativeResize(Eigen::NoChange,cols+numberOfTypeElements);
      else
        this->elemSidesConnectivity[static_cast<size_t>(elemType)].resize(Eigen::NoChange,numberOfTypeElements);
    }
  
    Connectivity::ElemSidesConnMatrixDynamic& getElemSidesConnForType(const typename Connectivity::MeshTypes<dim>::Types elemType)
    {
      return elemSidesConnectivity[static_cast<size_t>(elemType)];
    }
    const Connectivity::ElemSidesConnMatrixDynamic& getElemSidesConnForType(const typename Connectivity::MeshTypes<dim>::Types elemType) const 
    {
      return elemSidesConnectivity[static_cast<size_t>(elemType)];
    }
  
    void printInfo()
    {
      for (unsigned int type = 0; type < this->elemNodesConnectivity.size(); type++)
      {
        std::cout<<std::setw(15)<<"Elem Nodes"<<" Connectivity for Type "<<std::setw(5)<<SpaCN::ElemTypesMeshName(static_cast<ElemTypesMesh>(this->elemTypeFromDimType(type)))
						      <<std::setw(10)<<": nodes per elem "<<std::setw(2)<<this->elemNodesConnectivity[type].rows()
	                                              <<", elem card "<<this->elemNodesConnectivity[type].cols()<<std::endl;
      }
      for (unsigned int type = 0; type < elemSidesConnectivity.size(); type++)
      {
        std::cout<<std::setw(15)<<"Elem Sides"<<" Connectivity for Type "<<std::setw(5)<<SpaCN::ElemTypesMeshName(static_cast<ElemTypesMesh>(this->elemTypeFromDimType(type)))
						      <<std::setw(10)<<": sides per elem "<<std::setw(2)<<elemSidesConnectivity[type].rows()
	                                              <<", elem card "<<elemSidesConnectivity[type].cols()<<std::endl;
      }
      for (unsigned int type = 0; type < elemElemsConnectivity.size(); type++)
      {
        std::cout<<std::setw(15)<<"Elem Elems"<<" Connectivity for Type "<<std::setw(5)<<SpaCN::ElemTypesMeshName(static_cast<ElemTypesMesh>(this->elemTypeFromDimType(type)))
						     <<std::setw(10)<<": neighbors card "<<std::setw(2)<<elemElemsConnectivity[type].rows()
	                                             <<", elem card "<<elemElemsConnectivity[type].cols()<<std::endl;
      }
    }
    
    Eigen::VectorXi elemVec;
    ElemSidesConnectivity elemSidesConnectivity;
    ElemSidesConnectivity elemElemsConnectivity;
  };
  
  template<unsigned int dim>
  class FaceMesh : public DimMeshBase<dim>
  {
    public:
    using TypesCount =        std::array<int, static_cast<std::size_t>(Connectivity::MeshTypes<dim>::Types::NumberOfElemTypesMesh)>;
    using ElemNodesConnVector   = std::array<Connectivity::ElemNodesConnVectorDynamic, static_cast<std::size_t>(Connectivity::MeshTypes<dim>::Types::NumberOfElemTypesMesh)>;
    using SideElemsConnectivity = std::array<Connectivity::SideElemsConnMatrix,        static_cast<std::size_t>(Connectivity::MeshTypes<dim>::Types::NumberOfElemTypesMesh)>;
    using SideElemsConnVector   = std::array<Connectivity::SideElemsConnVector,        static_cast<std::size_t>(Connectivity::MeshTypes<dim>::Types::NumberOfElemTypesMesh)>;
  
    FaceMesh()
    {
      for (unsigned int i = 0; i < std::get<dim>(Connectivity::elemTypesNodeNumber).size(); i++)
      {
        sideElemsConnectivity[i].resize(Eigen::NoChange,0);
      }
    }
  
    //Side Elems Connectivity
    void resizeSideElemsConn(const typename Connectivity::MeshTypes<dim>::Types elemType, const std::size_t numberOfTypeElements)
    {
      this->sideElemsConnectivity[static_cast<size_t>(elemType)].resize(Eigen::NoChange,numberOfTypeElements);
    }
  
    void resizeSideElemsConnConservative(const typename Connectivity::MeshTypes<dim>::Types elemType, const std::size_t numberOfTypeElements, std::size_t& cols)
    {
      cols = this->sideElemsConnectivity[static_cast<size_t>(elemType)].cols();
      if (cols > 0)
        this->sideElemsConnectivity[static_cast<size_t>(elemType)].conservativeResize(Eigen::NoChange,cols+numberOfTypeElements);
      else
        this->sideElemsConnectivity[static_cast<size_t>(elemType)].resize(Eigen::NoChange,numberOfTypeElements);
    }
  
    Connectivity::SideElemsConnMatrix& getSideElemsConnForType(const typename Connectivity::MeshTypes<dim>::Types elemType)
    {
      return sideElemsConnectivity[static_cast<size_t>(elemType)];
    }
    const Connectivity::SideElemsConnMatrix& getSideElemsConnForType(const typename Connectivity::MeshTypes<dim>::Types elemType) const 
    {
      return sideElemsConnectivity[static_cast<size_t>(elemType)];
    }
  
    void printInfo()
    {
      for (unsigned int type = 0; type < this->elemNodesConnectivity.size(); type++)
      {
        std::cout<<std::setw(15)<<"Elem Nodes"<<" Connectivity for Type "<<std::setw(5)<<SpaCN::ElemTypesMeshName(static_cast<ElemTypesMesh>(this->elemTypeFromDimType(type)))
						      <<std::setw(10)<<": nodes per elem "<<std::setw(2)<<this->elemNodesConnectivity[type].rows()
	                                              <<", elem card "<<this->elemNodesConnectivity[type].cols()<<std::endl;
      }
      for (unsigned int type = 0; type < sideElemsConnectivity.size(); type++)
      {
        std::cout<<std::setw(15)<<"Side Elems"<<" Connectivity for Type "<<std::setw(5)<<SpaCN::ElemTypesMeshName(static_cast<ElemTypesMesh>(this->elemTypeFromDimType(type)))
						      <<std::setw(10)<<": neighbors card "<<std::setw(2)<<sideElemsConnectivity[type].rows()
	                                              <<", elem card "<<sideElemsConnectivity[type].cols()<<std::endl;
      }
    }
  
    SideElemsConnectivity sideElemsConnectivity;
  };
 

  template<unsigned int dim>
  class Mesh
  {
    public:
    typedef Connectivity::TopoIndexVector TopoIndexVector;
    typedef Connectivity::TopoPointVector TopoPointVector;
    Mesh()
    {
    }
  
    //Elem Nodes Connectivity
    void resizeElemNodesConn(const ElemTypesMesh elemType, const std::size_t numberOfTypeElements)
    {
      unsigned int elemDim = Connectivity::dimensionFromType(elemType);
      if (elemDim == dim)
      {
        std::size_t elemMeshType = elemMesh.dimMeshElemType(elemType);
        this->elemMesh.elemNodesConnectivity[elemMeshType].resize(Eigen::NoChange,numberOfTypeElements);
      }
      else if (elemDim == dim-1)
      {
        std::size_t faceMeshType = faceMesh.dimMeshElemType(elemType);
        this->faceMesh.elemNodesConnectivity[faceMeshType].resize(Eigen::NoChange,numberOfTypeElements);
      }
      else
      {
        std::logic_error("halt");
        std::abort();
      }
    }
  
    void resizeElemNodesConnConservative(const ElemTypesMesh elemType, const std::size_t numberOfTypeElements, std::size_t& cols)
    {
      unsigned int elemDim = Connectivity::dimensionFromType(elemType);
      if (elemDim == dim)
      {
        std::size_t elemMeshType = elemMesh.dimMeshElemType(elemType);
        cols = this->elemMesh.elemNodesConnectivity[elemMeshType].cols();
        if (cols > 0)
          this->elemMesh.elemNodesConnectivity[elemMeshType].conservativeResize(Eigen::NoChange,cols+numberOfTypeElements);
        else
          this->elemMesh.elemNodesConnectivity[elemMeshType].resize(Eigen::NoChange,numberOfTypeElements);
      }
      else if (elemDim == dim-1)
      {
        std::size_t faceMeshType = faceMesh.dimMeshElemType(elemType);
        cols = this->faceMesh.elemNodesConnectivity[faceMeshType].cols();
        if (cols > 0)
          this->faceMesh.elemNodesConnectivity[faceMeshType].conservativeResize(Eigen::NoChange,cols+numberOfTypeElements);
        else
          this->faceMesh.elemNodesConnectivity[faceMeshType].resize(Eigen::NoChange,numberOfTypeElements);
      }
      else
      {
        std::logic_error("halt");
      }
    }
  
    Connectivity::ElemNodesConnMatrixDynamic& getElemNodesConnForType(const ElemTypesMesh elemType)
    {
      unsigned int elemDim = Connectivity::dimensionFromType(elemType);
      if (elemDim == dim)
      {
        std::size_t elemMeshType = elemMesh.dimMeshElemType(elemType);
        return this->elemMesh.elemNodesConnectivity[elemMeshType];
      }
      else if (elemDim == dim-1)
      {
        std::size_t faceMeshType = faceMesh.dimMeshElemType(elemType);
        return this->faceMesh.elemNodesConnectivity[faceMeshType];
      }
      else
      {
        std::cout<<"Mesh error: Element of dimension "<<elemDim<<" requested for "<<dim<<" dimensional mesh "<<std::endl;
        std::logic_error("halt");
        std::exit(0);
        return this->faceMesh.elemNodesConnectivity[0];
      }
    }
    const Connectivity::ElemNodesConnMatrixDynamic& getElemNodesConnForType(const ElemTypesMesh elemType) const 
    {
      unsigned int elemDim = Connectivity::dimensionFromType(elemType);
      if (elemDim == dim)
      {
        std::size_t elemMeshType = elemMesh.dimMeshElemType(elemType);
        return this->elemMesh.elemNodesConnectivity[elemMeshType];
      }
      else if (elemDim == dim-1)
      {
        std::size_t faceMeshType = faceMesh.dimMeshElemType(elemType);
        return this->faceMesh.elemNodesConnectivity[faceMeshType];
      }
      else
      {
        std::cout<<"Mesh error: Element of dimension "<<elemDim<<" requested for "<<dim<<" dimensional mesh "<<std::endl;
        std::logic_error("halt");
        std::exit(0);
        return this->faceMesh.elemNodesConnectivity[0];
      }
    }
    const bool trueIfTypeExists(const ElemTypesMesh elemType) const 
    {
      unsigned int elemDim = Connectivity::dimensionFromType(elemType);
      if (elemDim == dim)
      {
        std::size_t elemMeshType = elemMesh.dimMeshElemType(elemType);
        return true;
      }
      else if (elemDim == dim-1)
      {
        std::size_t faceMeshType = faceMesh.dimMeshElemType(elemType);
        return true;
      }
      else
      {
        return false;
      }
    }
  
    unsigned long numberOfNodes() const
    {
      return nodesCoords.cols();
    }
    
    void printInfo()
    {
      std::cout<<"Elem Mesh info..."<<std::endl;
      elemMesh.printInfo();
      std::cout<<"Face Mesh info..."<<std::endl;
      faceMesh.printInfo();
      std::cout<<"Nodes info..."<<std::endl;
      std::cout<<std::setw(16)<<"Nodes card "<<nodesCoords.cols()<<std::endl;
    }
  
    typename FaceMesh<dim-1>::TypesCount computeElemNeighbors(const bool computeFaces)
    {
      const typename ElemMesh<dim>::PreviousTypesSize& pTypesSize = elemMesh.computePreviousTypesSize();
      std::vector<std::vector<int> > nodesToElemConn(this->numberOfNodes());
      for (unsigned int type = 0; type < elemMesh.elemNodesConnectivity.size(); type++)
      {
        const Connectivity::ElemNodesConnMatrixDynamic& elemNodesST = elemMesh.elemNodesConnectivity[type];
        for (unsigned int elemTypeID = 0; elemTypeID < elemNodesST.cols(); elemTypeID++)
        {
          int elemID = elemTypeID + pTypesSize[type]; 
          for (unsigned int row = 0; row < elemNodesST.rows(); row++)
            nodesToElemConn[elemNodesST(row,elemTypeID)].push_back(elemID);
        }
      }
      for (unsigned int i = 0; i < nodesToElemConn.size(); i++)
        std::sort(nodesToElemConn[i].begin(),nodesToElemConn[i].end());
      
      typename FaceMesh<dim-1>::ElemNodesConnVector iFaceNodesConnVector;
      typename FaceMesh<dim-1>::ElemNodesConnVector bFaceNodesConnVector;
      typename FaceMesh<dim-1>::TypesCount faceNumberEst;
      faceNumberEst.fill(0);
      TopoIndexVector elemSideIndicesRefSpace;
      TopoIndexVector elemSideNodeIDs;
      for (unsigned int type = 0; type < elemMesh.elemNodesConnectivity.size(); type++)
      {
        const Connectivity::ElemNodesConnMatrixDynamic& elemNodesST = elemMesh.elemNodesConnectivity[type];
        int elemType = elemMesh.elemTypeFromDimType(type);
        int numberOfSides = Connectivity::elemTypesSideNumberAll[elemType];
        ElemTypesMesh sideType;
        Connectivity::ElemSidesConnMatrixDynamic& elemElemsST = elemMesh.elemElemsConnectivity[type];
        elemElemsST.resize(Eigen::NoChange,elemNodesST.cols());
        for (unsigned int elemTypeID = 0; elemTypeID < elemNodesST.cols(); elemTypeID++)
        {
          int elemID = elemTypeID + pTypesSize[type];
          for (int sideNumber = 0; sideNumber < numberOfSides; sideNumber++)
          {
            sideType = SpaMesh::SpaCN::SubElemTypesMesh(static_cast<ElemTypesMesh>(elemType),dim-1,sideNumber);
            elemSideIndicesRefSpace.resize(Connectivity::elemTypesNodesNumberAll[sideType]);
            SpaMesh::SpaCN::SubEntityVertexIndices(static_cast<ElemTypesMesh>(elemType), 
		                                   dim-1, sideNumber, 
						   &elemSideIndicesRefSpace(0));
            elemSideNodeIDs.resize(elemSideIndicesRefSpace.size());
            for (unsigned int i = 0; i < elemSideIndicesRefSpace.size(); i++)
              elemSideNodeIDs(i) = elemNodesST(elemSideIndicesRefSpace[i],elemTypeID);
            std::vector<int> intersection = nodesToElemConn[elemSideNodeIDs[0]];
            for (unsigned int i = 1; i < elemSideNodeIDs.size(); i++)
            {
              std::vector<int> newIntersection;
              std::set_intersection(intersection.begin(),  intersection.end(),
                                    nodesToElemConn[elemSideNodeIDs[i]].begin(), nodesToElemConn[elemSideNodeIDs[i]].end(),
                  		    std::back_inserter(newIntersection));
              intersection = newIntersection;
            }
            if (intersection.size() == 2)
            {
              if (intersection[0] == elemID)
	      {
                elemElemsST(sideNumber, elemTypeID) = intersection[1];
                faceNumberEst[faceMesh.dimMeshElemType(sideType)]+=1;
		if (computeFaces)
		{
		  iFaceNodesConnVector[faceMesh.dimMeshElemType(sideType)].push_back(elemSideNodeIDs);
		}
	      }
              else if (intersection[1] == elemID)
                elemElemsST(sideNumber, elemTypeID) = intersection[0];
              else 
                std::cout<<"Error, two neighbors but neither is me"<<std::endl;
            }
            else if (intersection.size() == 1 && intersection[0] == elemID)
	    {
              elemElemsST(sideNumber, elemTypeID) = elemID;
              faceNumberEst[faceMesh.dimMeshElemType(sideType)]+=1;
      	      if (computeFaces)
	      {
       	        bFaceNodesConnVector[faceMesh.dimMeshElemType(sideType)].push_back(elemSideNodeIDs);
	      }
	    }
            else
            {
              std::cout<<"Error, number of neighbors "<<intersection.size()<<", elemID = "<<elemID<<std::endl;
              std::cout<<"Neighbors found ";
              for (unsigned int i = 0; i < intersection.size(); i++)
                std::cout<<intersection[i]<<" ";
              std::cout<<std::endl;
              std::cout<<"Elem type "<<elemType<<", Side type "<<sideType<<", Side number "<<sideNumber<<std::endl;
              std::cout<<"Number of side nodes "<<Connectivity::elemTypesNodesNumberAll[sideType]<<", computed "<<elemSideNodeIDs.size()<<std::endl;
              std::exit(0);
            }
          } //end side loop
        } //end elem loop
      } // end type loop

      if (computeFaces)
      {
	bool bFacesRead = true;
	bool iFacesRead = true;
	bool allFacesRead = true;
	bool someFacesRead = true;
        for (unsigned int type = 0; type < faceMesh.elemNodesConnectivity.size(); type++)
        {
          const Connectivity::ElemNodesConnMatrixDynamic& faceNodesST    = faceMesh.elemNodesConnectivity[type];
          const Connectivity::ElemNodesConnVectorDynamic& iFaceNodesSTVec = iFaceNodesConnVector[type];
          const Connectivity::ElemNodesConnVectorDynamic& bFaceNodesSTVec = bFaceNodesConnVector[type];
	  if (faceNodesST.cols() == 0)
	    someFacesRead = false;
	  if (faceNodesST.cols() != iFaceNodesSTVec.size())
	    iFacesRead = false;
	  if (faceNodesST.cols() != bFaceNodesSTVec.size())
	    bFacesRead = false;
	  if (faceNodesST.cols() != bFaceNodesSTVec.size()+iFaceNodesSTVec.size())
	    allFacesRead = false;
	}
	if (!iFacesRead && !bFacesRead && !allFacesRead)
	{
	  if (someFacesRead)
	    std::cout<<"The faces read are nor internal nor boundary faces (their number does not agree), delete them!";
          for (unsigned int type = 0; type < faceMesh.elemNodesConnectivity.size(); type++)
          {
            faceMesh.resizeElemNodesConn(static_cast<typename Connectivity::MeshTypes<dim-1>::Types>(type), faceNumberEst[type]);
	    int faceTypeID = 0;
            Connectivity::ElemNodesConnMatrixDynamic& faceNodesST = faceMesh.elemNodesConnectivity[type];
	    {
              const Connectivity::ElemNodesConnVectorDynamic& bFaceNodesSTVec = bFaceNodesConnVector[type];
	      for (std::size_t faceTypeIDVec = 0; faceTypeIDVec < bFaceNodesSTVec.size(); faceTypeIDVec++, faceTypeID++)
	        faceNodesST.col(faceTypeID) = bFaceNodesSTVec[faceTypeIDVec];
	    }
	    {
              const Connectivity::ElemNodesConnVectorDynamic& iFaceNodesSTVec = iFaceNodesConnVector[type];
	      for (std::size_t faceTypeIDVec = 0; faceTypeIDVec < iFaceNodesSTVec.size(); faceTypeIDVec++, faceTypeID++)
	        faceNodesST.col(faceTypeID) = iFaceNodesSTVec[faceTypeIDVec];
	    }
	  }
	}
	if (bFacesRead)
	{
	  for (unsigned int type = 0; type < faceMesh.elemNodesConnectivity.size(); type++)
          {
	    std::size_t firstID;
            const Connectivity::ElemNodesConnVectorDynamic& iFaceNodesSTVec = iFaceNodesConnVector[type];
            faceMesh.resizeElemNodesConnConservative(static_cast<typename Connectivity::MeshTypes<dim-1>::Types>(type), iFaceNodesSTVec.size(), firstID);
            Connectivity::ElemNodesConnMatrixDynamic& faceNodesST = faceMesh.elemNodesConnectivity[type];
	    std::size_t faceTypeIDVec = 0;
	    for (std::size_t faceTypeID = firstID; faceTypeID < faceNodesST.cols(); faceTypeID++, faceTypeIDVec++)
	      faceNodesST.col(faceTypeID) = iFaceNodesSTVec[faceTypeIDVec];
	  }
	}
	if (iFacesRead && !allFacesRead)
	{
	  std::cout<<"Internal faces read from file but boundary faces are not present, this possibility is not implemented yet!";
	  std::abort();
	}
        for (unsigned int type = 0; type < elemMesh.elemNodesConnectivity.size(); type++)
        {
          const Connectivity::ElemNodesConnMatrixDynamic& elemNodesST = elemMesh.elemNodesConnectivity[type];
          Connectivity::ElemSidesConnMatrixDynamic& elemSidesST = elemMesh.elemSidesConnectivity[type];
          elemSidesST.resize(Eigen::NoChange,elemNodesST.cols());
	}
      } //end if compute faces
      const typename FaceMesh<dim-1>::PreviousTypesSize& pTypesSizeFace = faceMesh.computePreviousTypesSize();
      for (unsigned int type = 0; type < faceMesh.elemNodesConnectivity.size(); type++)
      {
        const Connectivity::ElemNodesConnMatrixDynamic& faceNodesST = faceMesh.elemNodesConnectivity[type];
        Connectivity::SideElemsConnMatrix& faceElemsST = faceMesh.sideElemsConnectivity[type];
        faceElemsST.resize(Eigen::NoChange,faceNodesST.cols());
        int sideType = faceMesh.elemTypeFromDimType(type);
        int sense;
	int offset;
        for (unsigned int faceTypeID = 0; faceTypeID < faceNodesST.cols(); faceTypeID++)
        {
          int faceID = faceTypeID + pTypesSizeFace[type];
          std::vector<int> intersection = nodesToElemConn[faceNodesST(0,faceTypeID)];
          for (unsigned int i = 1; i < faceNodesST.rows(); i++)
          {
            std::vector<int> newIntersection;
            std::set_intersection(intersection.begin(),  intersection.end(),
                                  nodesToElemConn[faceNodesST(i,faceTypeID)].begin(), nodesToElemConn[faceNodesST(i,faceTypeID)].end(),
                		    std::back_inserter(newIntersection));
            intersection = newIntersection;
          }
          if (intersection.size() == 2)
          {
            faceElemsST(0,faceTypeID) = intersection[0];
            faceElemsST(1,faceTypeID) = intersection[1];
	    if (computeFaces)
	    {
	      int elemTypeID;
	      int elemType = elemMesh.typeFromLID(intersection[0], elemTypeID);
	      assert (elemTypeID < elemMesh.elemNodesConnectivity[elemType].cols());
              ElemTypesMesh elemTypeAll = static_cast<ElemTypesMesh>(elemMesh.elemTypeFromDimType(elemType));
	      int elemSideNumber;
	      SpaCN::SideNumber(elemTypeAll, 
	  	                &(elemMesh.elemNodesConnectivity[elemType](0,elemTypeID)), 
	  	                &(faceNodesST(0,faceTypeID)), 
	  		        faceNodesST.rows(), dim-1, elemSideNumber, sense, offset);
	      elemMesh.elemSidesConnectivity[elemType](elemSideNumber,elemTypeID) = faceID;
	    }
	    if (computeFaces)
	    {
	      int neighborTypeID;
	      int neighborType = elemMesh.typeFromLID(intersection[1], neighborTypeID);
	      assert (neighborTypeID < elemMesh.elemNodesConnectivity[neighborType].cols());
              ElemTypesMesh neighborTypeAll = static_cast<ElemTypesMesh>(elemMesh.elemTypeFromDimType(neighborType));
	      int neighborSideNumber;
	      SpaCN::SideNumber(neighborTypeAll, 
	  	                &(elemMesh.elemNodesConnectivity[neighborType](0,neighborTypeID)), 
	  	                &(faceNodesST(0,faceTypeID)), 
	  		        faceNodesST.rows(), dim-1, neighborSideNumber, sense, offset);
	      elemMesh.elemSidesConnectivity[neighborType](neighborSideNumber,neighborTypeID) = faceID;
	    }
          }
          else if (intersection.size() == 1)
          {
            faceElemsST(0,faceTypeID) = intersection[0];
            faceElemsST(1,faceTypeID) = intersection[0];
	    if (computeFaces)
	    {
	      int elemTypeID;
	      int elemType = elemMesh.typeFromLID(intersection[0], elemTypeID);
	      assert (elemTypeID < elemMesh.elemNodesConnectivity[elemType].cols());
              ElemTypesMesh elemTypeAll = static_cast<ElemTypesMesh>(elemMesh.elemTypeFromDimType(elemType));
	      int elemSideNumber;
	      SpaCN::SideNumber(elemTypeAll, 
	  	                &(elemMesh.elemNodesConnectivity[elemType](0,elemTypeID)), 
	  	                &(faceNodesST(0,faceTypeID)), 
	  		        faceNodesST.rows(), dim-1, elemSideNumber, sense, offset);
	      elemMesh.elemSidesConnectivity[elemType](elemSideNumber,elemTypeID) = faceID;
	    }
          }
          else
          {
            std::cout<<"Error, number of neighbors "<<intersection.size()<<std::endl;
            std::cout<<"Neighbors found ";
            for (unsigned int i = 0; i < intersection.size(); i++)
              std::cout<<intersection[i]<<" ";
            std::cout<<std::endl;
            std::cout<<", Side type "<<sideType<<std::endl;
            std::cout<<"Number of side nodes "<<Connectivity::elemTypesNodesNumberAll[sideType]<<std::endl;
            std::exit(0);
          }
        } //end face loop
      } // end type loop
      
      return faceNumberEst;
    }

    void getCoords (const Connectivity::ElemNodesConnMatrixDynamic& elemNodeConn, int elemId, TopoPointVector& elemCoords) const
    {
      elemCoords.resize(elemNodeConn.rows(),Eigen::NoChange);
      for (unsigned int i = 0; i < elemNodeConn.rows(); i++)
        elemCoords.row(i) = nodesCoords.col(elemNodeConn(i,elemId));
    }
 
    unsigned long elemCard() const 
    {
      unsigned long size = 0;
      for (unsigned int i = 0; i < elemMesh.elemNodesConnectivity.size(); i++)
        size += elemMesh.elemNodesConnectivity[i].cols();
      return size;
    }

    unsigned int elemNeighborsCard(const int elemId) const 
    {
      int spaMeshElemTypeDim = elemMesh.typeFromLID(elemId);
      return std::get<dim>(Connectivity::elemTypesSideNumber)[spaMeshElemTypeDim];
    }

    Connectivity::ElemConnVector elemNeighbors(const int elemId) const
    {
      int elemTypeId;
      int spaMeshElemTypeDim = elemMesh.typeFromLID(elemId, elemTypeId);
      return elemMesh.elemElemsConnectivity[spaMeshElemTypeDim].col(elemTypeId);
    }

    Connectivity::ElemConnVector elemFaces(const int elemId) const
    {
      int elemTypeId;
      int spaMeshElemTypeDim = elemMesh.typeFromLID(elemId, elemTypeId);
      return elemMesh.elemSidesConnectivity[spaMeshElemTypeDim].col(elemTypeId);
    }

    unsigned long faceCard() const 
    {
      unsigned long size = 0;
      for (unsigned int i = 0; i < faceMesh.elemNodesConnectivity.size(); i++)
        size += faceMesh.elemNodesConnectivity[i].cols();
      return size;
    }
  
    ElemMesh<dim> elemMesh;
    FaceMesh<dim-1> faceMesh;
    Eigen::Matrix<double, 3, Eigen::Dynamic, Eigen::ColMajor> nodesCoords;
  };


}; //namespace SpaMesh

#endif
