#include "IntelRayCaster.h"
#include "VssRay.h"
#include "Preprocessor.h"

#ifdef GTP_INTERNAL
#include "ArchModeler2MLRT.hxx"

#define DEBUG_RAYCAST 0 


namespace GtpVisibilityPreprocessor {


IntelRayCaster::IntelRayCaster(const Preprocessor &preprocessor, const string externKdTree):
RayCaster(preprocessor)
{
	InitRayCast(externKdTree);
}


IntelRayCaster::~IntelRayCaster()
{
}


bool IntelRayCaster::InitRayCast(const string externKdTree)
{
	cout<<"Ray Cast file: " << externKdTree << endl;
	return mlrtaLoadAS(externKdTree.c_str());
}


int IntelRayCaster::CastRay(
							const SimpleRay &simpleRay,
							VssRayContainer &vssRays,
							const AxisAlignedBox3 &box,
							const bool castDoubleRay,
							const bool pruneInvalidRays
							)
{
	//cout << "intel ray" << endl;
	VssRay *vssRay  = NULL;
	int hits = 0;
	int hittriangle;
	Intersection hitA(simpleRay.mOrigin), hitB(simpleRay.mOrigin);

	float dist;
	double normal[3];

	hittriangle = mlrtaIntersectAS(
		&simpleRay.mOrigin.x,
		&simpleRay.mDirection.x,
		normal,
		dist);

	if (hittriangle != -1 ) {
		Intersectable *intersect = mPreprocessor.GetParentObject(hittriangle);
		
		if (intersect)
		{
			hitA.mObject = intersect;
			hitA.mNormal = Vector3(normal[0], normal[1], normal[2]);
			// Get the normal of that face
			//		Mesh *mesh = ((MeshInstance *)objectA)->GetMesh();
			//		normalA = mesh->GetFacePlane(mFaceParents[forward_hit_triangles[i]].mFaceIndex).mNormal;
			//-rays[index+i].mDirection; // $$ temporary
			hitA.mPoint = simpleRay.Extrap(dist);
		}
	}

	if (castDoubleRay)
	{
		Vector3 dir = -simpleRay.mDirection;
		hittriangle = mlrtaIntersectAS(
			&simpleRay.mOrigin.x,
			&dir.x,
			normal,
			dist);

		Intersectable *intersect = mPreprocessor.GetParentObject(hittriangle);
		
		if (intersect)
		{
			hitB.mObject = intersect;
			hitB.mNormal = Vector3(normal[0], normal[1], normal[2]);
			// Get the normal of that face
			//		Mesh *mesh = ((MeshInstance *)objectB)->GetMesh();
			//		normalA = mesh->GetFacePlane(mFaceParents[forward_hit_triangles[i]].mFaceIndex).mNormal;
			//-rays[index+i].mDirection; // $$ temporary
			hitB.mPoint = simpleRay.Extrap(dist);
		}
	}

	return ProcessRay(
					  simpleRay,
					  hitA,
					  hitB,
					  vssRays,
					  box,
					  castDoubleRay,
					  pruneInvalidRays
					  );
}


void IntelRayCaster::CastRays16(
								const int index,
								SimpleRayContainer &rays, 
								VssRayContainer &vssRays,
								const AxisAlignedBox3 &sbox,
								const bool castDoubleRay,
								const bool pruneInvalidRays)
{
	int i;
	const int num = 16;

#if DEBUG_RAYCAST
	Debug<<"C16 "<<flush;
#endif

	int forward_hit_triangles[16];
	float forward_dist[16];

	int backward_hit_triangles[16];
	float backward_dist[16];


	Vector3 min = sbox.Min();
	Vector3 max = sbox.Max();

	for (i=0; i < num; i++) {
	  mlrtaStoreRayAS16(&rays[index + i].mOrigin.x,
						&rays[index + i].mDirection.x,
						i);
	}

#if DEBUG_RAYCAST
		Debug<<"TA\n"<<flush;
#endif

	mlrtaTraverseGroupAS16(&min.x,
						   &max.x,
						   forward_hit_triangles,
						   forward_dist);
	
	if (castDoubleRay)
	  {
		for (i=0; i < num; i++) 
		  {
			Vector3 dir = -rays[index + i].mDirection;
			mlrtaStoreRayAS16(&rays[index+i].mOrigin.x,
							  &dir.x,
							  i);
		  }

#if DEBUG_RAYCAST
		Debug<<"TB\n"<<flush;
#endif

		mlrtaTraverseGroupAS16(&min.x,
							   &max.x,
							   backward_hit_triangles,
							   backward_dist);
	  }

#if DEBUG_RAYCAST
	Debug<<"BBB\n"<<flush;
#endif

	for (i=0; i < num; i++) 
	{
		Intersection hitA(rays[i].mOrigin), hitB(rays[i].mOrigin);

#if DEBUG_RAYCAST
		Debug<<"FH\n"<<flush;
#endif
		Intersectable *intersect = mPreprocessor.GetParentObject(forward_hit_triangles[i]);

		if (intersect)
		{
			hitA.mObject = intersect;
			// Get the normal of that face
			hitA.mNormal = mPreprocessor.GetParentNormal(forward_hit_triangles[i]);

			//-rays[index+i].mDirection; // $$ temporary
			hitA.mPoint = rays[index+i].Extrap(forward_dist[i]);
		}
	
#if DEBUG_RAYCAST
		Debug<<"BH\n"<<flush;
#endif

		if (castDoubleRay) 
		{
			Intersectable *intersect = mPreprocessor.GetParentObject(backward_hit_triangles[i]);

			if (intersect)
			{ 
				hitB.mObject = intersect;
				hitB.mNormal = mPreprocessor.GetParentNormal(backward_hit_triangles[i]);

				// normalB = rays[index+i].mDirection; // $$ temporary
				hitB.mPoint = rays[index+i].Extrap(-backward_dist[i]);
			}
		}

#if DEBUG_RAYCAST
		Debug<<"PR\n"<<flush;
#endif

		ProcessRay(rays[index + i],
				   hitA,
				   hitB,
				   vssRays,
				   sbox,
				   castDoubleRay,
				   pruneInvalidRays
				   );
	}

#if DEBUG_RAYCAST
	Debug<<"C16F\n"<<flush;
#endif
}

}

#endif