#ifndef HALFSPACED3INDEXEDFULL_H
#define HALFSPACED3INDEXEDFULL_H

#include <cassert>
using namespace std;

#include <typedefs.h>
#include <partitionspace.h>
#include <mathlib.h>
#include <zero.h>

/*!
\brief Define a 3D half space with three ordered points.

The inside of the half space is on the anti clockwise
 side of the plane with the three points.
*/
template< typename PT, typename PD, typename INDX >
class halfspaceD3indexedfull : public partitionspace<PT>
{
public:

  PT const * pts;

  /** Normal. */
  PT normal;  

  /** Index to p0. */
  INDX i0;
  /** Index to p1. */
  INDX i1;
  /** Index to p2. */
  INDX i2;

  /** Construct the plane with anticlockwise point 
      ordering.  The normal is outwards on the 
      anticlockwise side of the plane. */
  halfspaceD3indexedfull
  (
    PT const * pts_,
    INDX i0_,
    INDX i1_,
    INDX i2_
  )
    : pts(pts_),
      i0(i0_), i1(i1_), i2(i2_)
    { normalcalculate(); }

  /** Calculate the normal the ordered points. */
  void normalcalculate();

  /** Is the point inside the half space? */
  boolc isInside( PT const & x ) const
    { return 0 < normal.x*(x.x-pts[i0].x)+normal.y*(x.y-pts[i0].y)+normal.z*(x.z-pts[i0].z); }
  /** Is the point on or inside the half space? */
  boolc isInsideOrOnBoundary( PT const & x ) const
    { return 0 < zero<PD>::val+normal.x*(x.x-pts[i0].x)+normal.y*(x.y-pts[i0].y)+normal.z*(x.z-pts[i0].z); }

  /** A measure of the distance from the plane to the
      point w without a square root. ie
      N^2*d^2 where d is the vanilla distance function. */
  PD const distancefromhalfspace(PT const & w) const
    { 
      PT p;
      p.x=normal.x*normal.x*(pts[i0].x-w.x);
      p.y=normal.y*normal.y*(pts[i0].y-w.y);
      p.z=normal.z*normal.z*(pts[i0].z-w.z);
      return p.x*p.x+p.y*p.y+p.z*p.z;
    }
};


// --------------------------------------------------------- 
// Implementation


template< typename PT, typename PD, typename INDX >
void halfspaceD3indexedfull<PT,PD,INDX>::normalcalculate()
{
  PT u(pts[i2] - pts[i1]);
  PT v(pts[i0] - pts[i1]);

  normal.x=u.y*v.z-v.y*u.z;
  normal.y=u.z*v.x-u.x*v.z;
  normal.z=u.x*v.y-u.y*v.x;
}


#endif