#include "Polyhedron.h"
#include "common.h"
#include "Polygon3.h"
#include "Plane3.h"


#ifdef _CRT_SET
	#define _CRTDBG_MAP_ALLOC
	#include <stdlib.h>
	#include <crtdbg.h>

	// redefine new operator
	#define DEBUG_NEW new(_NORMAL_BLOCK, __FILE__, __LINE__)
	#define new DEBUG_NEW
#endif


using namespace std;


namespace CHCDemoEngine 
{

static const float epsilon = 1e-6f;



/************************************************************/
/*              class Polyhedron Implementation             */
/************************************************************/


Polyhedron::Polyhedron()
{
	mBox.Initialize();
}


Polyhedron::Polyhedron(const PolygonContainer &polys)
{
	mPolygons = polys;

	mBox.Initialize();
	mBox.Include(mPolygons);
}


Polyhedron::Polyhedron(const Polyhedron &rhs)
{
	mPolygons.reserve(rhs.mPolygons.size());
	mBox = rhs.mBox;

	PolygonContainer::const_iterator it, it_end = rhs.mPolygons.end();

	for (it = rhs.mPolygons.begin(); it != it_end; ++ it)
		Add(new Polygon3(*(*it)));
}


Polyhedron *Polyhedron::CreatePolyhedron(const vector<Plane3> &planes, 
										 const AxisAlignedBox3 &sceneBox)
{
	// add the bounding box
	PolygonContainer polygons;
	sceneBox.ExtractPolygons(polygons);

	Polyhedron *oldPolyhedron = new Polyhedron(polygons);

	if (!oldPolyhedron->Valid()) cerr << "******************* not valid!! ************* " << endl;

	vector<Plane3>::const_iterator it, it_end = planes.end();

	Polyhedron *newPolyhedron = NULL;
	
	// intersect bounding box with planes
	for (it = planes.begin(); it != it_end; ++ it)
	{	
		Plane3 plane = *it;
		
		newPolyhedron = oldPolyhedron->CalcIntersection(plane);
	
		if (!newPolyhedron)
		{
			//cerr << "polyhedron not valid or NULL!" << endl; 
			return NULL;	
		}

		DEL_PTR(oldPolyhedron);
		oldPolyhedron = newPolyhedron;
	}

	return newPolyhedron;
}


Polyhedron::~Polyhedron()
{
	CLEAR_CONTAINER(mPolygons);
}


int Polyhedron::NumPolygons() const
{
	return (int)mPolygons.size();
}


void Polyhedron::Add(Polygon3 *p)
{
    mPolygons.push_back(p);
	mBox.Include(*p);
}


const PolygonContainer &Polyhedron::GetPolygons() const
{
	return mPolygons;
}


float Polyhedron::GetArea() const 
{ 
	float area = 0;

	for (size_t i = 0; i < mPolygons.size(); ++ i)
	{
		area += mPolygons[i]->GetArea();
	}

	return area;
}



float Polyhedron::GetVolume() const
{
	// computes volume by tetrahedralization of the geometry
	// We just compute the sum of the tetrahedron volumes
	float volume = 0;
	const float f = 1.0f / 6.0f;

	PolygonContainer::const_iterator pit, pit_end = mPolygons.end();

	// note: can take arbitrary point, e.g., the origin. However,
	// we rather take the center of mass to prevents precision errors
	const Vector3 center = CenterOfMass();

	for (pit = mPolygons.begin(); pit != pit_end; ++ pit)
	{
		Polygon3 *poly = *pit;
		const Vector3 v0 = poly->mVertices[0] - center;

		for (int i = 1; i < (int)poly->mVertices.size() - 1; ++ i)
		{
			const Vector3 v1 = poly->mVertices[i] - center;
			const Vector3 v2 = poly->mVertices[i + 1] - center;

			// more robust version using abs and the center of mass
			volume += fabs (f * (DotProd(v0, CrossProd(v1, v2))));
		}
	}

	return volume;
}


AxisAlignedBox3 Polyhedron::GetBoundingBox() const
{
	return mBox;
}


int Polyhedron::Side(const Plane3 &plane) const
{
	int boxSide = mBox.Side(plane);

	// plane does not intersect bounding box
	if (boxSide != 0) return boxSide;

	PolygonContainer::const_iterator it, it_end = mPolygons.end();

	int s = 0;

	for (it = mPolygons.begin(); it != it_end; ++ it)
	{
        const int side = (*it)->Side(plane, epsilon);

		if (side == 0) // intersects polygon => intersects
			return 0;
		else if (side != 0)
		{
			if (side == -s)
				return 0; // sign changed => intersects

			s = side;
		}
	}

	return s;
}


Vector3 Polyhedron::CenterOfMass() const
{
	int n = 0;

	Vector3 center(0,0,0);

	PolygonContainer::const_iterator pit, pit_end = mPolygons.end();

	for (pit = mPolygons.begin(); pit != pit_end; ++ pit)
	{
		Polygon3 *poly = *pit;
		
		VertexArray::const_iterator vit, vit_end = poly->mVertices.end();

		for(vit = poly->mVertices.begin(); vit != vit_end; ++ vit)
		{
			center += *vit;
			++ n;
		}
	}

	return center / (float)n;
}


bool Polyhedron::Valid() const
{
	// polyhedron is degenerated
	if (mPolygons.size() < 3)
		return false;
	
	const Vector3 center = CenterOfMass();

	PolygonContainer::const_iterator pit, pit_end = mPolygons.end();

	for (pit = mPolygons.begin(); pit != pit_end; ++ pit)
	{
		Polygon3 *poly = *pit;
		Plane3 plane = poly->GetSupportingPlane();

		float dist = plane.Distance(center);
		
		if (dist > 0) return false;
	}
	return true;
}


Polyhedron *Polyhedron::CalcIntersection(const Plane3 &splitPlane) const
{	
	// first test plane intersection with geometry
	const int intersect = Side(splitPlane);
	
	if (intersect == 1)
	{
		// on the other side of the plane, the intersection is empty
		return NULL;
	}
	else if (intersect == -1)
	{
		// does not intersect, just return a copy
		return new Polyhedron(*this);
	}

	// polyhedron is intersected: create new polyhedron
	Polyhedron *clippedPolyhedron = new Polyhedron();


	//////////
	//-- create and add new polygon to close polyhedron

	// get cross section of new polygon
	Polygon3 *planePoly = mBox.CrossSection(splitPlane);

	// create new face: split polygon with all other polygons
	planePoly = SplitPolygon(planePoly);

	if (planePoly) 
	{
		// we can now savely add the new polygon
		clippedPolyhedron->Add(planePoly);
	}
	else if (0)
	{
		// something is wrong, probably some numerical error
		cerr << "no polygon: should not happen" << endl; 
		return new Polyhedron(*this); 
	}
	

	/////////////
	//-- clip polyhedron: plane splits all polygons

	for (size_t i = 0; i < mPolygons.size(); ++ i)
	{
		Polygon3 *poly = mPolygons[i];

		const int cf = mPolygons[i]->ClassifyPlane(splitPlane, epsilon);
			
		switch (cf)
		{
			// split case: split polygon and add it
			case Polygon3::SPLIT:
				{
					Polygon3 *poly = mPolygons[i];

					Polygon3 *frontPoly = new Polygon3();
					Polygon3 *backPoly = new Polygon3();
				
					poly->Split(splitPlane, *frontPoly, *backPoly, epsilon);

					// we are only interested in the polytope behind the plane
					DEL_PTR(frontPoly);

					if (backPoly->Valid(epsilon))
						clippedPolyhedron->Add(backPoly);
					else
						DEL_PTR(backPoly);
				}
				
				break;
			case Polygon3::BACK_SIDE:
			case Polygon3::COINCIDENT:
				clippedPolyhedron->Add(new Polygon3(mPolygons[i]->mVertices));
				break;
			default:
				// polygon is not in intersection
				break;
		}
	}

	return clippedPolyhedron;
}


Polygon3 *Polyhedron::SplitPolygon(Polygon3 *polygon) const
{
	if (!polygon->Valid(epsilon)) DEL_PTR(polygon);

	// polygon is split by all other planes
	for (size_t i = 0; (i < mPolygons.size()) && polygon; ++ i)
	{
		const Plane3 plane = mPolygons[i]->GetSupportingPlane();

		int cf = polygon->ClassifyPlane(plane, epsilon);

		// split new polygon with all previous planes
		switch (cf)
		{
			case Polygon3::SPLIT:
				{
					Polygon3 *frontPoly = new Polygon3();
					Polygon3 *backPoly = new Polygon3();

					polygon->Split(plane, *frontPoly, *backPoly, epsilon);
					
					// don't need these anymore
					DEL_PTR(frontPoly);
					DEL_PTR(polygon);

					// back polygon belongs to geometry
					if (backPoly->Valid(epsilon))
						polygon = backPoly;
					else
						DEL_PTR(backPoly);
				}
				break;
			case Polygon3::FRONT_SIDE:
				//cerr << "SplitPolygon: should not come here" << endl;
				DEL_PTR(polygon);
                break;
			// polygon is taken as it is
			case Polygon3::BACK_SIDE:
			case Polygon3::COINCIDENT:
			default:
				break;
		}
	}

	return polygon;
}


void Polyhedron::CollectVertices(VertexArray &vertices) const
{
	PolygonContainer::const_iterator it, it_end = mPolygons.end();

	for (it = mPolygons.begin(); it != it_end; ++ it)
	{
		Polygon3 *poly = *it;

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

		for (vit = poly->mVertices.begin(); vit != vit_end; ++ vit)
		{
			vertices.push_back(*vit);
		}
	}
}


}