#define USE_THREADS 0

#ifdef UNICODE
#undef UNICODE
#endif

#define NOMINMAX
#ifdef __WINDOWS__
#include <windows.h>
#ifdef _CRT_SET
#include <crtdbg.h>
#endif // _CRT_SET
#endif
#include <cstdio>

#include "Camera.h"
#include "PreprocessorFactory.h"
#include "Parser.h"
#include "Environment.h"
#include "MeshKdTree.h"
#include "Preprocessor.h"
#include "common.h"
#include "PreprocessorThread.h"
#include "ObjExporter.h"
#include "SceneGraph.h"
#include "GlobalLinesRenderer.h"
#include "RayCaster.h"
#include "Triangle3.h"
#include "IntersectableWrapper.h"
#include "timer.h"
#include "raypack.h"

#include "ViewCellsManager.h"

#ifdef USE_QT  
	#include "QtPreprocessorThread.h"
	#include "QtGlViewer.h"
	#include "QtGlRenderer.h"
#else
	#if USE_THREADS
#include "BoostPreprocessorThread.h"
	#endif
#endif

#include "ResourceManager.h"
#include "GlRenderer.h"


#define USE_EXE_PATH false


using namespace GtpVisibilityPreprocessor;

//Preprocessor *preprocessor = NULL;
extern GlRendererWidget *rendererWidget;
//GlobalLinesRenderer *globalLinesRenderer = NULL;

// DLL function signature
typedef GlRendererWidget *(*importFunction)(Preprocessor *);


// Flag if the ray should be single sided (one direction)
// or double sided (shooting ray in both directions)
static bool castDoubleRays;

extern void Cleanup();

void
_SortRays2(SimpleRayContainer &rays,
	   const int l,
	   const int r,
	   const int depth,
	   float box[12])
{
  // pick-up a pivot
  int axis;
  
  float maxDiff = -1.0f;
  // get the largest axis
  int offset = 0;
  int i;

  //const int batchsize = 16384;
  //const int batchsize = 8192;
  //const int batchsize = 128;
  const int batchsize = 4;

  //if (r - l < 16*batchsize)
  //	offset = 3;
	
  //  if (depth%2==0)
  //	offset = 3;
  
  for (i=offset; i < offset + 6; i++) {
    float diff = box[i + 6] - box[i];
    assert(diff >= 0.f);
    if (diff > maxDiff) {
      // Find the maximum
      maxDiff = diff;
      axis = i;
    }
  }

  //  cout<<depth<<" "<<axis<<" "<<l<<" "<<r<<endl;
  
  i=l;
  int j=r;

  float x = (box[axis] + box[axis+6])*0.5f;
  //  float x = rays[(l+r)/2].GetParam(axis);
  do {
	while(i<j && rays[i].GetParam(axis) < x)
	  i++;
	while(i<j && x < rays[j].GetParam(axis))
	  j--;
	
	if (i <= j) {
	  swap(rays[i], rays[j]);
	  i++;
	  j--;
	}
  } while (i<=j);

  
  if (l + batchsize < j ) {
	// set new max
	float save = box[axis+6];
	box[axis+6] = x;
	_SortRays2(rays, l, j, depth+1, box);
	box[axis+6] = save;
  } else {
	//	for (int k=0; k < 6; k++)
	//	  cout<<k<<" "<<box[k]<<" - "<<box[k+6]<<endl;
  }
  
  if (i + batchsize < r) {
    // set new min
    box[axis] = x;
    _SortRays2(rays, i, r, depth+1, box);
  } else {
	//	for (int k=0; k < 6; k++)
	//	  cout<<k<<" "<<box[k]<<" - "<<box[k+6]<<endl;
  }	
}

void
SortRays2(SimpleRayContainer &rays, const AxisAlignedBox3 &box)
{
  const float sizeBox = Magnitude(box.Diagonal());
  // This is some size of the 
  const float sizeDir = 0.1f * sizeBox;
  
  float b[12]={
    box.Min().x,
    box.Min().y,
    box.Min().z,
    -sizeDir,
    -sizeDir,
    -sizeDir,
    box.Max().x,
    box.Max().y,
    box.Max().z,
    sizeDir,
    sizeDir,
    sizeDir
  };

  
  _SortRays2(rays, 0, (int)rays.size()-1, 0, b);

  return;
}					


static string GetInternFilename(const string &filename, const string newSuffix)
{
	vector<string> filenames;
	const int files = SplitFilenames(filename, filenames);

	vector<string>::const_iterator sit, sit_end = filenames.end();
	string kdFilename;

	int i = 0;
	for (sit = filenames.begin(); sit != sit_end; ++ sit, ++ i)
	{
		string currentFile = *sit;
		string strippedFilename;

		if (i == 0)
		{	
			strippedFilename = currentFile;
		}
		else
		{
			char *str = StripPath(currentFile.c_str());
			strippedFilename = string(str);

			delete [] str;
		}
		
		string suffix("_");

		if (i == (int)filenames.size() - 1)
		{
			suffix = newSuffix;
		}

		if (strstr(strippedFilename.c_str(), ".x3d")) 
		{
			kdFilename += ReplaceSuffix(strippedFilename, ".x3d", suffix);
		}
        else if (strstr(strippedFilename.c_str(), ".dat"))
		{
			kdFilename += ReplaceSuffix(strippedFilename, ".dat", suffix);
		} 
		else if (strstr(strippedFilename.c_str(), ".obj"))
		{
			kdFilename += ReplaceSuffix(strippedFilename, ".obj", suffix);
		}
		else
		{
			cerr << "Error: Currently unsupported format for kd, filename " << currentFile << endl;
		}
	}

	//cout << "kdfilename: " << kdFilename << endl;
	return kdFilename;
}


