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

#ifdef GTP_INTERNAL
#include "ArchModeler2MLRT.hxx"

#define DEBUG_RAYCAST 0


namespace GtpVisibilityPreprocessor {


FILE *fileOut = 0;
bool saveRays = false;
bool saveMutationRays = false;
const int saveRaysStart = 3000000;
int cntSavedRaysFLUSH = 0;
unsigned long int cntSavedRays = 0;
const int intSavedLIMIT = 1024;

void
InitSaving()
{
  fileOut = fopen("fileRays.txt", "wb");
  if (!fileOut) {
    cout << "ERROR: file fileRays.txt cannot be opened .. exiting" << endl;
    exit(3);
  }
}
void
FinishSaving()
{
  fclose(fileOut);
}

IntelRayCaster::IntelRayCaster(const Preprocessor &preprocessor, 
			       const string externKdTree):
RayCaster(preprocessor)
{
  if (!InitRayCast(externKdTree))
    cout << "warning: intel ray tracer could not be initialized!" << endl;
  if (saveRays || saveMutationRays)
    InitSaving();
}


IntelRayCaster::~IntelRayCaster()
{
  if (saveRays)
    FinishSaving();
}


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


// Using packet of 4 rays supposing that these are coherent
// We give a box to which each ray is clipped to before the
// ray shooting is computed !

// Using packet of 4 rays supposing that these are coherent
// We give a box to which each ray is clipped to before the
// ray shooting is computed !
void IntelRayCaster::CastRaysPacket4(const Vector3 &minBox,
									 const Vector3 &maxBox,
									 const Vector3 origin4[],
									 const Vector3 dirs4[],
									 int     result4[],
									 float   dist4[])
{
  for (int i = 0; i < 4; i++) {
    mlrtaStoreRayASEye4(&origin4[i].x, &dirs4[i].x, i);
  }
  
  rawCastTimer.Entry();
  mlrtaTraverseGroupASEye4(&minBox.x, &maxBox.x, result4, dist4);
  rawCastTimer.Exit();

  if (saveMutationRays) {
    fprintf(fileOut, "I %4.7f %4.7f %4.7f %4.7f %4.7f %4.7f\n",
			minBox.x, minBox.y, minBox.z, maxBox.x, maxBox.y, maxBox.z);
    for (int i = 0; i < 4; i++) {
      fprintf(fileOut, "%d %4.7f %4.7f %4.7f %4.7f %4.7f %4.7f %d %4.7f\n",
			  cntSavedRays,
			  origin4[i].x,
			  origin4[i].y,
			  origin4[i].z,
			  dirs4[i].x,
			  dirs4[i].y,
			  dirs4[i].z,
			  (result4[i] != -1) ? 1 : 0,
			  (result4[i] != -1) ? dist4[i] : 0);
      cntSavedRays ++;
      cntSavedRaysFLUSH++;
    }
    if (cntSavedRaysFLUSH > intSavedLIMIT) {
      fflush(fileOut);
      cntSavedRaysFLUSH = 0;
    }
  } // if we are saving rays

  return;
}


  
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;
  float dist1, dist2;
  double normal[3];

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

  rawCastTimer.Exit();
  dist1 = 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;

	rawCastTimer.Entry();

	hittriangle = mlrtaIntersectAS(
				   &simpleRay.mOrigin.x,
				   &dir.x,
				   normal,
				   dist);
	
	rawCastTimer.Exit();

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

