/*
 * Description  : Class to hold point data
 * Author       : Krystof Valenta <mail: valenkry@fel.cvut.cz> 19.5.2011
 *
 * TO DO		: Triangulate method
 *				  Computation of representant points for SPT - average color / normal
 *				  ?? Outer octree hierarchy level - view frustum culling ??
 *
 * Purpose      : GeometryPoints stores points in sequential point trees ( SPTs )
 */

#include "geometrypoints.h"
#include <wx/stopwatch.h>
#include <queue>

using namespace VRUT;
using namespace std;

#define TEST_MODE 0

///Builds sequential point tree from point cloud
///\param[in] inputPoints Points to build SPT from
///\param[in] maxDepth Max SPT depth ( number of LODs )
///\param[in] changeMainAABB True if AABB of whole Geometry is to be changed
///\param[in] discardStrategy If false all points are store, otherwise only one point is stored in Octree leaves and other points are discarded
///\return \c pointer to created SPT
SequentialPointTree *
GeometryPoints::BuildSPT( const std::vector<Point> &inputPoints,
						  unsigned int maxDepth, bool changeMainAABB, bool discardStrategy)
{
  SequentialPointTree * newSPT = new SequentialPointTree();
  

#if TEST_MODE
  for (size_t i=0; i < inputPoints.size(); i++)
	newSPT->points.push_back(inputPoints[i]);

  newSPT->levels = maxDepth + 1;
  newSPT->levelPointCounts = new unsigned int[maxDepth + 1];
  for( unsigned int l = 0; l < newSPT->levels; l++ ) {
	newSPT->levelPointCounts[ l ] = 0;
  }
  
  return newSPT;
#endif

  if( inputPoints.size() == 0 )
	{
	  LOGERROR(wxT("<GeometryPoints>Attemt to build SPT from 0 points"));
	  return NULL;
	}
  
  // Stopwatch - hierarchy build time
  wxStopWatch sw;
  
  // Build AABB of whole point cloud
  newSPT->SPTaabb = buildAABBfromPoints( inputPoints, changeMainAABB );
  
  // Root
  OctreeNode root;
  root.children[0] = NULL;
  root.parent = NULL;
  root.nodeDepth = 0;
  root.aabb = newSPT->SPTaabb;
  OctreeNode * currentNode = &root;
  
  newSPT->levels = maxDepth + 1;
  newSPT->levelPointCounts = new unsigned int[maxDepth + 1];
  for( unsigned int l = 0; l < newSPT->levels; l++ ) {
	newSPT->levelPointCounts[ l ] = 0;
  }
  
  bool done;
  
  // Insert points - store points only in leaf nodes
  for( unsigned int p = 0; p < inputPoints.size(); p++)
	{
	  done = false;
	  currentNode = &root;
	  
	  // Insert point to hierarchy
	  while(!done)
		{
		  // Leaf node
		  if(currentNode->children[0] == NULL)
			{
			  // Empty leaf node
			  if(currentNode->points.size() == 0)
				{
				  currentNode->points.push_back( (Point*)(&inputPoints[p]) );
				  done = true;
				}
			  
			  // Splittable leaf node with one point
			  else if( currentNode->nodeDepth < maxDepth )
				{
				  splitNode( currentNode );
				  
				  // Move point from node to one of his children
				  pickChild( currentNode, currentNode->points.back() )->points.push_back( currentNode->points.back() );
				  currentNode->points.pop_back();
				  
				  // Traverse to leaf, where the point we are trying to push lies
				  currentNode = pickChild( currentNode, (Point*)(&inputPoints[p]) );
				}
			  
			  // Nonsplittable leaf node with point(s)
			  else
				{
				  if(discardStrategy)
					{
					  done = true;
					}
				  else
					{
					  currentNode->points.push_back( (Point*)(&inputPoints[p]) );
					  done = true;
					}
				}				
			}
		  
		  // Node has children
		  else
			{
			  // Traverse to child, in which AABB point lies
			  currentNode = pickChild( currentNode, (Point*)(&inputPoints[p]) );
			}
		}
	}
  
  // Compute representant points - move some points from leaf nodes to inner nodes
  done = false;
  currentNode = &root;
  
  if(root.children != NULL)
	{
	  while(!done)
		{				
		  for( int i = 0; i < 8; i++ )
			{
			  if( currentNode->children[i]->points.size() == 0 &&
				  currentNode->children[i]->children[0] != NULL )
				{
				  currentNode = currentNode->children[i];
				  break;
				}
			  else if( i == 7 )
				{							
				  computeRepresentant( currentNode );
				  if( currentNode != &root ) currentNode = currentNode->parent;
				  else done = true;
				}
			}			
		}
	}
  
  // Push points sorted by levels
  std::queue<OctreeNode*> nodeQueue; // Queue for nodes
  std::vector<Point> pointSequence;  // final points
  nodeQueue.push( &root );		   // push root
  
  while( !nodeQueue.empty() )
	{
	  // Push points
	  for( unsigned int p = 0; p < nodeQueue.front()->points.size(); p++ )
		{
		  pointSequence.push_back( *(nodeQueue.front()->points[ p ]) );
		}
	  
	  // Increase level point count
	  newSPT->levelPointCounts[ nodeQueue.front()->nodeDepth ] += nodeQueue.front()->points.size();
	  
	  // Push children to queue
	  if( nodeQueue.front()->children[0] != NULL )
		{
		  for( unsigned int c = 0; c < 8; c++ )
			{
			  nodeQueue.push( nodeQueue.front()->children[ c ] );
			}
		}
	  
	  // Free and pop processed node
	  if( nodeQueue.front()->nodeDepth != 0 ) delete nodeQueue.front();		
	  nodeQueue.pop();
	}
  
  // Correct level point counts
  if( newSPT->levels > 1 )
	{
	  for( unsigned int i = 1; i < newSPT->levels; i++ )
		{			
		  newSPT->levelPointCounts[ i ] += newSPT->levelPointCounts[ i - 1 ];			
		}
	}
  
  // Assign points sorted by levels to newSPT
  newSPT->points = pointSequence;
  
  // SPT finished
  LOG(wxT("<GeometryPoints>SPT created"));
  for( unsigned int i = 0; i < newSPT->levels ; i++)
	{
	  LOG(wxString::Format(wxT("<GeometryPoints>SPT level%i size= %i"),i , newSPT->levelPointCounts[i]) );
	}
  
  // Log build time
  LOG(wxString::Format(wxT("<GeometryPoints>Hierarchy built in %.3f secs"), 0.001f * sw.Time()) );
  
  return newSPT;
}

