#include "munga.h"
#pragma hdrstop

#include "gaugmap.h"
#include "point3d.h"
#include "entity.h"

// #define LOCAL_TEST

#if defined(LOCAL_TEST)
#	define Test_Tell(n) std::cout << n
#else
#	define Test_Tell(n)
#endif

// 'FULL_EXTENT' is a value representing the total extent
// of the world in meters.
#define FULL_EXTENT 32767.0
#define HALF_EXTENT (FULL_EXTENT/2.0)

// 'LARGEST_RADIUS' defines a radius encompassing the largest existing entity
#define LARGEST_RADIUS 500.0		// HACK - this is a guess


//#############################################################################
// approximateDistance
// ...accurate to about 13%
// Graphics Gems, p.432, "A Fast Approximation To 3D Euclidean Distance"
//#############################################################################
Scalar
	approximateDistance(Point3D *point1, Point3D *point2)
{
	Check_Pointer(point1);
	Check_Pointer(point2);

	Scalar
		max, mid, min, temp;
	//-----------------------------------------------
	// Get the three components in arbitrary order
	//-----------------------------------------------
	max = abs(point2->x - point1->x);
	mid = abs(point2->y - point1->y);
	min = abs(point2->z - point1->z);
	//-----------------------------------------------
	// Sort only the largest value to the top.
	// We don't need to completely sort the values,
	// as we weight the middle and smallest equally. 
	//-----------------------------------------------
	if (max < mid)
	{
		temp=max; max=mid; mid=temp;	// swap max, mid
	}

	if (max < min)
	{
		temp=max; max=min; min=temp;	// swap max, min
	}

	//-----------------------------------------------
	// return largest + 1/4 middle + 1/4 smallest
	//-----------------------------------------------
	Check_Fpu();
	return max + ((mid+min)*.25);
}


//#############################################################################
// GaugeEntityList
//#############################################################################

//============================================================================
// Creator
//============================================================================
GaugeEntityList::GaugeEntityList() :
	Node(GaugeEntityList::GaugeEntityListClassID),
	instanceList(this)
{
	Check_Fpu();
}

//============================================================================
// Destructor
//============================================================================
GaugeEntityList::~GaugeEntityList()
{
	Check(this);
	Clear();		// is this necessary, or are nodes automatically deleted?
	Check_Fpu();
}

//============================================================================
// TestInstance
//============================================================================
Logical
	GaugeEntityList::TestInstance() const
{
	return Node::TestInstance();
}

//============================================================================
// Add
//============================================================================
void
	GaugeEntityList::Add(Entity *entity)
{
	Check(this);
	Check(entity);
	Check(&instanceList);
	instanceList.Add(entity);
	Check_Fpu();
}

//============================================================================
// Remove
//============================================================================
void
	GaugeEntityList::Remove(Entity *search_entity)
{
	Check(this);

	Test_Tell(
		"GaugeEntityList::Remove(" << search_entity <<
		")\n"
	);
	Check(&instanceList);
	ChainIteratorOf<Entity*>
		i(instanceList);

	Entity
		*entity;

	//-----------------------------------------------------
	// Search the list for the given entity
	//-----------------------------------------------------
	while((entity=i.GetCurrent()) != NULL)
	{
		Check(entity);

		//-----------------------------------------------------
		// If we find it, remove it and exit
		//-----------------------------------------------------
		if (entity == search_entity)
		{
			Test_Tell("Found!\n");
			i.Remove();
			break;
		}
		else
		{
			i.Next();
		}
	}
	Check_Fpu();
}

//============================================================================
// NumberOfItems
//============================================================================
int
	GaugeEntityList::NumberOfItems()
{
	Check(this);

	ChainIteratorOf<Entity*>
		i(instanceList);

	Check_Fpu();
	return i.GetSize();
}

//============================================================================
// Clear
//============================================================================
void
	GaugeEntityList::Clear()
{
//	Test_Tell("GaugeEntityList::Clear()\n");
	Check(this);

	ChainIteratorOf<Entity*>
		i(instanceList);

	//-----------------------------------------------------
	// Remove all objects from the list
	//-----------------------------------------------------
	while(i.GetCurrent() != NULL)
	{
		Check(i.GetCurrent());
		i.Remove();
	}
	Check_Fpu();
}

