#include "GeoTreeSimplifier.h"
#include "Leaf.h"

#include <iostream>
#include <fstream>

using namespace Geometry;
using namespace std;

const int VISIT_PARENTS_DEEP=1;

class LeafOctree
{
public:
	float left, right, bottom, top, front, back;
	std::vector<int> leaves; ///< leaves that are too much big to fit in any of the child leaves of this octree leaf
	LeafOctree* parent;
	LeafOctree* children[8];
	bool PointInsideOctree(float x, float y, float z) const {
		return (x>=left && x<=right && y>=bottom && y<=top && z>=front && z<=back);
	}
	bool HasChildren(void) const { return (children[0] && children[1] && children[2] && children[3] && children[4] && children[5] && children[6] && children[7]); }
};

TreeSimplifier::TreeSimplifier(const Mesh *m, TIPOFUNC upb)
{
	objmesh	= m;
	mtreesimpsequence = new Geometry::TreeSimplificationSequence();
	activeLeaves =	0;
	countLeaves	 =	0;

	// Output mesh
	mesh	=	new Geometry::Mesh();
	*mesh	=	*m;

	//	Sets the progress bar.
	mUPB	=	upb;
}

TreeSimplifier::~TreeSimplifier()
{
	delete	mtreesimpsequence;
}

// Returns the simplified mesh.
Geometry::Mesh *TreeSimplifier::GetMesh ()
{
	return	mesh;
}

Geometry::TreeSimplificationSequence *TreeSimplifier::GetSimplificationSequence()
{
	return	mtreesimpsequence;
}
/*#include <time.h> // necesario para gettickcount
#include <map>*/
void TreeSimplifier::Simplify(Real lodfactor,	Index meshLeaves)
{
	long	int	totalv;
	long	int	newleaf;
	float	diam;

	totalv	=	0;

/*	// DEBUG
	FILE *f = fopen("timelog.txt","wt");
	int initime = time(0);
	fprintf(f,"initime: %d", initime); fflush(f);
	// DEBUG*/
	
	Mesh2Structure(objmesh, meshLeaves);
	
	diam = BoundingSphereDiameter();

	// precalculate the octree to speed up the simplification process
	const int octree_deep = 2;
	octree=CreateLeafOctree(octree_deep);

	SetCriteriaOptimized(diam);

	int target_face_count = (int)((lodfactor*0.01f) * activeLeaves);
	int update_each = (activeLeaves - target_face_count)/68;
//	int prune_each = octree_deep?(activeLeaves - target_face_count)/octree_deep:0;
	
//	FILE *fp = fopen("pruning.txt","wt"); // DEBUG

/*	std::map<float,int> hojas_x_crit;
	for (int i=0; i<countLeaves; i++)
		hojas_x_crit[Leaves[i].criteria] = i;

	int *leaf_order = new int[countLeaves];
	std::map<float,int>::iterator kkit = hojas_x_crit.begin();
	for (int i=0; i<countLeaves; i++, kkit++)
		leaf_order[i] = kkit->second;*/

//	int erased_leaves = 0;
	while (activeLeaves > target_face_count)
	{
		static int erased_faces_since_last_update = 0;
		if (mUPB && erased_faces_since_last_update >= update_each)
		{
			erased_faces_since_last_update=0;
			mUPB(1);
		}

/*		static int erased_faces_since_last_prune = 0;
		if (octree_deep && erased_faces_since_last_prune >= prune_each)
		{
			erased_faces_since_last_prune=0;
			int pruneinitime = time(0);
			PruneOctree(octree);
			int prunefintime = time(0);
			// DEBUG			
			fprintf(fp,"pruning at %d activeLeaves [%d (s)]\n", activeLeaves, prunefintime-pruneinitime); fflush(f);
			// DEBUG
		}*/

		erased_faces_since_last_update++;				
//		erased_faces_since_last_prune++;

		newleaf	= Collapse(diam);
		SetCriteria2Optimized(diam, newleaf);
//		erased_leaves++;
	}

//	fclose(fp);

/*	int fintime = time(0);
	fprintf(f,"fintime: %d",fintime); fflush(f);
	fprintf(f,"total: %d (s)",fintime-initime); fflush(f);
	fclose(f);*/

	BuildOutputMesh(meshLeaves);	
}

void TreeSimplifier::Mesh2Structure(const Mesh *mesh, Index meshLeaves)
{
	size_t		countv=0;
	long int	pos=0;
	long int	v1, v2, v3;
	long int	triangleID=0;

	countv += vertex_count = mesh->mSubMesh[meshLeaves].mVertexBuffer->mVertexCount;	
	Vertex  = new float[2*countv][3];
		
	size_t	update_each = mesh->mSubMesh[meshLeaves].mVertexBuffer->mVertexCount/5;
	
	for (unsigned int j=0; j<mesh->mSubMesh[meshLeaves].mVertexBuffer->mVertexCount; j++)
	{
		static size_t	ticks_since_last_update = 0;
		if (mUPB && ticks_since_last_update>=update_each)
		{
			ticks_since_last_update=0;
			mUPB(1);
		}
		ticks_since_last_update++;
		
		Vertex[pos][0] = mesh->mSubMesh[meshLeaves].mVertexBuffer->mPosition[j].x; 
		Vertex[pos][1] = mesh->mSubMesh[meshLeaves].mVertexBuffer->mPosition[j].y; 
		Vertex[pos][2] = mesh->mSubMesh[meshLeaves].mVertexBuffer->mPosition[j].z;
		pos++;
	}

	// Calculate the number of leaves
	countLeaves += (long)mesh->mSubMesh[meshLeaves].mIndexCount;
	countLeaves=countLeaves/6; // Each leaf is composed of 6 indices

	if (countLeaves > 0)
		Leaves = new Leaf[2*countLeaves];

	activeLeaves = countLeaves;

	// Insert leaves into the structure
	pos=0;

	update_each = mesh->mSubMesh[meshLeaves].mIndexCount / 5;
	
	// Each leaf is composed of 6 vertices
	for (unsigned int j=0; j<mesh->mSubMesh[meshLeaves].mIndexCount; j=j+6) 
	{
		static size_t	ticks_since_last_update = 0;
		if (mUPB && ticks_since_last_update>=update_each)
		{
			ticks_since_last_update=0;
			mUPB(1);
		}
		ticks_since_last_update+=6;
		
		// first triangle
		v1=mesh->mSubMesh[meshLeaves].mIndex[j];
		v2=mesh->mSubMesh[meshLeaves].mIndex[j+1];
		v3=mesh->mSubMesh[meshLeaves].mIndex[j+2];
		Leaves[pos].vertsLeaf[0]=v1;
		Leaves[pos].vertsLeaf[1]=v2;
		Leaves[pos].vertsLeaf[2]=v3;
		Leaves[pos].idTriangle[0]=triangleID;
		Leaves[pos].exists=true;
		triangleID++;

		// second triangle
		v3=mesh->mSubMesh[meshLeaves].mIndex[j+5];
		Leaves[pos].vertsLeaf[3]=v3;
		Leaves[pos].idTriangle[1]=triangleID;
		triangleID++;

		CalculateLeafCenter(Leaves[pos]);
		CalculateLeafNormal(Leaves[pos]);
		pos++;
	}
}

float TreeSimplifier::max(float a,	float b) const
{
	if (a>b) return (a);
	else return(b);
}

float TreeSimplifier::min(float a,	float b) const
{
	if (a>b) return (b);
	else return(a);
}

float TreeSimplifier::distan(float x1, float y1, float z1, float x2, float y2, float z2) const
{
	return ((x2-x1)*(x2-x1)) + ((y2-y1)*(y2-y1)) + ((z2-z1)*(z2-z1));
}

