#include <iostream>
#include "AxisAlignedBox3.h"
#include "SepPlanesBox3.h"

#include <cassert>
using namespace std;

namespace GtpVisibilityPreprocessor {

int
Side(const Plane3 &pl, const AxisAlignedBox3 &box)
{
  // Better implementation than below, takes the
  // problem as computing the extrema of the function
  // Written by V.H. in November 2001.
  float sumMin = pl.mD;
  float sumMax = pl.mD;
  const Vector3 &min = box.Min();
  const Vector3 &max = box.Max();

  // maximize and minimize the value for the function
  // normal * vertex + d
  
  // for x-axis
  if (pl.mNormal.x >= 0.0) {
    sumMax += pl.mNormal.x * max.x;
    sumMin += pl.mNormal.x * min.x;
  }
  else {
    sumMin += pl.mNormal.x * max.x;
    sumMax += pl.mNormal.x * min.x;
  }
  
  // for y-axis
  if (pl.mNormal.y >= 0.0) {
    sumMax += pl.mNormal.y * max.y;
    sumMin += pl.mNormal.y * min.y;
  }
  else {
    sumMin += pl.mNormal.y * max.y;
    sumMax += pl.mNormal.y * min.y;
  }
  
  // for z-axis
  if (pl.mNormal.z >= 0.0) {
    sumMax += pl.mNormal.z * max.z;
    sumMin += pl.mNormal.z * min.z;
  }
  else {
    sumMin += pl.mNormal.z * max.z;
    sumMax += pl.mNormal.z * min.z;
  }
  
  if (sumMax >= 0.0) {
    if (sumMin >= 0.0)
      return 1; // box only in positive halfspace
    return 0; // box is intersected by the plane
  }

  // sumMax < 0.0
  assert(sumMin < 0.0);
  
  return -1; // box only in the negative halfspace
}

  
  // constructor, no planes
CSeparatingAxisTester::CSeparatingAxisTester()
{
  cntPlanes = 0;
}

void
CSeparatingAxisTester::InsertPlane(const Plane3 &pl)
{
  assert(cntPlanes < 16);
  planes[cntPlanes].mNormal = pl.mNormal;
  planes[cntPlanes].mD = pl.mD;
  cntPlanes++;
}

void
CSeparatingAxisTester::OnePhase(const AxisAlignedBox3 &origBox,
				const AxisAlignedBox3 &shadBox,
				const Vector3 &pointBACKSIDE)
{
  Vector3 centerOrig = origBox.Center();
  Vector3 centerShad = shadBox.Center();
  const float eps = 1e-3f;
  int cnt = 0;
  
  // We check first for all edges of the 'origBox' and one
  // vertex of the 'shadBox' existence of the plane.
  for (int i = 0; i < 12; i++) {
    Vector3 a,b;
    // You get one edge from the orig box
    origBox.GetEdge(i, &a, &b);
    // Move the vertices a bit outside the box
    a += eps * (a-centerOrig);
    b += eps * (b-centerOrig);

    // Now you take all the vertices from the 'shadBox',
    // which contains a new position of the moved object
    for (int j = 0; j < 8; j++) {
      Vector3 c;
      shadBox.GetVertex(j, c);
      c += eps * (c-centerShad);
      Plane3 pl(a, b, c);

      cnt++;
      // if (cnt == 17) { cout << "Problem " << endl; }
      
      int centerFlag = pl.Side(pointBACKSIDE, 1e-6f);
      if (centerFlag == Plane3:: INTERSECTS)
	continue; // plane crosses the center of the box!
      
      if (centerFlag == Plane3::FRONT_SIDE) {
	// we have to change the plane orientation
	pl.mNormal = -pl.mNormal;
	pl.mD = -pl.mD;
	centerFlag = Plane3::BACK_SIDE;
	assert(pl.Side(pointBACKSIDE, 1e-6) == Plane3::BACK_SIDE);
      } 

      // Check if the shad box is on one side of the plane
      if ((Side(pl, origBox) == Plane3::FRONT_SIDE) &&
	  (Side(pl, shadBox) == Plane3::BACK_SIDE)) {
	//cout << "Inserting plane normal=" << pl.mNormal
	//     << " mD=" << pl.mD << endl;
	InsertPlane(pl);
      }
    } // for j
  } // for i
}

// brute force separating planes construction algorithm
void
CSeparatingAxisTester::Init(const AxisAlignedBox3 &origBox,
							const AxisAlignedBox3 &shadBox)
{
  // test all edges of the axis aligned box
  Vector3 a,b;
  const float eps = 1e-3f;
  Vector3 centerShad = shadBox.Center();

  cntPlanes = 0;

  // First, we check for all edges of the 'origBox' and one
  // vertex of the 'shadBox' existence of the plane.
  OnePhase(origBox, shadBox, centerShad);

  // Second, we check for all edges of the 'shadBox' and one
  // vertex of the 'origBox' existence of the plane.
  OnePhase(shadBox, origBox, centerShad);
  
  // Finally, we add the separating plane, which touches the
  // 'shadBox', lies between 'origBox' and 'shadBox',
  // 'origBox' lies in the FRONT_SIDE of this plane, and
  // 'shadBox' lies in the BACK_SIDE of this plane.
  
  Vector3 centerOrig = origBox.Center();
  Vector3 normal = Normalize(centerOrig - centerShad);
  // Now we check all the vertices of the shadBox and find the
  // one closest to the box
  bool f = false;
  for (int i = 0; i < 8; i++) {
    Vector3 c;
    shadBox.GetVertex(i, c);
    c += eps * (c-centerShad);
    Plane3 pl(normal, c);
    if (Side(pl, shadBox) == -1) {
      assert(pl.Side(centerShad, 1e-6) == Plane3::BACK_SIDE);
      //assert(pl.Side(centerOrig, 1e-6) == Plane3::FRONT_SIDE);
      InsertPlane(pl);
      f = true;
      break;
    }
  } // for all vertices

  if (!f) {
    cout << "Problem finding last separating plane" << endl;
    abort();
  }
  
  return;
}
  
// This function returns true if a ray starting
// in the 'origBox' and intersecting 'shadBox'
// can intersect the box as input of this function
bool
CSeparatingAxisTester::TestIsInsideShaft(const AxisAlignedBox3 &testBox)
{
  for (int i = 0; i < cntPlanes; i++) {
    if (Side(planes[i], testBox) == Plane3::FRONT_SIDE)
      // there is such a plane which separates 'origBox' and 'shadBox'
      // that no ray can intersect all three boxes, starting in 'origBox'
      return false; 
  }

  // there is a ray intersecting 'origBox', 'shadBox', and 'testBox'
  // at the same time
  return true; 
}
  
// Testing code
void
TestSepAxis()
{
  CSeparatingAxisTester st;
#if 0
  Vector3 vo1(0, 0, 0);
  Vector3 vo2(1, 1, 1);

  Vector3 vs1(10, -1, -1);
  Vector3 vs2(11, 1, 1);
#else
  // JB test
  Vector3 vo1(1279.08, 211.942, -76.4611);
  Vector3 vo2(1395.34, 215.833, 37.5);
  Vector3 vs1(-2446.14, 1381.31, -2621.98);
  Vector3 vs2(-2426.14, 1391.57, -2608.9);
#endif
  
  st.Init(AxisAlignedBox3(vo1, vo2),
	  AxisAlignedBox3(vs1, vs2));

  // Box that should return true
  Vector3 vt1(10, -1, 1);
  Vector3 vt2(11, -1, 1);
  int res = st.TestIsInsideShaft(AxisAlignedBox3(vt1, vt2));
  cout << "Res1 = " << res << endl;

  // Box that should return true
  vt1.SetValue(11, 1, 1);
  vt2.SetValue(12, 2, 2);
  res = st.TestIsInsideShaft(AxisAlignedBox3(vt1, vt2));
  cout << "Res2 = " << res << endl;
  
  // Box that should return false
  vt1.SetValue(-1, -1, -1);
  vt2.SetValue(1, 1, 1);
  res = st.TestIsInsideShaft(AxisAlignedBox3(vt1, vt2));
  cout << "Res3 = " << res << endl;

  // Box that should also return false
  vt1.SetValue(12, 12, 12);
  vt2.SetValue(13, 13, 13);
  res = st.TestIsInsideShaft(AxisAlignedBox3(vt1, vt2));
  cout << "Res4 = " << res << endl;
}

}