#ifndef __JGEOMETRY2DTOOLKIT__
#define __JGEOMETRY2DTOOLKIT__

#include <utility>
#include <algorithm>
#include <limits>

#include "JGeometry2D/JPosition2D.hh"
#include "JMath/JMathToolkit.hh"


/**
 * \author mdejong
 */

/**
 * Auxiliary classes and methods for 2D geometrical objects and operations.
 */
namespace JGEOMETRY2D {}
namespace JPP { using namespace JGEOMETRY2D; }

namespace JGEOMETRY2D {

  using JMATH::getDistance;
  

  /**
   * Check sequence of three points in X-Y plane.
   *
   * \param  a       1st point
   * \param  b       2nd point
   * \param  c       3rd point
   * \return         true if points a, b, c are counter clockwise; else false
   */
  template<class T>
  inline bool getCCW(const T& a,
                     const T& b,
                     const T& c)
  {
    const double A = a.getX() - b.getX();
    const double B = a.getY() - b.getY();
    const double C = c.getX() - b.getX();
    const double D = c.getY() - b.getY();

    return A*D <= B*C;
  }


  /**
   * Center.
   */
  class JCenter2D :
    public JVector2D
  {
  public:
    /**
     * Constructor.
     *
     * \param  p0       first  point
     * \param  p1       second point
     */
    JCenter2D(const JVector2D& p0,
              const JVector2D& p1) :
      JVector2D()
    {
      add(p0);
      add(p1);

      div(2);
    }


    /**
     * Constructor.
     *
     * \param  p0       first  point
     * \param  p1       second point
     * \param  p2       third  point
     */
    JCenter2D(const JVector2D& p0,
              const JVector2D& p1,
              const JVector2D& p2) :
      JVector2D()
    {
      add(p0);
      add(p1);
      add(p2);

      div(3);
    }


    /**
     * Constructor.
     *
     * \param  __begin  begin of data
     * \param  __end    end   of data
     */
    template<class T>
    JCenter2D(T __begin,
	      T __end) :
      JVector2D()
    {
      if (__begin != __end) {
	
	for (T i = __begin; i != __end; ++i) {
	  add(*i);
	}

	div(std::distance(__begin, __end));
      }
    }
  };


  /**
   * Auxiliary class for convex hull determination in X-Y plane.
   */
  class JConvexHull2D {
  protected:    
    /**
     * Partition half Hull.
     *
     * \param  __begin     begin of data
     * \param  __end       end   of data
     * \param  compare     comparator
     * \return             end   of data
     */
    template<class T, class JCompare_t>
    static inline T getConvexHull2D(T __begin, 
				    T __end, 
				    JCompare_t compare)
    {
      using namespace std;

      if (__begin != __end) {
	
	sort(__begin, __end, compare);
	
	T l = __begin;
	
	if (++l != __end) {
	  
	  T i = __begin; 
	  
	  ++(++i);
	  
	  for (T j, k; i != __end; ++i) {
	    
	    for (j = k = l; j != __begin && getCCW(*i, *j, *--k); --j) {}
	    
	    l = j;
	    ++l;

	    iter_swap(l,i);
	  }
	}
	
	return l;
	
      } else {

	return __begin;
      }
    }


    /**
     * Auxiliary class for sorting elements.
     */
    struct JLowerHull {
      /**
       * Sort criterion for lower hull.
       *
       * \param  first   first  point
       * \param  second  second point
       * \return         true if first before second; else false
       */
      template<class T>
      inline bool operator()(const T& first, const T& second) const
      {
	if (first.getX() == second.getX()) 
	  return second.getY() > first.getY();
	else
	  return first.getX() > second.getX();
      }
    };
    

    /**
     * Auxiliary class for sorting elements.
     */
    struct JUpperHull {
      /**
       * Sort criterion for upper hull.
       *
       * \param  first   first  point
       * \param  second  second point
       * \return         true if first before second; else false
       */
      template<class T>
      inline bool operator()(const T& first, const T& second) const
      { 
	if (first.getX() == second.getX()) 
	  return second.getY() < first.getY();
	else
	  return first.getX() < second.getX();
      }
    };

  
  public:
    /**
     * Default constructor.
     */
    JConvexHull2D()
    {}


    static const JLowerHull  sortLowerHull;   //!< Function object for sorting elements.
    static const JUpperHull  sortUpperHull;   //!< Function object for sorting elements.