// Calculates the center of a leaf
void TreeSimplifier::CalculateLeafCenter(Leaf &auxleaf)
{
	float	max_x;
	float	max_y;
	float	max_z;
	float	min_x;
	float	min_y;
	float	min_z;
	
	//x1
	max_x	=	max(max(Vertex[auxleaf.vertsLeaf[0]][0],Vertex[auxleaf.vertsLeaf[1]][0]),
				    max(Vertex[auxleaf.vertsLeaf[2]][0],Vertex[auxleaf.vertsLeaf[3]][0]));
	
	min_x	=	min(min(Vertex[auxleaf.vertsLeaf[0]][0],Vertex[auxleaf.vertsLeaf[1]][0]),
				  	min(Vertex[auxleaf.vertsLeaf[2]][0],Vertex[auxleaf.vertsLeaf[3]][0]));

	auxleaf.center[0] = (max_x + min_x)/2;

	//y1
	max_y	=	max(max(Vertex[auxleaf.vertsLeaf[0]][1],Vertex[auxleaf.vertsLeaf[1]][1]),
				  	max(Vertex[auxleaf.vertsLeaf[2]][1],Vertex[auxleaf.vertsLeaf[3]][1]));
	
	min_y	=	min(min(Vertex[auxleaf.vertsLeaf[0]][1],Vertex[auxleaf.vertsLeaf[1]][1]),
				  	min(Vertex[auxleaf.vertsLeaf[2]][1],Vertex[auxleaf.vertsLeaf[3]][1]));

	auxleaf.center[1]	=	(max_y + min_y) / 2;
	
	//z1
	max_z	=	max(max(Vertex[auxleaf.vertsLeaf[0]][2],Vertex[auxleaf.vertsLeaf[1]][2]),
				  	max(Vertex[auxleaf.vertsLeaf[2]][2],Vertex[auxleaf.vertsLeaf[3]][2]));
	
	min_z	=	min(min(Vertex[auxleaf.vertsLeaf[0]][2],Vertex[auxleaf.vertsLeaf[1]][2]),
				  	min(Vertex[auxleaf.vertsLeaf[2]][2],Vertex[auxleaf.vertsLeaf[3]][2]));

	auxleaf.center[2]	=	(max_z + min_z) / 2;
}


//  calculate the normal vector of a leaf
void TreeSimplifier::CalculateLeafNormal(Leaf &auxleaf)
{
	float onex, oney, onez;
	float twox, twoy, twoz;
	float threex, threey, threez;

	onex   = Vertex[auxleaf.vertsLeaf[0]][0]; oney   = Vertex[auxleaf.vertsLeaf[0]][1]; onez   = Vertex[auxleaf.vertsLeaf[0]][2];
	twox   = Vertex[auxleaf.vertsLeaf[1]][0]; twoy   = Vertex[auxleaf.vertsLeaf[1]][1]; twoz   = Vertex[auxleaf.vertsLeaf[1]][2];
	threex = Vertex[auxleaf.vertsLeaf[2]][0]; threey = Vertex[auxleaf.vertsLeaf[2]][1]; threez = Vertex[auxleaf.vertsLeaf[2]][2];
	
	auxleaf.normal[0] = ((twoz-onez)*(threey-oney)) - ((twoy-oney)*(threez-onez));
	auxleaf.normal[1] = ((twox-onex)*(threez-onez)) - ((threex-onex)*(twoz-onez));
	auxleaf.normal[2] = ((threex-onex)*(twoy-oney)) - ((twox-onex)*(threey-oney));	
}


//  calculate the Hausdorff distance (distance between point clouds)
/*float TreeSimplifier::Hausdorff(Hoja &leaf1, Hoja& leaf2) const
{
	float onex, oney, onez;
	float twox, twoy, twoz;
	float threex, threey, threez;
	float fourx, foury, fourz;
	float x1, y1, z1;
	float x2, y2, z2;
	float x3, y3, z3;
	float x4, y4, z4;
	float dist1, dist2, dist3, dist4, distmp, dista, distb, dist;

	onex=Vertex[leaf1.vertsLeaf[0]][0]; oney= Vertex[leaf1.vertsLeaf[0]][1]; onez = Vertex[leaf1.vertsLeaf[0]][2];
	twox = Vertex[leaf1.vertsLeaf[1]][0]; twoy = Vertex[leaf1.vertsLeaf[1]][1]; twoz = Vertex[leaf1.vertsLeaf[1]][2];
	threex = Vertex[leaf1.vertsLeaf[2]][0]; threey = Vertex[leaf1.vertsLeaf[2]][1]; threez = Vertex[leaf1.vertsLeaf[2]][2];
	fourx = Vertex[leaf1.vertsLeaf[3]][0]; foury = Vertex[leaf1.vertsLeaf[3]][1]; fourz = Vertex[leaf1.vertsLeaf[3]][2];
	

	x1 = Vertex[leaf2.vertsLeaf[0]][0]; y1 = Vertex[leaf2.vertsLeaf[0]][1]; z1 = Vertex[leaf2.vertsLeaf[0]][2];
	x2 = Vertex[leaf2.vertsLeaf[1]][0]; y2 = Vertex[leaf2.vertsLeaf[1]][1]; z2 = Vertex[leaf2.vertsLeaf[1]][2];
	x3 = Vertex[leaf2.vertsLeaf[2]][0]; y3 = Vertex[leaf2.vertsLeaf[2]][1]; z3 = Vertex[leaf2.vertsLeaf[2]][2];
	x4 = Vertex[leaf2.vertsLeaf[3]][0]; y4 = Vertex[leaf2.vertsLeaf[3]][1]; z4 = Vertex[leaf2.vertsLeaf[3]][2];

	dist1 = distan ( onex, oney,onez,x1,y1,z1);

	distmp = distan ( onex, oney,onez,x2,y2,z2);
	if ( distmp < dist1) dist1 = distmp;

	distmp = distan ( onex, oney,onez,x3,y3,z3);
	if ( distmp < dist1) dist1 = distmp;

	distmp = distan ( onex, oney,onez,x4,y4,z4);
	if ( distmp < dist1) dist1 = distmp;

	dist2 = distan ( twox, twoy,twoz,x1,y1,z1);
	
	distmp = distan ( twox, twoy,twoz,x2,y2,z2);
	if ( distmp < dist2) dist2 = distmp;
	
	distmp = distan ( twox, twoy,twoz,x3,y3,z3);
	if ( distmp < dist2) dist2 = distmp;
	
	distmp = distan ( twox, twoy,twoz,x4,y4,z4);
	if ( distmp < dist2) dist2 = distmp;


	dist3 = distan ( threex, threey,threez,x1,y1,z1);
	
	distmp = distan ( threex, threey,threez,x2,y2,z2);
	if ( distmp < dist3) dist3 = distmp;

	distmp = distan ( threex, threey,threez,x3,y3,z3);
	if ( distmp < dist3) dist3 = distmp;

	distmp = distan ( threex, threey,threez,x4,y4,z4);
	if ( distmp < dist3) dist3 = distmp;



	dist4 = distan ( fourx, foury,fourz,x1,y1,z1);
	
	distmp = distan ( fourx, foury,fourz,x2,y2,z2);
	if ( distmp < dist4) dist4 = distmp;

	distmp = distan ( fourx, foury,fourz,x3,y3,z3);
	if ( distmp < dist4) dist4 = distmp;
	
	distmp = distan ( fourx, foury,fourz,x4,y4,z4);
	if ( distmp < dist4) dist4 = distmp;


	dista =  max(dist1, max(dist2, max(dist3, dist4)));

	dist1 = distan ( x1,y1,z1, onex, oney, onez);

	distmp = distan ( x1,y1,z1, twox, twoy, twoz);
	if ( distmp < dist1) dist1 = distmp;

	distmp = distan ( x1,y1,z1, threex, threey, threez);
	if ( distmp < dist1) dist1 = distmp;

	distmp = distan ( x1,y1,z1, fourx, foury, fourz);
	if ( distmp < dist1) dist1 = distmp;

	dist2 = distan ( x2,y2,z2, onex, oney, onez);

	distmp = distan ( x2,y2,z2, twox, twoy, twoz);
	if ( distmp < dist2) dist2 = distmp;

	distmp = distan ( x2,y2,z2, threex, threey, threez);
	if ( distmp < dist2) dist2 = distmp;

	distmp = distan ( x2,y2,z2, fourx, foury, fourz);
	if ( distmp < dist2) dist2 = distmp;


		//3
	dist3 = distan ( x3,y3,z3, onex, oney, onez);

	distmp = distan ( x3,y3,z3, twox, twoy, twoz);
	if ( distmp < dist3) dist3 = distmp;

	distmp = distan ( x3,y3,z3, threex, threey, threez);
	if ( distmp < dist3) dist3 = distmp;

	distmp = distan ( x3,y3,z3, fourx, foury, fourz);
	if ( distmp < dist3) dist3 = distmp;

		//4
	dist4 = distan ( x4,y4,z4, onex, oney, onez);

	distmp = distan ( x4,y4,z4, twox, twoy, twoz);
	if ( distmp < dist4) dist4 = distmp;

	distmp = distan ( x4,y4,z4, threex, threey, threez);
	if ( distmp < dist4) dist4 = distmp;

	distmp = distan ( x4,y4,z4, fourx, foury, fourz);
	if ( distmp < dist4) dist4 = distmp;

	//
	distb =  max(dist1, max(dist2, max(dist3, dist4)));

	dist  = max ( dista, distb);
	return ( dist);
	
}
*/

