#include <math.h>
#include "GR_misc.h"
#include "GR_Geometry.h"
#include "GR_Vertex.h"
#include "GR_Face.h"
#include "GR_Cell.h"
#include "GR_AdaptPred.h"
#include "GR_Vec.h"

#define MINMAX4(da, db, dc, dd, dMin, dMax) { \
  double dMax1, dMax2, dMin1, dMin2; \
  if (da > db) {dMax1 = da; dMin1 = db;} \
  else         {dMax1 = db; dMin1 = da;} \
  if (dc > dd) {dMax2 = dc; dMin2 = dd;} \
  else         {dMax2 = dd; dMin2 = dc;} \
  if (dMax1 > dMax2) dMax = dMax1; \
  else               dMax = dMax2; \
  if (dMin1 < dMin2) dMin = dMin1; \
  else               dMin = dMin2; \
}

void vNormal3D(const double adA[3], const double adB[3],
	       const double adC[3], double * const pdRes)
{
  assert(pdRes != NULL);
  double adDiffBA[3] = adDIFF3D(adB, adA);
  double adDiffCB[3] = adDIFF3D(adC, adB);
  vCROSS3D(adDiffCB, adDiffBA, pdRes);
}

//@ 3D orientation test
int iOrient3D(const Vert* const pVertA, const Vert* const pVertB,
	      const Vert* const pVertC, const Vert* const pVertD)
{
  assert(pVertA->qValid());
  assert(pVertB->qValid());
  assert(pVertC->qValid());
  assert(pVertD->qValid());

  return (iOrient3D(pVertA->adCoords(), pVertB->adCoords(),
		    pVertC->adCoords(), pVertD->adCoords()));
}

int iOrient3D(const Face* const pF, const double adA[3])
{
  assert(pF->qValid());
  return (iOrient3D(pF->pVVert(0)->adCoords(),
		    pF->pVVert(1)->adCoords(),
		    pF->pVVert(2)->adCoords(),
		    adA));
}

int iOrient3D(const Face* const pF, const Vert * const pV)
{
  assert(pF->qValid());
  return (iOrient3D(pF->pVVert(0)->adCoords(),
		    pF->pVVert(1)->adCoords(),
		    pF->pVVert(2)->adCoords(),
		    pV->adCoords()));
}