///Rebuils SPT - used, when some points were added
///\param[in] toRebuild Pointer to SPT which is to rebuild
///\param[in] maxDepth New max SPT depth ( number of LODs )
///\param[in] changeMainAABB If true, AABB of whole geometry is changed
///\param[in] discardStrategy If false all points are store, otherwise only one point is stored in Octree leaves and other points are discarded
void GeometryPoints::rebuildSPT( SequentialPointTree * toRebuild, unsigned int maxDepth, bool changeMainAABB, bool discardStrategy)
{
	if( toRebuild->points.size() == 0 )
	{
		LOGERROR(wxT("<GeometryPoints>Attemt to build SPT from 0 points"));
		return;
	}
	else
	{
		//LOG(wxT("<GeometryPoints>SPT rebuild begin"));
	}

	// Stopwatch - hierarchy build time
	wxStopWatch sw;		
	
	// Build AABB of whole point cloud
	toRebuild->SPTaabb = buildAABBfromPoints( toRebuild->points, changeMainAABB );

	// Root
	OctreeNode root;
	root.children[0] = NULL;
	root.parent = NULL;
	root.nodeDepth = 0;
	root.aabb = toRebuild->SPTaabb;
	OctreeNode * currentNode = &root;
		
	toRebuild->levels = maxDepth + 1;
	delete [] toRebuild->levelPointCounts;
	toRebuild->levelPointCounts = new unsigned int[maxDepth + 1];
	for( unsigned int l = 0; l < toRebuild->levels; l++ )
	{
		toRebuild->levelPointCounts[ l ] = 0;
	}

	bool done;
	
	// Insert points - store points only in leaf nodes
	for( unsigned int p = 0; p < toRebuild->points.size(); p++)
	{
		done = false;
		currentNode = &root;

		// Insert point to hierarchy
		while(!done)
		{
			// Leaf node
			if(currentNode->children[0] == NULL)
			{
				// Empty leaf node
				if(currentNode->points.size() == 0)
				{
					currentNode->points.push_back( (Point*)(&toRebuild->points[p]) );
					done = true;
				}

				// Splittable leaf node with one point
				else if( currentNode->nodeDepth < maxDepth )
				{
					splitNode( currentNode );

					// Move point from node to one of his children
					pickChild( currentNode, currentNode->points.back() )->points.push_back( currentNode->points.back() );
					currentNode->points.pop_back();

					// Traverse to leaf, where the point we are trying to push lies
					currentNode = pickChild( currentNode, (Point*)(&toRebuild->points[p]) );
				}

				// Nonsplittable leaf node with point(s)
				else
				{
					if(discardStrategy) done = true;
					else
					{
						currentNode->points.push_back( (Point*)(&toRebuild->points[p]) );
						done = true;
					}
				}				
			}

			// Node has children
			else
			{
				// Traverse to child, in which AABB point lies
				currentNode = pickChild( currentNode, (Point*)(&toRebuild->points[p]) );
			}
		}
	}

	// Compute representant points - move some points from leaf nodes to inner nodes
	done = false;
	currentNode = &root;

	if(root.children != NULL)
	{
		while(!done)
		{				
			for( int i = 0; i < 8; i++ )
			{
				if( currentNode->children[i]->points.size() == 0 &&
					currentNode->children[i]->children[0] != NULL )
				{
					currentNode = currentNode->children[i];
					break;
				}
				else if( i == 7 )
				{							
					computeRepresentant( currentNode );
					if( currentNode != &root ) currentNode = currentNode->parent;
					else done = true;
				}
			}			
		}
	}
	
	// Push points sorted by levels
	std::queue<OctreeNode*> nodeQueue; // Queue for nodes
	std::vector<Point> pointSequence;  // final points
	nodeQueue.push( &root );		   // push root

	while( !nodeQueue.empty() )
	{
		// Push points
		for( unsigned int p = 0; p < nodeQueue.front()->points.size(); p++ )
		{
			pointSequence.push_back( *(nodeQueue.front()->points[ p ]) );
		}

		// Increase level point count
		toRebuild->levelPointCounts[ nodeQueue.front()->nodeDepth ] += nodeQueue.front()->points.size();

		// Push children to queue
		if( nodeQueue.front()->children[0] != NULL )
		{
			for( unsigned int c = 0; c < 8; c++ )
			{
				nodeQueue.push( nodeQueue.front()->children[ c ] );
			}
		}

		// Free and pop processed node
		if( nodeQueue.front()->nodeDepth != 0 ) delete nodeQueue.front();
		nodeQueue.pop();
	}

	// Correct level point counts
	if( toRebuild->levels > 1 )
	{
		for( unsigned int i = 1; i < toRebuild->levels; i++ )
		{			
			toRebuild->levelPointCounts[ i ] += toRebuild->levelPointCounts[ i - 1 ];			
		}
	}

	// Assign points sorted by levels to newSPT
	toRebuild->points = pointSequence;

	// SPT finished
	/*LOG(wxT("<GeometryPoints>SPT created"));
	for( unsigned int i = 0; i < toRebuild->levels ; i++)
	{
		LOG(wxString::Format(wxT("<GeometryPoints>SPT level%i size= %i"),i , toRebuild->levelPointCounts[i]) );
	}*/

	// Log rebuild time
	LOG(wxString::Format(wxT("<GeometryPoints>Hierarchy rebuilt in %.3f secs"), 0.001f * sw.Time()) );
}