//  Calculate the Hausdorff distance (distance between point clouds) (Optimized)
float TreeSimplifier::HausdorffOptimized(const Leaf &leaf1, const Leaf& leaf2) const
{
	float onex, oney, onez;
	float twox, twoy, twoz;
	float threex, threey, threez;
	float fourx, foury, fourz;
	float x1, y1, z1;
	float x2, y2, z2;
	float x3, y3, z3;
	float x4, y4, z4;
	float dist1, dist2, dist3, dist4, distmp, dista, distb, dist;

	onex   = Vertex[leaf1.vertsLeaf[0]][0]; oney   = Vertex[leaf1.vertsLeaf[0]][1]; onez   = Vertex[leaf1.vertsLeaf[0]][2];
	twox   = Vertex[leaf1.vertsLeaf[1]][0]; twoy   = Vertex[leaf1.vertsLeaf[1]][1]; twoz   = Vertex[leaf1.vertsLeaf[1]][2];
	threex = Vertex[leaf1.vertsLeaf[2]][0]; threey = Vertex[leaf1.vertsLeaf[2]][1]; threez = Vertex[leaf1.vertsLeaf[2]][2];
	fourx  = Vertex[leaf1.vertsLeaf[3]][0]; foury  = Vertex[leaf1.vertsLeaf[3]][1]; fourz  = Vertex[leaf1.vertsLeaf[3]][2];

	x1 = Vertex[leaf2.vertsLeaf[0]][0]; y1 = Vertex[leaf2.vertsLeaf[0]][1]; z1 = Vertex[leaf2.vertsLeaf[0]][2];
	x2 = Vertex[leaf2.vertsLeaf[1]][0]; y2 = Vertex[leaf2.vertsLeaf[1]][1]; z2 = Vertex[leaf2.vertsLeaf[1]][2];
	x3 = Vertex[leaf2.vertsLeaf[2]][0]; y3 = Vertex[leaf2.vertsLeaf[2]][1]; z3 = Vertex[leaf2.vertsLeaf[2]][2];
	x4 = Vertex[leaf2.vertsLeaf[3]][0]; y4 = Vertex[leaf2.vertsLeaf[3]][1]; z4 = Vertex[leaf2.vertsLeaf[3]][2];

	// variables used to cache distances
	float one1,one2,one3,one4, two1,two2,two3,two4, three1,three2,three3,three4, four1,four2,four3,four4;

	// Store the minimal distances from each of the 4 vertices to the other 4 vertices
	one1 = dist1 = distan ( onex, oney,onez,x1,y1,z1);
	
	one2 = distmp = distan ( onex, oney,onez,x2,y2,z2);
	if ( distmp < dist1) dist1 = distmp;

	one3 = distmp = distan ( onex, oney,onez,x3,y3,z3);
	if ( distmp < dist1) dist1 = distmp;

	one4 = distmp = distan ( onex, oney,onez,x4,y4,z4);
	if ( distmp < dist1) dist1 = distmp;

	
	two1 = dist2 = distan ( twox, twoy,twoz,x1,y1,z1);
	
	two2 = distmp = distan ( twox, twoy,twoz,x2,y2,z2);
	if ( distmp < dist2) dist2 = distmp;
	
	two3 = distmp = distan ( twox, twoy,twoz,x3,y3,z3);
	if ( distmp < dist2) dist2 = distmp;
	
	two4 = distmp = distan ( twox, twoy,twoz,x4,y4,z4);
	if ( distmp < dist2) dist2 = distmp;

	
	three1 = dist3 = distan ( threex, threey,threez,x1,y1,z1);
	
	three2 = distmp = distan ( threex, threey,threez,x2,y2,z2);
	if ( distmp < dist3) dist3 = distmp;

	three3 = distmp = distan ( threex, threey,threez,x3,y3,z3);
	if ( distmp < dist3) dist3 = distmp;

	three4 = distmp = distan ( threex, threey,threez,x4,y4,z4);
	if ( distmp < dist3) dist3 = distmp;


	four1 = dist4 = distan ( fourx, foury,fourz,x1,y1,z1);
	
	four2 = distmp = distan ( fourx, foury,fourz,x2,y2,z2);
	if ( distmp < dist4) dist4 = distmp;

	four3 = distmp = distan ( fourx, foury,fourz,x3,y3,z3);
	if ( distmp < dist4) dist4 = distmp;
	
	four4 = distmp = distan ( fourx, foury,fourz,x4,y4,z4);
	if ( distmp < dist4) dist4 = distmp;

	// pick up the maximum value from those 4

	dista =  max(dist1, max(dist2, max(dist3, dist4)));

	// now use the cached distances to perform the reverse distances

	dist1 = one1;  //distan ( x1,y1,z1, onex, oney, onez);

	distmp = two1; //distan ( x1,y1,z1, twox, twoy, twoz);
	if ( distmp < dist1) dist1 = distmp;

	distmp =  three1; //distan ( x1,y1,z1, threex, threey, threez);
	if ( distmp < dist1) dist1 = distmp;

	distmp = four1; //distan ( x1,y1,z1, fourx, foury, fourz);
	if ( distmp < dist1) dist1 = distmp;

	//2
	dist2 = one2; //distan ( x2,y2,z2, onex, oney, onez);

	distmp = two2; //distan ( x2,y2,z2, twox, twoy, twoz);
	if ( distmp < dist2) dist2 = distmp;

	distmp = three2; //distan ( x2,y2,z2, threex, threey, threez);
	if ( distmp < dist2) dist2 = distmp;

	distmp = four2; //distan ( x2,y2,z2, fourx, foury, fourz);
	if ( distmp < dist2) dist2 = distmp;


		//3
	dist3 = one3; //distan ( x3,y3,z3, onex, oney, onez);

	distmp = two3; //distan ( x3,y3,z3, twox, twoy, twoz);
	if ( distmp < dist3) dist3 = distmp;

	distmp = three3; //distan ( x3,y3,z3, threex, threey, threez);
	if ( distmp < dist3) dist3 = distmp;

	distmp = four3; //distan ( x3,y3,z3, fourx, foury, fourz);
	if ( distmp < dist3) dist3 = distmp;

		//4
	dist4 = one4; //distan ( x4,y4,z4, onex, oney, onez);

	distmp = two4; //distan ( x4,y4,z4, twox, twoy, twoz);
	if ( distmp < dist4) dist4 = distmp;

	distmp = three4; //distan ( x4,y4,z4, threex, threey, threez);
	if ( distmp < dist4) dist4 = distmp;

	distmp = four4; //distan ( x4,y4,z4, fourx, foury, fourz);
	if ( distmp < dist4) dist4 = distmp;

	//
	distb =  max(dist1, max(dist2, max(dist3, dist4)));

	dist  = max ( dista, distb);
	return ( dist);
	
}


