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

const Vert *TriCell::
pVVert (const int i)
  const {
  assert (iFullCheck ());
  assert2 (i >= 0 && i <= 2, "Vertex index out of range");
  // New implementation aimed at speeding up this routine, since it's a
  // surprisingly large fraction of CPU time for ITAPS-based swapping ---
  // like around 10% of the total before the switch, shockingly
  // (probably even bigger without Babel overhead, so GRUMMP-native
  // swapping could be spend an even bigger fraction of time here).
  if (i == 2) {
    Vert *pV0 = ppFFaces[0]->pVVert(0);
    Vert *pV1 = ppFFaces[0]->pVVert(1);
    Vert *pV2 = ppFFaces[1]->pVVert(0);
    if (pV2 == pV0 || pV2 == pV1)
      return ppFFaces[1]->pVVert(1);
    else
      return pV2;
  }
  else {
    return ((ppFFaces[0]->pCCellLeft() == static_cast<const Cell*>(this)) ?
	    ppFFaces[0]->pVVert(i) : ppFFaces[0]->pVVert(1-i));
  }
  // The following is the original code, retained because it's much
  // clearer in intent than the modified version.
//   const Face *const pFBase = ppFFaces[0];
//   assert (pFBase != NULL);
//   const Vert *pVRetVal;
//   switch (i)
//     {
//     case 0:
//       if (pFBase->pCCellLeft () == this)
// 	pVRetVal = (pFBase->pVVert (0));
//       else
// 	pVRetVal = (pFBase->pVVert (1));
//       break;
//     case 1:
//       if (pFBase->pCCellLeft () == this)
// 	pVRetVal = (pFBase->pVVert (1));
//       else
// 	pVRetVal = (pFBase->pVVert (0));
//       break;
//     case 2:
//       pVRetVal = (pVVertOpposite (pFBase));
//       break;
//     default:
//       assert2 (0, "Invalid vertex index");
//       pVRetVal = (pVInvalidVert);
//       break;
//     }
//   return (pVRetVal);
}

void TriCell::vAllVertHandles(GRUMMP_Entity* aHandles[]) const
{
  assert (iFullCheck ());
  // New implementation aimed at speeding up this routine, since it's a
  // surprisingly large fraction of CPU time for ITAPS-based swapping ---
  // like around 10% of the total before the switch, shockingly
  // (probably even bigger without Babel overhead, so GRUMMP-native
  // swapping could be spending an even bigger fraction of time here).
  Vert *pV0 = ppFFaces[0]->pVVert(0);
  Vert *pV1 = ppFFaces[0]->pVVert(1);
  Vert *pV2 = ppFFaces[1]->pVVert(0);
  if (ppFFaces[0]->pCCellLeft() == static_cast<const Cell*>(this)) {
    aHandles[0] = pV0;
    aHandles[1] = pV1;
  }
  else {
    aHandles[0] = pV1;
    aHandles[1] = pV0;
  }
  if (pV2 == pV0 || pV2 == pV1)
    aHandles[2] = ppFFaces[1]->pVVert(1);
  else
    aHandles[2] = pV2;
}

double TriCell::
dSize ()
  const {
  assert (iFullCheck ());
  double adTemp[3];
  vVecSize (adTemp);
  if (ppFFaces[0]->pVVert (0)->iSpaceDimen () == 2)
    return (fabs (*adTemp));
  else
    return (dMAG3D (adTemp));
}

bool TriCell::
qIsClosed ()
  const {
  // Check to be sure that the tet has only four vertices and that each
  // appears in exactly three faces.
  const Vert *apV[] =
  {pVInvalidVert, pVInvalidVert, pVInvalidVert};
  // If the cell is deleted, it doesn't matter whether it's closed or
  // not. 
  if (qDeleted ())
    return true;
  int iFaceCount[3];
  for (int iF = 0; iF < 3; iF++)
    {
      const Face *pF = pFFace (iF);
      assert (pF->qValid () && !pF->qDeleted ());
      for (int iV = 0; iV < 2; iV++) {
	const Vert *pV = pF->pVVert (iV);
	assert (pV->qValid () && !pV->qDeleted ());
	int ii;
	for (ii = 0; ii < 3; ii++) {
	  if (apV[ii] == pVInvalidVert) {
	    apV[ii] = pV;
	    iFaceCount[ii] = 1;
	    break;
	  }
	  else if (apV[ii] == pV) {
	    iFaceCount[ii]++;
	    break;
	  }
	}
	if (ii == 3)
	  return (false);
      }				// Done with this vertex
      
    }				// Done with this face
  
  if (iFaceCount[0] != 2 || iFaceCount[1] != 2 ||
      iFaceCount[2] != 2)
    return (false);
  return (true);
}

void TriCell::
vAllDihed (double adDihed[], int *const piNDihed)
  const {
  assert (adDihed != NULL);
  assert (piNDihed != NULL);
  *piNDihed = 3;
  const double * const adCoord0 = pVVert(0)->adCoords();
  const double * const adCoord1 = pVVert(1)->adCoords();
  const double * const adCoord2 = pVVert(2)->adCoords();

  double adNorm0[] = {+adCoord1[1] - adCoord0[1],
		      -adCoord1[0] + adCoord0[0]};
  double adNorm1[] = {+adCoord2[1] - adCoord1[1],
		      -adCoord2[0] + adCoord1[0]};
  double adNorm2[] = {+adCoord0[1] - adCoord2[1],
		      -adCoord0[0] + adCoord2[0]};
  
  vNORMALIZE2D (adNorm0);
  vNORMALIZE2D (adNorm1);
  vNORMALIZE2D (adNorm2);
  
  double dArg0 = -dDOT2D (adNorm0, adNorm2);
  double dArg1 = -dDOT2D (adNorm0, adNorm1);
  double dArg2 = -dDOT2D (adNorm1, adNorm2);

  double dScale = 180. / M_PI;
  adDihed[0] = GR_acos (dArg0) * dScale; // angle @ 0
  adDihed[1] = GR_acos (dArg1) * dScale; // angle @ 1
  adDihed[2] = GR_acos (dArg2) * dScale; // angle @ 2
}

