#ifndef HALFSPACED3INDEXED_H
#define HALFSPACED3INDEXED_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 halfspaceD3indexed : public partitionspace<PT>
{
public:

  /** Start point. */
  PT const & p0;  
  /** Second point. */
  PT const & p1;
  /** End point. */
  PT const & p2;  
  /** 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. */
  halfspaceD3indexed
  (
    PT const * pts,
    INDX i0_,
    INDX i1_,
    INDX i2_
  )
    : p0(pts[i0_]), p1(pts[i1_]), p2(pts[i2_]),
      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-p0.x)+normal.y*(x.y-p0.y)+normal.z*(x.z-p0.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-p0.x)+normal.y*(x.y-p0.y)+normal.z*(x.z-p0.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*(p0.x-w.x);
      p.y=normal.y*normal.y*(p0.y-w.y);
      p.z=normal.z*normal.z*(p0.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 halfspaceD3indexed<PT,PD,INDX>::normalcalculate()
{
  PT u(p2 - p1);
  PT v(p0 - p1);

  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