//============================================================================
//	CopyEntities
//
// This method will create a reference in the destination list to
// all entities in the list
//============================================================================
int
	GaugeEntityList::CopyEntities(
		GaugeEntityList *destination,
		Derivation *derivation_type
	)
{
	Test_Tell("GaugeEntityList::CopyEntities(" << destination << ")\n");
	Check(this);
	Check(destination);

	ChainIteratorOf<Entity*>
		i(instanceList);
	Entity
		*entity;
	int
		count(0);

	//-----------------------------------------------------
	// Return all items in the list
	//-----------------------------------------------------
	while ((entity=i.ReadAndNext()) != NULL)
	{
		Check(entity);
		//-----------------------------------------------------
		// Discard if wrong type
		//-----------------------------------------------------
		if (derivation_type != NULL)
		{
			if (! entity->IsDerivedFrom(*derivation_type))
			{
				continue;
			}
		}
		//-----------------------------------------------------
		// Add to the new list
		//-----------------------------------------------------
		destination->Add(entity);
		++count;
	}
	Test_Tell("Found " << count << " items.\n");
	Check_Fpu();
	return count;
}

//============================================================================
//	CopyEntitiesWithinBounds
//
// This method will create a reference in the destination list to
// all entities in the list that are within the given bounds (if supplied).
//============================================================================
int
	GaugeEntityList::CopyEntitiesWithinBounds(
		GaugeEntityList *destination,
		Scalar min_x,
		Scalar /*min_y*/,
		Scalar min_z,
		Scalar max_x,
		Scalar /*max_y*/,
		Scalar max_z,
		Derivation *derivation_type
	)
{
	Test_Tell(
		"GaugeEntityList::CopyEntitiesWithinBounds(" << destination <<
		", " << min_x <<
		"... , " << min_z <<
		", " << max_x <<
		"... , " << max_z <<
		")\n"
	);
	Check(this);
	Check(destination);

	ChainIteratorOf<Entity*>
		i(instanceList);

	Entity
		*entity;

	Point3D
		pos;

	int
		count(0);

	//-----------------------------------------------------
	// Check all items in the list
	//-----------------------------------------------------
	while ((entity=i.ReadAndNext()) != NULL)
	{
		Check(entity);
		//-----------------------------------------------------
		// Discard if wrong type
		//-----------------------------------------------------
		if (derivation_type != NULL)
		{
			if (! entity->IsDerivedFrom(*derivation_type))
			{
				continue;
			}
		}
		//-----------------------------------------------------
		// Get the item's position and radius
		//-----------------------------------------------------
		pos = entity->localOrigin.linearPosition;

		//-----------------------------------------------------
		// Add to the new list only if inside the bounds
		//-----------------------------------------------------
		Test_Tell(
			"Testing (" << pos.x <<
			", " << pos.y <<
			", " << pos.z <<
			")\n"
		);
		//if (pos.z >= min_z)
		//{
		//	if (pos.z <= max_z)
		//	{
		//		if (pos.x >= min_x)
		//		{
		//			if (pos.x <= max_x)
					{
						Test_Tell("Accepted\n");
						destination->Add(entity);
						++count;
					}
		//		}
		//	}
		//}
	}
	Test_Tell("Found " << count << " items.\n");
	Check_Fpu();
	return count;
}

//#############################################################################
// GaugeEntityArray
//#############################################################################

//============================================================================
// Creator
//============================================================================
GaugeEntityArray::GaugeEntityArray()
{
	minimumX= 0.0;
	minimumY= 0.0;
	minimumZ= 0.0;
	maximumX= 0.0;
	maximumY= 0.0;
	maximumZ= 0.0;
	Check_Fpu();
}

//============================================================================
// Destructor
//============================================================================
GaugeEntityArray::~GaugeEntityArray()
{
	Check(this);
	Clear();
	Check_Fpu();
}

//============================================================================
//	TestInstance
//============================================================================
Logical
	GaugeEntityArray::TestInstance() const
{
	return True;
}