    /**
     * Get convex Hull.
     *
     * \param  __begin     begin of data
     * \param  __end       end of data
     * \return             end of lower and upper Hull data
     */
    template<class T>
    inline std::pair<T,T> operator()(T __begin, T __end) const
    {
      using namespace std;

      T __p = getConvexHull2D(__begin, __end, sortLowerHull);        // make lower hull
    
      if (__p == __begin || __p == __end) {
	return make_pair(__p, __p);
      }
    
      // add first point of lower hull to upper hull
    
      reverse(__begin, __p);
    
      T __q = getConvexHull2D(--__p, __end, sortUpperHull);          // make upper hull
    
      ++__q;
    
      // re-store order and keep the extra point
      
      reverse(__p,     __q);
      reverse(__begin, __q);
    
      const int n = distance(__p, __q);
    
      __p = __begin;

      advance(__p, n);
    
      return make_pair(__p, __q);
    }
  };


  /**
   * Function object for convex hull determination.
   */
  static const JConvexHull2D  getConvexHull2D;


  /**
   * Get area of a convex polygon.
   *
   * \param   __begin   begin of data
   * \param   __end     end   of data
   * \return            area
   */
  template<class T>
  inline double getArea2D(T __begin, T __end)
  {
    using namespace std;

    if (distance(__begin,__end) >= 3) {

      double A = 0.0;

      T i, j, k;
      
      i = j = k = __begin;
      
      for (++j, ++(++k); k != __end; ++i, ++j, ++k) {
	A += j->getX() * (k->getY()  -  i->getY());
      }
      
      k = __begin;
      
      A +=  j->getX() * (k->getY()  -  i->getY());
      
      ++i;
      j = k; 
      ++k;
      
      A +=  j->getX() * (k->getY()  -  i->getY());

      return 0.5 * fabs(A);

    } else {

      return 0.0;
    }
  }


  /**
   * Check if given point is inside a convex polygon.
   *
   * \param   __begin   begin of data
   * \param   __end     end   of data
   * \param   pos       position
   * \return            true if inside; else false
   */
  template<class T>
  inline bool inside2D(T __begin, T __end, const JVector2D& pos)
  {
    using namespace std;

    if (distance(__begin,__end) >= 2) {

      T i = __begin, j = __begin;
      
      for (++j; j != __end; ++i, ++j) {

	if (!getCCW(*i, *j, pos)) {
	  return false;
	}
      }

      j = __begin;

      return getCCW(*i, *j, pos);

    } else {

      return false;
    }
  }


  /**
   * Check if given point is inside a convex polygon.
   *
   * \param   __begin   begin of data
   * \param   __end1    end   of lower hull
   * \param   __end2    end   of upper hull
   * \param   pos       position
   * \return            true if inside; else false
   */
  template<class T>
  inline bool inside2D(T __begin, T __end1, T __end2, const JVector2D& pos)
  {
    using namespace std;

    if (distance(__begin,__end2) >= 3) {

      if (pos.getX() < __begin->getX()) {
	return false;
      }

      T i = lower_bound(__begin, __end1, pos, JConvexHull2D::sortUpperHull);

      if (i == __end1) {
	return false;
      }

      T j = i--;

      if (!getCCW(*i, *j, pos)) {
	return false;
      }

      i = lower_bound(__end1, __end2, pos, JConvexHull2D::sortLowerHull);
      j = i--;

      if (j == __end2) {
	j = __begin;
      }

      return getCCW(*i, *j, pos);

    } else {

      return false;
    }
  }


  /**
   * Auxiliary class for determination of smallest distance between pair of 2D points.
   *
   * Reference:
   * Computational Geometry
   * Algorithms and Applications
   * Authors: de Berg, M., Cheong, O., van Kreveld, M., Overmars, M.
   */
  class JSmallestDistance2D {
  protected:
    /**
     * Get distance beween the closest points within a strip around the median in x.\n
     * Note that this method runs not at O(n^2) but at O(6)!
     *
     * \param   __begin   begin of data
     * \param   __end     end   of data
     * \param   delta     width of strip
     * \return            minimal distance
     */
    template<class T>
    static double getDmin(T __begin, T __end, const double delta)
    {
      using namespace std;

      double Dmin = delta;
 
      sort(__begin, __end, compareY); 
 
      for (T i = __begin; i != __end; ++i) {
        for (T j = i; ++j != __end && (j->getY() - i->getY()) < Dmin; ) {

	  const double d = getDistance(*i, *j);

	  if (d < Dmin) {
	    Dmin = d;
	  }
	}
      }
      
      sort(__begin, __end, compareX);

      return Dmin;
    }