// Calculate the distance between leaves
/*
void TreeSimplifier::DistanciaEntreHojas(void)
{
	float dist, distmp ;
	int j;

	for ( int i=0;i<countLeaves;i++)
	{
		if ( Leaves[i].existe == true) // si la hoja aun existe
		{
			// inicializo para poder comparar
			if ( i == (countLeaves-1)) j = 0; 
				else j = i+1;
			while (Leaves[j].existe == false) j++;
			//dist = Leaves[i].Distancia(Leaves[j]);
			dist = HausdorffOptimizado( Leaves[i], Leaves[j]);

			Leaves[i].hoja_cerca = j;
			// empiezo los calculos
			for ( j =0; j<(countLeaves-1);j++)
			{
				if ( j == i) 
					break;
				if ( Leaves[j].existe == true) // si la hoja aun existe
				{
					distmp = HausdorffOptimizado(Leaves[i], Leaves[j]);
					if ( distmp < dist ) 
					{
						dist = distmp;
						Leaves[i].hoja_cerca = j;
					}
				}
				
			}
			Leaves[i].dist = dist;
		}
	}

}*/

// Returns the leaf which distance is the lowest
long int TreeSimplifier::MinDistance(void)
{
	float		mindist=0.0f;
	long int	which=-1;
	int			i=0;

/*	//init
	while (Leaves[i].existe != true)
		i++;
	
	mindist = Leaves[i].dist;
	which	= i;
*/	
	// search the minimum
	for (i = 0; i < countLeaves; i++)
	{
		if (Leaves[i].exists)
		{
			if (mindist > Leaves[i].dist || which==-1)
			{
				mindist = Leaves[i].dist;
				which = i;
			}
		}

	}
	return which;
}

// Calculate the coplanarity between leaves
void TreeSimplifier::CoplanarBetweenLeaves(void)
{
	float	cop;
	float	coptmp;
	int 	i;
	int 	j;

	for (i=0;i<countLeaves;i++)
	{
		if (Leaves[i].exists)
		{
			if (i == countLeaves-1)
				j	=	0;
			else
				j =	i + 1;
			
			while (Leaves[j].exists == false)
				j++;
			
			cop	= Leaves[i].Coplanarity(Leaves[j]);
			Leaves[i].leafCop	=	j;
			
			for (j=0; j<countLeaves-1; j++)
			{
				if (( j != i) && (Leaves[j].exists))
				{
					coptmp	=	Leaves[i].Coplanarity(Leaves[j]);

					// Take the most coplanar: close to 1
					if (coptmp > cop)
					{
						cop	= coptmp;
						Leaves[i].leafCop = j;
					}
				}
			}
			Leaves[i].coplanar = 1 - cop;
		}
	}
}

void TreeSimplifier::TwoGreater(float *maj, int	*indices)
{
	float	m1;
	int		i;

	if	(maj[0] < maj[1])
	{
		m1	= maj[0];
		maj[0] = maj[1];
		maj[1] = m1;

		i =	indices[0];
		indices[0] = indices[1];
		indices[1] = i;
	}

	if (maj[2] < maj[3])
	{
		m1	= maj[2];
		maj[2] = maj[3];
		maj[3]	= m1;
		i	= indices[2];
		indices[2] = indices[3];
		indices[3] = i;
	}

	if  (maj[0] < maj[2])
	{
		m1		=	maj[0];
		maj[0]	=	maj[2];
		maj[2]	=	m1;
		i		=	indices[0];
		indices[0]	=	indices[2];
		indices[2]	=	i;
	}

	if (maj[2] < maj[3])
	{
		m1		=	maj[2];
		maj[2]	=	maj[3];
		maj [3]	=	m1;

		i			=	indices[2];
		indices[2]	=	indices[3];
		indices[3]	=	i;
	}

	if (maj [1] < maj [2])
	{
		m1		=	maj[1];
		maj[1]	=	maj[2];
		maj [2]	=	m1;

		i			= indices[1];
		indices[1]	= indices[2];
		indices[2]	= i;
	}		
}

float TreeSimplifier::BoundingSphereDiameter(void) const
{
	float xmax, xmin, ymax, ymin, zmax, zmin;
	float diameter, cx, cy, cz;
	int j;

	// initialize all vertices to the first
	xmax = Vertex[Leaves[0].vertsLeaf[0]][0];
	xmin = xmax;

	ymax = Vertex[Leaves[0].vertsLeaf[0]][1];
	ymin = ymax;

	zmax = Vertex[Leaves[0].vertsLeaf[0]][2];
	zmin = zmax;

	// search for max and mins

	int update_each	= countLeaves / 5;
	for (int i = 1; i < countLeaves; i++)
	{
		static int ticks_since_last_update = 0;
		if (mUPB && ticks_since_last_update>=update_each)
		{
			ticks_since_last_update=0;
			mUPB(1);
		}
		ticks_since_last_update++;
		
		for (j=0;j<4;j++)
		{			
			if ( xmax < Vertex[Leaves[i].vertsLeaf[j]][0]) xmax = Vertex[Leaves[i].vertsLeaf[j]][0];
			if ( xmin > Vertex[Leaves[i].vertsLeaf[j]][0]) xmin = Vertex[Leaves[i].vertsLeaf[j]][0];

			if ( ymax < Vertex[Leaves[i].vertsLeaf[j]][1]) ymax = Vertex[Leaves[i].vertsLeaf[j]][1];
			if ( ymin > Vertex[Leaves[i].vertsLeaf[j]][1]) ymin = Vertex[Leaves[i].vertsLeaf[j]][1];
			
			if ( zmax < Vertex[Leaves[i].vertsLeaf[j]][2]) zmax = Vertex[Leaves[i].vertsLeaf[j]][2];
			if ( zmin > Vertex[Leaves[i].vertsLeaf[j]][2]) zmin = Vertex[Leaves[i].vertsLeaf[j]][2];
		}
	}

	cx = (xmax + xmin)/2;
	cy = (ymax + ymin)/2;
	cz = (zmax + zmin)/2;

	diameter = ((xmax-xmin)*(xmax-xmin)) + ((ymax-ymin)*(ymax-ymin)) + ((zmax-zmin)*(zmax-zmin));

	return (diameter);
}