//@@ 3D orientation predicate, with adaptive precision arithmetic
int iOrient3D(const double adA[3], const double adB[3],
	      const double adC[3], const double adD[3]) 
     // Computes the orientation of four verts in 3D, using as nearly
     // exact arithmetic as required.
{
  if (qAdaptPred) {
    double dResult = orient3d_shew(adA, adB, adC, adD);
    if (dResult > 0)
      return (-1);
    else if (dResult < 0) 
      return (1);
    else 
      return 0;
  }
  else {
    double dXa = adA[0];
    double dYa = adA[1];
    double dZa = adA[2];
    
    double dXb = adB[0];
    double dYb = adB[1];
    double dZb = adB[2];

    double dXc = adC[0];
    double dYc = adC[1];
    double dZc = adC[2];

    double dXd = adD[0];
    double dYd = adD[1];
    double dZd = adD[2];

    double dMaxX, dMinX, dMaxY, dMinY, dMaxZ, dMinZ;

    double dDX2 = dXb - dXa;
    double dDX3 = dXc - dXa;
    double dDX4 = dXd - dXa;
    MINMAX4(dXa, dXb, dXc, dXd, dMinX, dMaxX);

    double dDY2 = dYb - dYa;
    double dDY3 = dYc - dYa;
    double dDY4 = dYd - dYa;
    MINMAX4(dYa, dYb, dYc, dYd, dMinY, dMaxY);

    double dDZ2 = dZb - dZa;
    double dDZ3 = dZc - dZa;
    double dDZ4 = dZd - dZa;
    MINMAX4(dZa, dZb, dZc, dZd, dMinZ, dMaxZ);
    double dMax = max( max(dMaxX-dMinX, dMaxY-dMinY), dMaxZ-dMinZ);

    // dDet is proportional to the cell volume
    double dDet = (dDX2*(dDY3*dDZ4 - dDY4*dDZ3) +
		   dDX3*(dDY4*dDZ2 - dDY2*dDZ4) +
		   dDX4*(dDY2*dDZ3 - dDY3*dDZ2));

    //   // Compute a length scale based on edge lengths.
    //   double dScale = ( dDIST3D(adA, adB) + dDIST3D(adA, adC) +
    // 		    dDIST3D(adA, adD) + dDIST3D(adB, adC) +
    // 		    dDIST3D(adB, adD) + dDIST3D(adC, adD) ) / 6.;

    //   dDet /= (dScale*dScale*dScale);

//   double dError = 1.e-13;

  // Compute an upper bound on the error bound.

    const double dMachEps = 2.22044605e-13; // about 2^-52 * 1000;

    double dError = dMachEps * dMax*dMax*dMax;

    if (dDet > dError)
      return (1);
    else if (dDet < -dError) 
      return (-1);

  // If neither of those two worked, compute a more accurate error bound.
  // The stuff in parentheses is the result of perturbing each term in
  // the determinant by tweaking each multiplicand by the same amount.
  // That amount is a multiple of machine zero.
//   dError = dMachEps *
//     (fabs(dDX2*dDY3) + fabs(dDX2*dDZ4) + fabs(dDY3*dDZ4) +
//      fabs(dDX3*dDY4) + fabs(dDX3*dDZ2) + fabs(dDY4*dDZ2) +
//      fabs(dDX4*dDY2) + fabs(dDX4*dDZ3) + fabs(dDY2*dDZ3) -
//      fabs(dDZ2*dDY3) + fabs(dDZ2*dDX4) + fabs(dDY3*dDX4) +
//      fabs(dDZ3*dDY4) + fabs(dDZ3*dDX2) + fabs(dDY4*dDX2) +
//      fabs(dDZ4*dDY2) + fabs(dDZ4*dDX3) + fabs(dDY2*dDX3));
//   if (dDet > dError)
//     return (1);
//   else if (dDet < -dError) 
//     return (-1);
//   else
    return(0);
  }
}

//@@ Insphere primitive with adaptive arithmetic
int iInsphere(const Vert* const pVertA, const Vert* const pVertB,
	      const Vert* const pVertC, const Vert* const pVertD,
	      const Vert* const pVertE)
     // Determines whether pVertE is inside or outside the circumsphere
     // of the tet formed by pVertA-D, using as nearly exact arithmetic
     // as required.  Returns 1 if pVertE is inside the circumsphere of
     // the tet, -1 if outside, 0 if on.
{
  assert(pVertA->qValid());
  assert(pVertB->qValid());
  assert(pVertC->qValid());
  assert(pVertD->qValid());
  assert(pVertE->qValid());

  return iInsphere(pVertA->adCoords(), pVertB->adCoords(),
		   pVertC->adCoords(), pVertD->adCoords(),  
		   pVertE->adCoords());
}