  if (saveRays && preprocessor->mTotalRaysCast > saveRaysStart) {    
    if (castDoubleRay)
	{
      fprintf(fileOut, "D %d %4.7f %4.7f %4.7f %4.7f %4.7f %4.7f %d %4.7f %d %4.7f\n",
	      cntSavedRays,
	      simpleRay.mOrigin.x,
	      simpleRay.mOrigin.y,
	      simpleRay.mOrigin.z,
	      simpleRay.mDirection.x,
	      simpleRay.mDirection.y,
	      simpleRay.mDirection.z,
	      hitA.mObject ? 1 : 0,
	      hitA.mObject ? dist1 : 0,
	      hitB.mObject ? 1 : 0,
	      hitB.mObject ? dist2 : 0
	      );
	}
    else
      fprintf(fileOut, "S %d %4.7f %4.7f %4.7f %4.7f %4.7f %4.7f %d %4.7f\n",
	      cntSavedRays,
	      simpleRay.mOrigin.x,
	      simpleRay.mOrigin.y,
	      simpleRay.mOrigin.z,
	      simpleRay.mDirection.x,
	      simpleRay.mDirection.y,
	      simpleRay.mDirection.z,
	      hitA.mObject ? 1 : 0,
	      hitA.mObject ? dist1 : 0
	      );
    cntSavedRays++;
    cntSavedRaysFLUSH++;
    if (cntSavedRaysFLUSH > intSavedLIMIT) {
      fflush(fileOut);
      cntSavedRaysFLUSH = 0;
    }
  }      
  
  return ProcessRay(
		    simpleRay,
		    hitA,
		    hitB,
		    vssRays,
		    box,
		    castDoubleRay,
		    pruneInvalidRays
		    );
}
  

void IntelRayCaster::CastRays16(
				SimpleRayContainer &rays,
				VssRayContainer &vssRays,
				const AxisAlignedBox3 &sbox,
				const bool castDoubleRay,
				const bool pruneInvalidRays)
{
  CastRays16(rays, 0, vssRays, sbox, castDoubleRay, pruneInvalidRays);
}
  
void IntelRayCaster::CastRays16(
				SimpleRayContainer &rays,
				const int offset,
				VssRayContainer &vssRays,
				const AxisAlignedBox3 &sbox,
				const bool castDoubleRay,
				const bool pruneInvalidRays)
{  
  int i, k;
  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 (k=offset, i=0; i < num; i++, k++) {
#if DEBUG_RAYCAST
    if (counter == 43964) {
      Debug<<rays[k].mOrigin<<" "<<rays[k].mDirection<<endl;
    }
#endif

    mlrtaStoreRayAS16(&rays[k].mOrigin.x,
		      &rays[k].mDirection.x,
		      i);
  }
  
#if DEBUG_RAYCAST
  Debug<<"TA\n"<<flush;
#endif
  rawCastTimer.Entry();

  mlrtaTraverseGroupAS16(&min.x,
			 &max.x,
			 forward_hit_triangles,
			 forward_dist);
  
  rawCastTimer.Exit();
  
#if DEBUG_RAYCAST
  Debug<<"TAB\n"<<flush;
#endif

  if (castDoubleRay) {
    for (k=offset, i=0; i < num; i++, k++)  {
      Vector3 dir = -rays[k].mDirection;
      mlrtaStoreRayAS16(&rays[k].mOrigin.x,
			&dir.x,
			i);
    }
    
#if DEBUG_RAYCAST
    Debug<<"TB\n"<<flush;
#endif
    rawCastTimer.Entry();
    mlrtaTraverseGroupAS16(&min.x,
			   &max.x,
			   backward_hit_triangles,
			   backward_dist);

	rawCastTimer.Exit();
  }
	
#if DEBUG_RAYCAST
  Debug<<"BBB\n"<<flush;
#endif

  if (saveRays && preprocessor->mTotalRaysCast > saveRaysStart) {
    if (castDoubleRay)
      fprintf(fileOut, "G\n");
    else
      fprintf(fileOut, "H\n");
  }
  
  for (i=0, k=offset; i < num; i++, k++) 
  {
    Intersection hitA(rays[k].mOrigin), hitB(rays[k].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[k].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[k].Extrap(-backward_dist[i]);
      }
    }


    if (saveRays && preprocessor->mTotalRaysCast > saveRaysStart) {
      if (castDoubleRay)     
	fprintf(fileOut, "%d %4.7f %4.7f %4.7f %4.7f %4.7f %4.7f %d %4.7f %d %4.7f\n",
		cntSavedRays,
		rays[k].mOrigin.x,
		rays[k].mOrigin.y,
		rays[k].mOrigin.z,
		rays[k].mDirection.x,
		rays[k].mDirection.y,
		rays[k].mDirection.z,
		hitA.mObject ? 1 : 0,
		hitA.mObject ? forward_dist[i] : 0,
		hitB.mObject ? 1 : 0,
		hitB.mObject ? -backward_dist[i] : 0
		);
      else
	fprintf(fileOut, "%d %4.7f %4.7f %4.7f %4.7f %4.7f %4.7f %d %4.7f\n",
		cntSavedRays,
		rays[k].mOrigin.x,
		rays[k].mOrigin.y,
		rays[k].mOrigin.z,
		rays[k].mDirection.x,
		rays[k].mDirection.y,
		rays[k].mDirection.z,
		hitA.mObject ? 1 : 0,
		hitA.mObject ? forward_dist[i] : 0
		);
      cntSavedRays++;
      cntSavedRaysFLUSH++;
    }

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

#if 1
    ProcessRay(rays[k],
	       hitA,
	       hitB,
	       vssRays,
	       sbox,
	       castDoubleRay,
	       pruneInvalidRays
	       );
#endif
  } // for

  if (saveRays) {
    if (cntSavedRaysFLUSH > intSavedLIMIT) {
      fflush(fileOut);
      cntSavedRaysFLUSH = 0;
    }
  }

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





