#include <sstream>
using namespace std;

#include <mathlib.h>
#include <plane.h>



plane::plane
(
  bool & valid,
  point3<double> const & p0,
  point3<double> const & p1,
  point3<double> const & p2
)
{
  valid=false;
  point3<double> arm1(p1-p0);
  point3<double> arm2(p2-p0);

  // If the points are colinear then the plane can not
  // be determined.
  crossproduct::evalxyz(nml,arm1,arm2);
  if (nml.x*nml.x+nml.y*nml.y+nml.z*nml.z<zero<double>::val)
    return;

  valid=true;

  d0 = nml.dot(p0);
}


boolc plane::intersects
( 
  point3<double> & A,
  point3<double> & B,
  plane const & P2 
) const
{
  //crossprod< point3<double> > cp;
  //cp(B,nml,P2.nml);
  crossproduct::evalxyz(B,nml,P2.nml);
  if (zero<double>::test(B.dot(B)))
    return false;

  point3<double> w;
  bool res;
  res = solverInconsistent<double>::d2linearequ
  (
    A.x, A.y, A.z,
    nml.x, nml.y, nml.z, d0,
    P2.nml.x, P2.nml.y, P2.nml.z, P2.d0
  );

  return res;

//  if (B.dot(B)<zero<double>::val)
//    return false;

  // Find an intersection point.

/*
  if (nmlxIsZero())
    if (P2.nmlxIsZero())
    {
      A.x = 0.0;
    
      return solver<double>::d2linearequ
      (
        A.y,
        A.z,
        nml.y,
        nml.z,
        P2.nml.y,
        P2.nml.z,
        d0,
        P2.d0
      );
    }

  //if (abs(nml.y)+abs(P2.nml.y) < zero )
  if (nmlyIsZero())
    if (P2.nmlyIsZero())
    {
      A.y = 0.0;
    
      return solver<double>::d2linearequ
      (
        A.x,
        A.z,
        nml.x,
        nml.z,
        P2.nml.x,
        P2.nml.z,
        d0,
        P2.d0
      );
    }

  if (nmlzIsZero())
    if (P2.nmlzIsZero())
    {
      A.z = 0.0;
    
      return solver<double>::d2linearequ
      (
        A.x,
        A.y,
        nml.x,
        nml.y,
        P2.nml.x,
        P2.nml.y,
        d0,
        P2.d0
      );
    }

  A.z=1.0;
  return solver<double>::d2linearequ
  (
    A.x,
    A.y,
    nml.x,
    nml.y,
    P2.nml.x,
    P2.nml.y,
    d0-nml.z,
    P2.d0-P2.nml.z
  );
*/
}


plane::operator string const () const
{
  stringstream ss;
  ss << nml.x << " " << nml.y << " " << nml.z << " " << d0;

  return ss.str();
}