#include "Polygon3.h"
#include "AxisAlignedBox3.h"
#include "Plane3.h"


using namespace std;

namespace CHCDemoEngine 
{

Polygon3::Polygon3()
{
}


Polygon3::Polygon3(const VertexArray &vertices)
{
	mVertices.reserve(vertices.size());
	mVertices = vertices;
}


Polygon3::~Polygon3() 
{
}


Plane3 Polygon3::GetSupportingPlane() const
{
	return Plane3(mVertices[0], mVertices[1], mVertices[2]);
}


Vector3 Polygon3::GetNormal() const 
{
    return Normalize(CrossProd(mVertices[2] - mVertices[1],
							   mVertices[0] - mVertices[1]));
}


void Polygon3::Split(const Plane3 &partition, 
					 Polygon3 &front, 
					 Polygon3 &back, float epsilon)
{
	Vector3 ptA = mVertices.back();
	int sideA = partition.Side(ptA, epsilon);
	bool foundSplit = false;
    Vector3 lastSplit;
		
	/////////////
	//-- find line - plane intersections

	VertexArray::const_iterator it, it_end = mVertices.end();

	for (it = mVertices.begin(); it != it_end; ++ it)
	{
		Vector3 ptB = *it;
		int sideB = partition.Side(ptB, epsilon);
	
		// vertices on different sides => split
	    if (sideB > 0)
		{
			if (sideA < 0)
			{
				//-- plane - line intersection
				Vector3 splitPt = partition.FindIntersection(ptA, ptB);
			
				// test if split point not too close to previous split point
				if (!foundSplit || (!EpsilonEqualV3(splitPt, lastSplit, epsilon)))
				{
					// add vertex to both polygons
					front.mVertices.push_back(splitPt);
					back.mVertices.push_back(splitPt);
					
					lastSplit = splitPt;
					foundSplit = true;
				}
			}
			front.mVertices.push_back(ptB);
		}
		else if (sideB < 0)
		{
			if (sideA > 0)
			{
				////////////
				//-- plane - line intersection

				Vector3 splitPt = partition.FindIntersection(ptA, ptB);
				
				// test if split point not too close to previous split point
				if (!foundSplit || (!EpsilonEqualV3(splitPt, lastSplit, epsilon)))
				{
					// add vertex to both polygons
					front.mVertices.push_back(splitPt);
					back.mVertices.push_back(splitPt);

					lastSplit = splitPt;
					foundSplit = true;
				}	
			}

			back.mVertices.push_back(ptB);
		}
		else
		{
			// vertex on plane => add vertex to both polygons
			front.mVertices.push_back(ptB);
			back.mVertices.push_back(ptB); 
		}
	
		ptA = ptB;
		sideA = sideB;
	}
}


float Polygon3::GetArea() const
{
	Vector3 v = CrossProd(mVertices.back(), mVertices.front());
    
    for (int i=0; i < mVertices.size() - 1; ++i)
		v += CrossProd(mVertices[i], mVertices[i+1]);
    
    return 0.5f * fabs(DotProd(GetNormal(), v));
}


int Polygon3::Side(const Plane3 &plane, 
				   const float epsilon) const
{
	int classification = ClassifyPlane(plane, epsilon);
	
	if (classification == BACK_SIDE)
		return -1;
	else if (classification == FRONT_SIDE)
		return 1;

	// plane splits polygon or is coincident
	return 0;
}


int Polygon3::ClassifyPlane(const Plane3 &plane, 
							const float epsilon) const
{
	VertexArray::const_iterator it, it_end = mVertices.end();

	bool onFrontSide = false;
	bool onBackSide = false;
	
	int count = 0;
	// find possible line-plane intersections
	for (it = mVertices.begin(); it != it_end; ++ it)
	{
		const int side = plane.Side(*it, epsilon);
		
		if (side > 0)
		{
			onFrontSide = true;
		}
		else if (side < 0)
		{
			onBackSide = true;
		}

		//TODO: check if split goes through vertex
		if (onFrontSide && onBackSide) // split 
		{
			return SPLIT;
		}
		// 3 vertices enough to decide if coincident
		else if (((++ count) >= 3) && !onFrontSide && !onBackSide)
		{   
			return COINCIDENT; 
		}
	}

	if (onBackSide)
	{
		return BACK_SIDE;
	}
	else if (onFrontSide)
	{
		return FRONT_SIDE;
	}
	
	return COINCIDENT; // plane and polygon are coincident
}


Vector3 Polygon3::Center() const
{
	int i;
	Vector3 sum = mVertices[0];
	for (i=1; i < mVertices.size(); i++)
		sum += mVertices[i];
	
	return sum / (float)i;
}


bool Polygon3::Valid(const float epsilon) const
{
	if (mVertices.size() < 3)
		return false;
#if 0
	// matt: removed for performance issues
	// check if area exceeds certain size
	if (GetArea() < 1e-10)
	{
		cerr << "area too small: " << GetArea() << std::endl;
		return false;
	}
		
	Vector3 vtx = mVertices.back();
	VertexArray::const_iterator it, it_end = mVertices.end();

	for (it = mVertices.begin(); it != it_end; ++it)
	{
		if (EpsilonEqualV3(vtx, *it, epsilon))
		{
			cerr << "Double vertices:\n" << *this << endl;
			return false;
		}
		vtx = *it;
	}
#endif
	return true;
}


// int_lineseg returns 1 if the given line segment intersects a 2D
// ray travelling in the positive X direction.	This is used in the
// Jordan curve computation for polygon intersection.
inline int int_lineseg(float px,
					   float py,
					   float u1,
					   float v1,
					   float u2,
					   float v2)
{
	float t;
	float ydiff;

	u1 -= px; u2 -= px; 	  // translate line
	v1 -= py; v2 -= py;

	if ((v1 > 0 && v2 > 0) ||
		(v1 < 0 && v2 < 0) ||
		(u1 < 0 && u2 < 0))
		return 0;

	if (u1 > 0 && u2 > 0)
		return 1;

	ydiff = v2 - v1;

	if (fabs(ydiff) < Limits::Small) 
	{	 
		// denominator near 0
		if (((fabs(v1) > Limits::Small) || (u1 > 0) || (u2 > 0)))
			return 0;

		return 1;
	}

	t = -v1 / ydiff; // Compute parameter

	return (u1 + t * (u2 - u1)) > 0;
}


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

	VertexArray::const_iterator vit, vit_end = mVertices.end();

	for (vit = mVertices.begin(); vit != vit_end; ++ vit)
	{
		box.Include(*vit);
	}

	return box;
}


}