void TriCell::vAllSolid (double adSolid[], int *const piNSolid) const {
  vAllDihed (adSolid, piNSolid);
}

void TriCell::vCircumcenter (double adRes[]) const {
  const double *const adA = pVVert (0)->adCoords ();
  const double *const adB = pVVert (1)->adCoords ();
  const double *const adC = pVVert (2)->adCoords ();

  double adRow0[] = {adA[0] - adB[0],
		     adA[1] - adB[1]};
  double adRow1[] = {adA[0] - adC[0],
		     adA[1] - adC[1]};

  double dRHS0 = 0.5 * (dDOT2D (adA, adA) - dDOT2D (adB, adB));
  double dRHS1 = 0.5 * (dDOT2D (adA, adA) - dDOT2D (adC, adC));

  double dDet = adRow0[0] * adRow1[1] - adRow1[0] * adRow0[1];
  double dDet0 = dRHS0 * adRow1[1] - dRHS1 * adRow0[1];
  double dDet1 = dRHS1 * adRow0[0] - dRHS0 * adRow1[0];

  adRes[0] = dDet0 / dDet;
  adRes[1] = dDet1 / dDet;

  // The following is commented out because the code is known to work
  // properly, but on some machine/OS combinations, the assertions can
  // get triggered.
//  #ifndef NDEBUG
//    double dComp = dDIST2D (adRes, adA);
//    double dTest = dDIST2D (adRes, adB);
//    assert (0 == iFuzzyComp (dComp, dTest));
//    dTest = dDIST2D (adRes, adC);
//    assert (0 == iFuzzyComp (dComp, dTest));
//  #endif
}

double TriCell::dCircumradius () const
{
  // 14-06-99 - Charles Boivin
  // Modified this to use only 2 dimensions
  // since it's in TriCell.
  assert (qValid ());
  if (iOrient2D(pVVert(0), pVVert(1), pVVert(2)) == 0) {
    // Tri is totally flat.
    return LARGE_DBL;
  }
  else {
    double adTemp[2];
    vCircumcenter (adTemp);
    const double * const adCoord0 = pVVert(0)->adCoords();
    return dDIST2D(adTemp, adCoord0);
  }
}

bool TriCell::qCircumscribes(const double adPoint[]) const
{
  return (1 == iIncircle(pVVert(0)->adCoords(), pVVert(1)->adCoords(),
			 pVVert(2)->adCoords(), adPoint));
}

void TriCell::vContaincenter (double adRes[]) const {
  vCircumcenter (adRes);
}

double TriCell::dContainradius () const {
  assert (qValid ());
  double adTemp[3];
  vContaincenter (adTemp);
  adTemp[0] -= pVVert (0)->dX ();
  adTemp[1] -= pVVert (0)->dY ();
  adTemp[2] -= pVVert (0)->dZ ();
  return (dMAG3D (adTemp));
}

void TriCell::vBarycentrics (const double adPoint[], double adBary[]) const
  // This is written for triangles in 2D.  Triangles in 3D should
  // probably be projected into 2D, with assertions about coplanarity.
{
  assert (qValid ());

  const Vert *pVA = pVVert (0);
  const Vert *pVB = pVVert (1);
  const Vert *pVC = pVVert (2);

  assert (pVA->iSpaceDimen () == 2);

  double adRow1[] = {1., 1., 1.};
  double adRow2[] = {pVA->dX (), pVB->dX (), pVC->dX ()};
  double adRow3[] = {pVA->dY (), pVB->dY (), pVC->dY ()};

  vSolve3By3 (adRow1, adRow2, adRow3, 1., adPoint[0], adPoint[1], adBary);
}

double TriCell::dShortestEdgeLength() const
{
  register double dDX, dDY;
  register const Vert *pV0 = pVVert(0);
  register const Vert *pV1 = pVVert(1);
  register const Vert *pV2 = pVVert(2);

  dDX = pV0->dX() - pV1->dX();
  dDY = pV0->dY() - pV1->dY();
  double dLen01_Sq = dDX*dDX + dDY*dDY;

  dDX = pV0->dX() - pV2->dX();
  dDY = pV0->dY() - pV2->dY();
  double dLen02_Sq = dDX*dDX + dDY*dDY;
  
  dDX = pV1->dX() - pV2->dX();
  dDY = pV1->dY() - pV2->dY();
  double dLen12_Sq = dDX*dDX + dDY*dDY;

  double dShortest_Sq = min(min(dLen01_Sq, dLen02_Sq), dLen12_Sq);
  return (sqrt(dShortest_Sq));
}

void TriCell::vVecSize (double adRes[]) const {
  if (pVVert (0)->iSpaceDimen () == 2)
    {
      ::vNormal2D (pVVert (0)->adCoords (), pVVert (1)->adCoords (),
		   pVVert (2)->adCoords (), adRes);
      adRes[0] *= 0.5;
    }
  else {
    ::vNormal (pVVert (0)->adCoords (), pVVert (1)->adCoords (),
	       pVVert (2)->adCoords (), adRes);
    vSCALE3D (adRes, 0.5);
  }
}