void TreeSimplifier::NormalizeDistance(float diameter)
{
	float dtmp;
	for (int i=0; i<countLeaves;i++)
	{
		dtmp = Leaves[i].dist;
		Leaves[i].dist = dtmp / diameter;
		
	}
}
/*

	THIS FUNCTION IS OBSOLETE, AND NEEDS TO BE ERASED
//--------------------------------------------------------------------------------------------------------------------------------
//  establece el criterio como (K1 * dist + K2 * cop)/ K1+K2
//--------------------------------------------------------------------------------------------------------------------------------
void TreeSimplifier::EstableceCriterio(float diametro)
{
	//	Para empezar establezco K1 y K2 con 0,5
	float coptmp2, coptmp, distmp2, distmp, criteriotmp;
	int i, j, nhojasi, nhojasj;

	int update_each	= countLeaves / 30;
	
	for ( i=0; i<countLeaves;i++)
	{
		if (Leaves[i].existe == true)
		{
			//incializo criterio a un numero elevado
			Leaves[i].criteria = 1000;
			nhojasi = int(Leaves[i].parentLeafCount);
			//coplanaridad
			for ( j =0; j<countLeaves;j++)
			{
				if (( j != i) && ( Leaves[j].existe == true)) // si la hoja aun existe
				{
					//17/09/01 ANTES DE CALCULAR NADA, COMPRUEBO QUE ESTAS DOS HOJAS 
					// SE PODRIAN COLAPSAR, ED, QUE LA DIFERENCIA DE HOJAS QUE COLAPSAN
					// ES COMO MÁXIMO 1

					nhojasj = int(Leaves[j].parentLeafCount);

					if ( abs((nhojasi - nhojasj)) < 2)
					{
						//coplanaridad y lo invierto
						coptmp2 = Leaves[i].Coplanaridad(Leaves[j]); 
						coptmp = 1 - coptmp2;
						//distancia y la normalizo
						distmp2 = Hausdorff( Leaves[i], Leaves[j]); 
						distmp = distmp2 / diametro;
						// calculo el criterio para esa hoja
						criteriotmp = (( K1 * distmp * distmp ) + (K2 * coptmp * distmp))/ (K1 + K2);
						//selecciono el criterio menor
						if (Leaves[i].criteria > criteriotmp) 
						{
							Leaves[i].criteria = criteriotmp;
							Leaves[i].hoja_crit = j;
						}
					}
				}
				
			}
		}
	}
}

//--------------------------------------------------------------------------------------------------------------------------------
//  establece el criterio despues de colapsar
//--------------------------------------------------------------------------------------------------------------------------------
void TreeSimplifier::EstableceCriterio2 ( float			diametro,
																					long int	hojanueva)
{
	//Para empezar establezco K1 y K2 con 0,5
	float  coptmp2, coptmp, distmp2, distmp, criteriotmp;
	int i, j, nhojasi, nhojasj;
	//ESTAN EN tres PROCEDIMIENTOS: COLAPSA, ESTABLECECRITERIO Y ESTABLECECRITERIO2
	
	for (i = 0; i < countLeaves; i++)
	{
		if ((Leaves[i].existe == true) && (i != hojanueva))
		{	
			nhojasi = int(Leaves[i].parentLeafCount);
			//¿ SE HA DESACTIVADO LA HOJA_CRIT QUE GUARDABA LA HOJA?
			if ( Leaves[Leaves[i].hoja_crit].existe == false)
			{
				Leaves[i].criteria = 1000;

				//coplanaridad
				for ( j =0; j<countLeaves;j++)
				{
					if (( j != i) && ( Leaves[j].existe == true)) // si la hoja aun existe
					{
						//17/09/01 ANTES DE CALCULAR NADA, COMPRUEBO QUE ESTAS DOS HOJAS 
						// SE PODRIAN COLAPSAR, ED, QUE LA DIFERENCIA DE HOJAS QUE COLAPSAN
						// ES COMO MÁXIMO 1

						nhojasj = int(Leaves[j].parentLeafCount);

						if ( abs((nhojasi - nhojasj)) < 2)
						{

							//coplanaridad y lo invierto
							coptmp2 = Leaves[i].Coplanaridad(Leaves[j]); 
							coptmp = 1 - coptmp2;	
							//distancia y la normalizo	
						//	distmp2 = Leaves[i].Distancia(Leaves[j]); 
							distmp2 = Hausdorff( Leaves[i],  Leaves[j]); 
							distmp = distmp2 / diametro;
							// calculo el criterio para esa hoja
							criteriotmp = (( K1 * distmp * distmp ) + (K2 * coptmp * distmp))/ (K1 + K2);
							//selecciono el criterio menor
							if (Leaves[i].criteria > criteriotmp) 
							{
								Leaves[i].criteria = criteriotmp;
								Leaves[i].hoja_crit = j;
							}
						}
					}
				}
			}
			else
			{ // CALCULARE SI EL CRITERIO CON ESTA HOJA ES MENOR QUE EL ANTERIOR
				nhojasj = int(Leaves[hojanueva].parentLeafCount);

				if ( abs((nhojasi - nhojasj)) < 2)
				{
					//coplanaridad y lo invierto
					coptmp2 = Leaves[i].Coplanaridad(Leaves[hojanueva]); 
					coptmp = 1 - coptmp2;
					//distancia y la normalizo
					//distmp2 = Leaves[i].Distancia(Leaves[hojanueva]); 
					distmp2 = Hausdorff( Leaves[i], Leaves[hojanueva]); 
					distmp = distmp2 / diametro;
					// calculo el criterio para esa hoja
					criteriotmp = (( K1 * distmp * distmp ) + (K2 * coptmp * distmp))/ (K1 + K2);
					//selecciono el criterio menor
					if (Leaves[i].criteria > criteriotmp) 
					{	
						Leaves[i].criteria = criteriotmp;
						Leaves[i].hoja_crit = hojanueva;
					}
				}
			}
		}
	}
}*/



// Gets the laf with the minimum criteria
long int TreeSimplifier::MinCriteria (void)
{
	float mincrit=0.0f;
	long int which=-1;
		
/*	while (Leaves[i].existe != true) i++;
	mincrit = Leaves[i].criteria;
	which =i;
	*/
	for (int i=0;i<countLeaves;i++)
	{
		if (Leaves[i].exists)
		{
			if (mincrit>Leaves[i].criteria || which==-1)
			{
				mincrit = Leaves[i].criteria;
				which = i;
			}
		}
		
	}
	return which;
}


void TreeSimplifier::ChooseVertices(Leaf& leaf1, Leaf& leaf2, long int count)
{
	float a,b,c;
	float dist[4];
	int indices[4];
	
	a = Vertex[leaf1.vertsLeaf[0]][0];
	b = Vertex[leaf1.vertsLeaf[0]][1];
	c = Vertex[leaf1.vertsLeaf[0]][2];

	dist[0] = ((leaf2.center[0]-a)*(leaf2.center[0]-a)) + ((leaf2.center[1]-b)*(leaf2.center[1]-b)) +
		   ((leaf2.center[2]-c)*(leaf2.center[2]-c));

	
	a = Vertex[leaf1.vertsLeaf[1]][0];
	b = Vertex[leaf1.vertsLeaf[1]][1];
	c = Vertex[leaf1.vertsLeaf[1]][2];

	dist[1] = ((leaf2.center[0]-a)*(leaf2.center[0]-a)) + ((leaf2.center[1]-b)*(leaf2.center[1]-b)) +
		   ((leaf2.center[2]-c)*(leaf2.center[2]-c));

	a = Vertex[leaf1.vertsLeaf[2]][0];
	b = Vertex[leaf1.vertsLeaf[2]][1];
	c = Vertex[leaf1.vertsLeaf[2]][2];

	dist[2] = ((leaf2.center[0]-a)*(leaf2.center[0]-a)) + ((leaf2.center[1]-b)*(leaf2.center[1]-b)) +
		   ((leaf2.center[2]-c)*(leaf2.center[2]-c));


	a = Vertex[leaf1.vertsLeaf[3]][0];
	b = Vertex[leaf1.vertsLeaf[3]][1];
	c = Vertex[leaf1.vertsLeaf[3]][2];

	dist[3] = ((leaf2.center[0]-a)*(leaf2.center[0]-a)) + ((leaf2.center[1]-b)*(leaf2.center[1]-b)) +
		   ((leaf2.center[2]-c)*(leaf2.center[2]-c));

	for ( int i=0;i<4;i++) indices[i]=i;

	TwoGreater(dist, indices);
	
	Leaves[countLeaves].vertsLeaf[0] = leaf1.vertsLeaf[indices[0]]; 
	Leaves[countLeaves].vertsLeaf[1] = leaf1.vertsLeaf[indices[1]]; 
	

	
	a = Vertex[leaf2.vertsLeaf[0]][0];
	b = Vertex[leaf2.vertsLeaf[0]][1];
	c = Vertex[leaf2.vertsLeaf[0]][2];

	dist[0] = ((leaf1.center[0]-a)*(leaf1.center[0]-a)) + ((leaf1.center[1]-b)*(leaf1.center[1]-b)) +
		   ((leaf1.center[2]-c)*(leaf1.center[2]-c));

	
	a = Vertex[leaf2.vertsLeaf[1]][0];
	b = Vertex[leaf2.vertsLeaf[1]][1];
	c = Vertex[leaf2.vertsLeaf[1]][2];

	dist[1] = ((leaf2.center[0]-a)*(leaf2.center[0]-a)) + ((leaf1.center[1]-b)*(leaf1.center[1]-b)) +
		   ((leaf2.center[2]-c)*(leaf2.center[2]-c));

	a = Vertex[leaf2.vertsLeaf[2]][0];
	b = Vertex[leaf2.vertsLeaf[2]][1];
	c = Vertex[leaf2.vertsLeaf[2]][2];

	dist[2] = ((leaf1.center[0]-a)*(leaf1.center[0]-a)) + ((leaf1.center[1]-b)*(leaf1.center[1]-b)) +
		   ((leaf1.center[2]-c)*(leaf1.center[2]-c));


	a = Vertex[leaf2.vertsLeaf[3]][0];
	b = Vertex[leaf2.vertsLeaf[3]][1];
	c = Vertex[leaf2.vertsLeaf[3]][2];

	dist[3] = ((leaf1.center[0]-a)*(leaf1.center[0]-a)) + ((leaf1.center[1]-b)*(leaf1.center[1]-b)) +
		   ((leaf1.center[2]-c)*(leaf1.center[2]-c));

	for ( int i=0;i<4;i++) indices[i]=i;

	TwoGreater(dist, indices);
	
	Leaves[countLeaves].vertsLeaf[2] = leaf2.vertsLeaf[indices[0]]; 
	Leaves[countLeaves].vertsLeaf[3] = leaf2.vertsLeaf[indices[1]]; 



	
}

