#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 ) {
		if (hittriangle >= mPreprocessor.mFaceParents.size())
			cerr<<"Warning: triangle index out of range! "<<hittriangle<<endl;
		else {
			hitA.mObject = mPreprocessor.mFaceParents[hittriangle].mObject;
			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);

		if (hittriangle != -1 ) {
			if (hittriangle >= mPreprocessor.mFaceParents.size())
				cerr<<"Warning: triangle index out of range! "<<hittriangle<<endl;
			else 
			{
				hitB.mObject = mPreprocessor.mFaceParents[hittriangle].mObject;
				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);
	}

	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);
		}

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

	for (i=0; i < num; i++) 
	{
		Intersection hitA(rays[i].mOrigin), hitB(rays[i].mOrigin);
		
		if (forward_hit_triangles[i] != -1 ) {
			if (forward_hit_triangles[i] >= mPreprocessor.mFaceParents.size())
				cerr<<"Warning: triangle index out of range! "<<forward_hit_triangles[i]<<endl;
			else { 
				hitA.mObject = mPreprocessor.mFaceParents[forward_hit_triangles[i]].mObject;
				// Get the normal of that face
				hitA.mNormal = hitA.mObject->GetNormal(mPreprocessor.mFaceParents[forward_hit_triangles[i]].mFaceIndex);
				//-rays[index+i].mDirection; // $$ temporary
				hitA.mPoint = rays[index+i].Extrap(forward_dist[i]);
			}
		}

		if (castDoubleRay && (backward_hit_triangles[i] != -1)) {
			if (backward_hit_triangles[i] >= mPreprocessor.mFaceParents.size())
				cerr<<"Warning: triangle  index out of range! "<<backward_hit_triangles[i]<<endl;
			else { 
				hitB.mObject = mPreprocessor.mFaceParents[backward_hit_triangles[i]].mObject;
				hitB.mNormal = hitB.mObject->GetNormal(mPreprocessor.mFaceParents[forward_hit_triangles[i]].mFaceIndex);

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

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

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

}

#endif