#include "Plane3.h"
#include "Matrix4x4.h"
#include "Ray.h"
#include "SimpleRay.h"


namespace GtpVisibilityPreprocessor {


Plane3::Plane3(const Vector3 &a,
			   const Vector3 &b,
			   const Vector3 &c) 
{
    Vector3 v1=a-b, v2=c-b;
    mNormal = Normalize(CrossProd(v2,v1));
    mD = -DotProd(b, mNormal);
}

Plane3::Plane3(const Vector3 &normal,
			   const Vector3 &point):
mNormal(normal)
{
    mD = -DotProd(normal, point);
}


bool
PlaneIntersection(const Plane3 &a, const Plane3 &b, const Plane3 &c, Vector3 &result)
{
  Vector3
    sx(a.mNormal.x, b.mNormal.x, c.mNormal.x),
    sy(a.mNormal.y, b.mNormal.y, c.mNormal.y),
    sz(a.mNormal.z, b.mNormal.z, c.mNormal.z),
    sd(a.mD, b.mD, c.mD);

  Matrix4x4 md(a.mNormal, b.mNormal, c.mNormal), mx, my, mz;

  mx.SetColumns(sd, sy, sz);
  my.SetColumns(sx, sd, sz);
  mz.SetColumns(sx, sy, sd);
  
  const float det = md.Det3x3();

  if (fabs(det) < TRASH)
    return false;

  result.SetValue(mx.Det3x3()/det,
				  my.Det3x3()/det,
				  mz.Det3x3()/det);
  
  return true;
}


 bool PlaneIntersection(const Plane3 &p1, const Plane3 &p2)
 {
	 return 
		 p1.mNormal.x != p2.mNormal.x ||
		 p1.mNormal.y != p2.mNormal.y ||
		 p1.mNormal.z != p2.mNormal.z ||
		 p1.mD == p2.mD;
 }


/*
If the planes are known to intersect then determine the origin and unit direction vector of a 
line formed by the intersection of two planes. 
The direction of that line is just the vector product of the normals of the planes. 
Finding a point along the intersection line is theoretically easy but numerical precision 
issues mean that we need to determine which axis it is best to do the calculation in.
The point will lie on the axis that was used to determine the intersection.
*/
SimpleRay GetPlaneIntersection(const Plane3 &plane1, const Plane3 &plane2)
{
	Vector3 point, dir;
	dir = CrossProd(plane1.mNormal, plane2.mNormal);
    
	const int index = dir.DrivingAxis();
   
	switch (index)
	{
	case 0:
		point[0] = 0.0f;
		point[1] = (plane1.mNormal[2] * plane2.mD - plane2.mNormal[2] * plane1.mD) / dir[0] ;
		point[2] = (plane2.mNormal[1] * plane1.mD - plane1.mNormal[1] * plane2.mD) / dir[0] ;
		break;
	case 1:
		point[0] = (plane2.mNormal[2] * plane1.mD - plane1.mNormal[2] * plane2.mD) / dir[1] ;
		point[1] = 0.0f ;
		point[2] = (plane1.mNormal[0] * plane2.mD - plane2.mNormal[0] * plane1.mD) / dir[1] ;
		break;
	case 2:
		point[0] = (plane1.mNormal[1] * plane2.mD - plane2.mNormal[1] * plane1.mD) / dir[2] ;
		point[1] = (plane2.mNormal[0] * plane1.mD - plane1.mNormal[0] * plane2.mD) / dir[2] ;
		point[2] = 0.0f ;
		break;
	}

	Normalize(dir);
	return SimpleRay(point, dir, 0, 1.0f);
}


Vector3 Plane3::FindIntersection(const Vector3 &a,						   
								 const Vector3 &b,
								 float *t,
								 bool *coplanar) const 
{
	//cout << "a: " << a << " b: " << b << endl;
	const Vector3 v = b - a; // line from A to B
	float dv = DotProd(v, mNormal);
    
    if (signum(dv) == 0)
	{
		if (coplanar) 
		{
			(*coplanar) = true;	
		}

		if (t) 
		{
			(*t) = 0;
		}

		return a;
	}
	
	if (coplanar) 
	{
		(*coplanar) = false;
	}

    float u = - Distance(a) / dv; // TODO: could be done more efficiently
    
	if (t) 
	{
		(*t) = u;
	}

    return a + u * v;
}


int Plane3::Side(const Vector3 &v, const float threshold) const 
{
    return signum(Distance(v), threshold);
}


float Plane3::FindT(const Vector3 &a, const Vector3 &b) const 
{	  
	const Vector3 v = b - a; // line from A to B
	const float dv = DotProd(v, mNormal);
    
	// does not intersect or coincident
	if (signum(dv) == 0)
		return 0;
	
	return - Distance(a) / dv; // TODO: could be done more efficiently
}


float Plane3::FindT(const SimpleRay &a) const 
{	  
	const Vector3 v = a.mDirection; // line from A to B
	const float dv = DotProd(v, mNormal);
    
	// does not intersect or coincident
	if (signum(dv) == 0)
		return 0;
	
	return - Distance(a.mOrigin) / dv; // TODO: could be done more efficiently
}

}