// Add leaves
long int TreeSimplifier::Collapse(float diam)
{
	long int i=0, which=-1;
	long int other = -1;
	float coptmp, coptmp2, distmp, distmp2, criteriatmp;

	which=MinCriteria();
	other = Leaves[which].leafCrit;

	//desactivo las hojas cercanas
	Leaves[which].exists = false;
	Leaves[other].exists = false;
	//creo la hoja nueva

	Leaves[countLeaves].leafNear = -1;
	Leaves[countLeaves].dist = 0;
	Leaves[countLeaves].leafCop = -1;
	Leaves[countLeaves].coplanar = 0;

	Leaves[countLeaves].parentLeafCount = Leaves[which].parentLeafCount + Leaves[other].parentLeafCount;
//	Leaves[countLeaves].parentLeafCount = 10;
	ChooseVertices(Leaves[which], Leaves[other], countLeaves);
	CalculateLeafCenter (Leaves[countLeaves]);
	CalculateLeafNormal (Leaves[countLeaves]);

	// find out the minimal octree node that can contain this leaf
	LeafOctree *onode = GetMinOctreeNodeForLeaf(octree,Leaves[countLeaves]);
	octree_owning_leaf[countLeaves] = onode;
	onode->leaves.push_back(countLeaves);

/*	if (Leaves[countLeaves].parentLeafCount > 60 )
	{
			Leaves[countLeaves].existe = false;
			activeLeaves--;
	}
	else
	{		*/
     	Leaves[countLeaves].exists = true;
		Leaves[countLeaves].criteria = 1000000.0f;
		Leaves[countLeaves].idTriangle[0] = countLeaves*2;
		Leaves[countLeaves].idTriangle[1] = countLeaves*2+1;

 		float area_leaf_i = CalculateLeafArea(Leaves[i])/diam;

		for (int j = 0; j < (countLeaves+1); j++){
/*		int visit_parents = VISIT_PARENTS_DEEP;
		for (LeafOctree *octreenode = octree_owning_leaf[i]; octreenode && visit_parents>=0; octreenode=octreenode->parent, visit_parents--)
		{
			for (std::vector<int>::iterator it=octreenode->leaves.begin(); it!=octreenode->leaves.end(); it++)
			{
				int j = *it;*/

				if (j != countLeaves && Leaves[j].exists)
				{
					coptmp2 = Leaves[countLeaves].Coplanarity(Leaves[j]); 
					coptmp = 1 - coptmp2;
					//distmp2 = HausdorffOptimized(Leaves[countLeaves], Leaves[j]);
					distmp2 = DistanceFromCenters(Leaves[countLeaves], Leaves[j]);
					distmp = distmp2 / diam;
					
					criteriatmp = (( K1 * distmp * distmp ) + (K2 * coptmp * distmp))/ (K1 + K2);
					criteriatmp *= CalculateLeafArea(Leaves[j])/diam + area_leaf_i;
					criteriatmp *= Leaves[j].parentLeafCount + Leaves[i].parentLeafCount;
					
					if (Leaves[countLeaves].criteria > criteriatmp) 
					{
						Leaves[countLeaves].criteria = criteriatmp;
						Leaves[countLeaves].leafCrit = j;
					}
				}
	//		}
		}
//	}	

	// Crear el paso de simplificación
	Geometry::TreeSimplificationSequence::Step pasosimp;
	pasosimp.mV0=Leaves[which].idTriangle[0];
	pasosimp.mV1=Leaves[which].idTriangle[1];
	pasosimp.mT0=Leaves[other].idTriangle[0];
	pasosimp.mT1=Leaves[other].idTriangle[1];

	// Nuevos vértices
	pasosimp.mNewQuad[0]=Leaves[countLeaves].vertsLeaf[0];
	pasosimp.mNewQuad[1]=Leaves[countLeaves].vertsLeaf[1];
	pasosimp.mNewQuad[2]=Leaves[countLeaves].vertsLeaf[2];
	pasosimp.mNewQuad[3]=Leaves[countLeaves].vertsLeaf[3];

	// Insertar el paso de simplificación
	mtreesimpsequence->mSteps.push_back(pasosimp);

	//incremento el numero de hojas
	countLeaves++;
	activeLeaves--;
	return (countLeaves-1);//porque lo he incrementado en la linea de antes
}

void TreeSimplifier::BuildOutputMesh(int idMeshLeaves) 
{
	// Calculate the resulting index count after simplifying
	long int tamIndex	=	activeLeaves * 6; // Each leaf has got 6 indices

	delete [] mesh->mSubMesh[idMeshLeaves].mIndex;
	mesh->mSubMesh[idMeshLeaves].mIndexCount=tamIndex;
	mesh->mSubMesh[idMeshLeaves].mIndex=new Geometry::Index[tamIndex];

	// Iterate through all active leaves and dump them to the final mesh
	Geometry::Index index=0;

	int update_each	= countLeaves / 10;
	
	for (long j=0; j<countLeaves;j++)
	{
		static int ticks_since_last_update = 0;
		if (mUPB && ticks_since_last_update>=update_each)
		{
			ticks_since_last_update=0;
			mUPB(1);
		}
		ticks_since_last_update++;
		
		if (Leaves[j].exists)
		{
//			if (index<6)
//			{
				mesh->mSubMesh[idMeshLeaves].mIndex[index]=Leaves[j].vertsLeaf[0];
				mesh->mSubMesh[idMeshLeaves].mIndex[index+1]=Leaves[j].vertsLeaf[1];
				mesh->mSubMesh[idMeshLeaves].mIndex[index+2]=Leaves[j].vertsLeaf[2];
				mesh->mSubMesh[idMeshLeaves].mIndex[index+3]=Leaves[j].vertsLeaf[2];
				mesh->mSubMesh[idMeshLeaves].mIndex[index+4]=Leaves[j].vertsLeaf[1];
				mesh->mSubMesh[idMeshLeaves].mIndex[index+5]=Leaves[j].vertsLeaf[3];
/*			}
			else
			{
				mesh->mSubMesh[idMeshLeaves].mIndex[index]=0;
				mesh->mSubMesh[idMeshLeaves].mIndex[index+1]=0;
				mesh->mSubMesh[idMeshLeaves].mIndex[index+2]=0;
				mesh->mSubMesh[idMeshLeaves].mIndex[index+3]=0;
				mesh->mSubMesh[idMeshLeaves].mIndex[index+4]=0;
				mesh->mSubMesh[idMeshLeaves].mIndex[index+5]=0;
			}*/
			index=index+6;
		}
	}
}

LeafOctree* TreeSimplifier::CreateLeafOctree(int deep)
{
	LeafOctree *leafoctree = new LeafOctree;
	leafoctree->parent=NULL;
	leafoctree->left = Vertex[0][0];
	leafoctree->right = Vertex[0][0];
	leafoctree->bottom = Vertex[0][1];
	leafoctree->top = Vertex[0][1];
	leafoctree->front = Vertex[0][2];
	leafoctree->back = Vertex[0][2];

	// calcualte the limits of the octree
	for (size_t	i	=	0; i < vertex_count; i++)
	{
		if (leafoctree->left>Vertex[i][0])
			leafoctree->left=Vertex[i][0];
		if (leafoctree->right<Vertex[i][0])
			leafoctree->right=Vertex[i][0];
		if (leafoctree->bottom>Vertex[i][1])
			leafoctree->bottom=Vertex[i][1];
		if (leafoctree->top<Vertex[i][1])
			leafoctree->top=Vertex[i][1];
		if (leafoctree->front>Vertex[i][2])
			leafoctree->front=Vertex[i][2];
		if (leafoctree->back<Vertex[i][2])
			leafoctree->back=Vertex[i][2];
	}

	// setup the initial leaves buffer
	leafoctree->leaves.clear();
	octree_owning_leaf=new LeafOctree*[countLeaves*2]; // *2 to reserve memory for all simplified leaves
	for (int i=0; i<countLeaves; i++)
	{
		leafoctree->leaves.push_back(i);
		octree_owning_leaf[i] = leafoctree;
	}

	// generate the octree given an arbitrary depth
	RecursiveCreateLeafOctree(leafoctree,deep);

	// fill the octree
	RecursiveFillOctreeWithLeaves(leafoctree);

	return leafoctree;
}