// -------------------------------------------------------
// Written by Vlastimil Havran

// This is for testing RT implementation
void
TestRTcamera(int argc, char **argv)
{
  int returnCode = 0;

  InitTiming();
  Debug.open("debug.log");

  Environment::GetSingleton()->Parse(argc, argv, USE_EXE_PATH);
  MeshKdTree::ParseEnvironment();

  char buff[128];
  Environment::GetSingleton()->GetStringValue("Preprocessor.type", buff);
  string preprocessorType(buff);

  if (!(preprocessor = PreprocessorFactory::CreatePreprocessor(preprocessorType)))
  {
    Environment::DelSingleton();
    cerr << "Unknown preprocessor type" << endl;
    exit(1);
  }


  Environment::GetSingleton()->GetStringValue("Scene.filename", buff);
  string filename(buff);

  const string externKdTree = ReplaceSuffix(filename, ".obj", ".kdf");
  const string internKdTree = GetInternFilename(filename, preprocessor->mLoadMeshes ?
						".kdm" : ".kdt");

  if (preprocessor->InitRayCast(externKdTree, internKdTree))
  {
    cout << "ray casting initialized!" << endl;
  }
  else
  {
    cout << "ray casting initialization failed!" << endl;
    Cleanup();
    exit(1);
  }
  
  //Debug << "using pvs type " << PVS_TYPE << endl;

  /////////////
  //-- load scene 
  
  if (!preprocessor->LoadScene(filename))
  {
    cout << "loading file " << filename << " failed" << endl;
    Cleanup();
    exit(1);
  }
	
  ////////////
  //-- initialize external ray caster
  
  if (preprocessor->LoadInternKdTree(internKdTree))
  {
    cout << "intern kd tree loaded!" << endl;
  }
  else
  {
    cout << "loading intern kd tree failed!" << endl;
    Cleanup();
    exit(1);
  }
  
  // export objects as obj
  if (preprocessor->mExportObj)
  {
    if (strstr(filename.c_str(), ".obj"))
      {
	cerr << "already in obj format" << endl;
	if (0) 	preprocessor->ExportObj("test.obj", preprocessor->mObjects);
      }
    else
      {
	
	const string objname = GetInternFilename(filename, ".obj");
	
	cout << "exporting scene to " << objname << endl;
	bool success = preprocessor->ExportObj(objname, preprocessor->mObjects);
	
	
	if (success)
	  cout << "finished exporting obj" << endl;
	else
	  cerr << "error exporting " << objname << endl;
      }
  }

  int width = 1000;
  int height = 500;
  float fieldOfView = 115.f;
  Camera cam(width, height, fieldOfView);
  
  AxisAlignedBox3 bboxOrig = preprocessor->mSceneGraph->GetBox();
  AxisAlignedBox3 bbox = bboxOrig;
  float sizeDiag = Magnitude(bbox.Diagonal());
  bbox.Enlarge(sizeDiag * 1.5f);

  Vector3 origin, dir;

#define WIEN1
//#define WIEN2
//#define ARENA1
    
#ifdef WIEN1
  // 1099.9 183.0 -387
  origin = Vector3(1099.9f, 183.0f, -387.0f);
  dir = Vector3(-0.6f, 0.0001f, -0.8f);
  //dir = -dir;
#define DIREXISTS
#endif  
#ifdef WIEN2
  // 935.6 215.8 -1012.3
  origin = Vector3(935.6, 215.8, -1012.4);
  dir = Vector3(0.01f, 0.01f, 1.0f);
#define DIREXISTS
#endif
#ifdef ARENA1
  origin = Vector3(22.16, 19.63, -950.44);
  dir = Vector3(0.9, 0.001f, -0.5f);
#define DIREXISTS
#endif  

  cam.SetPosition(origin);

#ifndef DIREXISTS
  Vector3 center = bbox.Center();
  dir = center - origin;
#endif
  
  dir.Normalize();
  cam.SetDirection(dir);
  cout << "Computing image\n" << endl;

  int id = preprocessor->mObjects.size() + 1;
    
  //We add here a few objects
  ObjectContainer dynObjects;
  Vector3 baseVec = origin + dir * 2.0f;
  Triangle3 tr1(baseVec, baseVec + Vector3(1, 0, 0),
		baseVec + Vector3(0, 1, 0));	       
  TriangleIntersectable ti1(tr1);
  dynObjects.push_back(&ti1);
  ti1.mId = id+1;
  
  tr1.Init(baseVec, baseVec + Vector3(1, 0, 0),
	   baseVec + Vector3(0, 0, 1));	       
  TriangleIntersectable ti2(tr1);
  dynObjects.push_back(&ti2);
  ti2.mId = id+2;

  tr1.Init(baseVec, baseVec + Vector3(0, 1, 0),
	   baseVec + Vector3(0, 0, 1));	       
  TriangleIntersectable ti3(tr1);
  dynObjects.push_back(&ti3);
  ti3.mId = id+3;

#if 1
  // This is required for cam.SnapImagePacket2
  // otherwise dynamic object cannot be identified!
  preprocessor->mObjects.push_back(&ti1);
  preprocessor->mObjects.push_back(&ti2);
  preprocessor->mObjects.push_back(&ti3);
#endif

  if (0) {
    Matrix4x4 mat;
    mat = IdentityMatrix();
    mat = mat * TranslationMatrix(Vector3(-1, 0, 0));
    preprocessor->mRayCaster->AddDynamicObjecs(dynObjects, mat);
  
#if 1
    // ray by ray
    cam.SnapImage("test-rays.tga",
		  preprocessor->mRayCaster,
		  bboxOrig,
		  preprocessor->mSceneGraph);
#endif
#if 1
    // using ray packets
    cam.SnapImage2("test-oneDir.tga",
		   preprocessor->mRayCaster,
		   bboxOrig,
		   preprocessor->mSceneGraph);
#endif
    
#if 1
#ifdef _USE_HAVRAN_SSE
    // using ray packets
    cam.SnapImagePacket("test-packet.tga",
			preprocessor->mRayCaster,
			bboxOrig,
			preprocessor->mSceneGraph);
#endif
#endif    
#if 1
    // using ray packets
    cam.SnapImagePacket2("test-packet4.tga",
			 preprocessor->mRayCaster,
			 bboxOrig,
			 preprocessor->mSceneGraph);
#endif
  }
  else {
    cout << "Computing animation" << endl;    
    for (int i = 0; i < 20; i++) {
      preprocessor->mRayCaster->DeleteDynamicObjects();
      Matrix4x4 mat;
      mat = IdentityMatrix();
      mat = mat * TranslationMatrix(Vector3(-0.1*(float)i, 0, 0));
      preprocessor->mRayCaster->AddDynamicObjecs(dynObjects, mat);
      char name[200];      
#if 1
    // ray by ray
      sprintf(name, "test-rays-%03d.tga", i);
      cam.SnapImage(name,
		    preprocessor->mRayCaster,
		    bboxOrig,
		    preprocessor->mSceneGraph);
#endif
#if 1
      // using ray packets
      sprintf(name, "test-oneDir-%03d.tga", i);
      cam.SnapImage2(name,
		     preprocessor->mRayCaster,
		     bboxOrig,
		     preprocessor->mSceneGraph);
#endif

#if 1
#ifdef _USE_HAVRAN_SSE  
      // using ray packets
      sprintf(name, "test-packet-%03d.tga", i);
      cam.SnapImagePacket(name,
			  preprocessor->mRayCaster,
			  bboxOrig,
			  preprocessor->mSceneGraph);
#endif
#endif
#if 1
      // using ray packets
      sprintf(name, "test-packet4-%03d.tga", i);
      cam.SnapImagePacket2(name,
			   preprocessor->mRayCaster,
			   bboxOrig,
			   preprocessor->mSceneGraph);
#endif
    } // for
  } // animation

  
  cout << "Done\n" << endl;
  return;
}

