#include "Triangle3.h"
#include "Ray.h"
#include "AxisAlignedBox3.h"
#include "Containers.h"
#include "Polygon3.h"


namespace GtpVisibilityPreprocessor {

	
Triangle3::Triangle3(const Vector3 &a, const Vector3 &b, const Vector3 &c) 
{
	Init(a, b, c);
}


void Triangle3::Init(const Vector3 &a, const Vector3 &b, const Vector3 &c) 
{
	mVertices[0] = a;
	mVertices[1] = b;
	mVertices[2] = c;
}


float Triangle3::GetSpatialAngle(const Vector3 &point) const
{
	return 0.0f;
}


int
Triangle3::CastRay(const Ray &ray,
				   float &t,
				   const float nearestT,
				   Vector3 &normal) const
{
#if 0
	VertexContainer vertices;
	vertices.push_back(mVertices[0]);
	vertices.push_back(mVertices[1]);
	vertices.push_back(mVertices[2]);

	Polygon3 poly(vertices);

	int dummy = poly.CastRay(ray, t, nearestT);
	normal = poly.GetNormal();
	
	//	cout << "polyversion code: " << dummy << " t: " << t << " nearestT: " << nearestT << endl;
	return dummy;
#endif
	
	//////////////
	// specialised triangle ray casting version
	// using ray-plane intersection
	
	// get triangle edge vectors and plane normal
	const Vector3 u = mVertices[0] - mVertices[1];
    const Vector3 v = mVertices[2] - mVertices[1];

	// cross product
    normal = Normalize(CrossProd(v, u));

	// ray direction vector
    const Vector3 dir = ray.GetDir();
    const Vector3 w0 = ray.GetLoc() - mVertices[1];

	// params to calc ray-plane intersect
    const float a = -DotProd(normal, w0);
    const float b = DotProd(normal, dir);

	// check for division by zero
	if (fabs(b) < Limits::Small) 
	{   
		// ray is parallel to triangle plane
        if (a == 0)
		{
			// ray lies in triangle plane
            return Ray::INTERSECTION_OUT_OF_LIMITS;
		}
        else
		{
			// ray disjoint from plane
			return Ray::NO_INTERSECTION;
		}
    }

    // distance from origin of ray to plane
    t = a / b;

    if (t <= Limits::Small) // ray goes away from triangle
	{
	  return Ray::INTERSECTION_OUT_OF_LIMITS;
	}
	// already found nearer intersection
	else if ((ray.GetType() == Ray::LOCAL_RAY) && (t >= nearestT))
	{
		return Ray::NO_INTERSECTION;
	}

	/////////////////
    //-- found intersection point
	//-- check if it is inside triangle
  
	const Vector3 pt = ray.GetLoc() + t * dir; 
#if GTP_DEBUG
	if (!pt.CheckValidity())
	{
		cout << "tr: " << *this << endl;
		cout << "v: " << pt << " t: " << t << " a: " << a << " b: " << b << " n: " << normal << endl;
	}
#endif
	const Vector3 w = pt - mVertices[1];

	const float uu = DotProd(u, u);
    const float uv = DotProd(u, v);
    const float vv = DotProd(v, v);
    
	const float wu = DotProd(w, u);
    const float wv = DotProd(w, v);


	const float D = uv * uv - uu * vv;

    // get and test parametric coords
    const float s = (uv * wv - vv * wu) / D;

    if ((s < -Limits::Small) || (s > 1.0f + Limits::Small)) // pt is outside triangle
	{	
        return Ray::NO_INTERSECTION;
	}

	const float s2 = (uv * wu - uu * wv) / D;

    if ((s2 < -Limits::Small) || ((s + s2) > 1.0f + Limits::Small)) // pt is outside triangle
	{	
        return Ray::NO_INTERSECTION;
	}

	return Ray::INTERSECTION; // I is in T
}


AxisAlignedBox3 Triangle3::GetBoundingBox() const
{
	AxisAlignedBox3 box;
	box.Initialize();

	box.Include(mVertices[0]);
	box.Include(mVertices[1]);
	box.Include(mVertices[2]);

	return box;
}


Vector3 Triangle3::GetNormal() const 
{
	const Vector3 v1 = mVertices[0] - mVertices[1];
	const Vector3 v2 = mVertices[2] - mVertices[1];

	return Normalize(CrossProd(v2, v1));
}


Vector3 Triangle3::GetCenter() const 
{
	return (mVertices[0] + mVertices[1] + mVertices[2]) / 3.0f;
}


float Triangle3::GetArea() const 
{
	Vector3 v1 = mVertices[0] - mVertices[1], v2=mVertices[2] - mVertices[1];
	return 0.5f * Magnitude(CrossProd(v2, v1));
}


bool Triangle3::CheckValidity() const
{
	return !(
		EpsilonEqualV3(mVertices[0], mVertices[1]) ||
		EpsilonEqualV3(mVertices[0], mVertices[2]) ||
		EpsilonEqualV3(mVertices[1], mVertices[2])
		);
}


bool Triangle3::GetPlaneIntersection(const Plane3 &plane, 
									 Vector3 &intersectA, 
									 Vector3 &intersectB) const
{
	int side[3];

	// compute distance from plane
	for (int i = 0; i < 3; ++ i)
	{
		side[i] = plane.Side(mVertices[i], Limits::Small);
	}

	/////
	// no intersection => early exit
	if (((side[0] > 0) && (side[1] > 0) && (side[2] > 0)) ||
		((side[0] < 0) && (side[1] < 0) && (side[2] < 0)))
	{
		return false;
	}

	/////////////
	// at least 2 triangle vertices lie in plane => early exit

	for (int i = 0; i < 3; ++ i)
	{
		if (!side[i] && !side[(i + 1) % 3])
		{
			intersectA = mVertices[i];
			intersectB = mVertices[(i + 1) % 3];
			
			return true;
		}
	}

	bool foundA = false;

	// compute intersection points
	for (int i = 0; i < 3; ++ i)
	{
		const int i_2 = (i + 1) % 3;

		// intersection found
		if ((side[i] >= 0) && (side[i_2] <= 0) ||
			(side[i] <= 0) && (side[i_2] >= 0))
		{	
			const float t = plane.FindT(mVertices[i], mVertices[i_2]);
			
			if (!foundA)
			{
				intersectA = mVertices[i] + t * (mVertices[i_2] - mVertices[i]);
			
				foundA = true;
			}
			else
			{
				intersectB = mVertices[i] + t * (mVertices[i_2] - mVertices[i]);
			
				return true;
			}
		}
	}

	cout << "warning! wrong triangle - plane intersection" << endl;
	return false; // something went wrong!
}


}