///Replaces active SPT with new SPT and pushes, old SPT to SPThistory - used by CameraToPoints
///\param[in] inputPoints Points to build SPT from
///\param[in] maxDepth Max SPT depth ( number of LODs )
///\param[in] discardStrategy If false all points are store, otherwise only one point is stored in Octree leaves and other points are discarded
void GeometryPoints::PushNewSPT( const std::vector<Point> &inputPoints, unsigned int maxDepth, bool discardStrategy)
{
	// If there is no history, simply replace current SPT
	if( historySize == 0 )
	{
		// Empty SPTS = first SPT
		if( SPTS.size() == 0 )
		{
			// Build new SPT	
			SPTS.push_back( BuildSPT( inputPoints, maxDepth, true, discardStrategy ) );
		}
		else
		{
			// History has been shortened
			if( SPTS.size() > 1 )
			{
				// Delete obsolete SPTS
				for( unsigned int del = SPTS.size() - 1; del > 0; del-- )
				{
					SequentialPointTree * toDelete = SPTS[ del ];
					delete toDelete;

					SPTS.pop_back();
				}
			}

			SequentialPointTree * toDelete = SPTS[0];
			delete toDelete;

			// Build new SPT	
			SPTS[0] = BuildSPT( inputPoints, maxDepth, true, discardStrategy );
		}
	}
	else
	{
		// Empty SPTS
		if( SPTS.size() == 0 )
		{
			//PUSH NEW SPT
		}
		// History is not full
		else if ( SPTS.size() < historySize + 1 ) // SPTS contain current SPT + history = history + 1
		{
			// Duplicate last SPT
			SPTS.push_back( SPTS[ SPTS.size() - 1 ] );

			// Shift history
			for( unsigned int shiftAt = SPTS.size() - 2; shiftAt > 0; shiftAt--)
			{
				SPTS[ shiftAt ] = SPTS[ shiftAt - 1 ];
			}
		}
		// History has been shortened
		else if ( SPTS.size() > historySize + 1 )
		{
			// Delete obsolete SPTS
			for( unsigned int del = SPTS.size() - 1; del > historySize - 1; del-- )
			{
				SequentialPointTree * toDelete = SPTS[ del ];
				delete toDelete;

				SPTS.pop_back();
			}

			// Shift history
			for( unsigned int shiftAt = historySize; shiftAt > 0; shiftAt--)
			{
				SPTS[ shiftAt ] = SPTS[ shiftAt - 1 ];
			}
		}
		// History is full
		else
		{
			// Delete too old SPT
			SequentialPointTree * toDelete = SPTS[ historySize ];
			delete toDelete;

			// Shift history
			for( unsigned int shiftAt = historySize; shiftAt > 0; shiftAt--)
			{
				SPTS[ shiftAt ] = SPTS[ shiftAt - 1 ];
			}
		}

		// Build new SPT	
		if( frameCounter > 1 ) SPTS[0] = BuildSPT( inputPoints, maxDepth, true, discardStrategy );
		else SPTS.push_back( BuildSPT( inputPoints, maxDepth, true, discardStrategy ) );
	}
}