int iInsphere(const double adA[3], const double adB[3],
	      const double adC[3], const double adD[3],
	      const double adE[3])
{    
  if (qAdaptPred) {
    double dResult = insphere_shew(adA, adB, adC, adD, adE)
      * orient3d_shew(adA, adB, adC, adD);
    if (dResult > 0)
      return 1;
    else if (dResult < 0)
      return -1;
    else
      return 0;
  }
  else {
    double dXa = adA[0];
    double dYa = adA[1];
    double dZa = adA[2];

    double dXb = adB[0];
    double dYb = adB[1];
    double dZb = adB[2];

    double dXc = adC[0];
    double dYc = adC[1];
    double dZc = adC[2];

    double dXd = adD[0];
    double dYd = adD[1];
    double dZd = adD[2];

    double dXe = adE[0];
    double dYe = adE[1];
    double dZe = adE[2];

    double a2dInSphMat[4][4], dWa;
    dWa = dXa*dXa + dYa*dYa + dZa*dZa;

    a2dInSphMat[0][0] = dXb - dXa;
    a2dInSphMat[0][1] = dYb - dYa;
    a2dInSphMat[0][2] = dZb - dZa;
    a2dInSphMat[0][3] = dXb*dXb + dYb*dYb + dZb*dZb - dWa;

    a2dInSphMat[1][0] = dXc - dXa;
    a2dInSphMat[1][1] = dYc - dYa;
    a2dInSphMat[1][2] = dZc - dZa;
    a2dInSphMat[1][3] = dXc*dXc + dYc*dYc + dZc*dZc - dWa;

    a2dInSphMat[2][0] = dXd - dXa;
    a2dInSphMat[2][1] = dYd - dYa;
    a2dInSphMat[2][2] = dZd - dZa;
    a2dInSphMat[2][3] = dXd*dXd + dYd*dYd + dZd*dZd - dWa;

    a2dInSphMat[3][0] = dXe - dXa;
    a2dInSphMat[3][1] = dYe - dYa;
    a2dInSphMat[3][2] = dZe - dZa;
    a2dInSphMat[3][3] = dXe*dXe + dYe*dYe + dZe*dZe - dWa;

    // Set up a scale that is the average of the distance from A to each
    // of the other verts.  Use some trickery to take advantage of
    // already knowing what the differences in coordinates are.
    double dAveLen = 0.25 * (dMAG3D(a2dInSphMat[0]) +
			     dMAG3D(a2dInSphMat[1]) +
			     dMAG3D(a2dInSphMat[2]) +
			     dMAG3D(a2dInSphMat[3]));
    double dScale = pow(dAveLen, 5);
    double dDet = dDet4By4(a2dInSphMat) / dScale *
      iOrient3D(adA, adB, adC, adD);
    double dEps = 1.e-12;
    if      (dDet >  dEps) return (-1);
    else if (dDet < -dEps) return ( 1);
    else                   return ( 0);
  }
}


//@ Determine whether a vertex lies inside, on or outside a tet
int iIsInTet(const Vert* const pV,     const Vert* const pVertA,
	     const Vert* const pVertB,	const Vert* const pVertC,
	     const Vert* const pVertD)
     // For pV to be inside the tet formed by pVertA-D, the orientation
     // has to be right wrt every face.  The return value is -1 if the
     // vert lies outside the tet, 0 if it's coplanar with all four
     // faces (impossible for valid tets), 1 if it coincides with a vert
     // in the tet, 2 if it lies on an edge, 3 if it lies on a face, and
     // a 4 if it lies strictly inside.
{
  assert(pV->qValid()     && pV->iSpaceDimen() == 3);
  assert(pVertA->qValid() && pVertA->iSpaceDimen() == 3);
  assert(pVertB->qValid() && pVertB->iSpaceDimen() == 3);
  assert(pVertC->qValid() && pVertC->iSpaceDimen() == 3);
  assert(pVertD->qValid() && pVertD->iSpaceDimen() == 3);
  int iRetVal = 0;
  int iCheck = iOrient3D(pVertB, pVertD, pVertC, pV);
  if (iCheck == -1) return -1;
  iRetVal += iCheck;
  
  iCheck = iOrient3D(pVertC, pVertD, pVertA, pV);
  if (iCheck == -1) return -1;
  iRetVal += iCheck;
  
  iCheck = iOrient3D(pVertA, pVertD, pVertB, pV);
  if (iCheck == -1) return -1;
  iRetVal += iCheck;

  iCheck = iOrient3D(pVertA, pVertB, pVertC, pV);
  if (iCheck == -1) return -1;
  iRetVal += iCheck;

  return (iRetVal);
}