//============================================================================
// Add
//============================================================================
void
	GaugeEntityArray::Add(Entity *entity)
{
	Test_Tell(
		"GaugeEntityArray::Add(" << entity <<
		")\n"
	);
	Check(this);
	Check(entity);

	//-----------------------------------------------------
	// Add to the proper grid list
	//-----------------------------------------------------
	Point3D
		pos(entity->localOrigin.linearPosition);

	grid[ArrayOffset(pos.x)][ArrayOffset(pos.z)].Add(entity);
	//-----------------------------------------------------
	// Keep track of the total extents
	//-----------------------------------------------------
	if (pos.x < minimumX)
	{
		minimumX = pos.x;
	}
	if (pos.y < minimumY)
	{
		minimumY = pos.y;
	}
	if (pos.z < minimumZ)
	{
		minimumZ = pos.z;
	}
	if (pos.x > maximumX)
	{
		maximumX = pos.x;
	}
	if (pos.y > maximumY)
	{
		maximumY = pos.y;
	}
	if (pos.z > maximumZ)
	{
		maximumZ = pos.z;
	}
	Check_Fpu();
}

//============================================================================
// Remove
//============================================================================
void
	GaugeEntityArray::Remove(Entity *entity)
{
	Test_Tell(
		"GaugeEntityArray::Remove(" << entity <<
		")\n"
	);
	Check(this);
	Check(entity);

	Point3D
		pos(entity->localOrigin.linearPosition);

	//-----------------------------------------------------
	// Remove from the proper grid list
	//-----------------------------------------------------
	grid[ArrayOffset(pos.x)][ArrayOffset(pos.z)].Remove(entity);
	Check_Fpu();
}

//============================================================================
// Clear
//============================================================================
void
	GaugeEntityArray::Clear()
{
	Check(this);

	int
		x,
		z;

	//-----------------------------------------------------
	// Clear all grid lists
	//-----------------------------------------------------
	for(z=0; z<gaugeMapArraySize; ++z)
	{
		for(x=0; x<gaugeMapArraySize; ++x)
		{
			grid[x][z].Clear();
		}
	}
	//-----------------------------------------------------
	// Set the bounds to zero
	//-----------------------------------------------------
	minimumX= 0.0;
	minimumY= 0.0;
	minimumZ= 0.0;
	maximumX= 0.0;
	maximumY= 0.0;
	maximumZ= 0.0;
	Check_Fpu();
}

//============================================================================
// PrintStatistics
//============================================================================
void
	GaugeEntityArray::PrintStatistics()
{
	Check(this);

	int
		x,
		z,
		n,
		total(0);

	//-----------------------------------------------------
	// Count all grid lists
	//-----------------------------------------------------
	for(z=0; z<gaugeMapArraySize; ++z)
	{
		if (z < 10)
		{
			std::cout << " ";
		}
		std::cout << z << ":";
		for(x=0; x<gaugeMapArraySize; ++x)
		{
			n = grid[x][z].NumberOfItems();
			total += n;
			if (n < 100)
			{
				std::cout << " ";
				if (n < 10)
				{
					std::cout << " ";
				}
			}
			std::cout << n << " ";
		}
		std::cout << "\n";
	}
	std::cout << "Total= " << total << "\n";
	Check_Fpu();
}

//============================================================================
//	CopyEntities
//
// This method will create a reference in the destination list to
// all GaugeImages in the array.
//============================================================================
int
	GaugeEntityArray::CopyEntities(
		GaugeEntityList *destination,
		Derivation *derivation_type
	)
{
	Test_Tell(
		"GaugeEntityArray::CopyEntities(" << destination << ")\n");
	Check(this);
	Check(destination);

	int
		x,
		low_x,
		high_x,
		z,
		high_z,
		count(0);

	//-----------------------------------------------------
	// Generate the z search limits
	//-----------------------------------------------------
	z = 0;
	high_z = gaugeMapArraySize - 1;
	//-----------------------------------------------------
	// Generate the x search limits
	//-----------------------------------------------------
	low_x  = 0;
	high_x = gaugeMapArraySize - 1;

	//-------------------------------------------------------
	// Search the grid for items
	//-------------------------------------------------------
	for( ; z<high_z; ++z)
	{
		for(x=low_x; x<high_x; ++x)
		{
			Test_Tell("Searching [" << x << "][" << z << "]\n");
			count += grid[x][z].CopyEntities(destination, derivation_type);
		}
	}
	Test_Tell("Search done, total found = " << count << " items.\n");
	Check_Fpu();
	return count;
}

