#ifndef __JGEOMETRY3DTOOLKIT__
#define __JGEOMETRY3DTOOLKIT__

#include <iterator>

#include "JGeometry3D/JPosition3D.hh"
#include "JGeometry2D/JGeometry2DToolkit.hh"
#include "JMath/JMathToolkit.hh"


/**
 * \author mdejong
 */

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

namespace JGEOMETRY3D {

  using JMATH::getDistance;


  /**
   * Center.
   */
  class JCenter3D :
    public JPosition3D
  {
  public:
    /**
     * Constructor.
     *
     * \param  p0       first  position
     * \param  p1       second position
     */
    JCenter3D(const JVector3D& p0,
              const JVector3D& p1) :
      JPosition3D()
    {
      add(p0);
      add(p1);

      div(2);
    }


    /**
     * Constructor.
     *
     * \param  p0       first  position
     * \param  p1       second position
     * \param  p2       third  position
     */
    JCenter3D(const JVector3D& p0,
              const JVector3D& p1,
              const JVector3D& p2) :
      JPosition3D()
    {
      add(p0);
      add(p1);
      add(p2);

      div(3);
    }


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

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


  /**
   * Weighed center.
   */
  class JWeighedCenter3D :
    public JPosition3D
  {
  public:
    /**
     * Constructor.
     *
     * \param  __begin  begin of data
     * \param  __end    end   of data
     */
    template<class T>
    JWeighedCenter3D(T __begin,
		     T __end) :
      JPosition3D()
    {
      if (__begin != __end) {
	
	double __w = 0.0;
	
	for (T i = __begin; i != __end; ++i) {
	  __x += i->getX() * i->getW();
	  __y += i->getY() * i->getW();
	  __z += i->getZ() * i->getW();
	  __w += i->getW();
	}

	div(__w);
      }
    }
  };


  /**
   * Auxiliary class for determination of smallest distance between pair of 3D points.
   */
  class JSmallestDistance3D {
  protected:
    /**
     * 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 ->getZ() - il->getZ() < Dmin) {}
        while (++ir != __end   && ir->getZ() - i ->getZ() < Dmin) {}

	const double dz   = JGEOMETRY2D::getSmallestDistance2D(++il, ir, Dmin);

	sort(il, ir, compareZ);

	return min(Dmin, dz);
      }
    }


    /**
     * Auxiliary class for sorting elements.
     */
    struct JCompareZ {
      /**
       * 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.getZ() < second.getZ();
      }
    };


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


    static const JCompareZ  compareZ;   //!< 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, compareZ);

      return getDmin(__begin, __end);
    }
  };


  /**
   * Function object for smallest distance determination.
   */
  static const JSmallestDistance3D  getSmallestDistance3D;
}

#endif