#ifndef XDKWRL_TRIANGULATOR_H
#define XDKWRL_TRIANGULATOR_H

#include <vector>  
#include <list>  
#include <numeric>  
#include <stdexcept>  

/*!
 * A very useful class to triangulate a sequence a of point
 * forming a simple polygon (without holes).
 *
 * The code is fully templated which means it can be used with any type
 * of point and any type of sequence.
 *
 * Here is an exemple of use, where Vec2 is a trivial 2D point type:
 \code 
 list<Vec2> polygon;
 
 polygon.push_back(Vec2(0.0f,0.0f));
 polygon.push_back(Vec2(1.0f,0.0f));
 polygon.push_back(Vec2(1.0f,1.0f));
 polygon.push_back(Vec2(0.0f,1.0f));
 
 Triangulator<Vec2> triangulator(polygon.begin(),polygon.end());
 triangulator.process();
 for (Triangulator<Vec2>::face_const_iterator fter = triangulator.faces_begin();
 fter != triangulator.faces_end();++fter)
 {
   cout<<"face joining\n"
       <<"  "<<*(fter->vertex(0))
       <<"  "<<*(fter->vertex(1))<<endl;
 }
 \endcode
 */
template<class P>
class Triangulator
{
public:
  class Face;
  typedef P PointType;
  typedef typename std::list<Face>::const_iterator face_const_iterator;
  template<class T>  
  inline Triangulator(T first,const T& last);
  inline void process() throw(std::runtime_error);
  inline unsigned int faces_size() const;
  inline face_const_iterator faces_begin() const;
  inline face_const_iterator faces_end() const;  
protected:
  static inline bool isInTriangle(const PointType& A,
				  const PointType& B,
				  const PointType& C,
				  const PointType& M);
  inline bool snip(const int u,const int v,const int w,const int nv);
  inline int nbPoints() const;
  inline const PointType& point(const int i) const;
  inline void addFace(const int u,const int v,const int w);
private:
  std::vector<const PointType*> contour_;
  std::vector<int>              V_;
  std::list<Face>               faces_;
};
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Interface of Triangulator::Face
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
template<class P>
class Triangulator<P>::Face
{
public:
  inline const typename Triangulator<P>::PointType* vertex(const int i) const;
protected:
  friend class Triangulator;
  inline Face(const Triangulator<P>::PointType* const p0,
	      const Triangulator<P>::PointType* const p1,
	      const Triangulator<P>::PointType* const p2);
private:
  const typename Triangulator<P>::PointType* vertices_[3];
};
//************************************************************
// Implementation of Triangulator::Face
//************************************************************
template<class P>
inline const typename Triangulator<P>::PointType*
Triangulator<P>::Face::vertex(const int i) const
{
  return vertices_[i];
}
template<class P>
inline
Triangulator<P>::Face::Face(const Triangulator<P>::PointType* const p0,
			    const Triangulator<P>::PointType* const p1,
			    const Triangulator<P>::PointType* const p2)
{
  vertices_[0] = p0;
  vertices_[1] = p1;
  vertices_[2] = p2;
}
//************************************************************
// Implementation of Triangulator::Face
//************************************************************
template<class P>
template<class T>  
inline
Triangulator<P>::Triangulator(T first,const T& last)  
{
  while (first != last)
  {
    contour_.push_back(&(*first++));
  }
  V_ = std::vector<int>(nbPoints());
  float A=0.0f;      
  for(int p=nbPoints()-1,q=0; q<nbPoints(); p=q++)
  {
    A +=
      (*contour_[p])[0]*(*contour_[q])[1] -
      (*contour_[q])[0]*(*contour_[p])[1];
  }
  if (A > 0.0f)
  {
    // This should be done with the iota function but gcc-3.2 seems to have
    // lost it on RedHat.
//     iota(V_.begin(),V_.end(),0);
    int n=0;
    for (std::vector<int>::iterator iter =V_.begin();
	 iter != V_.end();++iter)
    {
      *iter = n++;
    }
  }
  else
  {
    int n=0;
    for (std::vector<int>::reverse_iterator iter =V_.rbegin();
	 iter != V_.rend();++iter)
    {
      *iter == ++n;
    }
  }
}
template<class P>
inline void
Triangulator<P>::process() throw(std::runtime_error)
{
  if (nbPoints() < 3)
  {
    throw std::runtime_error("Empty polygon");
  }
  int nv = nbPoints();
  //  remove nv-2 Vertices, creating 1 triangle every time 
  int count = 2*nv;   // error detection 
  for(int m=0, v=nv-1; nv>2; )
  {
    // if we loop, it is probably a non-simple polygon 
    if (0 >= (count--))
    {
      throw std::runtime_error("Bad polygon");
    }    
    // three consecutive vertices in current polygon, <u,v,w> 
    const int u = nv <= v   ? 0 : v;     
    v           = nv <= u+1 ? 0 : u+1;
    const int w = nv <= v+1 ? 0 : v+1;     
    if ( snip(u,v,w,nv) )
    {
      // output Triangle 
      addFace(u,v,w);
      ++m;
      // remove v from remaining polygon 
      for(int s=v,t=v+1;t<nv;++s,++t)
      {
	V_[s] = V_[t];
      }
      // resest error detection counter 
      count = 2*(--nv);
    }
  }
}
template<class P>
inline unsigned int
Triangulator<P>::faces_size() const
{
  return faces_.size();
}
template<class P>
inline typename Triangulator<P>::face_const_iterator
Triangulator<P>::faces_begin() const
{
  return faces_.begin();
}
template<class P>
inline typename Triangulator<P>::face_const_iterator
Triangulator<P>::faces_end() const
{
  return faces_.end();
}
template<class P>
inline bool
Triangulator<P>::isInTriangle(const Triangulator<P>::PointType& A,
			      const Triangulator<P>::PointType& B,
			      const Triangulator<P>::PointType& C,
			      const Triangulator<P>::PointType& M)
{
  const float a[2] = { C[0]-B[0],C[1]-B[1] };
  const float b[2] = { A[0]-C[0],A[1]-C[1] };
  const float c[2] = { B[0]-A[0],B[1]-A[1] };
  const float am[2]= { M[0]-A[0],M[1]-A[1] };
  const float bm[2]= { M[0]-B[0],M[1]-B[1] };
  const float cm[2]= { M[0]-C[0],M[1]-C[1] };

  return ((a[0]*bm[1] >= a[1]*bm[0]) &&
	  (c[0]*am[1] >= c[1]*am[0]) &&
	  (b[0]*cm[1] >= b[1]*cm[0]));
}
template<class P>
inline bool
Triangulator<P>::snip(const int u,const int v,const int w,const int nv)
{
  static const float EPSILON=0.0000000001f;
  const PointType& A = point(u);
  const PointType& B = point(v);
  const PointType& C = point(w);
  if (EPSILON > (((B[0]-A[0])*(C[1]-A[1]))-((B[1]-A[1])*(C[0]-A[0]))))
  {
    return false;
  }
  for (int p=0;p < nv;++p)
  {
    if( (p == u) || (p == v) || (p == w) )
    {
      continue;
    }
    if (isInTriangle(A,B,C,point(p)))
    {
      return false;
    }
  }
  return true;
}
template<class P>
inline int
Triangulator<P>::nbPoints() const
{
  return contour_.size();
}
template<class P>
inline const P&
Triangulator<P>::point(const int i) const
{
  return *contour_[V_[i]];
}
template<class P>
inline void
Triangulator<P>::addFace(const int u,const int v,const int w)
{
  faces_.push_back(Face(contour_[V_[u]],
			contour_[V_[v]],
			contour_[V_[w]]));
};
#endif // XDKWRL_TRIANGULATOR_H

// Local variables section.
// This is only used by emacs!
// Local Variables:
// ff-search-directories: ("." "../../../src/xdkwrl/tools/")
// End:
