#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"

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;
  GetRay(rayIndex, origin, direction);
  return origin + direction*mSamples[rayIndex].mIntersections[depth].mT;
}

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 &sample,
					 const Vector3 &point
					 )
{
  const int sampleIndices[][2]={
    (0,1,2),
    (0,2,3)
  };
  
  float sum = 0.0f;
  int i;
  
  for (i=0; i < 2; i++) {
    Triangle3 triangle(sample.GetIntersectionPoint(sampleIndices[i][0], 0),
		       sample.GetIntersectionPoint(sampleIndices[i][1], 0),
		       sample.GetIntersectionPoint(sampleIndices[i][2], 0));
    sum += triangle.GetSpatialAngle(point);
  }

  return sum;
}


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

  float maxAngle = GetSpatialAngle(sample,
				   sample.mSource.GetCenter());
  
  for (int i=0; i < 3; i++) {
    float angle = GetSpatialAngle(sample,
				  sample.mSource.mVertices[i]);
    if (angle > maxAngle)
      maxAngle = angle;
  }
  
  sample.mError = maxAngle;
}


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 = 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;

  
  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;

  AddInitialSamples(sourceRect, targetRect, samples);

  //    Plane3 bottomPlane(xBasis, source.Center());
  //    Plane3 sidePlane(yBasis, source.Center());
  
  //    // cast rays between corresponding vertices of the boxes
  //    for (i=0; i < 8; i++) {
  //      Vector3 v;
  //      bool coplanar;
  //      v = sourcePlane.FindIntersection(source.GetVertex(i),
  //  				     target.GetVertex(i),
  //  				     NULL,
  //  				     &coplanar);
  //      if (!coplanar) {
  //        // evaluate source and 
  
  //      }
  //    }
  
  //  AddInitialSamples(sourceRectangle, targetRectangle, 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::ExportSamples(vector<RayShaft *> &samples)
{
  static int id = 0;
  char filename[64];
  if (id > 20)
    return;
			    
  for (int i=0; i < samples.size(); i++) {
    sprintf(filename, "samples%04d-%02d.x3d", id++, i);
    Exporter *exporter = Exporter::GetExporter(filename);

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

    exporter->SetFilled();


    RayShaft *sample = samples[i];
    Mesh *mesh = new Mesh;
    mesh->AddRectangle(sample->mSource);
    mesh->AddRectangle(sample->mTarget);
    vector<Ray> rays;
    for (int j=0; j < 4; j++) {
      Vector3 origin, direction;
      sample->GetRay(j, origin, direction);
      Ray ray(origin, direction, Ray::LINE_SEGMENT);
      rays.push_back(ray);
    }
    
    Material m = RandomMaterial();
    exporter->SetForcedMaterial(m);
    MeshInstance mi(mesh);
    exporter->ExportIntersectable(&mi);
    exporter->ExportRays(rays, -1.0f, m.mDiffuseColor);
    delete exporter;
  }
}

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

  for (i=0; i < 4; i++) {
    Vector3 origin, direction;
    shaft.GetRay(i, origin, direction);
    // determine intersections with the boxes
    ray.Init(origin, direction, Ray::LINE_SEGMENT);
    float stmin, stmax, ttmin, ttmax;
    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);
      if (!mKdTree->CastRay(ray))
	return VISIBLE;
    } else {
      shaft.mSamples[i].mMaxT = -1.0;
    }
  }
  return INVISIBLE;
}

int
MutualVisibilitySampler::ComputeVisibility()
{

  vector<RayShaft *> shafts;
  ConstructInitialSamples(mSource, mTarget, shafts);
  ExportSamples(shafts);

  stack<RayShaft *> shaftStack;
  
  
  
  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)
      return VISIBLE;
    
    //      // generate 2 new samples
    //      RayShaft newSamples[2];
    //      sample.Split(triangleSplitEdge, ray, newSamples[0], newSamples[1]);
    //      for (i=0; i < 2; i++) {
    //        newSamples[i].ComputeError();
    //        if (newSamples[i].mError > solidAngleThreshold) {
    //  	sampleStack.push(newSamples[i]);
    //        }
    //      }
    //    }
  }

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

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

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

  
  int visibility = sampler.ComputeVisibility();

  return visibility;
  
}