///Pushes points to current SPT, deletes points with pointOrigin < deleteOlderThan and rebuilds SPT
///\param[in] inputPoints Points to insert to current SPT
///\param[in] maxDepth If GeometryPoints does not contain any SPT, maxDepth will be used as parameter for new SPT creation
///\param[in] deleteOlderThan Points with pointOrigin < than deleteolderThan will be deleted
///\param[in] forceSPTdepth If true SPT will be rebuilt with maxDepth depth insead of SPTs current depth
void GeometryPoints::PushPoints( const std::vector<Point> &inputPoints,
								 unsigned int maxDepth,
								 unsigned int deleteOlderThan,
								 bool forceSPTdepth,
								 bool discardStrategy )
{
	// Empty SPTS = first SPT
	if( SPTS.size() == 0 )
	{
		// Build new SPT	
		SPTS.push_back( BuildSPT( inputPoints, maxDepth, true, discardStrategy ) );
	}
	else
	{
#if TEST_MODE
	  // just clear the the point array and push new points in the tree
	  SPTS[0]->points.clear();
	  for (size_t i=0; i < inputPoints.size(); i++)
		SPTS[0]->points.push_back(inputPoints[i]);
	  return;
#else
	  // Delete old points
		std::vector<Point> newPoints;
		for( unsigned long p = 0; p < SPTS[0]->points.size(); p++)
		{
			if( SPTS[0]->points[p].pointOrigin >= deleteOlderThan )
			{
				newPoints.push_back( SPTS[0]->points[p] );
			}
		}

		// Push new points
		for( unsigned long p = 0; p < inputPoints.size(); p++)
		{			
			newPoints.push_back( inputPoints[p] );		
		}

		// Assign new points
		SPTS[0]->points = newPoints;
		
		// Rebuild SPT
		if( forceSPTdepth ) rebuildSPT( SPTS[0], maxDepth, true, discardStrategy );
		else rebuildSPT( SPTS[0], SPTS[0]->levels - 1, true, discardStrategy );
#endif
	}
}