struct RESult {
  int hitA;
  float hitAT;
  int hitB;
  float hitBT;
  RESult(int hA, float tA, int hB, float tB):
    hitA(hA), hitAT(tA), hitB(hB), hitBT(tB) { }
};


// This is for testing RT implementation
void
TestRTfromFile(int argc, char **argv)
{
  int returnCode = 0;

  InitTiming();
  Debug.open("debug.log");

  Environment::GetSingleton()->Parse(argc, argv, USE_EXE_PATH);
  MeshKdTree::ParseEnvironment();

  char buff[128];
  Environment::GetSingleton()->GetStringValue("Preprocessor.type", buff);
  string preprocessorType(buff);

  if (!(preprocessor = PreprocessorFactory::CreatePreprocessor(preprocessorType)))
  {
    Environment::DelSingleton();
    cerr << "Unknown preprocessor type" << endl;
    exit(1);
  }


  Environment::GetSingleton()->GetStringValue("Scene.filename", buff);
  string filename(buff);

  const string externKdTree = ReplaceSuffix(filename, ".obj", ".kdf");
  const string internKdTree = GetInternFilename(filename, preprocessor->mLoadMeshes ?
						".kdm" : ".kdt");

  if (preprocessor->InitRayCast(externKdTree, internKdTree))
  {
    cout << "ray casting initialized!" << endl;
  }
  else
  {
    cout << "ray casting initialization failed!" << endl;
    Cleanup();
    exit(1);
  }
  
  //Debug << "using pvs type " << PVS_TYPE << endl;

  /////////////
  //-- load scene 
  
  if (!preprocessor->LoadScene(filename))
  {
    cout << "loading file " << filename << " failed" << endl;
    Cleanup();
    exit(1);
  }
	
  ////////////
  //-- initialize external ray caster
  
  if (preprocessor->LoadInternKdTree(internKdTree))
  {
    cout << "intern kd tree loaded!" << endl;
  }
  else
  {
    cout << "loading intern kd tree failed!" << endl;
    Cleanup();
    exit(1);
  }
  
  // export objects as obj
  if (preprocessor->mExportObj)
  {
    if (strstr(filename.c_str(), ".obj"))
    {
      cerr << "already in obj format" << endl;
      if (0) 	preprocessor->ExportObj("test.obj", preprocessor->mObjects);
    }
    else
    {
      const string objname = GetInternFilename(filename, ".obj");
	
      cout << "exporting scene to " << objname << endl;
      bool success = preprocessor->ExportObj(objname, preprocessor->mObjects);	
	
      if (success)
	cout << "finished exporting obj" << endl;
      else
	cerr << "error exporting " << objname << endl;
    }
  }

  Environment::GetSingleton()->GetStringValue("Rays.file", buff);
  FILE *fp = fopen(buff, "rb");
  if (!fp) {
    cerr << "ERROR: file " << buff << " cannot be opened for reading" << endl;
    cerr << "EXITING" << endl;
    exit(3);
  }
  cout << "File " << buff << " was opened successfully" << endl;
  int cntMaxRays = 100000;
  Environment::GetSingleton()->GetIntValue("Rays.cnt", cntMaxRays);
  SimpleRayContainer rays;
  SimpleRay rayTest;
  vector<RESult> results;
  
  int cntRays = 0;
  for (int i = 0; cntRays < cntMaxRays; i++) {
    int ch = fgetc(fp);
    switch (ch) {
    case 'G': { // two-sided rays of 16
      for (int j = 0; j < 16; j++) {
	int order;
	float ox, oy, oz, dx, dy, dz;
	int hitA; float hitAT;
	int hitB; float hitBT;	
	if (fscanf(fp, "%d %f %f %f %f %f %f %d %f %d %f\n",
		   &order, &ox, &oy, &oz, &dx, &dy, &dz, &hitA, &hitAT, &hitB, &hitBT) != 11) {
	  cerr << "Problem parsing the ray file" << endl;
	  cerr << "EXITING for ray order" << order << endl;
	  goto FINISH;
	}
	Vector3 mPosition(ox, oy, oz);
	Vector3 mDirection(dx, dy, dz);
	rayTest.Set(mPosition, mDirection, 0, 1.0, true);
	rays.push_back(rayTest);
	results.push_back(RESult(hitA, hitAT, hitB, hitBT));
	cntRays++;
      }
      break;
    }
    case 'H': // one-sided rays of 16
      cerr << "Not yet implemented " << endl; abort();
      
    case 'D': // two-sided ray
      cerr << "Not yet implemented " << endl; abort();
      
    case 'S': // one-sided ray
      cerr << "Not yet implemented " << endl; abort();
    } // switch
  } // for i

FINISH:
  fclose(fp);

  Environment::GetSingleton()->GetBoolValue("TestDoubleRays", castDoubleRays);

  double mult = 1.0;
  if (castDoubleRays)
    mult = 2.0;

  cout << "Press a key to start ray shooting" << endl;
  getchar();
  cout << "Ray shooting " << cntRays << " rays started - "
       << (castDoubleRays ? " double " : " single ")
       << " dir " << endl;
  
  AxisAlignedBox3 bboxOrig = preprocessor->mSceneGraph->GetBox();
  
  cout << "Sorting rays " << endl;

  CTimer timer;
  timer.Reset();
  timer.Start();
  long t1 = GetTime();

  //SortRays2(rays, bboxOrig);

  timer.Stop();
  long t2 = GetTime();
  cout<<"\n#SORTING_TIME = ";
  cout << TimeDiff(t1, t2)<<" [mikrosec]"
       << " userTime = " << timer.UserTime() << " realTime = "
       << timer.RealTime() << endl;

  cout << "Starting to shoot " << cntRays * mult << " rays" << endl;
  timer.Start();
  
  RayCaster *raycaster = preprocessor->mRayCaster;
  VssRayContainer vssRays;

//#define DEBUGRESULTS

#ifdef DEBUGRESULTS
  int cntIntelYesWeNo = 0;
  int cntIntelNoWeYes = 0;
  int cntIntelOurDistMismatch = 0;
  bool printOut = false;
  double sumDistIntel = 0.f;
  double sumDistOurAlg = 0.f;
  double sumDistAbsDiff = 0.f;
#endif

  SimpleRayContainer raysToTest;
  for (int i = 0; i < cntRays - 16; i++) {

#if 0
    int res = raycaster->CastRay(rays[i],
				 vssRays,
				 bboxOrig,
				 castDoubleRays, // castDoubleRay,
				 false); // pruneInvalidRays
#else
    raysToTest.erase(raysToTest.begin(), raysToTest.end());
    for (int j = 0; j < 16; j++, i++) {
      raysToTest.push_back(rays[i]);
    }
#if 0
    for (int j = 0; j < 16; j++) {
      cout << "orig = " << raysToTest[j].mOrigin << " dir = "
	   << raysToTest[j].mDirection << endl;
    }
#endif    
        
    raycaster->CastRays16(raysToTest,
			  vssRays,
			  bboxOrig,
			  castDoubleRays,
			  false);
#endif
    
#ifdef DEBUGRESULTS
    if (!castDoubleRays) {
      float T = rays[i].IntersectionRes[0].tdist;
      if ( (res != results[i].hitA) ||
	   ((res)&&(results[i].hitA)&& (fabs(T - results[i].hitAT)) > 0.1f)
	   )
	{
	  if (printOut)
	    cout << " i = " << i << " ";
	  if ((res != 0) && (results[i].hitA == 0)) {
	    cntIntelNoWeYes++;
	    if (printOut)
	      cout << "Intel no intersection, our alg intersection at t = " << T << endl;
	  }
	  else {
	    if ((res == 0) && (results[i].hitA)) {
	      cntIntelYesWeNo++;
	      if (printOut)
		cout << "Intel intersection at = " << results[i].hitAT
		     << " , our alg no intersection = " << endl;
	      
	    }
	    else {
	      cntIntelOurDistMismatch++;
	      sumDistOurAlg += T;
	      sumDistIntel += results[i].hitAT;
	      sumDistAbsDiff += fabs(T - results[i].hitAT);
	      if (printOut)
		cout << "Intel intersection at = " << results[i].hitAT
		     << " , our alg intersection at = " << T << endl;	    
	    }
	  }
	}
    } // double rays
    else {
      // checking for results of double rays - not yet implemented
    }
#endif    
  }

  timer.Stop();
  t2 = GetTime();
  cout<<"\n#SORTING + RAY_CAST_TIME = ";
  cout << TimeDiff(t1, t2)<<" [mikrosec]"
       << " userTime = " << timer.UserTime() << " realTime = "
       << timer.RealTime() << endl;
  cout << "Rays shot per milisecond [userTimer] = "
       << ((double)cntRays * mult/(double)timer.UserTime()) / 1000.f << endl;

#ifdef DEBUGRESULTS  
  cout << "cntIntelYesWeNo = " << cntIntelYesWeNo << endl;
  cout << "cntIntelNoWeYes = " << cntIntelNoWeYes << endl;
  cout << "cntIntelOurDistMismatch = " << cntIntelOurDistMismatch++ << endl;
  cout << " sumDistIntel = " << sumDistIntel/(double)cntIntelOurDistMismatch << endl;
  cout << " sumDistOur = " << sumDistOurAlg/(double)cntIntelOurDistMismatch << endl;
  cout << " sumAbsDiffDist = " << sumDistAbsDiff/(double)cntIntelOurDistMismatch
       << endl;
#endif  
  cout << "Done\n" << endl;
  return;
}

