#include "IntelRayCaster.h"
#include "VssRay.h"
#include "Preprocessor.h"
#include "SceneGraph.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,
							const bool keepOrigin
							)
{
	//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)
	{
	  cerr<<"HERE"<<endl;
	  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,
					  keepOrigin
					  );
}


void IntelRayCaster::CastRays16(
								SimpleRayContainer &rays, 
								VssRayContainer &vssRays,
								const AxisAlignedBox3 &sbox,
								const bool castDoubleRay,
								const bool pruneInvalidRays,
								const bool keepOrigin)
{
	int i;
	const int num = 16;
	
#if DEBUG_RAYCAST
	Debug<<"C16 "<<flush;
	static int counter=0;
	Debug<<counter++<<endl;
#endif

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

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

	
	Vector3 min = mPreprocessor.mSceneGraph->GetBox().Min();
	Vector3 max = mPreprocessor.mSceneGraph->GetBox().Max();

	for (i=0; i < num; i++) {
#if DEBUG_RAYCAST
	  if (counter == 43964) {
		Debug<<rays[i].mOrigin<<" "<<rays[i].mDirection<<endl;
	  }
#endif
	  mlrtaStoreRayAS16(&rays[i].mOrigin.x,
						&rays[i].mDirection.x,
						i);
	}

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

	mlrtaTraverseGroupAS16(&min.x,
						   &max.x,
						   forward_hit_triangles,
						   forward_dist);

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

	if (castDoubleRay) {
	  for (i=0; i < num; i++)  {
		Vector3 dir = -rays[i].mDirection;
		mlrtaStoreRayAS16(&rays[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[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[i].mDirection; // $$ temporary
				hitB.mPoint = rays[i].Extrap(-backward_dist[i]);
			}
		}

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

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

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

}

#endif