#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 Vector3 &viewPoint,
							const Vector3 &direction,
							const float probability,
							VssRayContainer &vssRays,
							const AxisAlignedBox3 &box,
							const bool castDoubleRay
							)
{
	//cout << "intel ray" << endl;
	VssRay *vssRay  = NULL;
	int hits = 0;
	int hittriangle;
	Vector3 pointA, pointB;
	Vector3 normalA, normalB;
	Intersectable *objectA = NULL, *objectB = NULL;
	float dist;
	double normal[3];

	hittriangle = mlrtaIntersectAS(
		&viewPoint.x,
		&direction.x,
		normal,
		dist);

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

	if (castDoubleRay)
	{
		Vector3 dir = -direction;
		hittriangle = mlrtaIntersectAS(
			&viewPoint.x,
			&dir.x,
			normal,
			dist);

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

	return ProcessRay(
		viewPoint,
		direction,
		objectA, pointA, normalA,
		objectB, pointB, normalB,
		probability,
		vssRays,
		box
		);
}


void IntelRayCaster::CastRays16(
								const int index,
								SimpleRayContainer &rays, 
								VssRayContainer &vssRays,
								const AxisAlignedBox3 &sbox,
								const bool castDoubleRay)
{
	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++) 
	{
		Intersectable *objectA = NULL, *objectB = NULL;
		Vector3 pointA, pointB;
		Vector3 normalA, normalB;

		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 { 
				objectA = mPreprocessor.mFaceParents[forward_hit_triangles[i]].mObject;
				// Get the normal of that face
				normalA = objectA->GetNormal(mPreprocessor.mFaceParents[forward_hit_triangles[i]].mFaceIndex);
				//-rays[index+i].mDirection; // $$ temporary
				pointA = 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 { 
				objectB = mPreprocessor.mFaceParents[backward_hit_triangles[i]].mObject;
				normalB = objectB->GetNormal(mPreprocessor.mFaceParents[forward_hit_triangles[i]].mFaceIndex);

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

		ProcessRay(
			rays[index+i].mOrigin,
			rays[index+i].mDirection,
			objectA, pointA, normalA,
			objectB, pointB, normalB,
			rays[index+i].mPdf,
			vssRays,
			sbox
			);
	}

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

}

#endif