    /**
     * Recursive method to find the smallest distance.
     *
     * \param   __begin   begin of data
     * \param   __end     end   of data
     * \return            minimal distance
     */
    template<class T>
    static double getDmin(T __begin, T __end)
    {
      using namespace std;

      const int N = distance(__begin, __end);

      if (N <= 3) {

	double Dmin = numeric_limits<double>::max();
 
	for (T i = __begin; i != __end; ++i) {
	  for (T j = i; ++j != __end; ) {

	    const double d = getDistance(*i, *j);

	    if (d < Dmin) {
	      Dmin = d;
	    }
	  }
	}

	return Dmin;

      } else {
 
	T i = __begin;

	advance(i, N/2);

	const double dl   = getDmin(__begin, i);
	const double dr   = getDmin(i,   __end);

	const double Dmin = min(dl, dr);

	T il = i;
	T ir = i;
 
	while (--il != __begin && i ->getX() - il->getX() < Dmin) {}
	while (++ir != __end   && ir->getX() - i ->getX() < Dmin) {}

	return min(Dmin, getDmin(++il, ir, Dmin));
      }
    }


    /**
     * Auxiliary class for sorting elements.
     */
    struct JCompareX {
      /**
       * Compare x-positions of given points.
       *
       * \param   first     first  point
       * \param   second    second point
       * \return            true if x of first point less than that of second; else false
       */
      template<class T>
      inline bool operator()(const T& first, const T& second) const
      {
	return first.getX() < second.getX();
      }
    };


    /**
     * Auxiliary class for sorting elements.
     */
    struct JCompareY {
      /**
       * Compare y-positions of given points.
       *
       * \param   first     first  point
       * \param   second    second point
       * \return            true if y of first point less than that of second; else false
       */
      template<class T>
      inline bool operator()(const T& first, const T& second) const
      {
	return first.getY() < second.getY();
      }
    };


  public:
    /**
     * Default constructor.
     */
    JSmallestDistance2D()
    {}


    static const JCompareX  compareX;   //!< Function object for sorting elements.
    static const JCompareY  compareY;   //!< Function object for sorting elements.


    /**
     * Get smallest distance between two points.\n
     * Note that this method changes the order of the elements.
     *
     * \param   __begin   begin of data
     * \param   __end     end   of data
     * \return            minimal distance
     */
    template<class T>
    double operator()(T __begin, T __end) const
    {
      using namespace std;

      sort(__begin, __end, compareX);
      
      return getDmin(__begin, __end);
    }


    /**
     * Get distance beween the closest points within a strip around the median in z.\n
     * Note that this method changes the order of the elements.
     *
     * \param   __begin   begin of data
     * \param   __end     end   of data
     * \param   delta     width of strip
     * \return            minimal distance
     */
    template<class T>
    double operator()(T __begin, T __end, const double delta) const
    {
      using namespace std;

      double Dmin = delta;

      sort(__begin, __end, compareX);
      
      for (T i = __begin; i != __end; ) {
	
	T j = i;

        for ( ; ++j != __end && (j->getX() - i->getX()) < Dmin; ) {}

	const double d = getDmin(i, j);

	if (d < Dmin) {
	  Dmin = d;
	}

	i = j;
      }

      return Dmin;
    }


    /**
     * Get pairs with smaller or equal given maximal distance.\n
     * Note that this method changes the order of the elements.
     *
     * \param   __begin   begin of data
     * \param   __end     end   of data
     * \param   Dmax      maximal distance
     * \return            pair of elements
     */
    template<class T>
    static inline std::pair<T,T> getPair(T __begin, T __end, const double Dmax)
    {
      using namespace std;

      sort(__begin, __end, compareX);
      
      for (T i = __begin; i != __end; ++i) {
      
	T j = i;
	     
	while (++j != __end && (j->getX() - i->getX()) <= Dmax) {}
      
	if (distance(i,j) != 1) {
	
	  if (distance(i,j) == 2) {
	  
	    if (getDistance(*i, *--j) <= Dmax) {
	      return make_pair(i,j);
	    }
	  
	  } else {

	    sort(i, j, compareY); 
	  
	    for (T __i = i; __i != j; ++__i) {
	      for (T __j = __i; ++__j != j && (__j->getY() - __i->getY()) <= Dmax; ) {
	      
		const double d = getDistance(*__i, *__j);
	      
		if (d <= Dmax) {
		  return make_pair(__i,__j);
		}
	      }
	    }
	  
	    sort(i, j, compareX);
	  }
	}
      }
    
      return make_pair(__end,__end);
    }
  };

 
  /**
   * Function object for smallest distance determination.
   */
  static const JSmallestDistance2D  getSmallestDistance2D;
}

#endif