///Deletes points with pointOrigin < deleteOlderThan
///\param[in] deleteOlderThan Delete threshold
void GeometryPoints::DeletePoints( unsigned int deleteOlderThan )
{
	if( SPTS.size() > 0 )
	{
		// Delete old points
		std::vector<Point> newPoints;
		for( unsigned long p = 0; p < SPTS[0]->points.size(); p++)
		{
			if( SPTS[0]->points[p].pointOrigin >= deleteOlderThan )
			{
				newPoints.push_back( SPTS[0]->points[p] );
			}
		}

		// Assign new points
		SPTS[0]->points = newPoints;

		// Rebuild SPT
		rebuildSPT( SPTS[0], SPTS[0]->levels - 1, true, false );
	}
}

///Returns total number of points including history
long GeometryPoints::GetTotalPoints() const
{
	long count = 0;

	for( unsigned int i = 0; i < SPTS.size(); i++ )
	{
		count += SPTS[i]->points.size();
	}

	return count;
}

///Build AABB from points
///\param[in] inputPoints Points over which to build AABB
///\param[in] changeMainAABB If true, AABB of whole geometry is changed
AABB GeometryPoints::buildAABBfromPoints( const std::vector<Point> &inputPoints, bool changeMainAABB)
{
	AABB newAABB;

	if ( inputPoints.size() > 0 )
	{
		std::vector<Point>::const_iterator v = inputPoints.begin();

		newAABB.MinBound = newAABB.MaxBound = v->position;
		Vector3 * minB = &newAABB.MinBound;
		Vector3 * maxB = &newAABB.MaxBound;
		for (; v != inputPoints.end(); ++v)
		{
			if (v->position.x < minB->x)
				minB->x = v->position.x;
			else if (v->position.x > maxB->x)
				maxB->x = v->position.x;
			if (v->position.y < minB->y)
				minB->y = v->position.y;
			else if (v->position.y > maxB->y)
				maxB->y = v->position.y;
			if (v->position.z < minB->z)
				minB->z = v->position.z;
			else if (v->position.z > maxB->z)
				maxB->z = v->position.z;
		}
		///Fix zero volume AABB
		if (fabs(minB->x - maxB->x) < EPSILON)
		{
			minB->x -= EPSILON;
			maxB->x += EPSILON;
		}
		if (fabs(minB->y - maxB->y) < EPSILON)
		{
			minB->y -= EPSILON;
			maxB->y += EPSILON;
		}
		if (fabs(minB->z - maxB->z) < EPSILON)
		{
			minB->z -= EPSILON;
			maxB->z += EPSILON;
		}
	}
	else
	{
		newAABB.MinBound = newAABB.MaxBound = Vector3(0,0,0);		
	}

	if(changeMainAABB) aabb = newAABB;

	return newAABB;
}

