#include <boxtree.hpp>
#include <boxlist.hpp>
#include <random.hpp>
#include <boxsolid.hpp>

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Logical
	BoxedSolid::TestClass()
{
//ofstream testout("gene.dat",ios::app);     //GY
	Tell("Start BoxedSolid::TestClass()..\n");
	BoxedSolidTree tree;
	BoxedSolidList list;
	Point3D test_point;

	ExtentBox eb;
	BoxedSolid* solids[TEST_CLASS];

	BoxedSolid *list_ptr;
	BoxedSolid *tree_ptr;

	//
	//------------------------------------------------------------
	// Create a bunch of boxes to test, and add them into the tree
	//------------------------------------------------------------
	//
	int i;
	for (i=0; i<ELEMENTS(solids); ++i)
	{

		eb.minX = 70.0f * Random - 40.0f;
		eb.minY = 70.0f * Random - 40.0f;
		eb.minZ = 70.0f * Random - 40.0f;
		eb.maxX = 10.0f * Random + eb.minX;
		eb.maxY = 10.0f * Random + eb.minY;
		eb.maxZ = 10.0f * Random + eb.minZ;

		Scalar min_size = 1.0f;

		if ((eb.maxX - eb.minX) < min_size)	eb.maxX = (eb.minX +min_size);
		if ((eb.maxY - eb.minY) < min_size)	eb.maxY = (eb.minY +min_size);
		if ((eb.maxZ - eb.minZ) < min_size)	eb.maxZ = (eb.minZ +min_size);

		Type c = (Type)Random(SolidTypeCount);

		//WedgeFacingNegativeZAndPositiveXType  ok
		//WedgeFacingNegativeZAndNegativeXType   ok
		//WedgeFacingPositiveZAndNegativeXType   ok
		//WedgeFacingPositiveZAndPositiveXType   ok

		//XAxisCylinderType   ok
		//YAxisCylinderType   ok
		//ZAxisCylinderType   ok

		//RampFacingNegativeZType  ok
		//RampFacingNegativeXType  ok
		//RampFacingPositiveZType  ok
		//RampFacingPositiveXType  ok

		//InvertedRampFacingNegativeXType ok
		//InvertedRampFacingNegativeZType  ok
		//InvertedRampFacingPositiveXType  ok
		//InvertedRampFacingPositiveZType ok

		//ConeType   ok  with  avoiding special case of a=0 in Hit()

		//c = ConeType;


		switch (c)
		{
			case BlockType:
				eb.maxZ = (eb.maxX - eb.minX) + eb.minZ;
				solids[i] = new BoxedSolid(eb, StoneMaterial, NULL, NULL);
				break;

//			case SphereType:											// Not implemented yet
			case ConeType:
				eb.maxZ = (eb.maxX - eb.minX) + eb.minZ;
				solids[i] = new BoxedCone(eb, StoneMaterial, NULL, NULL);
				break;

//			case RampType=4,
			case RampFacingNegativeZType:
				solids[i] = new BoxedRampFacingNegativeZ(eb, StoneMaterial, NULL, NULL);
				break;
			case RampFacingNegativeXType:
				solids[i] = new BoxedRampFacingNegativeX(eb, StoneMaterial, NULL, NULL);
				break;
			case RampFacingPositiveZType:
				solids[i] = new BoxedRampFacingPositiveZ(eb, StoneMaterial, NULL, NULL);
				break;
			case RampFacingPositiveXType:
				solids[i] = new BoxedRampFacingPositiveX(eb, StoneMaterial, NULL, NULL);
				break;
//			case InvertedRampType=8,
			case InvertedRampFacingNegativeZType:
				solids[i] = new BoxedInvertedRampFacingNegativeZ(eb, StoneMaterial, NULL, NULL);
				break;
			case InvertedRampFacingNegativeXType:
				solids[i] = new BoxedInvertedRampFacingNegativeX(eb, StoneMaterial, NULL, NULL);
				break;
			case InvertedRampFacingPositiveZType:
				solids[i] = new BoxedInvertedRampFacingPositiveZ(eb, StoneMaterial, NULL, NULL);
				break;
			case InvertedRampFacingPositiveXType:
				solids[i] = new BoxedInvertedRampFacingPositiveX(eb, StoneMaterial, NULL, NULL);
				break;
//			case WedgeType=12,
			case WedgeFacingNegativeZAndPositiveXType:
				solids[i] = new BoxedWedgeFacingNegativeZAndPositiveX(eb, StoneMaterial, NULL, NULL);
				break;
			case WedgeFacingNegativeZAndNegativeXType:
				solids[i] = new BoxedWedgeFacingNegativeZAndNegativeX(eb, StoneMaterial, NULL, NULL);
				break;
			case WedgeFacingPositiveZAndNegativeXType:
				solids[i] = new BoxedWedgeFacingPositiveZAndNegativeX(eb, StoneMaterial, NULL, NULL);
				break;
			case WedgeFacingPositiveZAndPositiveXType:
				solids[i] = new BoxedWedgeFacingPositiveZAndPositiveX(eb, StoneMaterial, NULL, NULL);
				break;
			case XAxisCylinderType:
				eb.maxZ = (eb.maxY - eb.minY) + eb.minZ;
				solids[i] = new BoxedXAxisCylinder(eb, StoneMaterial, NULL, NULL);
				break;
			case YAxisCylinderType:
				eb.maxZ = (eb.maxX - eb.minX) + eb.minZ;
				solids[i] = new BoxedYAxisCylinder(eb, StoneMaterial, NULL, NULL);
				break;
			case ZAxisCylinderType:
				eb.maxY = (eb.maxX - eb.minX) + eb.minY;
				solids[i] = new BoxedZAxisCylinder(eb, StoneMaterial, NULL, NULL);
				break;
			default:
				--i;
				continue;
		}

		Register_Pointer(solids[i]);
		tree.Add(solids[i], *solids[i]);
		list.Add(solids[i], *solids[i]);
	}

	//
	//---------------------------------------------------------------------
	// Test the centroids of the boxes against the tree and see if they hit
	// where they are supposed to
	//---------------------------------------------------------------------
	//
	for (i=0; i<ELEMENTS(solids); ++i)
	{
		test_point.x = (solids[i]->minX + solids[i]->maxX) * 0.5f;
		test_point.y = (solids[i]->minY + solids[i]->maxY) * 0.5f;
		test_point.z = (solids[i]->minZ + solids[i]->maxZ) * 0.5f;

		tree_ptr = tree.FindBoundingBoxContaining(test_point);
		list_ptr = list.FindBoundingBoxContaining(test_point);
		Test(tree_ptr == list_ptr);
	}

	//
	//---------------------------------------------------
	// Now test a bunch of random points against the tree
	//---------------------------------------------------
	//
	for (i=0; i<TEST_CLASS; ++i)
	{
		test_point.x = 80.0f * Random - 40.0f;
		test_point.y = 80.0f * Random - 40.0f;
		test_point.z = 80.0f * Random - 40.0f;

		tree_ptr = tree.FindBoundingBoxContaining(test_point);
		list_ptr = list.FindBoundingBoxContaining(test_point);

		Test(tree_ptr == list_ptr);
	}

	//
	//-------------------------------------------------------
	// Try finding the boxes contained in random test volumes
	//-------------------------------------------------------
	//
	for (i=0; i<TEST_CLASS; ++i)
	{
		BoundingBoxCollisionList tree_collisions(10);
		BoundingBoxCollisionList list_collisions(10);

		ExtentBox test_volume;
		test_volume.minX = 72.0f * Random - 40.0f;
		test_volume.minY = 78.0f * Random - 40.0f;
		test_volume.minZ = 72.0f * Random - 40.0f;
		test_volume.maxX = test_volume.minX + 8.0f;
		test_volume.maxY = test_volume.minY + 2.0f;
		test_volume.maxZ = test_volume.minZ + 8.0f;

		BoxedYAxisCylinder test_disk(test_volume, SteelMaterial, NULL, NULL);

		tree.FindBoundingBoxesContaining(&test_disk, tree_collisions);
		list.FindBoundingBoxesContaining(&test_disk, list_collisions);

		//
		//--------------------------------------------------------------------
		// If no collisions were found in the list, none should be in the tree
		//--------------------------------------------------------------------
		//
		if (!list_collisions.GetCollisionCount())
		{
			Test(!tree_collisions.GetCollisionCount());
		}

		//
		//-------------------------------------------------------------------
		// If less then 10 collisions were found, then every collision in the
		// tree should be in the list
		//-------------------------------------------------------------------
		//
		else if (list_collisions.GetCollisionCount() < 10)
		{
			Test(tree_collisions.GetCollisionCount());

			for (int c=0; c<tree_collisions.GetCollisionCount(); ++c)
			{
				int c2;
				for (c2=0; c2<list_collisions.GetCollisionCount(); ++c2)
				{
					if (
						tree_collisions[c].treeVolume
						 == list_collisions[c2].treeVolume
					)
					{
						break;
					}
				}
				Test(c2 != list_collisions.GetCollisionCount());
			}
		}

		//
		//----------------------------------------------------------------------
		// Otherwise, anything could happen, so just make sure that the tree was
		// not empty
		//----------------------------------------------------------------------
		//
		else
		{
			Test(tree_collisions.GetCollisionCount());
		}
	}

	//
	//----------------------------------------------------------------------
	// Try finding heights of random test points over any blocks, and verify
	// if the rays should hit or not
	//----------------------------------------------------------------------
	//
	Scalar tree_height;
	Scalar list_height;
	for (i=0; i<TEST_CLASS; ++i)
	{
		test_point.x = 80.0f * Random - 40.0f;
		test_point.y = 20.0f * Random + 40.0f;
		test_point.z = 80.0f * Random - 40.0f;

		tree_ptr = tree.FindBoundingBoxUnder(test_point, &tree_height);
		list_ptr = list.FindBoundingBoxUnder(test_point, &list_height);

		Test(tree_ptr == list_ptr);
		Test(tree_height == list_height);

		Line line;
		line.origin = test_point;
		line.direction.x = 0.0f;
		line.direction.y = -1.0f;
		line.direction.z = 0.0f;
		line.length = 200.0f;

		BoundingBox *ray_ptr;

		ray_ptr = list.FindBoundingBoxHitBy(&line);
		Test(ray_ptr == list_ptr);
		if (ray_ptr)
		{
			Test(Close_Enough(line.length, tree_height,1.0e-2));
		}                                  //a cone apix is the worst case


		line.length = 200.0f;
		ray_ptr = tree.FindBoundingBoxHitBy(&line);
		Test(ray_ptr == tree_ptr);
		if (ray_ptr)
		{
			Test(Close_Enough(line.length, tree_height,1.0e-2));
		}
	}

	//
	//-----------------------------------------------------------------
	// Now test a bunch of random rays with origins outside the solids
	//-----------------------------------------------------------------
	//
	for (i=0; i<TEST_CLASS; ++i)
	{
		test_point.x = 80.0f * Random - 40.0f;
		test_point.y = 80.0f * Random - 40.0f;
		test_point.z = 80.0f * Random - 40.0f;

		tree_ptr = tree.FindBoundingBoxContaining(test_point);
		list_ptr = list.FindBoundingBoxContaining(test_point);
		Test(tree_ptr == list_ptr);

		if (tree_ptr == NULL)
		{
			Scalar
				ray_length = Random * 40.0f;
			Line
				line;
			line.origin = test_point;
			line.direction.x = Random;
			line.direction.y = Random;
			line.direction.z = Random;

			
			Scalar temp = Sqrt(line.direction.x * line.direction.x +
									 line.direction.y * line.direction.y +
									 line.direction.z * line.direction.z);
			if (!Close_Enough(temp, 0.0f, SMALL))
			{
				line.direction.x /= temp;
				line.direction.y /= temp;
				line.direction.z /= temp;

				temp = Sqrt(line.direction.x * line.direction.x +
							line.direction.y * line.direction.y +
							line.direction.z * line.direction.z);
				Test( Close_Enough(temp, 1.0f, SMALL));
				line.length = ray_length;
				list_ptr = list.FindBoundingBoxHitBy(&line);
				Scalar list_length = line.length;

				line.length = ray_length;
				tree_ptr = tree.FindBoundingBoxHitBy(&line);
				Scalar tree_length = line.length;

				Test(list_ptr == tree_ptr);
				if (list_ptr)
				{
					Test(Close_Enough(list_length, tree_length, 1.0e-2));
				}
			}
			else
			{
				--i;
				continue;
			}
		}
		else
		{
			--i;
			continue;
		}
	}


	for (i=0; i<ELEMENTS(solids); ++i)
	{
		Unregister_Pointer(solids[i]);
		delete solids[i];
	}

//testout<<".......\n";  //GY
//testout.close();
//cerr<<"End \n";
	return True;
}