//============================================================================
//	CopyEntitiesWithinBounds
//
// This method will create a reference in the destination list to
// all GaugeImages in the array that are within the given bounds.
//============================================================================
int
	GaugeEntityArray::CopyEntitiesWithinBounds(
		GaugeEntityList *destination,
		Scalar min_x,
		Scalar min_y,
		Scalar min_z,
		Scalar max_x,
		Scalar max_y,
		Scalar max_z,
		Derivation *derivation_type
	)
{
	Test_Tell(
		"GaugeEntityArray::CopyEntitiesWithinBounds(" << destination <<
		", " << min_x <<
		", " << min_y <<
		", " << min_z <<
		", " << max_x <<
		", " << max_y <<
		", " << max_z <<
		")\n"
	);
	Check(this);
	Check(destination);

	int
		x,
		low_x,
		high_x,
		z,
		high_z,
		count(0);

	//-----------------------------------------------------
	// Generate the z search limits
	//-----------------------------------------------------
	z      = ArrayOffset(min_z - LARGEST_RADIUS);
	high_z = ArrayOffset(max_z + LARGEST_RADIUS);

	Test_Tell(
		"Initial z =" << z <<
		", high_z=" << high_z <<
		"\n"
	);
	if (z < 0)
	{
		z = 0;
	}
	else if (z >= gaugeMapArraySize)
	{
		z = gaugeMapArraySize - 1;
	}
	if (high_z < 0)
	{
		high_z = 0;
	}
	else if (high_z >= gaugeMapArraySize)
	{
		high_z = gaugeMapArraySize - 1;
	}

	//-----------------------------------------------------
	// Generate the x search limits
	//-----------------------------------------------------
	low_x  = ArrayOffset(min_x - LARGEST_RADIUS);
	high_x = ArrayOffset(max_x + LARGEST_RADIUS);

	if (low_x < 0)
	{
		low_x = 0;
	}
	else if (low_x >= gaugeMapArraySize)
	{
		low_x = gaugeMapArraySize - 1;
	}
	if (high_x < 0)
	{
		high_x = 0;
	}
	else if (high_x >= gaugeMapArraySize)
	{
		high_x = gaugeMapArraySize - 1;
	}

	Test_Tell("Search x=(" << low_x << "..." << high_x << ")\n");
	Test_Tell("Search z=(" << z     << "..." << high_z << ")\n");
	//-------------------------------------------------------
	// Search the (limited) grid for items within the bounds
	//-------------------------------------------------------
	for( ; z<high_z; ++z)
	{
		for(x=low_x; x<high_x; ++x)
		{
			Test_Tell("Searching [" << x << "][" << z << "]\n");
			count += grid[x][z].CopyEntitiesWithinBounds(
				destination,
				min_x,
				min_y,
				min_z,
				max_x,
				max_y,
				max_z,
				derivation_type
			);
		}
	}
	Test_Tell("Search done, total found = " << count << " items.\n");
	Check_Fpu();
	return count;
}

//============================================================================
// GetBounds
//============================================================================
void
	GaugeEntityArray::GetBounds(
		Scalar *min_x,
		Scalar *min_y,
		Scalar *min_z,
		Scalar *max_x,
		Scalar *max_y,
		Scalar *max_z
	)
{
	*min_x = minimumX;
	*min_y = minimumY;
	*min_z = minimumZ;
	*max_x = maximumX;
	*max_y = maximumY;
	*max_z = maximumZ;
	Check_Fpu();
}

//============================================================================
// ArrayOffset - generate offset into array based on position value
//============================================================================
int
	GaugeEntityArray::ArrayOffset(Scalar n)
{
	int	q;

	q = (int)(gaugeMapArraySize*(n/HALF_EXTENT)+.5)+(gaugeMapArraySize>>1);

	if (q < 0)
	{
		q = 0;
	}
	if (q >= gaugeMapArraySize)
	{
		q = (gaugeMapArraySize-1);
	}

	Check_Fpu();
   return q;
}