namespace X3DTK {
namespace MESH {

template<class MData, class VData, class EData, class FData, bool RW>
TemplateNormalsUpdaterVerticesCoreVisitor<MData, VData, EData, FData, RW>::TemplateNormalsUpdaterVerticesCoreVisitor()
{
  define(Recorder<TemplateVertex<MData, VData, EData, FData, RW> >::getEnterFunction(&TemplateNormalsUpdaterVerticesCoreVisitor::enterVertex));
  define(Recorder<Shape>::getWalkOnFunction(&TemplateNormalsUpdaterVerticesCoreVisitor::walkOnShape));
}

template<class MData, class VData, class EData, class FData, bool RW>
bool TemplateNormalsUpdaterVerticesCoreVisitor<MData, VData, EData, FData, RW>::walkOnShape(Shape *, SFNode child)
{
  TemplateNormalsUpdaterStateVariables<MData, VData, EData, FData, RW> *stateVariables = Singleton<TemplateNormalsUpdaterStateVariables<MData, VData, EData, FData, RW> >::getInstance();
  MFNode selectedNodes = stateVariables->getSelectedNodes();
  return (selectedNodes.empty() || (std::find(selectedNodes.begin(), selectedNodes.end(), child) != selectedNodes.end()));
}

template<class MData, class VData, class EData, class FData, bool RW>
void TemplateNormalsUpdaterVerticesCoreVisitor<MData, VData, EData, FData, RW>::enterVertex(TemplateVertex<MData, VData, EData, FData, RW> *V)
{ 
  TemplateNormalsUpdaterStateVariables<MData, VData, EData, FData, RW> *stateVariables = Singleton<TemplateNormalsUpdaterStateVariables<MData, VData, EData, FData, RW> >::getInstance();
  if (!stateVariables->getNode(V))  
  {
    if (VData::template find<MESH::VertexNormalData>() 
     && FData::template find<MESH::FaceNormalData>())
    { 
      const typename TemplateVertex<MData, VData, EData, FData, RW>::MFVertex &vertices = V->getVertices();
      for (typename TemplateVertex<MData, VData, EData, FData, RW>::MFVertex::const_iterator v = vertices.begin(); v != vertices.end(); ++v)
      {
//        std::cout << "for vertex " << **v << std::endl;
        std::list<std::pair<BaseSFFace *, BaseSFFace *> > faceGraph;
        typename SFTemplateVertex<MData, VData, EData, FData, RW>::MFFace mf = (*v)->getFaces();

        const typename SFTemplateVertex<MData, VData, EData, FData, RW>::MFEdge &edges = (*v)->getEdges();
        for (typename SFTemplateVertex<MData, VData, EData, FData, RW>::MFEdge::const_iterator e = edges.begin(); e != edges.end(); ++e)
        {
          typename SFTemplateEdge<MData, VData, EData, FData, RW>::MFFace faces = (*e)->getFaces();
        
          // Take faces 2 by 2
          for (typename SFTemplateEdge<MData, VData, EData, FData, RW>::MFFace::const_iterator fi = faces.begin(); fi != faces.end(); ++fi)
          {
            float scalarProductReference = cosf(0.5f*((*fi)->getMesh()->data().getCreaseAngle() + (*fi)->getMesh()->data().getCreaseAngle()));
            typename SFTemplateEdge<MData, VData, EData, FData, RW>::MFFace::const_iterator fj = fi;
            ++fj;
            while (fj != faces.end())
            { 
              if (*fi != *fj)
              {
                // compare angle between fi and fj
                const SFVec3f &ni = (*fi)->data().getNormal();
                const SFVec3f &nj = (*fj)->data().getNormal();
              
                float scalarProduct = ni*nj;
              
/*              std::cout << "sp = " << scalarProduct << std::endl;
              std::cout << "cos = " << cosf(0.5f*((*fi)->getMesh()->data().getCreaseAngle() + (*fi)->getMesh()->data().getCreaseAngle())) << std::endl;

              std::cout << "treating " << *static_cast<SFTemplateFace<MData, VData, EData, FData, RW> *>(*fi) << " - "
                                       << *static_cast<SFTemplateFace<MData, VData, EData, FData, RW> *>(*fj);
*/                                          
                if (scalarProduct >= scalarProductReference)
                {
                  faceGraph.push_back(std::pair<BaseSFFace *, BaseSFFace *>(*fi, *fj));
//                std::cout << " ok" << std::endl;
                } 
//              else
//                std::cout << std::endl; 
              }
              ++fj;
            }
          }
        }  

        // Traversing the face graph
        while (!faceGraph.empty())
        {
          std::list<BaseSFFace *> faceSet;
          faceSet.push_back(faceGraph.front().first);
        
          typename SFTemplateVertex<MData, VData, EData, FData, RW>::MFFace::iterator r = std::find(mf.begin(), mf.end(), static_cast<SFTemplateFace<MData, VData, EData, FData, RW> *>(faceGraph.front().first));
          if (r != mf.end())
            mf.erase(r);
        
          std::list<BaseSFFace *>::const_iterator f = faceSet.begin();
          while (f != faceSet.end())
          {
            // iterating the face graph to find out the connected faces
            std::list<std::pair<BaseSFFace *, BaseSFFace *> >::iterator p = faceGraph.begin();
            while (p != faceGraph.end())
            {
              bool found = false;
              if ((*p).first == *f)
              {
                r = std::find(mf.begin(), mf.end(), static_cast<SFTemplateFace<MData, VData, EData, FData, RW> *>((*p).second));
                if (r != mf.end())
                {
                  faceSet.push_back((*p).second);
                  mf.erase(r);
                }  
                p = faceGraph.erase(p);
                found = true;
              }  
              if ((*p).second == *f)
              {
                r = std::find(mf.begin(), mf.end(), static_cast<SFTemplateFace<MData, VData, EData, FData, RW> *>((*p).first));
                if (r != mf.end())
                {
                  mf.erase(r);
                  faceSet.push_back((*p).first);
                }  
                p = faceGraph.erase(p);                
                found = true;
              }  
              if (!found)
                ++p;
            }
            ++f;
          }
          // the connected component is computed
          SFVec3f normal = SFVec3f::null;
//         std::cout << "faceSet" << std::endl;
          for (std::list<BaseSFFace *>::const_iterator fc = faceSet.begin(); fc != faceSet.end(); ++fc)
          {
//          std::cout << *static_cast<SFTemplateFace<MData, VData, EData, FData, RW> *>(*fc) << std::endl;
            normal += static_cast<SFTemplateFace<MData, VData, EData, FData, RW> *>(*fc)->data().getNormal();
          }
          normal.normalize();
          for (std::list<BaseSFFace *>::const_iterator fa = faceSet.begin(); fa != faceSet.end(); ++fa)
            (*v)->data().setNormalOfFace(normal, *fa);
        }
        for (typename SFTemplateVertex<MData, VData, EData, FData, RW>::MFFace::const_iterator fid = mf.begin(); fid != mf.end(); ++fid)
        {
//        std::cout << "faceSet" << std::endl;
//        std::cout << **fid << std::endl;
          (*v)->data().setNormalOfFace((*fid)->data().getNormal(), *fid);
        }  
//        std::cout << "end..." << std::endl;
      }
    }
    stateVariables->addNode(V);
  }
}

/*

template<class MData, class VData, class EData, class FData, bool RW>
void TemplateNormalsUpdaterVerticesCoreVisitor<MData, VData, EData, FData, RW>::enterVertex(TemplateVertex<MData, VData, EData, FData, RW> *M)
{
  #ifdef TEMPLATE_SPECIALIZATION_SUPPORTED
  if (VData::template find<MESH::VertexNormalData>() 
   && FData::template find<MESH::FaceNormalData>())
   #endif
  { 
    const typename TemplateVertex<MData, VData, EData, FData, RW>::MFVertex &vertices = M->getVertices();
    for (typename TemplateVertex<MData, VData, EData, FData, RW>::MFVertex::const_iterator v = vertices.begin(); v != vertices.end(); ++v)
    {
      SFVec3f normal;

      // Compute point normal as an area weighted average of faces' normals
      const typename SFTemplateVertex<MData, VData, EData, FData, RW>::MFFace &faces = (*v)->getFaces();
      typename SFTemplateVertex<MData, VData, EData, FData, RW>::MFFace::const_iterator f = faces.begin();

      while (f != faces.end())
      {
        #ifdef TEMPLATE_SPECIALIZATION_SUPPORTED
        normal = normal + (*f)->template ogetData<MESH::FaceNormalData>().getNormal();
        #else
        normal = normal + (*f)->data().getNormal();
        #endif
        
        ++f;
      }  
      normal.normalize();

      // Store computed normal in VertexNormalData
      f = faces.begin();
      
      while (f != faces.end())
      {  
        #ifdef TEMPLATE_SPECIALIZATION_SUPPORTED
        (*v)->template ogetData<MESH::VertexNormalData>().setNormalOfFace(normal, *f);
        #else
        (*v)->data().setNormalOfFace(normal, *f);
        #endif
        
        ++f;
      } 
    }
  }
}


*/

}
}