void TreeSimplifier::RecursiveCreateLeafOctree(LeafOctree *parent, int deep)
{
	for (int i=0; i<8; i++)
	{
		parent->children[i] = NULL;
		if (deep>0)
		{
			LeafOctree *child = parent->children[i] = new LeafOctree;

			child->parent = parent;
			switch(i)
			{
			case 0: 
				child->left=parent->left; 
				child->right=(parent->left+parent->right)*0.5f;
				child->bottom=(parent->bottom+parent->top)*0.5f; 
				child->top=parent->top;
				child->front=parent->front;
				child->back=(parent->front+parent->back)*0.5f; break;
			case 1: 
				child->left=(parent->left+parent->right)*0.5f; 
				child->right=parent->right;
				child->bottom=(parent->bottom+parent->top)*0.5f; 
				child->top=parent->top;
				child->front=parent->front;
				child->back=(parent->front+parent->back)*0.5f; break;
			case 2: 
				child->left=parent->left;
				child->right=(parent->left+parent->right)*0.5f;
				child->bottom=parent->bottom;
				child->top=(parent->bottom+parent->top)*0.5f;
				child->front=parent->front;
				child->back=(parent->front+parent->back)*0.5f; break;
			case 3: 
				child->left=(parent->left+parent->right)*0.5f;
				child->right=parent->right;
				child->bottom=parent->bottom;
				child->top=(parent->bottom+parent->top)*0.5f;
				child->front=parent->front;
				child->back=(parent->front+parent->back)*0.5f; break;
			case 4: 
				child->left=parent->left; 
				child->right=(parent->left+parent->right)*0.5f;
				child->bottom=(parent->bottom+parent->top)*0.5f; 
				child->top=parent->top;
				child->back=parent->back;
				child->front=(parent->front+parent->back)*0.5f; break;
			case 5: 
				child->left=(parent->left+parent->right)*0.5f; 
				child->right=parent->right;
				child->bottom=(parent->bottom+parent->top)*0.5f; 
				child->top=parent->top;
				child->back=parent->back;
				child->front=(parent->front+parent->back)*0.5f; break;
			case 6: 
				child->left=parent->left;
				child->right=(parent->left+parent->right)*0.5f;
				child->bottom=parent->bottom;
				child->top=(parent->bottom+parent->top)*0.5f;
				child->back=parent->back;
				child->front=(parent->front+parent->back)*0.5f; break;
			case 7: 
				child->left=(parent->left+parent->right)*0.5f;
				child->right=parent->right;
				child->bottom=parent->bottom;
				child->top=(parent->bottom+parent->top)*0.5f;
				child->back=parent->back;
				child->front=(parent->front+parent->back)*0.5f; break;
			}
			RecursiveCreateLeafOctree(parent->children[i],deep-1);
		}
	}

}

void TreeSimplifier::RecursiveFillOctreeWithLeaves(LeafOctree *o)
{   
	for (int i=0; i<8; i++)
	{
		LeafOctree *child = o->children[i];
		if (child)
		{
			for (std::vector<int>::iterator it=o->leaves.begin(); it!=o->leaves.end(); )
			{
				int idleaf = *it;
				int idv0 = Leaves[idleaf].vertsLeaf[0];
				int idv1 = Leaves[idleaf].vertsLeaf[1];
				int idv2 = Leaves[idleaf].vertsLeaf[2];
				int idv3 = Leaves[idleaf].vertsLeaf[3];

				float * v0 = Vertex[idv0];
				float * v1 = Vertex[idv1];
				float * v2 = Vertex[idv2];
				float * v3 = Vertex[idv3];

				// if the leaf fits completely inside a child, the child owns it
				if (child->PointInsideOctree(v0[0],v0[1],v0[2]) &&
					child->PointInsideOctree(v1[0],v1[1],v1[2]) &&
					child->PointInsideOctree(v2[0],v2[1],v2[2]) &&
					child->PointInsideOctree(v3[0],v3[1],v3[2]))
				{
					child->leaves.push_back(idleaf);
					it = o->leaves.erase(it);
					octree_owning_leaf[idleaf] = child;
				}
				else
					it++; 
			}
			// recurse all valid children
			RecursiveFillOctreeWithLeaves(o->children[i]);
		}
	}
}

void TreeSimplifier::SetCriteriaOptimized(float diam)
{
	float coptmp2, coptmp, distmp2, distmp, criteriatmp;
	int i, j, nleavesi, nleavesj;

	int update_each	= countLeaves / 20;
	
	for ( i=0; i<countLeaves;i++)
	{
		static int ticks_since_last_update = 0;
		if (mUPB && ticks_since_last_update>=update_each)
		{
			ticks_since_last_update=0;
			mUPB(1);
		}
		ticks_since_last_update++;
		
		if (Leaves[i].exists == true)
		{
			Leaves[i].criteria = 1000000.0f;
			nleavesi = int(Leaves[i].parentLeafCount);
 			float area_leaf_i = CalculateLeafArea(Leaves[i])/diam;

			// Use only the leaves which belong the the same octree node as leaf i or to its parents		
			int visit_parents = VISIT_PARENTS_DEEP;
			for (LeafOctree *octreenode = octree_owning_leaf[i]; octreenode; octreenode=octreenode->parent, visit_parents--)
			{
				for (std::vector<int>::iterator it=octreenode->leaves.begin(); it!=octreenode->leaves.end(); it++)
				{
					j = *it;
//				for (j=0; j<countLeaves; j++)
//				{
					if (j!=i && Leaves[j].exists)
					{
						nleavesj = int(Leaves[j].parentLeafCount);

	//					if ( abs((nleavesi - nleavesj)) < 2)
	//					{							
							coptmp2 = Leaves[i].Coplanarity(Leaves[j]);
							coptmp = 1 - coptmp2;

							//distmp2 = HausdorffOptimized( Leaves[i], Leaves[j]); 
							distmp2 = DistanceFromCenters(Leaves[i], Leaves[j]); 
							distmp = distmp2 / diam;

							//criteriatmp = (( K1 * distmp * distmp ) + (K2 * coptmp * distmp))/ (K1 + K2);
							criteriatmp = distmp;
							// select the lowest
							criteriatmp *= CalculateLeafArea(Leaves[j])/diam + area_leaf_i;
							criteriatmp *= Leaves[j].parentLeafCount + Leaves[i].parentLeafCount;
							if (Leaves[i].criteria > criteriatmp)
							{
								Leaves[i].criteria = criteriatmp;
								Leaves[i].leafCrit = j;
							}
		//				}
					}
				}				
			}
		}
	}
}