///Splits node, creating its 8 children
///\param[in] n Node to split
void GeometryPoints::splitNode( OctreeNode * n)
{
	Vector3 center(n->aabb.Center());
	// Right up front
	n->children[0] = new OctreeNode();
	n->children[0]->parent = n;
	n->children[0]->nodeDepth = n->nodeDepth + 1;
	n->children[0]->children[0] = NULL;
	n->children[0]->aabb.MinBound = center;
	n->children[0]->aabb.MaxBound = n->aabb.MaxBound;
	
	// Right up back
	n->children[1] = new OctreeNode();
	n->children[1]->parent = n;
	n->children[1]->nodeDepth = n->nodeDepth + 1;
	n->children[1]->children[0] = NULL;
	n->children[1]->aabb.MinBound = Vector3(center.x, center.y, n->aabb.MinBound.z);
	n->children[1]->aabb.MaxBound = Vector3(n->aabb.MaxBound.x, n->aabb.MaxBound.y, center.z);

	// Right down front
	n->children[2] = new OctreeNode();
	n->children[2]->parent = n;
	n->children[2]->nodeDepth = n->nodeDepth + 1;
	n->children[2]->children[0] = NULL;
	n->children[2]->aabb.MinBound = Vector3(center.x, n->aabb.MinBound.y, center.z);
	n->children[2]->aabb.MaxBound = Vector3(n->aabb.MaxBound.x, center.y, n->aabb.MaxBound.z);

	// Right down back
	n->children[3] = new OctreeNode();
	n->children[3]->parent = n;
	n->children[3]->nodeDepth = n->nodeDepth + 1;
	n->children[3]->children[0] = NULL;
	n->children[3]->aabb.MinBound = Vector3(center.x, n->aabb.MinBound.y, n->aabb.MinBound.z);
	n->children[3]->aabb.MaxBound = Vector3(n->aabb.MaxBound.x, center.y, center.z);

	// Left up front
	n->children[4] = new OctreeNode();
	n->children[4]->parent = n;
	n->children[4]->nodeDepth = n->nodeDepth + 1;
	n->children[4]->children[0] = NULL;
	n->children[4]->aabb.MinBound = Vector3(n->aabb.MinBound.x, center.y, center.z);
	n->children[4]->aabb.MaxBound = Vector3(center.x, n->aabb.MaxBound.y, n->aabb.MaxBound.z);

	// Left up back
	n->children[5] = new OctreeNode();
	n->children[5]->parent = n;
	n->children[5]->nodeDepth = n->nodeDepth + 1;
	n->children[5]->children[0] = NULL;
	n->children[5]->aabb.MinBound = Vector3(n->aabb.MinBound.x, center.y, n->aabb.MinBound.z);
	n->children[5]->aabb.MaxBound = Vector3(center.x, n->aabb.MaxBound.y, center.z);

	// Left down front
	n->children[6] = new OctreeNode();
	n->children[6]->parent = n;
	n->children[6]->nodeDepth = n->nodeDepth + 1;
	n->children[6]->children[0] = NULL;
	n->children[6]->aabb.MinBound = Vector3(n->aabb.MinBound.x, n->aabb.MinBound.y, center.z);
	n->children[6]->aabb.MaxBound = Vector3(center.x, center.y, n->aabb.MaxBound.z);
	
	// Left down back
	n->children[7] = new OctreeNode();
	n->children[7]->parent = n;
	n->children[7]->nodeDepth = n->nodeDepth + 1;
	n->children[7]->children[0] = NULL;
	n->children[7]->aabb.MinBound = n->aabb.MinBound;
	n->children[7]->aabb.MaxBound = ( n->aabb.MinBound + n->aabb.MaxBound ) * 0.5f;
}

///Gets child of input octreeNode, where is point p
///\param[in] n Parent OctreeNode
///\param[in] p Point to pick
///\return \c Pointer to child of node 'n', where point 'p' lies
GeometryPoints::OctreeNode * GeometryPoints::pickChild( OctreeNode * n, Point * p)
{
	Vector3 center = n->aabb.Center();

	int pointer = 0;

	if( p->position.x < center.x ) pointer += 4;
	if( p->position.y < center.y ) pointer += 2;
	if( p->position.z < center.z ) pointer += 1;

	return n->children[pointer];
}

///Computes representant point for node n - only from its direct children!
///\param[in] n Node for which to compute representant point
void GeometryPoints::computeRepresentant( OctreeNode * n)
{
	//TODO: select point closest to average color
	bool done = false;
	unsigned int child = 0;

	while( !done )
	{
		if( n->children[ child ]->points.size() > 0)
		{
			n->points.push_back( n->children[ child ]->points.back() );
			n->children[ child ]->points.pop_back();
			done = true;
		}
		else if( child < 7 )
		{
			child++;
		}
		else
		{
			LOGERROR(wxT("<GeometryPoints>Node has children, and none of them has any point"));
		}
	}
}
