#include <assert.h>
#include <stack>
using namespace std;
#include "KdTree.h"
#include "AxisAlignedBox3.h"
#include "Ray.h"
#include "MutualVisibility.h"
#include "Exporter.h"
#include "Mesh.h"
#include "Triangle3.h"
#include "SceneGraph.h"

namespace GtpVisibilityPreprocessor {

void
RayShaft::Init(
	       const Rectangle3 &source,
	       const Rectangle3 &target)
{
  mDepth = 0;
  mSource = source;
  mTarget = target;
}


Vector3
RayShaft::GetIntersectionPoint(const int rayIndex,
			       const int depth) const
{
  Vector3 origin, direction;
  Ray ray;
  GetRaySegment(rayIndex, ray);
  if (depth >= mSamples[rayIndex].mIntersections.size()) {
    cerr<<"depth of sample out of limits"<<endl;
    exit(1);
  }
  return ray.Extrap(mSamples[rayIndex].mIntersections[depth].mT);
}

void
RayShaft::GetRaySegment(const int i, Ray &ray) const
{
  Vector3 origin, direction;
  GetRay(i, origin, direction);
  ray.Init(origin, direction, Ray::LINE_SEGMENT);
  if ( mSamples[i].IsValid() ) {
    origin = ray.Extrap(mSamples[i].mMinT);
    direction = ray.Extrap(mSamples[i].mMaxT) - origin;
    ray.Init(origin, direction, Ray::LINE_SEGMENT);
  }
}

void
RayShaft::GetRay(const int rayIndex,
		 Vector3 &origin,
		 Vector3 &direction) const
{
  
  assert(rayIndex < 4);
  
  origin = mSource.mVertices[rayIndex];
  direction = mTarget.mVertices[rayIndex] - origin;
}

void
MutualVisibilitySampler::PerformSplit(
				      const RayShaft &sample,
				      const bool splitSource,
				      const int axis,
				      RayShaft &sample1,
				      RayShaft &sample2
				      )
{
  

  // split the triangles

  if (splitSource) {
    sample1.mTarget = sample.mTarget;
    sample2.mTarget = sample.mTarget;
    sample.mSource.Split(
			 axis,
			 sample1.mSource,
			 sample2.mSource);
  } else {
    
    sample1.mSource = sample.mSource;
    sample2.mSource = sample.mSource;
    
    sample.mTarget.Split(
			 axis,
			 sample1.mTarget,
			 sample2.mTarget);
  }
  
  // split the intersections
  switch (axis) {
  case 0:
    sample1.mSamples[0].mIntersections = sample.mSamples[0].mIntersections;
    sample1.mSamples[3].mIntersections = sample.mSamples[3].mIntersections;
    
    sample2.mSamples[1].mIntersections = sample.mSamples[1].mIntersections;
    sample2.mSamples[2].mIntersections = sample.mSamples[2].mIntersections;
    break;

  case 1:
    sample1.mSamples[0].mIntersections = sample.mSamples[0].mIntersections;
    sample1.mSamples[1].mIntersections = sample.mSamples[1].mIntersections;

    sample2.mSamples[2].mIntersections = sample.mSamples[2].mIntersections;
    sample2.mSamples[3].mIntersections = sample.mSamples[3].mIntersections;
    break;
  }

  // the intersections for the new shaft rays will be established
  // later
  sample1.mDepth = sample2.mDepth = sample.mDepth+1;
}

float
MutualVisibilitySampler::GetSpatialAngle(const RayShaft &shaft,
					 const Vector3 &point
					 )
{
  const int sampleIndices[]={
    0,1,2,
    0,2,3
  };
  
  float sum = 0.0f;
  int i, j=0;

  if (!shaft.IsValid())
    return 2.0f*mSolidAngleThreshold;

  for (i=0; i < 2; i++) {
    Triangle3 triangle(shaft.GetIntersectionPoint(sampleIndices[j++], 0),
		       shaft.GetIntersectionPoint(sampleIndices[j++], 0),
		       shaft.GetIntersectionPoint(sampleIndices[j++], 0));
    sum += triangle.GetSpatialAngle(point);
  }

  return sum;
}


void
MutualVisibilitySampler::ComputeError(RayShaft &shaft)
{
  // evaluate minimal error which can be achieved by more precise evaluation
  // if this is above the threshold do not proceed further

  // check whether the samples pierce the same mesh
  Intersectable::NewMail();
  int i, j;
  for (i=0; i < 4; i++) {
    RaySample *sample = &shaft.mSamples[i];
    for (j=0; j < sample->mIntersections.size(); j++)
      sample->mIntersections[j].mObject->Mail();
  }

  for (i=0; i < 4; i++) {
    RaySample *sample = &shaft.mSamples[i];
    for (j=0; j < sample->mIntersections.size(); j++) {
      if (sample->mIntersections[j].mObject->IncMail() == 4) {
	cerr<<"T";
	shaft.mError = 0.0f;
	return;
      }
    }
  }
    
  
  float maxAngle = GetSpatialAngle(shaft,
				   shaft.mTarget.GetCenter());
  
  for (i=0; i < 3; i++) {
    float angle = GetSpatialAngle(shaft,
				  shaft.mTarget.mVertices[i]);
    if (angle > maxAngle)
      maxAngle = angle;
  }

  maxAngle = MAX_FLOAT;
  shaft.mError = maxAngle;
}


void
MutualVisibilitySampler::ConstructInitialSamples2(
						  const AxisAlignedBox3 &source,
						  const AxisAlignedBox3 &target,
						  vector<RayShaft *> &samples
						  )
{
  // get all rectangles potentially visible from the source box
  int i;
  int sourceMask = 0;
  for (i=0; i < 8; i++)
    sourceMask |= source.GetFaceVisibilityMask(target.GetVertex(i));

  // now for each visble source face find all visible target faces
  for (i=0; i < 6; i++) {
    Rectangle3 sourceFace = source.GetFace(i);
    if ( sourceMask &(1<<i) ) {
      int mask = target.GetFaceVisibilityMask(sourceFace);
      // construct triangle samples for all visible rectangles
      for (int j=0; j < 6; j++)
	if (mask & (1<<j)) {
	  AddInitialSamples2(sourceFace, target.GetFace(j), samples);
	}
    }
  }
}

void
MutualVisibilitySampler::ConstructInitialSamples3(
						  const AxisAlignedBox3 &source,
						  const AxisAlignedBox3 &target,
						  vector<RayShaft *> &samples
						  )
{
  // get all rectangles potentially visible from the source box
  int i;
  int sourceMask = 0;
  for (i=0; i < 8; i++)
    sourceMask |= source.GetFaceVisibilityMask(target.GetVertex(i));

  // now for each visble source face find all visible target faces
  int face;
  for (face=0; face < 6; face++) {
    if ( sourceMask & (1<<face) ) {
      Rectangle3 sourceRect = source.GetFace(face);

      Vector3 targetCenter = target.Center();
      
      Vector3 normal = sourceRect.GetNormal();
      
      Plane3 sourcePlane(normal, sourceRect.GetVertex(0));
      
      Plane3 targetPlane(normal, target.GetVertex(0));

      int i;
      for (i=1; i < 8; i++) {
	Vector3 v = target.GetVertex(i);
	if (targetPlane.Distance(v) < 0)
	  targetPlane = Plane3(normal, v);
      }
  
      Vector3 xBasis = Normalize(sourceRect.GetVertex(1) - sourceRect.GetVertex(0));
      Vector3 yBasis = Normalize(sourceRect.GetVertex(3) - sourceRect.GetVertex(0));

      // cast rays between the centers of the boxes
      Vector3 targetRCenter = targetPlane.FindIntersection(sourceRect.GetCenter(),
							   targetCenter);
      
      Rectangle3 targetRect;
      
      float targetDist[4];
      targetDist[0] = targetDist[1] = targetDist[2] = targetDist[3] = 0.0f;
      
      // cast rays between corresponding vertices of the boxes
      int j;
      int intersections=0;
      for (i=0; i < 4; i++)
	for (j=0; j < 8; j++) {
	  Vector3 v;
	  Vector3 diff;
	  bool coplanar;
	  float dist;
	  
	  v = targetPlane.FindIntersection(sourceRect.GetVertex(i),
					   target.GetVertex(j),
					   NULL,
					   &coplanar);
	  if (!coplanar) {
	    // evaluate target and 
	    diff = targetRCenter - v;
	    dist = DotProd(diff, xBasis);
	    if (dist < targetDist[0])
	      targetDist[0] = dist;
	    if (dist > targetDist[1])
	      targetDist[1] = dist;

	    dist = DotProd(diff, yBasis);

	    if (dist < targetDist[2])
	      targetDist[2] = dist;
	    if (dist > targetDist[3])
	      targetDist[3] = dist;
	    intersections++;
	  }
	}
      
      if (intersections>=4) {
	targetRect.mVertices[0] = targetRCenter + targetDist[0]*xBasis + targetDist[2]*yBasis;
	targetRect.mVertices[1] = targetRCenter + targetDist[1]*xBasis + targetDist[2]*yBasis;
	targetRect.mVertices[2] = targetRCenter + targetDist[1]*xBasis + targetDist[3]*yBasis;
	targetRect.mVertices[3] = targetRCenter + targetDist[0]*xBasis + targetDist[3]*yBasis;
	
	//	cout<<sourceRect<<targetRect<<endl;
	AddInitialSamples(sourceRect, targetRect, samples);
      }
    }
  }
}

void
MutualVisibilitySampler::ConstructInitialSamples(
						 const AxisAlignedBox3 &source,
						 const AxisAlignedBox3 &target,
						 vector<RayShaft *> &samples
						 )
{
  Vector3 sourceCenter = source.Center();
  Vector3 targetCenter = target.Center();
  Vector3 normal = Normalize(sourceCenter - targetCenter );
  
  Plane3 sourcePlane(normal, source.GetVertex(0));
  int i;
  for (i=1; i < 8; i++) {
    Vector3 v = source.GetVertex(i);
    if (sourcePlane.Distance(v) > 0)
      sourcePlane = Plane3(normal, v);
  }
  
  Plane3 targetPlane(normal, target.GetVertex(0));
  for (i=1; i < 8; i++) {
    Vector3 v = target.GetVertex(i);
    if (targetPlane.Distance(v) < 0)
      targetPlane = Plane3(normal, v);
  }

  
  Vector3 xBasis = CrossProd(Vector3(0,1,0), normal);

  if (Magnitude(xBasis) > 1e-6)
    xBasis.Normalize();
  else {
    xBasis = Normalize(CrossProd(Vector3(0,0,1), normal));
  }

  Vector3 yBasis = Normalize( CrossProd(normal, xBasis) );
  // cast rays between the centers of the boxes
  Vector3 targetRCenter = targetPlane.FindIntersection(sourceCenter,
						       targetCenter);
  
  Vector3 sourceRCenter = sourcePlane.FindIntersection(sourceCenter,
						       targetCenter);
  

  Rectangle3 sourceRect;
  Rectangle3 targetRect;


  if (0) {
  float scale = Magnitude(source.Size())*0.7f;
  sourceRect.mVertices[0] = sourceRCenter - scale*xBasis - scale*yBasis;
  sourceRect.mVertices[1] = sourceRCenter + scale*xBasis - scale*yBasis;
  sourceRect.mVertices[2] = sourceRCenter + scale*xBasis + scale*yBasis;
  sourceRect.mVertices[3] = sourceRCenter - scale*xBasis + scale*yBasis;

  scale = Magnitude(target.Size())*0.7f;
  targetRect.mVertices[0] = targetRCenter - scale*xBasis - scale*yBasis;
  targetRect.mVertices[1] = targetRCenter + scale*xBasis - scale*yBasis;
  targetRect.mVertices[2] = targetRCenter + scale*xBasis + scale*yBasis;
  targetRect.mVertices[3] = targetRCenter - scale*xBasis + scale*yBasis;
  }


  float sourceDist[4];
  float targetDist[4];
  sourceDist[0] = sourceDist[1] = sourceDist[2] = sourceDist[3] = 0.0f;
  targetDist[0] = targetDist[1] = targetDist[2] = targetDist[3] = 0.0f;

  // cast rays between corresponding vertices of the boxes
  int j;
  for (i=0; i < 8; i++)
    for (j=0; j < 8; j++) {
      Vector3 v;
      Vector3 diff;
      bool coplanar;
      float dist;
      v = sourcePlane.FindIntersection(source.GetVertex(i),
				       target.GetVertex(j),
				       NULL,
				       &coplanar);
      if (!coplanar) {
	// evaluate source and 
	diff = sourceRCenter - v;
	dist = DotProd(diff, xBasis);
	if (dist < sourceDist[0])
	  sourceDist[0] = dist;
	if (dist > sourceDist[1])
	  sourceDist[1] = dist;

	dist = DotProd(diff, yBasis);
	if (dist < sourceDist[2])
	  sourceDist[2] = dist;
	if (dist > sourceDist[3])
	  sourceDist[3] = dist;
      }
      
      v = targetPlane.FindIntersection(source.GetVertex(i),
				       target.GetVertex(j),
				       NULL,
				       &coplanar);
      if (!coplanar) {
	// evaluate target and 
	diff = targetRCenter - v;
	dist = DotProd(diff, xBasis);
	if (dist < targetDist[0])
	  targetDist[0] = dist;
	if (dist > targetDist[1])
	  targetDist[1] = dist;
	dist = DotProd(diff, yBasis);
	if (dist < targetDist[2])
	  targetDist[2] = dist;
	if (dist > targetDist[3])
	  targetDist[3] = dist;
      }
    }

  sourceRect.mVertices[0] = sourceRCenter + sourceDist[0]*xBasis + sourceDist[2]*yBasis;
  sourceRect.mVertices[1] = sourceRCenter + sourceDist[1]*xBasis + sourceDist[2]*yBasis;
  sourceRect.mVertices[2] = sourceRCenter + sourceDist[1]*xBasis + sourceDist[3]*yBasis;
  sourceRect.mVertices[3] = sourceRCenter + sourceDist[0]*xBasis + sourceDist[3]*yBasis;

  targetRect.mVertices[0] = targetRCenter + targetDist[0]*xBasis + targetDist[2]*yBasis;
  targetRect.mVertices[1] = targetRCenter + targetDist[1]*xBasis + targetDist[2]*yBasis;
  targetRect.mVertices[2] = targetRCenter + targetDist[1]*xBasis + targetDist[3]*yBasis;
  targetRect.mVertices[3] = targetRCenter + targetDist[0]*xBasis + targetDist[3]*yBasis;


  cout<<sourceRect<<targetRect<<endl;
  AddInitialSamples(sourceRect, targetRect, samples);
}

void
MutualVisibilitySampler::AddInitialSamples(
					   const Rectangle3 &sourceRect,
					   const Rectangle3 &targetRect,
					   vector<RayShaft *> &samples
					   )
{

  RayShaft *sample = new RayShaft(sourceRect, 
				  targetRect);
  samples.push_back(sample);
}

void
MutualVisibilitySampler::AddInitialSamples2(
					    const Rectangle3 &sourceRect,
					    const Rectangle3 &targetRect,
					    vector<RayShaft *> &samples
					    )
{
  // align the rectangles properly
  Rectangle3 sRect, tRect;
  if (DotProd(sourceRect.GetNormal(), targetRect.GetNormal()) < -0.99f) {
    int i;
    for (i=0; i < 4; i++) {
      tRect.mVertices[i] = targetRect.mVertices[( 7 - i )%4];
    }
    
    RayShaft *sample = new RayShaft(sourceRect, 
				    tRect);
    samples.push_back(sample);
  }
  
  return;
  
  float minDist = MAX_FLOAT;
  int startI;
  int startJ;
  int i, j;

  
  for (i=0; i < 4; i++) {
    for (j=0; j < 4; j++) {
      float dist = Distance(sourceRect.mVertices[i], targetRect.mVertices[j]);
      if (dist < minDist) {
	minDist = dist;
	startI = i;
	startJ = j;
      }
    }
  }
  for (i=0; i < 4; i++) {
    sRect.mVertices[i] = sourceRect.mVertices[(startI + i )%4];
    tRect.mVertices[i] = targetRect.mVertices[(4 + startJ - i )%4];
  }
  
  RayShaft *sample = new RayShaft(sRect, 
				  tRect);
  samples.push_back(sample);
}


void
MutualVisibilitySampler::ExportShafts(vector<RayShaft *> &shafts,
				      const bool singleFile)
{
  static int id = 0;
  char filename[64];
  if (id > 20)
    return;
  
  Exporter *exporter = NULL;
  for (int i=0; i < shafts.size(); i++) {
    if (!exporter) {
      if (singleFile)
	sprintf(filename, "shafts-single-%04d.x3d", id++);
      else
	sprintf(filename, "shafts%04d-%02d.x3d", id++, i);
      exporter = Exporter::GetExporter(filename);
    }
    
    exporter->SetWireframe();
    //    exporter->ExportScene(mSceneGraph->mRoot);

    exporter->ExportBox(mSource);
    exporter->ExportBox(mTarget);

    exporter->SetFilled();


    RayShaft *shaft = shafts[i];
    Mesh *mesh = new Mesh;
    mesh->AddRectangle(shaft->mSource);
    mesh->AddRectangle(shaft->mTarget);
    vector<Ray *> rays;
    for (int j=0; j < 4; j++) {
      Ray *ray = new Ray;
      shaft->GetRaySegment(j, *ray);
      rays.push_back(ray);
    }
    
    Material m = RandomMaterial();
    exporter->SetForcedMaterial(m);
    MeshInstance mi(mesh);
    exporter->ExportIntersectable(&mi);
    exporter->ExportRays(rays, -1.0f, m.mDiffuseColor);
    if (!singleFile) {
      delete exporter;
      exporter = NULL;
    }
  }
  if (exporter)
    delete exporter;
  
}

int
MutualVisibilitySampler::CastRays(RayShaft &shaft)
{
  Ray ray;
  int i;

  for (i=0; i < 4; i++)
    if (!shaft.mSamples[i].IsProcessed()) {
      Vector3 origin, direction;
      shaft.GetRay(i, origin, direction);
      // determine intersections with the boxes
      ray.Init(origin, direction, Ray::LINE_SEGMENT);
      float stmin, stmax = 0.0f, ttmin=1.0f, ttmax;
      bool valid = true;
      
      if (mUseBoxes) {
	if (mSource.GetMinMaxT(ray, &stmin, &stmax) &&
	    mTarget.GetMinMaxT(ray, &ttmin, &ttmax)) {
	  shaft.mSamples[i].mMinT = stmax;
	  shaft.mSamples[i].mMaxT = ttmin;
	  origin = ray.Extrap(stmax);
	  direction = ray.Extrap(ttmin) - origin;
	  // reinit the ray
	  ray.Init(origin, direction, Ray::LINE_SEGMENT);
	} else
	  valid = false;
      } else {
	shaft.mSamples[i].mMinT = 0.0f;
	shaft.mSamples[i].mMaxT = 1.0f;
      }
      if (valid) {
	if (!mKdTree->CastRay(ray)) {
	  cerr<<"V"<<endl;
	  return VISIBLE;
	}
	shaft.mSamples[i].mIntersections = ray.intersections;
	cerr<<"I";
      } else {
	cerr<<"X";
	shaft.mSamples[i].SetInvalid();
      }
    }
  return INVISIBLE;
}

int
MutualVisibilitySampler::ComputeVisibility()
{
  int result = INVISIBLE;

  vector<RayShaft *> shafts;
  ConstructInitialSamples3(mSource, mTarget, shafts);
	
  if (1)
    ExportShafts(shafts, false);

  stack<RayShaft *> shaftStack;
  int i;
  for (i=0; i < shafts.size(); i++)
    shaftStack.push(shafts[i]);

  shafts.clear();
  Ray ray;
  
// now process the shafts as long as we have something to do
  while (!shaftStack.empty()) {
    RayShaft *shaft = shaftStack.top();
    shaftStack.pop();
    
//      // cast a new ray
//      int triangleSplitEdge = SetupExtremalRay(sample, source, ray);
    
    if (CastRays(*shaft) == VISIBLE) {
      result = VISIBLE;
      break;
    }
		
    // compute error ....
    ComputeError(*shaft);
    cout<<shaft->mDepth<<"|";
    if (shaft->IsValid())
      shafts.push_back(shaft);

    if (shaft->mDepth < 10 &&
				shaft->mError > mSolidAngleThreshold) {
      
      // generate 2 new samples
      RayShaft *newSamples[2];
      newSamples[0] = new RayShaft;
      newSamples[1] = new RayShaft;
      
      // chose what to split
      bool splitSource = shaft->mSource.GetArea() > shaft->mTarget.GetArea();
      int axis;
      if (splitSource) {
				axis = shaft->mSource.DominantAxis();
      } else {
				axis = shaft->mTarget.DominantAxis();
      }
      
      PerformSplit(*shaft, splitSource, axis, *newSamples[0], *newSamples[1]);
      //      delete shaft;
      shaftStack.push(newSamples[0]);
      shaftStack.push(newSamples[1]);
    } else {
      // store a terminal shaft
    }

  }
  
  while (!shaftStack.empty()) {
    RayShaft *shaft = shaftStack.top();
    shaftStack.pop();
    delete shaft;
  }

  if (0)
    ExportShafts(shafts, true);

  for (i=0; i < shafts.size(); i++) {
    delete shafts[i];
  }

  return result;
}

MutualVisibilitySampler::MutualVisibilitySampler(SceneGraph *sceneGraph,
																								 KdTree *kdTree,
																								 const AxisAlignedBox3 &source,
																								 const AxisAlignedBox3 &target,
																								 const float solidAngleThreshold)
{
  mSceneGraph = sceneGraph;
  mKdTree = kdTree;
  mSource = source;
  mTarget = target;
  mUseBoxes = true;
  mSolidAngleThreshold = solidAngleThreshold;
}
  

int
ComputeBoxVisibility(SceneGraph *sceneGraph,
										 KdTree *kdTree,
										 const AxisAlignedBox3 &source,
										 const AxisAlignedBox3 &target,
										 const float solidAngleThreshold)
{
  MutualVisibilitySampler
		sampler(sceneGraph, kdTree, source, target, solidAngleThreshold);

  
  int visibility = sampler.ComputeVisibility();

  return visibility;
  
}
  
}