//-------------------------------------------------------------------------
// This is for testing RT implementation using ray packets
void
TestRTfromFilePackets(int argc, char **argv)
{
  int returnCode = 0;

  InitTiming();
  Debug.open("debug.log");

  Environment::GetSingleton()->Parse(argc, argv, USE_EXE_PATH);
  MeshKdTree::ParseEnvironment();

  char buff[128];
  Environment::GetSingleton()->GetStringValue("Preprocessor.type", buff);
  string preprocessorType(buff);

  if (!(preprocessor = PreprocessorFactory::CreatePreprocessor(preprocessorType)))
  {
    Environment::DelSingleton();
    cerr << "Unknown preprocessor type" << endl;
    exit(1);
  }


  Environment::GetSingleton()->GetStringValue("Scene.filename", buff);
  string filename(buff);

  const string externKdTree = ReplaceSuffix(filename, ".obj", ".kdf");
  const string internKdTree = GetInternFilename(filename, preprocessor->mLoadMeshes ?
						".kdm" : ".kdt");

  if (preprocessor->InitRayCast(externKdTree, internKdTree))
  {
    cout << "ray casting initialized!" << endl;
  }
  else
  {
    cout << "ray casting initialization failed!" << endl;
    Cleanup();
    exit(1);
  }
  
  //Debug << "using pvs type " << PVS_TYPE << endl;

  /////////////
  //-- load scene 
  
  if (!preprocessor->LoadScene(filename))
  {
    cout << "loading file " << filename << " failed" << endl;
    Cleanup();
    exit(1);
  }
	
  ////////////
  //-- initialize external ray caster
  
  if (preprocessor->LoadInternKdTree(internKdTree))
  {
    cout << "intern kd tree loaded!" << endl;
  }
  else
  {
    cout << "loading intern kd tree failed!" << endl;
    Cleanup();
    exit(1);
  }
  
  // export objects as obj
  if (preprocessor->mExportObj)
  {
    if (strstr(filename.c_str(), ".obj"))
    {
      cerr << "already in obj format" << endl;
      if (0) 	preprocessor->ExportObj("test.obj", preprocessor->mObjects);
    }
    else
    {
      const string objname = GetInternFilename(filename, ".obj");
	
      cout << "exporting scene to " << objname << endl;
      bool success = preprocessor->ExportObj(objname, preprocessor->mObjects);	
	
      if (success)
	cout << "finished exporting obj" << endl;
      else
	cerr << "error exporting " << objname << endl;
    }
  }

  Environment::GetSingleton()->GetStringValue("Rays.file", buff);
  FILE *fp = fopen(buff, "rb");
  if (!fp) {
    cerr << "ERROR: file " << buff << " cannot be opened for reading" << endl;
    cerr << "EXITING" << endl;
    exit(3);
  }
  cout << "File " << buff << " was opened successfully" << endl;
  int cntMaxRays = 100000;
  Environment::GetSingleton()->GetIntValue("Rays.cnt", cntMaxRays);
  SimpleRayContainer rays;
  SimpleRay rayTest;
  vector<RESult> results;
  
  int cntRays = 0;
  for (int i = 0; cntRays < cntMaxRays; i++) {
    int ch = fgetc(fp);
    switch (ch) {
    case 'G': { // two-sided rays of 16
      for (int j = 0; j < 16; j++) {
	int order;
	float ox, oy, oz, dx, dy, dz;
	int hitA; float hitAT;
	int hitB; float hitBT;	
	if (fscanf(fp, "%d %f %f %f %f %f %f %d %f %d %f\n",
		   &order, &ox, &oy, &oz, &dx, &dy, &dz, &hitA, &hitAT, &hitB, &hitBT) != 11) {
	  cerr << "Problem parsing the ray file" << endl;
	  cerr << "EXITING for ray order" << order << endl;
	  goto FINISH;
	}
	Vector3 mPosition(ox, oy, oz);
	Vector3 mDirection(dx, dy, dz);
	rayTest.Set(mPosition, mDirection, 0, 1.0, true);
	rays.push_back(rayTest);
	results.push_back(RESult(hitA, hitAT, hitB, hitBT));
	cntRays++;
      }
      break;
    }
    case 'H': // one-sided rays of 16
      cerr << "Not yet implemented " << endl; abort();
      
    case 'D': // two-sided ray
      cerr << "Not yet implemented " << endl; abort();
      
    case 'S': // one-sided ray
      cerr << "Not yet implemented " << endl; abort();
    } // switch
  } // for i
FINISH:
  fclose(fp);
  
  Environment::GetSingleton()->GetBoolValue("TestDoubleRays", castDoubleRays);

  double mult = 1.0;
  if (castDoubleRays)
    mult = 2.0;

  cout << "Starting to shoot " << cntRays * mult << " rays" << endl;
  
  RayCaster *raycaster = preprocessor->mRayCaster;
  VssRayContainer vssRays;
  AxisAlignedBox3 bboxOrig = preprocessor->mSceneGraph->GetBox();

#undef DEBUGRESULTS
//#define DEBUGRESULTS

#ifdef DEBUGRESULTS
  int cntIntelYesWeNo = 0;
  int cntIntelNoWeYes = 0;
  int cntIntelOurDistMismatch = 0;
  bool printOut = false;
  double sumDistIntel = 0.;
  double sumDistOurAlg = 0.;
  double sumDistAbsDiff = 0.f;
#endif

  cout << "Press a key to start ray shooting" << endl;
  getchar();
  cout << "Ray packs shooting " << cntRays << " rays started - "
       << (castDoubleRays ? " double " : " single ")
       << " dir " << endl;

  long t1 = GetTime();
  CTimer timer;
  timer.Reset();
  timer.Start();

#ifdef _USE_HAVRAN_SSE
#ifdef __SSE__  
  RayPacket2x2 raysPack;
  
  for (int i = 0; i < cntRays - 16; i++) {
    for (int j = 0; j < 4; j++, i++) {
      raysPack.SetLoc(j, rays[i].mOrigin);
      raysPack.SetDir(j, rays[i].mDirection);
    } // for
#if 0
    for (int j = 0; j < 16; j++) {
      cout << "orig = " << raysToTest[j].mOrigin << " dir = "
	   << raysToTest[j].mDirection << endl;
    }
#endif
    raysPack.ComputeDirSign();
    raycaster->CastRaysPacket2x2(raysPack, castDoubleRays);
    
#ifdef DEBUGRESULTS
    if (!castDoubleRays) {
      float T = rays[i].IntersectionRes[0].tdist;
      if ( (res != results[i].hitA) ||
	   ((res)&&(results[i].hitA)&& (fabs(T - results[i].hitAT)) > 2.0f)
	   )
	{
	  if (printOut)
	    cout << " i = " << i << " ";
	  if ((res != 0) && (results[i].hitA == 0)) {
	    cntIntelNoWeYes++;
	    if (printOut)
	      cout << "Intel no intersection, our alg intersection at t = " << T << endl;
	  }
	  else {
	    if ((res = 0) && (results[i].hitA)) {
	      cntIntelYesWeNo++;
	      if (printOut)
		cout << "Intel intersection at = " << results[i].hitAT
		     << " , our alg no intersection = " << endl;
	      
	    }
	    else {
	      cntIntelOurDistMismatch++;
	      sumDistOurAlg += T;
	      sumDistIntel += results[i].hitAT;
	      sumDistAbsDiff += fabs(T - results[i].hitAT);
	      if (printOut)
		cout << "Intel intersection at = " << results[i].hitAT
		     << " , our alg intersection at = " << T << endl;	    
	    }
	  }
	}
    } // double rays
    else {
      // checking for results of double rays - not yet implemented
    }
#endif    
  }

#endif // __SSE__ 
#endif // _USE_HAVRAN_SSE

  timer.Stop();
  long t2 = GetTime();
  cout<<"\n#RAY_CAST_TIME = ";
  cout << TimeDiff(t1, t2)<<" [mikrosec]"
       << " userTime = " << timer.UserTime() << " realTime = "
       << timer.RealTime() << endl;
    ;
  cout << "Rays shot per milisecond [userTimer] = "
       << ((double)cntRays * mult/(double)timer.UserTime()) / 1000.f << endl;

#ifdef DEBUGRESULTS  
  cout << "cntIntelYesWeNo = " << cntIntelYesWeNo << endl;
  cout << "cntIntelNoWeYes = " << cntIntelNoWeYes << endl;
  cout << "cntIntelOurDistMismatch = " << cntIntelOurDistMismatch++ << endl;
  cout << " sumDistIntel = " << sumDistIntel/(double)cntIntelOurDistMismatch << endl;
  cout << " sumDistOur = " << sumDistOurAlg/(double)cntIntelOurDistMismatch << endl;
  cout << " sumAbsDiffDist = " << sumDistAbsDiff/(double)cntIntelOurDistMismatch
       << endl;
#endif  
  cout << "Done\n" << endl;
  return;
}