void TreeSimplifier::SetCriteria2Optimized(float diam, long int newleaf)
{
	float  coptmp2, coptmp, distmp2, distmp, criteriatmp;
	int i, j, nleavesi, nleavesj;
	
	for (i = 0; i < countLeaves; i++)
	{
		if (Leaves[i].exists && i!=newleaf)
		{	
 			float area_leaf_i = CalculateLeafArea(Leaves[i])/diam;
			nleavesi = int(Leaves[i].parentLeafCount);
			if ( Leaves[Leaves[i].leafCrit].exists == false)
			{
				Leaves[i].criteria = 1000000.0f;

				int visit_parents = VISIT_PARENTS_DEEP;
				for (LeafOctree *octreenode = octree_owning_leaf[i]; octreenode; octreenode=octreenode->parent, visit_parents--)
				{
					for (std::vector<int>::iterator it=octreenode->leaves.begin(); it!=octreenode->leaves.end(); it++)
					{
						j = *it;
	//			for (j=0; j<countLeaves; j++)
	//			{
						if (j!=i && Leaves[j].exists)
						{
							nleavesj = int(Leaves[j].parentLeafCount);

//							if ( abs((nleavesi - nleavesj)) < 2)
//							{
								coptmp2 = Leaves[i].Coplanarity(Leaves[j]); 
								coptmp = 1 - coptmp2;	
							
							
								//distmp2 = HausdorffOptimized( Leaves[i],  Leaves[j]); 
								distmp2 = DistanceFromCenters( Leaves[i], Leaves[j]); 
								distmp = distmp2 / diam;

								criteriatmp = (( K1 * distmp * distmp ) + (K2 * coptmp * distmp))/ (K1 + K2);
								criteriatmp *= CalculateLeafArea(Leaves[j])/diam + area_leaf_i;
								criteriatmp *= Leaves[j].parentLeafCount + Leaves[i].parentLeafCount;
								
								// select the leaf with the lowest criteria
								if (Leaves[i].criteria > criteriatmp) 
								{
									Leaves[i].criteria = criteriatmp;
									Leaves[i].leafCrit = j;
								}
						//	}
						}
					}
				}
			}
			else
			{ 
				nleavesj = int(Leaves[newleaf].parentLeafCount);

//				if ( abs((nleavesi - nleavesj)) < 2)
//				{
					coptmp2 = Leaves[i].Coplanarity(Leaves[newleaf]); 
					coptmp = 1 - coptmp2;

					//distmp2 = HausdorffOptimized( Leaves[i], Leaves[newleaf]); 
					distmp2 = DistanceFromCenters( Leaves[i], Leaves[newleaf]); 
					distmp = distmp2 / diam;
					
					criteriatmp = (( K1 * distmp * distmp ) + (K2 * coptmp * distmp))/ (K1 + K2);
					criteriatmp *= CalculateLeafArea(Leaves[newleaf])/diam + area_leaf_i;
					criteriatmp *= Leaves[newleaf].parentLeafCount + Leaves[i].parentLeafCount;
					
					if (Leaves[i].criteria > criteriatmp) 
					{	
						Leaves[i].criteria = criteriatmp;
						Leaves[i].leafCrit = newleaf;
					}
//				}
			}
		}
	}
}


LeafOctree *TreeSimplifier::GetMinOctreeNodeForLeaf(LeafOctree *start, const Leaf &leaf)
{
	int idv0 = leaf.vertsLeaf[0];
	int idv1 = leaf.vertsLeaf[1];
	int idv2 = leaf.vertsLeaf[2];
	int idv3 = leaf.vertsLeaf[3];

	float * v0 = Vertex[idv0];
	float * v1 = Vertex[idv1];
	float * v2 = Vertex[idv2];
	float * v3 = Vertex[idv3];

	if (start->PointInsideOctree(v0[0],v0[1],v0[2]) &&
		start->PointInsideOctree(v1[0],v1[1],v1[2]) &&
		start->PointInsideOctree(v2[0],v2[1],v2[2]) &&
		start->PointInsideOctree(v3[0],v3[1],v3[2]))
	{
		return start;
	}

	for (int i=0; i<8; i++)
		if (start->children[i])
		{
			LeafOctree *selectedNode = GetMinOctreeNodeForLeaf(start->children[i],leaf);
			if (selectedNode)
				return selectedNode;
		}

	return NULL;
}

float TreeSimplifier::DistanceFromCenters(const Leaf &leaf1, const Leaf &leaf2) const
{
	float onex, oney, onez;
	float twox, twoy, twoz;
	float threex, threey, threez;
	float fourx, foury, fourz;
	float x1, y1, z1;
	float x2, y2, z2;
	float x3, y3, z3;
	float x4, y4, z4;

	onex   = Vertex[leaf1.vertsLeaf[0]][0]; oney   = Vertex[leaf1.vertsLeaf[0]][1]; onez   = Vertex[leaf1.vertsLeaf[0]][2];
	twox   = Vertex[leaf1.vertsLeaf[1]][0]; twoy   = Vertex[leaf1.vertsLeaf[1]][1]; twoz   = Vertex[leaf1.vertsLeaf[1]][2];
	threex = Vertex[leaf1.vertsLeaf[2]][0]; threey = Vertex[leaf1.vertsLeaf[2]][1]; threez = Vertex[leaf1.vertsLeaf[2]][2];
	fourx  = Vertex[leaf1.vertsLeaf[3]][0]; foury  = Vertex[leaf1.vertsLeaf[3]][1]; fourz  = Vertex[leaf1.vertsLeaf[3]][2];

	float center1x = (onex + twox + threex + fourx)*0.25f;
	float center1y = (oney + twoy + threey + foury)*0.25f;
	float center1z = (onez + twoz + threez + fourz)*0.25f;

	x1 = Vertex[leaf2.vertsLeaf[0]][0]; y1 = Vertex[leaf2.vertsLeaf[0]][1]; z1 = Vertex[leaf2.vertsLeaf[0]][2];
	x2 = Vertex[leaf2.vertsLeaf[1]][0]; y2 = Vertex[leaf2.vertsLeaf[1]][1]; z2 = Vertex[leaf2.vertsLeaf[1]][2];
	x3 = Vertex[leaf2.vertsLeaf[2]][0]; y3 = Vertex[leaf2.vertsLeaf[2]][1]; z3 = Vertex[leaf2.vertsLeaf[2]][2];
	x4 = Vertex[leaf2.vertsLeaf[3]][0]; y4 = Vertex[leaf2.vertsLeaf[3]][1]; z4 = Vertex[leaf2.vertsLeaf[3]][2];

	float center2x = (x1 + x2 + x3 + x4)*0.25f;
	float center2y = (y1 + y2 + y3 + y4)*0.25f;
	float center2z = (z1 + z2 + z3 + z4)*0.25f;

	return distan(center1x,center1y,center1z,center2x,center2y,center2z);
}


bool TreeSimplifier::PruneOctree(LeafOctree *octree)
{
	if (octree->HasChildren())
	{
		for (int i=0; i<8; i++)
			if (octree->children[i])
				if (PruneOctree(octree->children[i])) // if child was pruned, nullify it
					octree->children[i]=NULL;			
	}
	else
	{
		if (octree->parent)
		{
			for (std::vector<int>::iterator it=octree->leaves.begin(); it!=octree->leaves.end(); it++)
			{
				int idleaf = *it;
				octree->parent->leaves.push_back(idleaf);
				octree_owning_leaf[idleaf] = octree->parent;
			}
			return true;
		}
	}
	return false;
}

void TreeSimplifier::CrossProduct(const float *v1, const float *v2, float *res) const
{
	res[0] = v1[1]*v2[2] - v1[2]*v2[1];
	res[1] = v1[2]*v2[0] - v1[0]*v2[2];
	res[2] = v1[0]*v2[1] - v1[1]*v2[0];
}

float TreeSimplifier::SquaredModule(const float *v) const
{
	return (v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
}


float TreeSimplifier::CalculateLeafArea(Leaf &leaf) const
{
	int idv0 = leaf.vertsLeaf[0];
	int idv1 = leaf.vertsLeaf[1];
	int idv2 = leaf.vertsLeaf[2];
	int idv3 = leaf.vertsLeaf[3];

	float * v0 = Vertex[idv0];
	float * v1 = Vertex[idv1];
	float * v2 = Vertex[idv2];
	float * v3 = Vertex[idv3];

	float v1v0[3] = {v1[0]-v0[0],v1[1]-v0[1],v1[2]-v0[2]};
	float v2v0[3] = {v2[0]-v0[0],v2[1]-v0[1],v2[2]-v0[2]};
	float v2v1[3] = {v2[0]-v1[0],v2[1]-v1[1],v2[2]-v1[2]};
	float v3v1[3] = {v3[0]-v1[0],v3[1]-v1[1],v3[2]-v1[2]};
	
	float cross1[3], cross2[3];

	CrossProduct(v1v0,v2v0,cross1);
	CrossProduct(v2v1,v3v1,cross2);

	float mod1 = SquaredModule(cross1);
	float mod2 = SquaredModule(cross2);

	return mod1*0.5f + mod2*0.5f;
}