void
IntelRayCaster::CastSimpleForwardRays(
				      SimpleRayContainer &rays,
				      const AxisAlignedBox3 &sbox
				      )
{
  int hit_triangles[16];
  float dist[16];
  Vector3 normals[16];
  Vector3 min = sbox.Min();
  Vector3 max = sbox.Max();
  
  int packets = (int)rays.size() / 16;
  
  int i, j, k = 0;
  Vector3 dir;
  
  for (i=0; i < packets; i++) {
    for (j=0; j < 16; j++, k++)
      mlrtaStoreRayAS16(&rays[k].mOrigin.x,
			&rays[k].mDirection.x,
			j);
    rawCastTimer.Entry();

    mlrtaTraverseGroupAS16(&min.x,
			   &max.x,
			   hit_triangles,
			   dist);	
	rawCastTimer.Exit();
  } // for


  for (; k < rays.size(); k++) {
	double normal[3];
	rawCastTimer.Entry();
	hit_triangles[0] = mlrtaIntersectAS(
					    &rays[k].mOrigin.x,
					    &rays[k].mDirection.x,
					    normal,
					    dist[0]);
	rawCastTimer.Exit();
  }
  
}


void
IntelRayCaster::CastRays(SimpleRayContainer &rays,
						 VssRayContainer &vssRays,
						 const AxisAlignedBox3 &sbox,
						 bool castDoubleRay,
						 bool pruneInvalidRays)
{

  int buckets = (int)rays.size() / 16;
  int offset = 0;

#if 0
  int time = GetTime();
  CastSimpleForwardRays(rays, sbox);
  cout<<1e-3*2*rays.size()/TimeDiff(time, GetTime())<<" Mrays/sec"<<endl;
#endif
  
  for (int i=0; i < buckets; i++, offset+=16) {
	CastRays16(rays, offset, vssRays, sbox, castDoubleRay, pruneInvalidRays);

	if ((int)rays.size() > 100000 && i % (100000/16) == 0)
	  cout<<"\r"<<offset<<"/"<<(int)rays.size()<<"\r";
  }

  for (; offset < (int)rays.size(); offset++)
  	CastRay(rays[offset], vssRays, sbox, castDoubleRay, pruneInvalidRays);

}

}

#endif