// This is for testing RT implementation
void
TestRT_4_fromFile(int argc, char **argv)
{
  int returnCode = 0;

  InitTiming();
  Debug.open("debug.log");

  Environment::GetSingleton()->Parse(argc, argv, USE_EXE_PATH);
  MeshKdTree::ParseEnvironment();

  char buff[128];
  Environment::GetSingleton()->GetStringValue("Preprocessor.type", buff);
  string preprocessorType(buff);

  if (!(preprocessor = PreprocessorFactory::CreatePreprocessor(preprocessorType)))
  {
    Environment::DelSingleton();
    cerr << "Unknown preprocessor type" << endl;
    exit(1);
  }


  Environment::GetSingleton()->GetStringValue("Scene.filename", buff);
  string filename(buff);

  const string externKdTree = ReplaceSuffix(filename, ".obj", ".kdf");
  const string internKdTree = GetInternFilename(filename, preprocessor->mLoadMeshes ?
						".kdm" : ".kdt");

  if (preprocessor->InitRayCast(externKdTree, internKdTree))
  {
    cout << "ray casting initialized!" << endl;
  }
  else
  {
    cout << "ray casting initialization failed!" << endl;
    Cleanup();
    exit(1);
  }
  
  //Debug << "using pvs type " << PVS_TYPE << endl;

  /////////////
  //-- load scene 
  
  if (!preprocessor->LoadScene(filename))
  {
    cout << "loading file " << filename << " failed" << endl;
    Cleanup();
    exit(1);
  }
	
  ////////////
  //-- initialize external ray caster
  
  if (preprocessor->LoadInternKdTree(internKdTree))
  {
    cout << "intern kd tree loaded!" << endl;
  }
  else
  {
    cout << "loading intern kd tree failed!" << endl;
    Cleanup();
    exit(1);
  }
  
  // export objects as obj
  if (preprocessor->mExportObj)
  {
    if (strstr(filename.c_str(), ".obj"))
    {
      cerr << "already in obj format" << endl;
      if (0) 	preprocessor->ExportObj("test.obj", preprocessor->mObjects);
    }
    else
    {
      const string objname = GetInternFilename(filename, ".obj");
	
      cout << "exporting scene to " << objname << endl;
      bool success = preprocessor->ExportObj(objname, preprocessor->mObjects);	
	
      if (success)
	cout << "finished exporting obj" << endl;
      else
	cerr << "error exporting " << objname << endl;
    }
  }

  Environment::GetSingleton()->GetStringValue("Rays.file", buff);
  FILE *fp = fopen(buff, "rb");
  if (!fp) {
    cerr << "ERROR: file " << buff << " cannot be opened for reading" << endl;
    cerr << "EXITING" << endl;
    exit(3);
  }
  cout << "File " << buff << " was opened successfully" << endl;
  int cntMaxRays = 100000;
  Environment::GetSingleton()->GetIntValue("Rays.cnt", cntMaxRays);
  SimpleRayContainer rays;
  SimpleRay rayTest;
  vector<RESult> results;
  vector<AxisAlignedBox3> boxes;

  int cntRays = 0;
  for (int i = 0; cntRays < cntMaxRays;) {
    int ch = fgetc(fp);
    switch (ch) {
    case 'I': { // two-sided rays of 16
      Vector3 minv, maxv;
      if (fscanf(fp, "%f %f %f %f %f %f\n",
		 &minv.x, &minv.y, &minv.z,
		 &maxv.x, &maxv.y, &maxv.z) != 6) {
	cerr << "Problem parsing the ray file" << endl;
	cerr << "EXITING for ray order" << i << endl;
	goto FINISH;
      }
      boxes.push_back(AxisAlignedBox3(minv, maxv));      

      for (int j = 0; j < 4; j++) {
	int order;
	float ox, oy, oz, dx, dy, dz;
	int hitA; float hitAT;
	int hitB; float hitBT;	
	if (fscanf(fp, "%d %f %f %f %f %f %f %d %f\n",
		   &order, &ox, &oy, &oz, &dx, &dy, &dz,
		   &hitA, &hitAT) != 9) {
	  cerr << "Problem parsing the ray file" << endl;
	  cerr << "EXITING for ray order" << order << endl;
	  goto FINISH;
	}
	Vector3 mPosition(ox, oy, oz);
	Vector3 mDirection(dx, dy, dz);
	rayTest.Set(mPosition, mDirection, 0, 1.0, true);
	rays.push_back(rayTest);
	results.push_back(RESult(hitA, hitAT, 0, -1.f));
	cntRays++;
	i++;
      }
      break;
    }
    default: {
      cerr << "Not yet implemented or end of file" << endl;
      goto FINISH;
    }
    } // switch
  } // for i

FINISH:
  
  fclose(fp);
  cout << "Starting to shoot " << cntRays << " rays" << endl;
  
  RayCaster *raycaster = preprocessor->mRayCaster;
  VssRayContainer vssRays;
  AxisAlignedBox3 bboxOrig = preprocessor->mSceneGraph->GetBox();

//#define DEBUGRESULTS

#ifdef DEBUGRESULTS
  int cntIntelYesWeNo = 0;
  int cntIntelNoWeYes = 0;
  int cntIntelOurDistMismatch = 0;
  bool printOut = false;
  double sumDistIntel = 0.f;
  double sumDistOurAlg = 0.f;
  double sumDistAbsDiff = 0.f;
#endif

  //cout << "Press a key to start ray shooting" << endl;
  //getchar();
  cout << "Ray shooting " << cntRays << " rays started - "
       << (castDoubleRays ? " double " : " single ")
       << " dir " << endl;

  long t1 = GetTime();
  CTimer timer;
  timer.Reset();
  timer.Start();
  Vector3 boxMin, boxMax;

  bool printOut = false;
  
  SimpleRayContainer raysToTest;
  int boxI = 0;
  for (int i = 0; i < cntRays - 4; i+= 4, boxI++) {
    Vector3 origin4[4];
    Vector3 direction4[4];
    int     result4[4];
    float   dist4[4];
    boxMin = boxes[boxI].Min();
    boxMax = boxes[boxI].Max();
    for (int j = 0; j < 4; j++) {
      int o = i+j;
      origin4[j] = rays[o].mOrigin;
      direction4[j] = rays[o].mDirection;
    }
    raycaster->CastRaysPacket4(boxMin, boxMax,
			       origin4, direction4,
			       result4, dist4);
    if (printOut) {
      printf("I %4.7f %4.7f %4.7f %4.7f %4.7f %4.7f\n",
	     boxMin.x, boxMin.y, boxMin.z, boxMax.x, boxMax.y, boxMax.z);
    }

    for (int j = 0; j < 4; j++) {
#if 0
      if (result4[j] == 0) {
	int res = raycaster->CastRay(rays[i+j],
				     vssRays,
				     bboxOrig,
				     false,
				     true);
	if (res) {
	  float tdist = SimpleRay::IntersectionRes[0].tdist;
	  Vector3 point = rays[i+j].Extrap(tdist);
	  AxisAlignedBox3 testbox(boxMin, boxMax);
	  
	  if (testbox.IsInside(point)) {
	    cout << "Error in the algorithm - computed not in the box, but"
		 << " it is later found in the box" << endl;
	    cout << " j = " << j << endl;
	    cout << " box = " << testbox << endl;
	    cout << " point = " << point << endl;
	    raycaster->CastRaysPacket4(boxMin, boxMax,
			       origin4, direction4,
			       result4, dist4);
	  }
	}
      }
#endif      
      
      if (printOut) {
	printf("%d %4.7f %4.7f %4.7f %4.7f %4.7f %4.7f %d %4.7f\n",
	       i+j,
	       origin4[j].x,
	       origin4[j].y,
	       origin4[j].z,
	       direction4[j].x,
	       direction4[j].y,
	       direction4[j].z,
	       (result4[j] != -1) ? 1 : 0,
	       (result4[j] != -1) ? dist4[j] : 0);
      }
    } // for j    
  } // for i

  timer.Stop();
  long t2 = GetTime();
  cout<<"\n#RAY_CAST_TIME = ";
  cout << TimeDiff(t1, t2)<<" [mikrosec]"
       << " userTime = " << timer.UserTime() << " realTime = "
       << timer.RealTime() << endl;
  cout << "Rays shot per milisecond [userTimer] = "
       << ((double)cntRays/(double)timer.UserTime()) / 1000.f << endl;

#ifdef DEBUGRESULTS  
  cout << "cntIntelYesWeNo = " << cntIntelYesWeNo << endl;
  cout << "cntIntelNoWeYes = " << cntIntelNoWeYes << endl;
  cout << "cntIntelOurDistMismatch = " << cntIntelOurDistMismatch++ << endl;
  cout << " sumDistIntel = " << sumDistIntel/(double)cntIntelOurDistMismatch << endl;
  cout << " sumDistOur = " << sumDistOurAlg/(double)cntIntelOurDistMismatch << endl;
  cout << " sumAbsDiffDist = " << sumDistAbsDiff/(double)cntIntelOurDistMismatch
       << endl;
#endif  
  cout << "Done\n" << endl;
  return;
}