//@ Determine whether a vertex lies inside, on or outside a tet
int iCoplanarInTet(const Vert* const pV,     const Vert* const pVertA,
		   const Vert* const pVertB, const Vert* const pVertC,
		   const Vert* const pVertD)
     // For pV to be inside the tet formed by pVertA-D, the orientation
     // has to be right wrt every face.  The return value is -1 if the
     // vert lies outside the tet, 0 if it's coplanar with all four
     // faces (impossible for valid tets), 1 if it coincides with a vert
     // in the tet, 2 if it lies on an edge, 3 if it lies on a face, and
     // a 4 if it lies strictly inside.
{
  assert(pV->qValid()     && pV->iSpaceDimen() == 3);
  assert(pVertA->qValid() && pVertA->iSpaceDimen() == 3);
  assert(pVertB->qValid() && pVertB->iSpaceDimen() == 3);
  assert(pVertC->qValid() && pVertC->iSpaceDimen() == 3);
  assert(pVertD->qValid() && pVertD->iSpaceDimen() == 3);

  // Find a normal and two vectors in the plane.
  double adNorm[3];
  vNormal(pVertA->adCoords(), pVertB->adCoords(),
	  pVertC->adCoords(), adNorm);
  vNORMALIZE3D(adNorm);

  double adBasisX[] = {pVertA->dX() - pVertB->dX(),
		       pVertA->dY() - pVertB->dY(),
		       pVertA->dZ() - pVertB->dZ()};
  vNORMALIZE3D(adBasisX);

  double adBasisY[3];
  vCROSS3D(adNorm, adBasisX, adBasisY);

  assert(iFuzzyComp(dMAG3D(adBasisY), 1.) == 0);
  assert(qFuzzyPerp3D(adBasisX, adBasisY));
  assert(qFuzzyPerp3D(adNorm  , adBasisY));
  assert(qFuzzyPerp3D(adBasisX, adNorm  ));

  // Project onto a plane
  Vert VProjA, VProjB, VProjC, VProjD, VProj;
  double adTemp[2];

  adTemp[0] = dDOT3D(pVertA->adCoords(), adBasisX);
  adTemp[1] = dDOT3D(pVertA->adCoords(), adBasisY);
  VProjA.vSetCoords( 2, adTemp );

  adTemp[0] = dDOT3D(pVertB->adCoords(), adBasisX);
  adTemp[1] = dDOT3D(pVertB->adCoords(), adBasisY);
  VProjB.vSetCoords( 2, adTemp );

  adTemp[0] = dDOT3D(pVertC->adCoords(), adBasisX);
  adTemp[1] = dDOT3D(pVertC->adCoords(), adBasisY);
  VProjC.vSetCoords( 2, adTemp );

  adTemp[0] = dDOT3D(pVertD->adCoords(), adBasisX);
  adTemp[1] = dDOT3D(pVertD->adCoords(), adBasisY);
  VProjD.vSetCoords( 2, adTemp );

  adTemp[0] = dDOT3D(pV ->adCoords(), adBasisX);
  adTemp[1] = dDOT3D(pV ->adCoords(), adBasisY);
  VProj .vSetCoords( 2, adTemp );

  // Check to see if pV lies inside any of the triangles implied by
  // verts A-D
  int iCheck = iIsInTri(&VProjA, &VProjB, &VProjC, &VProj);
  if (iCheck != -1) return iCheck;

  iCheck = iIsInTri(&VProjA, &VProjD, &VProjB, &VProj);
  if (iCheck != -1) return iCheck;

  iCheck = iIsInTri(&VProjB, &VProjD, &VProjC, &VProj);
  if (iCheck != -1) return iCheck;

  iCheck = iIsInTri(&VProjC, &VProjD, &VProjA, &VProj);
  if (iCheck != -1) return iCheck;

  return -1;
}

int iIsReflex(const EdgeFace* const pF)
{
  assert(pF->eType() == Face::eEdgeFace);
  assert(pF->pCCellLeft()->eType()  == Cell::eTriCell);
  assert(pF->pCCellRight()->eType() == Cell::eTriCell);
  TriCell* pTC0 = dynamic_cast<TriCell*>(pF->pCCellLeft ());
  TriCell* pTC1 = dynamic_cast<TriCell*>(pF->pCCellRight());
  const Vert* pVA = pF->pVVert(0);
  const Vert* pVB = pF->pVVert(1);
  const Vert* pVC = pTC0->pVVertOpposite(pF);
  const Vert* pVD = pTC1->pVVertOpposite(pF);
  return (- iOrient3D(pVA, pVB, pVC, pVD));
}

//@ End of file
