#include <algorithm>

// the devil library
#include <IL/il.h>
#include <IL/ilu.h>
#include <IL/ilut.h>
#include <cassert>

#include "common.h"
#include "Camera.h"
#include "Ray.h"
#include "SimpleRay.h"
#include "KdTree.h"
#include "Mesh.h"
#include "Exporter.h"
#include "SceneGraph.h"
#include "Preprocessor.h"
#include "RayCaster.h"
#include "tgaimg.h"


#ifdef USE_HAVRAN_RAYCASTER
#include "raypack.h"
#endif

using namespace std;

namespace GtpVisibilityPreprocessor {


void
InitDevIl()
{
  ilInit();
  ILuint ImageName;
  ilGenImages(1, &ImageName);
  ilBindImage(ImageName);
  ilEnable(IL_FILE_OVERWRITE);

  //  ilRegisterFormat(IL_RGBA);
  //  ilRegisterType(IL_FLOAT);

  //  ilEnable(IL_ORIGIN_SET);
  //  ilOriginFunc(IL_ORIGIN_UPPER_LEFT);
}

bool
Camera::SnapImage(string filename,
		  KdTree *tree,
		  SceneGraph *sceneGraph
		 )
{
  int x;
  int y;

  bool exportRays = false;
  CTGAImage tgaimg; tgaimg.Init(mWidth, mHeight);
  
  vector<Ray *> rays;

  long t1 = GetTime();

  Ray ray;

  for (y = 0; y < mHeight; y++) {
    cout<<"+" << flush;
    for (x = 0; x < mWidth; x++) {
      SetupRay(ray, mWidth - (x + 1), mHeight - (y + 1));

	  bool debug = true;
	  //	  (y == mHeight/2) && (x== mWidth/3);
	  //	  MeshDebug = debug;
	  
	  if (debug) 
		ray.mFlags = (Ray::STORE_TESTED_OBJECTS | Ray::STORE_KDLEAVES);
	  else 
		ray.mFlags &= ~(Ray::STORE_TESTED_OBJECTS|Ray::STORE_KDLEAVES);
	  
	  if (tree->CastRay(ray)) {
		//		cout<<"I1";

		if (ray.intersections.size()) {
		  sort(ray.intersections.begin(), ray.intersections.end());
		  MeshInstance *mesh = (MeshInstance*)ray.intersections[0].mObject;
		  RgbColor color(1,1,1);
		  if (mesh->GetMesh()->mMaterial)
			color = mesh->GetMesh()->mMaterial->mDiffuseColor;

		  tgaimg.SetPixel(x, y, color.r * 255.f, color.g * 255.f, color.b * 255.f);
		}
	  }
	  if (debug) {
	    Ray *nray = new Ray(ray);
	    rays.push_back(nray);
	  }
    }
  }
  
  long t2 = GetTime();
  cout<<"#RAY_CAST_TIME\n";
  cout<<TimeDiff(t1, t2)<<"\n";
  
  
  cout<<"Saving image"<<endl;
  tgaimg.SaveToFile(filename.c_str());  

  cout<<"done."<<endl<<flush;
  
  Exporter *exporter = NULL;
  if (1) {
    exporter = Exporter::GetExporter(filename + "-rays" + ".x3d");
    //exporter->SetFilled();

	exporter->SetWireframe();

	if (sceneGraph) 
	  exporter->ExportScene(sceneGraph->GetRoot());
	
    //exporter->ExportKdTree(*tree);
	//exporter->ExportBspTree(*bsptree);
    exporter->ExportRays(rays, 2000);
    int k =0;
    for (int j=0; j < rays.size(); j++)
      if (rays[j]->kdLeaves.size()) {
		Ray *ray = rays[j];
		int i;
		exporter->SetWireframe();

		if (1)
		  for (i= 0; i < ray->kdLeaves.size(); i++)
			exporter->ExportBox(tree->GetBox(ray->kdLeaves[i]));

		exporter->SetFilled();
		
		if (1)
		  for (i= 0; i < ray->testedObjects.size(); i++)
			exporter->ExportIntersectable(ray->testedObjects[i]);
      }
	
    delete exporter;
  }
  
  return true;
}


void
Camera::SetupRay(Ray &ray, const int x, const int y)
{
  Vector3 xv = mRight*((x - mWidth/2)/(float)mWidth);
  Vector3 yv = mUp*((y - mHeight/2)/(float)mHeight);
  Vector3 target = xv + yv + mDirection;

  ray.Clear();
  ray.Init(mPosition, target, Ray::LOCAL_RAY);
  ray.mFlags &= ~Ray::CULL_BACKFACES;
}


void
Camera::SetupRay(SimpleRay &ray, const int x, const int y)
{
  Vector3 xv = mRight*((x - mWidth/2)/(float)mWidth);
  Vector3 yv = mUp*((y - mHeight/2)/(float)mHeight);
  Vector3 dir = mDirection - xv + yv;
  dir.Normalize();
  
  ray.Set(mPosition, dir, 0,
	  1.0f, ~(SimpleRay::F_BIDIRECTIONAL));
}

bool
Camera::SnapImage(string filename,
		  RayCaster *raycaster,
		  AxisAlignedBox3 &bbox,
		  SceneGraph *sceneGraph
		  )
{
  int x;
  int y;

  bool exportRays = false;
  CTGAImage tgaimg; tgaimg.Init(mWidth, mHeight);
  tgaimg.FillInImage(0.0, 0.0f, 200.f);
  
  vector<Ray *> rays;

  long t1 = GetTime();

  SimpleRay ray;
  VssRayContainer vssRays;

  bool doubleRays = true;
  bool exploitDoubleRays = true;
  if (!doubleRays)
    exploitDoubleRays = false;
  
  //CTimer timer;
  //timer.Start();
  
  //int ystart = 92;
  int xstart = 0;
  int ymax = mHeight;
  for (y = 0; y < ymax; y++) {
    cout<<"+" << flush;
    for (x = 0; x < mWidth; x++) {
      SetupRay(ray, x, y);
      if (exploitDoubleRays)
        ray.mDirection = - ray.mDirection;

      bool debug = false;
      //	  (y == mHeight/2) && (x== mWidth/3);
      //	  MeshDebug = debug;

      int res = raycaster->CastRay(ray,
				   vssRays,
				   bbox,
				   doubleRays, // castDoubleRay,
				   false); // pruneInvalidRays

      // cout << "End ------------------------ "
      // << SimpleRay::IntersectionRes[0].intersectable
      // << " t = " << SimpleRay::IntersectionRes[0].tdist
      // << endl;
      
      if (res) {
	Vector3 normal = SimpleRay::IntersectionRes[0].intersectable->GetNormal(0);
	float v =
	  ray.mDirection.x * normal.x +
	  ray.mDirection.y * normal.y +
	  ray.mDirection.z * normal.z;
	v = fabs(v);
	v *= 200.0f;
	if (v < 50.f) v = 50.f;
	tgaimg.SetPixel(x, y, v, v, v);
      }
      
      if (debug) {
	Ray *nray = new Ray(ray.mOrigin, ray.mDirection,
			    Ray::LOCAL_RAY);
	rays.push_back(nray);
      }
    }
  } // for y

  //timer.Stop();
  
  long t2 = GetTime();
  cout<<"\n#RAY_CAST_TIME = ";
  cout << TimeDiff(t1, t2)<<" [mikrosec]\n";  
  
  cout<<"Saving image"<<endl;

  tgaimg.SaveToFile(filename.c_str());  
  
  //ilRegisterType(IL_FLOAT);
  //ilTexImage(mWidth, mHeight, 1, 4, IL_RGBA, IL_FLOAT, buffer);
  //ilSaveImage((char *const)filename.c_str());
  //delete buffer;

  cout<<"done."<<endl<<flush;
    
  return true;
}


bool
Camera::SnapImage2(string filename,
		   RayCaster *raycaster,
		   AxisAlignedBox3 &bbox,
		   SceneGraph *sceneGraph)
{
  int x;
  int y;

  bool exportRays = false;
  CTGAImage tgaimg; tgaimg.Init(mWidth, mHeight);
  tgaimg.FillInImage(0.0, 0.0f, 200.f);
  
  vector<Ray *> raysDebug;

  long t1 = GetTime();

  SimpleRay ray;
  SimpleRayContainer rays;
  VssRayContainer vssRays;

  bool doubleRays = false;
  bool exploitDoubleRays = false;
  int batchSize = 16;
  if (!doubleRays)
    exploitDoubleRays = false;
  int shiftIndex = 0;
  if (exploitDoubleRays)
    shiftIndex = batchSize;
    
  //CTimer timer;
  //timer.Start();
  
  //int ystart = 92;
  int xstart = 0;
  int ymax = mHeight;
  for (y = 0; y < ymax; y++) {
    if (y == 250) {
      cout << "Debug y = " << y << endl;
    }
    
    cout<<"+" << flush;
    for (x = 0; x < mWidth; x += batchSize ) {
      rays.erase(rays.begin(), rays.end());
      int xi;
      for (xi = 0; (xi < batchSize) && (x+xi < mWidth); xi++ ) {
	SetupRay(ray, x + xi, y);
	if (exploitDoubleRays)
	  ray.mDirection = -ray.mDirection;
	rays.push_back(ray);
      } // for

      bool debug = false;
      //	  (y == mHeight/2) && (x== mWidth/3);
      //	  MeshDebug = debug;

#if 1
      assert(batchSize == 16);
      raycaster->CastRays16(rays,
			    vssRays,
			    bbox,
			    doubleRays, // castDoubleRay,
			    false); // pruneInvalidRays
#else
      raycaster->CastSimpleForwardRays(rays, bbox);
#endif
      
      // cout << "End ------------------------ "
      // << SimpleRay::IntersectionRes[0].intersectable
      // << " t = " << SimpleRay::IntersectionRes[0].tdist
      // << endl;
      
      for (xi = 0; (xi < batchSize) && (x+xi < mWidth); xi++ ) {
	Intersectable* res =
	  SimpleRay::IntersectionRes[xi + shiftIndex].intersectable;

	if (res) {
	  Vector3 normal = res->GetNormal(0);
	  float v =
	    rays[xi].mDirection.x * normal.x +
	    rays[xi].mDirection.y * normal.y +
	    rays[xi].mDirection.z * normal.z;
	  v = fabs(v);
	  v *= 200.0f;
	  if (v < 50.f) v = 50.f;
	  tgaimg.SetPixel(x + xi, y, v, v, v);
	}
      }
      
      if (debug) {
	Ray *nray = new Ray(ray.mOrigin, ray.mDirection,
			    Ray::LOCAL_RAY);
	raysDebug.push_back(nray);
      }
    } // for x
  } // for y

  //timer.Stop();
  
  long t2 = GetTime();
  cout<<"\n#RAY_CAST_TIME = ";
  cout << TimeDiff(t1, t2)<<" [mikrosec]\n";  
  
  cout<<"Saving image"<<endl;

  tgaimg.SaveToFile(filename.c_str());  
  
  //ilRegisterType(IL_FLOAT);
  //ilTexImage(mWidth, mHeight, 1, 4, IL_RGBA, IL_FLOAT, buffer);
  //ilSaveImage((char *const)filename.c_str());
  //delete buffer;

  cout<<"done."<<endl<<flush;
    
  return true;
}

  
bool
Camera::SnapImagePacket(string filename,
			RayCaster *raycaster,
			AxisAlignedBox3 &bbox,
			SceneGraph *sceneGraph
			)
{
#ifdef USE_HAVRAN_RAYCASTER 
#ifdef _USE_HAVRAN_SSE
  
  int x;
  int y;

  bool exportRays = false;  

  //  - components*mWidth;
  CTGAImage tgaimg; tgaimg.Init(mWidth, mHeight);
  tgaimg.FillInImage(0.0, 0.0f, 200.f); // blue color image
  
  vector<Ray *> rays;

  long t1 = GetTime();

  SimpleRay ray;
  VssRayContainer vssRays;

  //CTimer timer;
  //timer.Start();

  bool doubleRays = false;
  bool exploitDoubleRays = false;
  if (!doubleRays)
    exploitDoubleRays = false;
  
  GALIGN16 RayPacket2x2 rp;
  //int ystart = 92;
  //int xstart = 738;
  int pixels = 0;;
  for (y = 0; y < mHeight-1; y+=2) {
  //for (y = ystart-2; y > 0; y-=2) {
    for (x = 0; x < mWidth-1; x+=2) {
      int i = 0;
      for (int yi = 0; yi < 2; yi++) {
	for (int xi = 0; xi < 2; xi++) {
	  SetupRay(ray, x+xi, y+yi);
	  rp.SetLoc(i, ray.mOrigin);
	  rp.SetDir(i, ray.mDirection);
	  if (exploitDoubleRays)
	    rp.SetDir(i, -ray.mDirection);
	  i++;
	} // for xi
      } // for yi      
      pixels += 4;
      if (pixels > mWidth) {
	cout << "+" << flush;
	pixels = 0;
      }
      
      raycaster->CastRaysPacket2x2(rp, doubleRays, false);

      i = 0;
      for (int yi = 0; yi < 2; yi++) {
	for (int xi = 0; xi < 2; xi++) {
	  Intersectable* res = rp.GetObject(i);
	  if (res) {
#if 0
	    // This is debugging code to find a bug
	    for (int j = 0; j < 4; j++) {
	      cout << " j = " << j << " obj = "
		   << rp.GetObject(j)
		   << " t = " << rp.GetT(j);
	      ray.Set(rp.GetLoc(j), rp.GetDir(j), 0, 1.0f, 0);
	      int res = raycaster->CastRay(ray,
					   vssRays,
					   bbox,
					   doubleRays, // castDoubleRay,
					   false); // pruneInvalidRays
	      cout << " correct result obj = "
		   << SimpleRay::IntersectionRes[0].intersectable
		   << " t = "
		   << SimpleRay::IntersectionRes[0].tdist
		   << endl;		
	    } // for

	    raycaster->CastRaysPacket2x2(rp, false, false);
#endif
	    
	    Vector3 normal = res->GetNormal(0);
	    float v =
	      ray.mDirection.x * normal.x +
	      ray.mDirection.y * normal.y +
	      ray.mDirection.z * normal.z;
	    v = fabs(v);
	    v *= 200.0f;
	    if (v < 50.f) v = 50.f;
	    tgaimg.SetPixel(x+xi, y+yi, v, v, v);
	  }
	  i++;
	} // xi
      } // yi
    } // for x
  } // for y

  //timer.Stop();
  
  long t2 = GetTime();
  cout<<"\n#RAY_CAST_TIME = ";
  cout << TimeDiff(t1, t2)<<" [mikrosec]\n";  
  
  cout<<"Saving image"<<endl;
  tgaimg.SaveToFile(filename.c_str());  
    
#endif // _USE_HAVRAN_SSE
#endif // USE_HAVRAN_RAYCASTER 
  cout<<"done."<<endl<<flush;
    
  return true;
}
  
inline static bool ilt(Intersectable *obj1, Intersectable *obj2)
{
  return obj1->mId < obj2->mId;
}


bool
Camera::SnapImagePacket2(string filename,
			RayCaster *raycaster,
			AxisAlignedBox3 &bbox,
			SceneGraph *sceneGraph
			)
{
#ifdef USE_HAVRAN_RAYCASTER 
#ifdef _USE_HAVRAN_SSE
  
  int x;
  int y;

  bool exportRays = false;  

  //  - components*mWidth;
  CTGAImage tgaimg; tgaimg.Init(mWidth, mHeight);
  tgaimg.FillInImage(0.0, 0.0f, 200.f); // blue color image
  
  vector<Ray *> rays;

  long t1 = GetTime();

  SimpleRay ray;
  VssRayContainer vssRays;

  //CTimer timer;
  //timer.Start();

  bool doubleRays = false;
  bool exploitDoubleRays = false;
  if (!doubleRays)
    exploitDoubleRays = false;
  
  Vector3 origin4[4];
  Vector3 direction4[4];
  int     result4[4];
  float   dist4[4];
  
  GALIGN16 RayPacket2x2 rp;
  //int ystart = 92;
  //int xstart = 738;
  int pixels = 0;;
  for (y = 0; y < mHeight-1; y+=2) {
  //for (y = ystart-2; y > 0; y-=2) {
    for (x = 0; x < mWidth-1; x+=2) {
      int i = 0;
      for (int yi = 0; yi < 2; yi++) {
	for (int xi = 0; xi < 2; xi++) {
	  SetupRay(ray, x+xi, y+yi);
	  origin4[i] = ray.mOrigin;
	  direction4[i] = ray.mDirection;
	  if (exploitDoubleRays)
	    direction4[i] = -ray.mDirection;
	  i++;
	} // for xi
      } // for yi      
      pixels += 4;
      if (pixels > mWidth) {
	cout << "+" << flush;
	pixels = 0;
      }
      
      raycaster->CastRaysPacket4(bbox.Min(), bbox.Max(),
				 origin4,
				 direction4,
				 result4,
				 dist4);

      i = 0;
      for (int yi = 0; yi < 2; yi++) {
	for (int xi = 0; xi < 2; xi++) {
	  int objId = result4[i];
	  if (objId != -1) {
	    MeshInstance dummyInst(NULL);
	    dummyInst.SetId(objId);
	    ObjectContainer &objects = preprocessor->mObjects;
	    ObjectContainer::const_iterator oit =
	      lower_bound(objects.begin(), objects.end(),
			  (Intersectable *)&dummyInst, ilt);
      
	    if ((oit != objects.end()) && ((*oit)->GetId() == objId)) {
	      Intersectable *res = *oit;
	    
	      Vector3 normal = res->GetNormal(0);
	      float v =
		ray.mDirection.x * normal.x +
		ray.mDirection.y * normal.y +
		ray.mDirection.z * normal.z;
	      v = fabs(v);
	      v *= 200.0f;
	      if (v < 50.f) v = 50.f;
	      tgaimg.SetPixel(x+xi, y+yi, v, v, v);
	    }
	    else {
	      cout <<"*" << endl;
	    }
	    
	  } // object intersected
	  i++;
	} // xi
      } // yi
    } // for x
  } // for y

  //timer.Stop();
  
  long t2 = GetTime();
  cout<<"\n#RAY_CAST_TIME = ";
  cout << TimeDiff(t1, t2)<<" [mikrosec]\n";  
  
  cout<<"Saving image"<<endl;
  tgaimg.SaveToFile(filename.c_str());  
    
#endif // _USE_HAVRAN_SSE
#endif // USE_HAVRAN_RAYCASTER 
  cout<<"done."<<endl<<flush;
    
  return true;
}

}