#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* QuadCell::
pVVert (const int i)
  const {
  assert (iFullCheck ());
  assert2 (i >= 0 && i <= 3, "Vertex index out of range");
  // Assume that faces are returned in cyclic order by pFFace().
  const Face *const pFBase = pFFace(0);
  const Face *const pFOpp = pFFace(2);
  switch (i) {
  case 0:
    if (pFBase->pCCellLeft () == static_cast<const Cell*>(this)) {
      return pFBase->pVVert(0);
    }
    else {
      return pFBase->pVVert(1);
    }
    break;
  case 1:
    if (pFBase->pCCellLeft () == static_cast<const Cell*>(this)) {
      return pFBase->pVVert(1);
    }
    else {
      return pFBase->pVVert(0);
    }
    break;
  case 2:
    if (pFOpp->pCCellLeft () == static_cast<const Cell*>(this)) {
      return pFOpp->pVVert(0);
    }
    else {
      return pFOpp->pVVert(1);
    }
    break;
  case 3:
    if (pFOpp->pCCellLeft () == static_cast<const Cell*>(this)) {
      return pFOpp->pVVert(1);
    }
    else {
      return pFOpp->pVVert(0);
    }
    break;
  default:
    return pVInvalidVert;
  }
}

double QuadCell::dSize ()
  const {
  assert2 (0, "No area calculation yet for quads");
  assert (iFullCheck ());
  double adVecSize[3];
  vVecSize (adVecSize);
  if (pVVert (1)->iSpaceDimen () == 2)
    return fabs (*adVecSize);
  else
    return dMAG3D (adVecSize);
}

void QuadCell::
vAllDihed (double adDihed[], int *const piNDihed)
  const {
  *piNDihed = 4;
  double adNorm0[3], adNorm1[3], adNorm2[3], adNorm3[3];
  pFFace (0)->vUnitNormal (adNorm0);
  pFFace (1)->vUnitNormal (adNorm1);
  pFFace (2)->vUnitNormal (adNorm2);
  pFFace (3)->vUnitNormal (adNorm3);
  
  int iSign0 = 1, iSign1 = 1, iSign2 = 1, iSign3 = 1;
  if (pFFace (0)->pCCellLeft () == static_cast<const Cell*>(this))
    iSign0 = -1;
  if (pFFace (1)->pCCellLeft () == static_cast<const Cell*>(this))
    iSign1 = -1;
  if (pFFace (2)->pCCellLeft () == static_cast<const Cell*>(this))
    iSign2 = -1;
  if (pFFace (3)->pCCellLeft () == static_cast<const Cell*>(this))
    iSign3 = -1;
  
  double dScale = 180 / M_PI;
  adDihed[0] = GR_acos (dDOT3D (adNorm0, adNorm1)) * dScale * (iSign0 * iSign1);
  adDihed[1] = GR_acos (dDOT3D (adNorm1, adNorm2)) * dScale * (iSign1 * iSign2);
  adDihed[2] = GR_acos (dDOT3D (adNorm2, adNorm3)) * dScale * (iSign2 * iSign3);
  adDihed[3] = GR_acos (dDOT3D (adNorm3, adNorm0)) * dScale * (iSign3 * iSign0);
} void QuadCell::vAllSolid (double adSolid[], int *const piNSolid)
  const {
  vAllDihed (adSolid, piNSolid);
} void QuadCell::vVecSize (double adRes[])
  const {
  if (pVVert (0)->iSpaceDimen () == 2)
    {
      double adDiff1[] =
      {pVVert (1)->dX () - pVVert (0)->dX (),
       pVVert (1)->dY () - pVVert (0)->dY ()};
      double adDiff2[] =
      {pVVert (2)->dX () - pVVert (0)->dX (),
       pVVert (2)->dY () - pVVert (0)->dY ()};
      double adDiff3[] =
      {pVVert (3)->dX () - pVVert (0)->dX (),
       pVVert (3)->dY () - pVVert (0)->dY ()};
      double dCross1 = dCROSS2D (adDiff1, adDiff2);
      double dCross2 = dCROSS2D (adDiff2, adDiff3);
      *adRes = (dCross1 + dCross2) * 0.5;
    }
  else {
    double adVec0[3], adVec1[3];
    vNormal (pVVert (0)->adCoords (), pVVert (1)->adCoords (),
	     pVVert (2)->adCoords (), adVec0);
    vNormal (pVVert (0)->adCoords (), pVVert (2)->adCoords (),
	     pVVert (3)->adCoords (), adVec1);
    adRes[0] = (adVec0[0] + adVec1[0]) * 0.5;
    adRes[1] = (adVec0[1] + adVec1[1]) * 0.5;
    adRes[2] = (adVec0[2] + adVec1[2]) * 0.5;
  }
}

bool QuadCell::
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, pVInvalidVert};
  // If the cell is deleted, it doesn't matter whether it's closed or
  // not. 
  if (qDeleted ())
    return true;
  int iFaceCount[4];
  for (int iF = 0; iF < 4; 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 < 4; ii++) {
	  if (apV[ii] == pVInvalidVert) {
	    apV[ii] = pV;
	    iFaceCount[ii] = 1;
	    break;
	  }
	  else if (apV[ii] == pV) {
	    iFaceCount[ii]++;
	    break;
	  }
	}
	if (ii == 4)
	  return (false);
      }				// Done with this vertex
      
    }				// Done with this face
  
  if (iFaceCount[0] != 2 || iFaceCount[1] != 2 ||
      iFaceCount[2] != 2 || iFaceCount[3] != 2)
    return (false);
  return (true);
}

#define vSwitchFaces(pFA, pFB) do { \
    Face *pFTmp = pFA; \
    pFA = pFB; \
    pFB = pFTmp; \
  } while (0);

void QuadCell::vCanonicalizeFaceOrder()
{
  // This isn't particularly pretty, but it has to be done...
  Vert *apV[4];
  if (ppFFaces[0]->pCCellLeft() == this) {
    apV[0] = ppFFaces[0]->pVVert(0);
    apV[1] = ppFFaces[0]->pVVert(1);
  }
  else {
    apV[0] = ppFFaces[0]->pVVert(1);
    apV[1] = ppFFaces[0]->pVVert(0);
  }

  if (!ppFFaces[1]->qHasVert(apV[1])) {
    // Need to find out which face is actually the next in order
    if (ppFFaces[2]->qHasVert(apV[1])) {
      vSwitchFaces(ppFFaces[1], ppFFaces[2]);
    }
    else {
      assert(ppFFaces[3]->qHasVert(apV[1]));
      vSwitchFaces(ppFFaces[1], ppFFaces[3]);
    }
  }

  if (!ppFFaces[3]->qHasVert(apV[0])) {
    vSwitchFaces(ppFFaces[2], ppFFaces[3]);
  }

  // Check correctness
  assert( ppFFaces[0]->qHasVert(apV[0]) &&  ppFFaces[0]->qHasVert(apV[1]));
  assert(!ppFFaces[1]->qHasVert(apV[0]) &&  ppFFaces[1]->qHasVert(apV[1]));
  assert(!ppFFaces[2]->qHasVert(apV[0]) && !ppFFaces[2]->qHasVert(apV[1]));
  assert( ppFFaces[3]->qHasVert(apV[0]) && !ppFFaces[3]->qHasVert(apV[1]));
}

