#include <math.h>
#include "GR_assert.h"
#include "GR_events.h"
#include "GR_AnisoFront.h"
#include "GR_AnisoFrontEntry.h"
#include "GR_BFace.h"
#include "GR_Geometry.h"
#include "GR_InsertionQueue.h"
#include "GR_Mesh2D.h"
#include "GR_Vec.h"

#ifdef IRIX6
#include <values.h>
#endif

static const double dDefaultThick = 0.001;
static const double dStretch = 1.15;
static const double dDirectionWeight = 2;

void AnisoFrontEntry::vAssign(Vert * const pVLIn, Vert * const pVRIn,
			      AnisoFrontEntry * const pAFEPrevious)
{
  assert(pVLIn->qValid() && pVRIn->qValid());
  pVL = pVLIn;
  pVR = pVRIn;
  if (pAFEPrevious) {
    // Set up connectivity: connect to parent
    pAFEPrev = pAFEPrevious;
    pAFEPrev->vSetChild(this);

    // Connect to left face, if it exists, and tell it to connect to us.
    if (pAFEPrev->pAFELeftNeighbor()) {
      // Might assign null data, but so what?
      pAFELeft = pAFEPrev->pAFELeftNeighbor()->pAFEChild();
      if (pAFELeft) {
	pAFELeft->vSetRightNeighbor(this);
	pAFELeft->vSetRightVert(pVL);
      }
    }

    // Ditto for right face
    if (pAFEPrev->pAFERightNeighbor()) {
      // Might assign null data, but so what?
      pAFERight = pAFEPrev->pAFERightNeighbor()->pAFEChild();
      if (pAFERight) {
	pAFERight->vSetLeftNeighbor(this);
	pAFERight->vSetLeftVert(pVR);
      }
    }

    pAFEPrev->vSetLeftChildVert(pVL);
    pAFEPrev->vSetRightChildVert(pVR);
  } // Previous front entry was given
  else {
    pAFEPrev = pAFERight = pAFELeft = pAFEInvalid;
  } // Previous front entry was not given; this is the case for the
  // initial front, and the left and right neighbors must be set
  // explicitly.
} // Done with assignment

bool AnisoFrontEntry::qValid() const
{
  if (this == pAFEInvalid) return false;
  // This following line is, I think, a bug, so it's commented out.  CFO-G 5/8/07
  //  if (qNullEntry()) return true;
  bool qRetVal = true;
  if (pAFEParent() != pAFEInvalid
      &&
      pAFEParent()->pAFEChild() != pAFEInvalid) {
    qRetVal &= (pAFEParent()->pAFEChild() == this);
  }
  if (pAFEChild() != pAFEInvalid
      &&
      pAFEChild()->pAFEParent() != pAFEInvalid) {
    qRetVal &= (pAFEChild()->pAFEParent() == this);
  }
  if (pAFELeftNeighbor() != pAFEInvalid
      &&
      pAFELeftNeighbor()->pAFERightNeighbor() != pAFEInvalid) {
    qRetVal &= (pAFELeftNeighbor()->pAFERightNeighbor() == this);
  }
  if (pAFERightNeighbor() != pAFEInvalid
      &&
      pAFERightNeighbor()->pAFELeftNeighbor() != pAFEInvalid) {
    qRetVal &= (pAFERightNeighbor()->pAFELeftNeighbor() == this);
  }
  return qRetVal;
}

void AnisoFrontEntry::vLeftMarchingDirection(double adDir[2])
{
  if (pAFEPrev->qValid()) {
    // Marching direction is through the left-hand verts of this and the
    // previous front entry.
    adDir[0] = pVLeft()->dX() - pAFEParent()->pVLeft()->dX();
    adDir[1] = pVLeft()->dY() - pAFEParent()->pVLeft()->dY();
    vNORMALIZE2D(adDir);
  }
  else {
    if (pAFELeft->qValid() && !pAFELeft->qDegenerate()) {
      double adNormLeft[2];
      vUnitNormal(adDir);
      pAFELeft->vUnitNormal(adNormLeft);
      // If these normals are similar, take their average.  Otherwise,
      // stick with the normal of this face.
//        double dDot = dDOT2D(adDir, adNormLeft);
//        if (dDot > 0.75) {
	adDir[0] += adNormLeft[0];
	adDir[1] += adNormLeft[1];
	vNORMALIZE2D(adDir);
//        }
    }
    else {
      vUnitNormal(adDir);
    }
  }
}

void AnisoFrontEntry::vRightMarchingDirection(double adDir[2])
{
  if (pAFEPrev->qValid()) {
    // Marching direction is through the right-hand verts of this and the
    // previous front entry.
    adDir[0] = pVRight()->dX() - pAFEParent()->pVRight()->dX();
    adDir[1] = pVRight()->dY() - pAFEParent()->pVRight()->dY();
    vNORMALIZE2D(adDir);
  }
  else {
    if (pAFERight->qValid() && !pAFERight->qDegenerate()) {
      double adNormRight[2];
      vUnitNormal(adDir);
      pAFERight->vUnitNormal(adNormRight);
      // If these normals are similar, take their average.  Otherwise,
      // stick with the normal of this face.
      //      double dDot = dDOT2D(adDir, adNormRight);
      //      if (dDot > 0.75) {
      adDir[0] += adNormRight[0];
      adDir[1] += adNormRight[1];
      vNORMALIZE2D(adDir);
      //      }
    }
    else {
      vUnitNormal(adDir);
    }
  }
}

Vert* Mesh2D::pVInsertNearVert(Vert* const pVOld, const double adLoc[2])
{
  vMessage(4, "Finding a cell that contains the new location...\n");

  Cell *pCSeed = pCInvalidCell, *pCNearest = pCInvalidCell;
  double dBestRatio = 1.e300;
  std::set<Cell*> spCInc;
  {
    std::set<Vert*> spVTemp;
    vNeighborhood(pVOld, spCInc, spVTemp);
  }
  Vert VDummy;
  VDummy.vSetCoords(2, adLoc);
  for (std::set<Cell*>::iterator iterC = spCInc.begin();
       iterC != spCInc.end(); iterC++) {
    Cell *pCTemp = *iterC;
    if (pCTemp->eType() == Cell::eTriCell) {

      if (iIncircle(pCTemp->pVVert(0), pCTemp->pVVert(1),
		    pCTemp->pVVert(2), &VDummy) >= 0) {
		pCSeed = pCTemp;
		break;
      }
      else {
		double adCent[2];
      	dynamic_cast<TriCell*>(pCTemp)->vCircumcenter(adCent);
      	double dRad = dynamic_cast<TriCell*>(pCTemp)->dCircumradius();
      	double dDist = dDIST2D(adCent, adLoc);
      	vMessage(4, "     Center at: (%10g, %10g), %14.10g from point; radius %14.10g\n",
	    		 adCent[0], adCent[1], dDist, dRad);
	    double dRatio = dDist / dRad;
	    if (dRatio < dBestRatio) {
	      pCNearest = pCTemp;
	    }
      }
    }
  }


  if (pCSeed->qValid()) {
    vMessage(4, "got it\n");
  }
  else {
    // This is a fallback position, in case the incircle tests flake,
    // which they occasionally do.
    pCSeed = *(spCInc.begin());
  }
  assert(pCSeed->qValid());

  int iNSwaps;
  bool qRetVal = qInsertPoint(adLoc, pCSeed, &iNSwaps, true, true);
  if (qRetVal) {
    vUpdateLengthScale(pVVert(iNumVerts() - 1));
    return pVVert(iNumVerts() - 1);
  }
  else return pVInvalidVert;
}

// Anisotropic meshing keeps track of faces active in layer growth as a
// queue.  The queue is initialized by using the bdry faces (or
// specially-marked internal faces, for anisotropic solution-based
// refinement).  Each entry in the queue is visited in turn, a new layer
// is built on its face, and (if needed) a replacement queue entry is
// created.  When iteration reaches the end of the queue, then
// anisotropic generation is done.

// The data structure for each element in the front contains:
//  o A pointer to a valid face in the mesh.
//  o The vertices on that face, stored so that
//  o The step size taken to create the face
//  o Pointers to
//      Adjacent front entries (can be NULL if this face is
//          at the end of a front)
//      The front entry that this one replaces (parent)
//      The front entry that replaces this one (child)
// Yep, that's a quadruply-linked data structure.  Remember, you saw
// it here first.

void Mesh2D::
vRefineBoundaryToLengthScale(const int iNStructBC, const int aiStructBC[])
{
  int iVertsInserted = 0, iInsertedThisPass;
  // The following factor will depend on stretching ratio and desired
  // final aspect ratio. For now, I'm using a hard constant.
  double dFactor = 10.;
  do {
    iInsertedThisPass = 0;
    for (int iBF = iNumBdryFaces() - 1; iBF >= 0; iBF--) {
      BFace *pBF = pBFBFace(iBF);

      // Don't refine the "wrong" BC's.
      int iThisBC = pBF->iBdryCond();
      bool qRefineThisOne = false;
      for (int ii = 0; ii < iNStructBC; ii++) {
	if (iThisBC == aiStructBC[ii]) {
	  qRefineThisOne = true;
	  break;
	}
      }
      if (!qRefineThisOne) continue;

      Vert *pV0 = pBF->pVVert(0);
      Vert *pV1 = pBF->pVVert(1);
      double dLenScale0 = pV0->dLS();
      double dLenScale1 = pV1->dLS();
      double dEdgeLen = dDIST2D(pV0->adCoords(), pV1->adCoords());
      vMessage(4, "Looking at bdry face #%d (verts %d, %d) of length %10.6g\n",
	       iBF, iVertIndex(pV0), iVertIndex(pV1), dEdgeLen);
      vMessage(4, "  Vert %5d (located at (%10.6g, %10.6g)) has len scale %10.6g\n",
	       iVertIndex(pV0), pV0->dX(), pV0->dY(), dLenScale0);
      vMessage(4, "  Vert %5d (located at (%10.6g, %10.6g)) has len scale %10.6g\n",
	       iVertIndex(pV1), pV1->dX(), pV1->dY(), dLenScale1);

      // Split if the edge is too long.
      bool qNeedToSplit = (dEdgeLen > max(dLenScale0, dLenScale1)/dFactor);

      if (qNeedToSplit) {
	// Split the edge at its orientation-change midpoint.
	double adNewLoc[2];
	(dynamic_cast<BdryEdge*>(pBF))->vGetBisectionPoint(adNewLoc);
	vMessage(4, "  Too long!  Splitting with new vertex at (%10.6g, %10.6g)\n",
		 adNewLoc[0], adNewLoc[1]);
	Face *pF = pBF->pFFace();
	Cell *pC = pF->pCCellOpposite(pBF);
	int iFaceInd = 0;
	for (iFaceInd = pC->iNumFaces() - 1; iFaceInd >= 0; iFaceInd--) {
	  if (pC->pFFace(iFaceInd) == pF) break;
	}
	assert(iFaceInd != -1);
	// The following lines assume, so far incorrectly, that the bdry
	// Watson stuff has been purged of List<>'s.  This is likely
	// fixed is Serge's branch already, but that merge hasn't
	// happened yet.
//	std::set<BdryEdgeBase*> spBE;
//	spBE.insert(dynamic_cast<BdryEdgeBase*>(pBF));
//	int iAdded = iInsertPointOnBdryWatson(pC, iFaceInd, adNewLoc,
//					      spBE, false);
	// Here's the hack to be able to use the old call.
	List<BdryEdgeBase*> LpBE;
	LpBE.vAppendItem(dynamic_cast<BdryEdgeBase*>(pBF));
	int iAdded = iInsertPointOnBdryWatson(pC, iFaceInd, adNewLoc,
					      LpBE, false);
	iVertsInserted += iAdded;
	iInsertedThisPass += iAdded;
      } // Done inserting vertex
      vUpdateLengthScale();
    } // Done looking at all bdry edges
  } while (iInsertedThisPass > 0);
}

AnisoFront::AnisoFront(Mesh2D& M2D, const int iNStructBC,
		       const int aiStructBC[])
  : VAAF(M2D.iNumBdryFaces()), pM2D(&M2D), iStartOfLayer(0)
{
  // Loop through all the bdry faces, checking each one to see whether
  // it should be added to the front, and also setting up its
  // connectivity.
  vMessage(2, "Setting up initial front for anisotropic insertion\n");
  int iBF;
  for (iBF = 0; iBF < pM2D->iNumBdryFaces(); iBF++) {
    BFace* pBF = pM2D->pBFBFace(iBF);
    Face* pF = pBF->pFFace();
    int iBC = pBF->iBdryCond();
    // Check whether we want to grow a layer from BFace's with this BC
    {
      bool qAnisoFromHere = false;
      for (int ii = 0; ii < iNStructBC && !qAnisoFromHere; ii ++) {
	if (aiStructBC[ii] == iBC) {
	  qAnisoFromHere = true;
	  break;
	}
      }
      if (!qAnisoFromHere) continue;
    }

    // Yes, we want to add this face to the list, or we wouldn't be
    // here.

    // Must get vert data from the BFace, not the Face, so that the
    // front edges will be oriented properly.
    Vert *pV0 = pBF->pVVert(0);
    Vert *pV1 = pBF->pVVert(1);
    pV0->vMarkStructured();
    pV1->vMarkStructured();
    pBF->pFFace()->vMarkStructured();

    VAAF[iBF].vAssign(pV0, pV1);
    vMessage(3, "  Setting up front entry %d with verts %d and %d.  (BC: %d) %p %p\n",
	     iBF, pM2D->iVertIndex(pV0), pM2D->iVertIndex(pV1), iBC,
	     pF->pCCellLeft(), pBF);
    double dLen = dDIST2D(pV0->adCoords(), pV1->adCoords());
    VAAF[iBF].vSetSize(dLen * dDefaultThick);

    // Now the harder bit:  finding the BFace's on either side.  The
    // following may not be stunningly efficient, but you've got to walk
    // through the neighborhood of the vertex anyway to find the
    // adjacent BFace's, so this approach is not in principle too bad.
    {
      std::set<BFace*> spBFInc;
      bool qBdryVert;

      std::set<Cell*> spCTemp;
      std::set<Vert*> spVTemp;

      vNeighborhood(pV0, spCTemp, spVTemp, &spBFInc, &qBdryVert);
      assert(qBdryVert);
      assert(spBFInc.size() == 2);
      assert(spBFInc.count(pBF) == 1);

      std::set<BFace*>::iterator iterBF = spBFInc.begin();
      BFace *pBFOther = *iterBF;
      if (pBFOther == pBF) {
	iterBF++;
	pBFOther = *iterBF;
      }
      assert(pBFOther->qHasVert(pV0));

      if (pBFOther->iBdryCond() != iBC) {
	// This face doesn't have a winning BC!
	VAAF[iBF].vClearLeftNeighbor();
	vMessage(4, "  No left neighbor (bad BC %d).\n",
		 pBFOther->iBdryCond());
      }
      else {
	int iLeftIndex = pM2D->iBFaceIndex(pBFOther);
	vMessage(3, "  Left  hand neighbor: %d\n", iLeftIndex);
	VAAF[iBF].vSetLeftNeighbor(&(VAAF[iLeftIndex]));
      }

      vNeighborhood(pV1, spCTemp, spVTemp, &spBFInc, &qBdryVert);
      assert(qBdryVert);
      assert(spBFInc.size() == 2);
      assert(spBFInc.count(pBF) == 1);

      iterBF = spBFInc.begin();
      pBFOther = *iterBF;
      if (pBFOther == pBF) {
	iterBF++;
	pBFOther = *iterBF;
      }
      assert(pBFOther->qHasVert(pV1));

      if (pBFOther->iBdryCond() != iBC) {
	// This face doesn't have a winning BC!
	VAAF[iBF].vClearRightNeighbor();
	vMessage(4, "  No right neighbor (bad BC %d).\n",
		 pBFOther->iBdryCond());
      }
      else {
	int iRightIndex = pM2D->iBFaceIndex(pBFOther);
	vMessage(3, "  Right hand neighbor: %d\n", iRightIndex);
	VAAF[iBF].vSetRightNeighbor(&(VAAF[iRightIndex]));
      }
    }
  }

  // Clean up front entries at small angles.
  for (int i = VAAF.iSize() - 1; i >= 0; i--) {
    AnisoFrontEntry *pAFE = &(VAAF[i]);
    if (!pAFE->qNullEntry() &&
	pAFE->pVRight()->qIsSmallAngleCorner()) {
      // Need to merge this entry with the one on the right.
      assert(pAFE->pAFERightNeighbor()->qValid());
      AnisoFrontEntry *pAFENew = pAFENewEntry();
      vMergeFE_2for1(pAFE, pAFE->pAFERightNeighbor(), pAFENew);
      vMessage(2, "Merged entries %p and %p; new entry %p.\n",
	       pAFE, pAFE->pAFERightNeighbor(), pAFENew);
    }
  }

//   // Clean up front entries at very big angles (like trailing edges).
//   for (int i = VAAF.iSize() - 1; i >= 0; i--) {
//     AnisoFrontEntry *pAFE = &(VAAF[i]);
//     if (pAFE->qNullEntry()) continue;
//     AnisoFrontEntry *pAFERight = pAFE->pAFERightNeighbor();
//     if (pAFERight == pAFEInvalid || pAFERight->qNullEntry()) continue;
//     double adNorm[2], adNormRight[2];
//     pAFE->vUnitNormal(adNorm);
//     pAFERight->vUnitNormal(adNormRight);
//     if (dDOT2D(adNorm, adNormRight) < -M_SQRT1_2) {
//       // If the angle is more than 135 degrees, then break it.
//       pAFE->vClearRightNeighbor();
//       pAFERight->vClearLeftNeighbor();
//       vMessage(2, "Separated entries %p and %p at a corner (%f, %f).\n",
// 	       pAFE, pAFERight, pAFE->pVRight()->dX(), pAFE->pVRight()->dY());
//     }
//   }

#ifndef NDEBUG
  // Check whether the front is okay.
  for (int i_ = 0; i_ < VAAF.iSize(); i_ ++) {
    if (!VAAF[i_].qNullEntry()) {
      if (!VAAF[i_].qValid()) {
	vMessage(2, "Problem with entry %p\n", &(VAAF[i_]));
      }
      assert(VAAF[i_].qValid());
    }
  }
#endif

  // Finish at AR = 2.  This should eventually be passed in from above...
  vInitFrontQueue(2);
}

int AnisoFront::iMarchFromRightEndPoint(AnisoFrontEntry * const pAFE)
{
  assert(pAFE->qValid());
  assert(!pAFE->pVRightChild()->qValid());
  assert(!pAFE->pAFERightNeighbor()->qValid());

  // Find out how close the marching directions at the outside points
  // are to being in the same direction.
  double adDir[2];
  pAFE->vRightMarchingDirection(adDir);

  double dStepSize = pAFE->dSize() / pAFE->dLength();
//   AnisoFrontEntry *pAFEParent = pAFE->pAFEParent();
//   if (pAFEParent->qValid()) {
//     // Unless this is the first layer, base layer thickness on
//     // stretching from the last layer.
//     const double *adCurrentLoc = pAFE->pVRight()->adCoords();
//     const double *adPreviousLoc = pAFEParent->pVRight()->adCoords();
//     const double dDistance = dDIST2D(adCurrentLoc, adPreviousLoc);

//     dStepSize = dStretch * dDistance;
//   }
//   else {
//     dStepSize = dDefaultThick;
//   }

  int iAdded = 0;

  // Use the old marching direction to find where to insert a single
  // point.
  double adNewPoint[2];
  adNewPoint[0] = pAFE->pVRight()->dX() + dStepSize * adDir[0];
  adNewPoint[1] = pAFE->pVRight()->dY() + dStepSize * adDir[1];

  vMessage(4, "  New point location: (%12.6g, %12.6g)\n",
	   adNewPoint[0], adNewPoint[1]);

  Vert *pVNew = pM2D->pVInsertNearVert(pAFE->pVRight(), adNewPoint);
  if (pVNew->qValid()) iAdded = 1;
  if (iAdded > 0) {
    int iTmp = pM2D->iNumVerts() - 1;
    assert(iFuzzyComp(0, dDIST2D(adNewPoint,
				 pM2D->pVVert(iTmp)->adCoords())) == 0);
    pAFE->pVRight()->vMarkStructured();
    pAFE->vSetRightChildVert(pVNew);
  }
  return iAdded;
}

int AnisoFront::iMarchFromLeftEndPoint(AnisoFrontEntry * const pAFE)
{
  assert(pAFE->qValid());
  assert(!pAFE->pVLeftChild()->qValid());
  assert(!pAFE->pAFELeftNeighbor()->qValid());

  // Find out how close the marching directions at the outside points
  // are to being in the same direction.
  double adDir[2];
  pAFE->vLeftMarchingDirection(adDir);

  double dStepSize = pAFE->dSize() / pAFE->dLength();
//   AnisoFrontEntry *pAFEParent = pAFE->pAFEParent();
//   double dStepSize;
//   if (pAFEParent->qValid()) {
//     // Unless this is the first layer, base layer thickness on
//     // stretching from the last layer.
//     const double *adCurrentLoc = pAFE->pVLeft()->adCoords();
//     const double *adPreviousLoc = pAFEParent->pVLeft()->adCoords();
//     const double dDistance = dDIST2D(adCurrentLoc, adPreviousLoc);

//     dStepSize = dStretch * dDistance;
//   }
//   else {
//     dStepSize = dDefaultThick;
//   }


  int iAdded = 0;

  // Use the old marching direction to find where to insert a single
  // point.
  double adNewPoint[2];
  adNewPoint[0] = pAFE->pVLeft()->dX() + dStepSize * adDir[0];
  adNewPoint[1] = pAFE->pVLeft()->dY() + dStepSize * adDir[1];

  vMessage(4, "  New point location: (%12.6g, %12.6g)\n",
	   adNewPoint[0], adNewPoint[1]);

  Vert *pVNew = pM2D->pVInsertNearVert(pAFE->pVLeft(), adNewPoint);
  if (pVNew->qValid()) iAdded = 1;
  if (iAdded > 0) {
    assert(iFuzzyComp(0, dDIST2D(adNewPoint, pVNew->adCoords())) == 0);
    pAFE->pVLeft()->vMarkStructured();
    pAFE->vSetLeftChildVert(pVNew);
  }
  return iAdded;
}

int AnisoFront::iMarchFromMeetingPoint(AnisoFrontEntry * const pAFELeft,
				     AnisoFrontEntry * const pAFERight)
{
  assert(pAFELeft->qValid() && pAFERight->qValid());
  assert(!pAFELeft->pVRightChild()->qValid());
  assert(!pAFERight->pVLeftChild()->qValid());
  assert(pAFELeft->pAFERightNeighbor() == pAFERight);
  assert(pAFERight->pAFELeftNeighbor() == pAFELeft);

  bool qIsFirstLayer = !(pAFELeft->pAFEParent()->qValid());
  vMessage(3, "Marching from the intersection of front faces %p and %p\n",
	   pAFELeft, pAFERight);
  vMessage(3, " Verts: %12p (%10.6f, %10.6f)\n", pAFELeft->pVLeft(),
	   pAFELeft->pVLeft()->dX(), pAFELeft->pVLeft()->dY());
  vMessage(3, "        %12p (%10.6f, %10.6f)\n", pAFELeft->pVRight(),
	   pAFELeft->pVRight()->dX(), pAFELeft->pVRight()->dY());
  vMessage(3, "        %12p (%10.6f, %10.6f)\n", pAFERight->pVLeft(),
	   pAFERight->pVLeft()->dX(), pAFERight->pVLeft()->dY());
  vMessage(3, "        %12p (%10.6f, %10.6f)\n", pAFERight->pVRight(),
	   pAFERight->pVRight()->dX(), pAFERight->pVRight()->dY());


  // Find out how close the marching directions at the outside points
  // are to being in the same direction.
  double adDirLeftLine[2], adDirRightLine[2], adDirMiddle[2];
  pAFELeft->vLeftMarchingDirection(adDirLeftLine);
  pAFERight->vRightMarchingDirection(adDirRightLine);
  pAFELeft->vRightMarchingDirection(adDirMiddle);
//   pAFELeft->vUnitNormal(adDirLeftLine);
//   pAFERight->vUnitNormal(adDirRightLine);


#ifndef NDEBUG
  double adDirMid2[2];
  pAFERight->vLeftMarchingDirection(adDirMid2);
  assert(iFuzzyComp(adDirMiddle[0], adDirMid2[0]) == 0);
  assert(iFuzzyComp(adDirMiddle[1], adDirMid2[1]) == 0);
#endif

  double dCosine = dDOT2D(adDirRightLine, adDirMiddle);
  double dSine = dCROSS2D(adDirRightLine, adDirMiddle);
  // This angle is zero for parallel marching directions, with positive
  // angles indicating divergence of marching directions (something like
  // this: \ | /), and negative angles convergence (like this: / | \).
  double dAngleRight = atan2(dSine, dCosine);

  dCosine = dDOT2D(adDirMiddle, adDirLeftLine);
  dSine = dCROSS2D(adDirMiddle, adDirLeftLine);
  double dAngleLeft = atan2(dSine, dCosine);
  vMessage(4, " Right angle: %f.  Left angle: %f.\n",
	   dAngleRight, dAngleLeft);
  double dAngle = dAngleRight + dAngleLeft;

  double dStepSize;

  int iAdded = 0;
  if (dAngle >= M_PI/2 && qIsFirstLayer) {
    // In this case (front faces meeting at more than 270 degrees),
    // march from both sides -and- start a new line of verts, bisecting
    // the marching angles on the two sides.
    dStepSize = dDefaultThick;
    pAFELeft->vClearRightNeighbor();
    iAdded = iMarchFromRightEndPoint(pAFELeft);

    pAFERight->vClearLeftNeighbor();
    iAdded += iMarchFromLeftEndPoint(pAFERight);

    // Now add a new point bisecting the marching angles to the left and
    // right.
    double adNewDir[2];
    adNewDir[0] = adDirLeftLine[0] + adDirRightLine[0];
    adNewDir[1] = adDirLeftLine[1] + adDirRightLine[1];
    vNORMALIZE2D(adNewDir);

    Vert *pVOld = pAFELeft->pVRight();
    double adNewLoc[2];
    adNewLoc[0] = pVOld->dX() + dStepSize * adNewDir[0];
    adNewLoc[1] = pVOld->dY() + dStepSize * adNewDir[1];

    Vert *pVNew = pM2D->pVInsertNearVert(pVOld, adNewLoc);
    if (pVNew->qValid())
      iAdded++;
    else
      vFoundBug("adding anisotropic marching line");

    // Create two new front entries between pAFELeft and pAFERight.
    // These entry will have no parents, but they will have neighbors
    // and children.  To make this happen properly, each new entry must
    // be connected to its neighbors and know what its child verts are.

    // There's only one current vert in the two new front entries
    // combined, since they're degenerate.
    AnisoFrontEntry *pAFENew1 = pAFENewEntry();
    AnisoFrontEntry *pAFENew2 = pAFENewEntry();
    pAFENew1->vAssign(pAFELeft->pVRight(), pAFELeft->pVRight());
    pAFENew2->vAssign(pAFELeft->pVRight(), pAFELeft->pVRight());

    // Now sew the front back together.
    pAFELeft->vSetRightNeighbor(pAFENew1);
    pAFENew1->vSetLeftNeighbor(pAFELeft);

    pAFENew1->vSetRightNeighbor(pAFENew2);
    pAFENew2->vSetLeftNeighbor(pAFENew1);

    pAFENew2->vSetRightNeighbor(pAFERight);
    pAFERight->vSetLeftNeighbor(pAFENew2);

    // Set up the child verts for the two new entries.
    pAFENew1->vSetLeftChildVert(pAFELeft->pVRightChild());
    pAFENew1->vSetRightChildVert(pVNew);
    pAFENew2->vSetLeftChildVert(pVNew);
    pAFENew2->vSetRightChildVert(pAFERight->pVLeftChild());

    // Finally, create the child entry in the front for each of the new
    // entries, since that won't happen elsewhere.
    assert(pAFENew1->qValid());
    assert(pAFENew2->qValid());
    assert(pAFERight->qValid());
    assert(pAFELeft->qValid());
    vCreateChildEntry(pAFENew1);
    vCreateChildEntry(pAFENew2);
    assert(pAFENew1->pAFEChild()->qValid());
    assert(pAFENew2->pAFEChild()->qValid());

    AnisoFrontEntry *pAFEChild1 = pAFENew1->pAFEChild();
    AnisoFrontEntry *pAFEChild2 = pAFENew2->pAFEChild();

    pAFEChild1->vMarkAddedConvex();
    pAFEChild2->vMarkAddedConvex();
    pAFEChild1->vSetSize(pAFEChild1->dLength() * dDefaultThick);
    pAFEChild2->vSetSize(pAFEChild2->dLength() * dDefaultThick);
  }
  else {
    // Either we aren't strongly diverging, or we're past the first
    // layer of cells.

    // Use a weighted sum of normals to find where to insert a single
    // point.  This isn't likely to work terribly well for sharp
    // converging corners.

    // Create a weighted sum to get the new marching direction.
    double adNewDir[2];

    adNewDir[0] = (dDirectionWeight * adDirMiddle[0] +
		   adDirLeftLine[0] + adDirRightLine[0]);
    adNewDir[1] = (dDirectionWeight * adDirMiddle[1] +
		   adDirLeftLine[1] + adDirRightLine[1]);
    vNORMALIZE2D(adNewDir);

    double adNewPoint[2];

    double dArea = 0.5*(pAFELeft->dSize() + pAFERight->dSize());
    double adNormRight[2], adNormLeft[2];
    pAFELeft->vUnitNormal(adNormLeft);
    pAFERight->vUnitNormal(adNormRight);

    double dCosAlphaR = dDOT2D(adNormRight, adNewDir);
    double dCosAlphaL = dDOT2D(adNormLeft, adNewDir);


    double dLenLeft = pAFELeft->dLength();
    double dLenRight = pAFERight->dLength();

    dStepSize = dArea*2 / (dLenLeft*dCosAlphaL + dLenRight*dCosAlphaR);

    if (pAFELeft->qAddedInConvexCorner() &&
	pAFERight->qAddedInConvexCorner()) {
      double dLeftStep = 0, dRightStep = 0;
      if (pAFELeft->pVLeftChild()->qValid()) {
	dLeftStep = dDIST2D(pAFELeft->pVLeft()->adCoords(),
			    pAFELeft->pVLeftChild()->adCoords());
      }
      if (pAFERight->pVLeftChild()->qValid()) {
	dRightStep = dDIST2D(pAFERight->pVRight()->adCoords(),
			     pAFERight->pAFEParent()->pVRight()->adCoords());
      }
      dStepSize = std::max(dStepSize, std::max(dLeftStep, dRightStep));
    }

    adNewPoint[0] = pAFELeft->pVRight()->dX() + dStepSize * adNewDir[0];
    adNewPoint[1] = pAFELeft->pVRight()->dY() + dStepSize * adNewDir[1];

    vMessage(4, "  New point location: (%12.6g, %12.6g)\n",
	     adNewPoint[0], adNewPoint[1]);

    int iBefore = pM2D->iNumVerts();
    Vert *pVNew = pM2D->pVInsertNearVert(pAFELeft->pVRight(),
					 adNewPoint);
    if (pVNew->qValid()) iAdded = 1;
    if (iAdded > 0) {
      int iTmp = pM2D->iNumVerts() - 1;
      if (iAdded > 1) {
	vWarning("Anisotropically inserted point encroached a bdry edge!");
	vMessage(1, "Number of verts before = %d\n", iBefore);
      }
      while (dDIST2D(adNewPoint, pM2D->pVVert(iTmp)->adCoords()) > 0 &&
	     iTmp >= iBefore) {
	iTmp--;
      }
      assert(iTmp >= iBefore);
      pVNew = pM2D->pVVert(iTmp);
      pAFELeft->pVRight()->vMarkStructured();
      vMessage(4, "  Inserted left vert (and/or children): %d verts\n",
	       iAdded);
      pAFELeft->vSetRightChildVert(pVNew);
      pAFERight->vSetLeftChildVert(pVNew);
    }
    else {
      vWarning("Anisotropic insertion attempt failed!");
      vMessage(1, "Number of verts before = %d\n", iBefore);
    }
  }
//    else if (dAngle < 2*M_PI/3) {
//      // In this case (front face angle between 240 and 300 degrees),
//      // and march out from both of the original front faces and add a new
//      // front face in between them.
//      pAFELeft->vClearRightNeighbor();
//      iAdded = iMarchFromRightEndPoint(pAFELeft);

//      pAFERight->vClearLeftNeighbor();
//      iAdded += iMarchFromLeftEndPoint(pAFERight);

//      // Create a new front entry between pAFELeft and pAFERight.  This
//      // entry will have no parent, but it will have neighbors and
//      // children.  To make this happen properly, the new entry must be
//      // connected to its neighbors and know what its child verts are.
//      AnisoFrontEntry *pAFENew = pAFENewEntry();
//      pAFENew->vAssign(pAFELeft->pVRight(), pAFERight->pVLeft());

//      pAFELeft->vSetRightNeighbor(pAFENew);
//      pAFENew->vSetLeftNeighbor(pAFELeft);
//      pAFENew->vSetRightNeighbor(pAFERight);
//      pAFERight->vSetLeftNeighbor(pAFENew);

//      pAFENew->vSetRightChildVert(pAFERight->pVLeftChild());
//      pAFENew->vSetLeftChildVert(pAFELeft->pVRightChild());
//      assert(pAFENew->qValid());
//      assert(pAFERight->qValid());
//      assert(pAFELeft->qValid());
//      vCreateChildEntry(pAFENew);
//      assert(pAFENew->pAFEChild()->qValid());
//    }
  return iAdded;
}

int AnisoFront::iAddLayer(const double dFinishAR_In)
{
  int iBegin = iStartOfLayer;
  int iEnd = VAAF.iSize();
  int iVertsAdded = 0;
  dFinishAR = dFinishAR_In;

  // Pass 1:  Tag faces in the front which are too short to be marched
  // forward again.

  vMessage(2, "Checking anisotropic front:  ");
  int iAdvancing = 0, iNonAdvancing = 0, iInvalid = 0;
  int iAF;
  for (iAF = iBegin; iAF < iEnd; iAF++) {
    AnisoFrontEntry *pAFEHere = pAFEEntry(iAF);
    // Skip this entry if it doesn't contain real data.  Some of these
    // may occur, depending on how initialization is done.
    if (!pAFEHere->qValid() || pAFEHere->qNullEntry()) {
      pAFEHere->vMarkNonAdvancing();
      iInvalid++;
      continue;
    }

    // Skip this entry without comment if it's an entry wedged into the
    // original front as a post-fix to handle sharp corners.
    if (pAFEHere->pVLeft() == pAFEHere->pVRight()) {
      pAFEHere->vMarkNonAdvancing();
      continue;
    }

    // If this edge is too short compared to the space to its parent,
    // mark this entry so we won't march from it.
    AnisoFrontEntry *pAFEParent = pAFEHere->pAFEParent();
    bool qParentValid = pAFEParent->qValid();

    double dComparisonDist = dDefaultThick;
    if (qParentValid) {
      double dDistLeft  = dDIST2D(pAFEParent->pVLeft()->adCoords(),
				  pAFEHere->pVLeft()->adCoords());
      double dDistRight = dDIST2D(pAFEParent->pVRight()->adCoords(),
				  pAFEHere->pVRight()->adCoords());
      dComparisonDist = (dDistRight + dDistLeft)*0.5;
    }
    double dDistTop   = dDIST2D(pAFEHere->pVRight()->adCoords(),
				pAFEHere->pVLeft()->adCoords());

    // Check to see if this face should be stopped on procedural
    // grounds: specfically, if an entry has no entry on either
    // side, or no entry on one side and one added for convex corner
    // handling on the other, it should not be advanced (or we march
    // forever from convex corners.  A "normal" entry gives a score of
    // 2, a convex-added entry a score of 1.  A total score (both sides)
    // of 2 is required for the face to be continued.

    int iScore = 0;
    {
      AnisoFrontEntry *pAFELeft = pAFEHere->pAFELeftNeighbor();
      if (pAFELeft->qValid() && pAFELeft->qAdvanceFromHere()) {
	iScore ++;
	if (!pAFELeft->qAddedInConvexCorner()) iScore++;
      }
      AnisoFrontEntry *pAFERight = pAFEHere->pAFERightNeighbor();
      if (pAFERight->qValid() && pAFERight->qAdvanceFromHere()) {
	iScore ++;
	if (!pAFERight->qAddedInConvexCorner()) iScore++;
      }
    }

    if ((dDistTop < dFinishAR * dComparisonDist) || iScore < 2) {
      iNonAdvancing++;
      pAFEHere->vMarkNonAdvancing();
    }
    else {
      iAdvancing++;
    }
  }
  vMessage(2, "advancing from %d faces, skipping %d, %d invalid\n",
	   iAdvancing, iNonAdvancing, iInvalid);

  // Pass 2: Create new child verts for all entries that will be
  // advanced, and add new entries to the front for each face advanced.
  for (iAF = iBegin; iAF < iEnd; iAF++) {
    AnisoFrontEntry *pAFEHere = pAFEEntry(iAF);
    // Skip this entry if it doesn't contain real data or we aren't
    // advancing from here.
    if ((!pAFEHere->qValid()) || (!pAFEHere->qAdvanceFromHere()) ||
	pAFEHere->qNullEntry()) continue;

    if (pAFEHere->pVLeftChild()) {
      vMessage(4, "Left child vertex exists.\n");
    }
    else {
      AnisoFrontEntry *pAFELeft = pAFEHere->pAFELeftNeighbor();
      bool qLeftValid = pAFELeft->qValid();
      if (qLeftValid) {
	iVertsAdded += iMarchFromMeetingPoint(pAFELeft, pAFEHere);
	vMessage(4, "Left child vertex created using front neighbor.\n");
      }
      else {
	iVertsAdded += iMarchFromLeftEndPoint(pAFEHere);
	vMessage(4, "Left child vertex created solo.\n");
      }
    }

    if (pAFEHere->pVRightChild()) {
      vMessage(4, "Right child vertex exists.\n");
    }
    else {
      AnisoFrontEntry *pAFERight = pAFEHere->pAFERightNeighbor();
      bool qRightValid = pAFERight->qValid();
      if (qRightValid) {
	iVertsAdded += iMarchFromMeetingPoint(pAFEHere, pAFERight);
	vMessage(4, "Right child vertex created using front neighbor.\n");
      }
      else {
	iVertsAdded += iMarchFromRightEndPoint(pAFEHere);
	vMessage(4, "Right child vertex created solo.\n");
      }
    }

    // Add a new entry to the front.
    assert(pAFEHere->qValid());
    vCreateChildEntry(pAFEHere);
    assert(pAFEHere->pAFEChild()->qValid());
  } // Done looping over this layer.

  iStartOfLayer = iEnd;
  return iVertsAdded;
}

void Mesh2D::
vGenerateAnisotropic(const int iNStructBC, const int aiStructBC[])
{
//    iMessageStdoutLevel = 3;

  vMessage(2, "Refining boundary to match length scale...\n");
  vRefineBoundaryToLengthScale(iNStructBC, aiStructBC);
  vMessage(2, "Done refining boundary to match length scale; now have %d cells.\n", iNumCells());

  // FIX ME:  Can't currently build from either side of an internal face
  // (or both sides of an internal BFace) simultaneously, because of
  // limitations in how the initial queue is built.

  // Begin by constructing an initial anisotropic advancing layers
  // front.

  AnisoFront AF(*this, iNStructBC, aiStructBC);

  // Now work your way through the queue, processing each front entry in
  // turn.  Note that the connectivity on the new level takes care of
  // itself from parenthood, so there's no need to set it explicitly.  I
  // feel pretty smug about that...

  int iVertsAdded = 0, iLayers = 50;
//   do {
//     iVertsAdded = AF.iAddLayer(dFinishAR);
//     iLayer++;
//     if (iLayer >= 5) dFinishAR = 2;
//     vMessage(2, "Anisotropic layer %2d: added %3d verts (total of %5d in mesh).\n",
//	     iLayer, iVertsAdded, iNumVerts());
//   } while (iVertsAdded > 0 && iLayer < 380);

  AF.vMarchChanSteger(iLayers, iVertsAdded);

  // Traverse all anisotropic front faces that have ever existed.
  // Create a set of the recoverable ones.  Then protect all triangles
  // that have two of those recovered edges, as well as triangles with
  // one recovered edge that is at least half as long as its longest
  // edge.  This should protect every possible triangle in the
  // structured region, while making it so that the occasional false
  // positive will still be well-shaped.
  std::set<Face*> spFRecovered;
  AF.vRecoverEdges(spFRecovered);

  std::set<Face*>::iterator iterEnd = spFRecovered.end();
  for (int iC = iNumCells() - 1; iC >= 0; iC--) {
    Cell *pC = pCCell(iC);
    if (pC->qDeleted()) continue;
    double dLongestLen = 0, dRecLen = -1;
    int iNStruct = 0;
    for (int iF = pC->iNumFaces() - 1; iF >= 0; iF--) {
      Face *pF = pC->pFFace(iF);
      double dLen = pF->dSize();
      dLongestLen = std::max(dLongestLen, dLen);
      if (spFRecovered.find(pF) != iterEnd) {
	iNStruct ++;
	dRecLen = std::max(dRecLen, dLen);
      }
    }
    if (iNStruct >= 2 || (iNStruct == 1 && dRecLen*2 > dLongestLen)) {
      pC->vMarkQuasiStructured();
      vMessage(3, "Protected cell with verts at: (%10.6f, %10.6f)\n",
	       pC->pVVert(0)->dX(), pC->pVVert(0)->dY());
      vMessage(3, "                              (%10.6f, %10.6f)\n",
	       pC->pVVert(1)->dX(), pC->pVVert(1)->dY());
      vMessage(3, "                              (%10.6f, %10.6f)\n",
	       pC->pVVert(2)->dX(), pC->pVVert(2)->dY());
    }
    else {
      vMessage(3, "Unprotected cell with verts at: (%10.6f, %10.6f)\n",
	       pC->pVVert(0)->dX(), pC->pVVert(0)->dY());
      vMessage(3, "                                (%10.6f, %10.6f)\n",
	       pC->pVVert(1)->dX(), pC->pVVert(1)->dY());
      vMessage(3, "                                (%10.6f, %10.6f)\n",
	       pC->pVVert(2)->dX(), pC->pVVert(2)->dY());
    }
  }

  vAllowNonSimplicial();
  AF.vCreateQuads();
  assert(qValid());

  // Fix boundary encroachment safely.
  vCheckForLargeAngles();
  InsertionQueue IQ(this);
  IQ.vQualityRefine();
  assert(qValid());
}

void AnisoFront::vRecoverEdges(std::set<Face*>& spFRecovered)
// Try to recover every edge implied by all the aniso front entries.
// This includes both the long edges (parallel to the surface) and the
// short ones (perpendicular to the surface).
{
  // For every front entry, try to recover the edge that it lies on, and
  // any child edge on its left end.  Also, if it's got no right
  // neighbor, try to recover any child edge on its right end.

  for (int iE = iSize() - 1; iE >= 0 ; iE--) {
    Face *pF;
    AnisoFrontEntry *pAFE = pAFEEntry(iE);

    // Grab the verts of the front entry, and try to recover that edge.
    Vert *pVL = pAFE->pVLeft();
    Vert *pVR = pAFE->pVRight();

    // If there was a 3->2 aniso front entry merge, then the vert may
    // have been deleted.
    if (pVL == pVInvalidVert || pVL->qDeleted() || pVL->qDeletionRequested() ||
	pVR == pVInvalidVert ||	pVR->qDeleted() || pVR->qDeletionRequested())
      continue; 

    // Front faces artificially created to allow more rays from a sharp
    // corner in the mesh shouldn't be considered here.
    if (pVL != pVR && pM2D->qRecoverEdge(pVL, pVR, pF))
      spFRecovered.insert(pF);

    // Now the left end, if this front entry was advanced another level.
    Vert *pVLChild = pAFE->pVLeftChild();
    if (!(pVLChild == pVInvalidVert) && !(pVLChild->qDeleted()) &&
	pVLChild->qValid() && pM2D->qRecoverEdge(pVL, pVLChild, pF))
      spFRecovered.insert(pF);

    // Finally, the right end, if this front entry was advanced another
    // level, and there isn't a neighbor to trigger recovery of that
    // face already.
    Vert *pVRChild = pAFE->pVRightChild();
    if (!pAFE->pAFERightNeighbor()->qValid() &&
	!(pVRChild == pVInvalidVert) && !(pVRChild->qDeleted()) &&
	pVRChild->qValid() && pM2D->qRecoverEdge(pVR, pVRChild, pF))
      spFRecovered.insert(pF);
  }
  // And that'll do it.
}

void AnisoFront::vCreateQuads()
// For each front face that was successfully marched forwards, create a
// quad.
{
  vMessage(2, "Creating anisotropic quads...\n");

  // This step has to be done before creating quads, after the last
  // pre-quad purge, and inside an AnisoFront member function.  The only
  // place to do that is right here (or create another function solely
  // for that... 
  std::set<Vert*>::iterator iter;
  int iSwaps = 0, iDel = 0;
  vMessage(3, "Deleting extra verts from 3-to=2 aniso front entry merges.\n");
  for (iter = spVToDelete.begin(); iter != spVToDelete.end(); iter++) {
    bool qOK = pM2D->qRemoveVert(*iter, iSwaps);
    assert(qOK);
  }
  vMessage(3, "Deleted %d verts, required %d swaps.\n", iDel, iSwaps);

  {
    // First, force edges into the mesh.
    std::set<Face*> spFRecover;
    vRecoverEdges(spFRecover);
  }

  // Now, for every front face with a descendent, create a quad from its
  // four verts.
  int iAttempt = 0, iSuccess = 0;
  for (int iE = iSize() - 1; iE >= 0 ; iE--) {
    AnisoFrontEntry *pAFE = pAFEEntry(iE);

    if (pAFE->pAFEChild() != pAFEInvalid) {
      iAttempt++;
      // Grab the verts of the front entry, and try to create a quad
      Vert *pVL = pAFE->pVLeft();
      Vert *pVR = pAFE->pVRight();
      if (pVL != pVR) {
	Vert *pVLChild = pAFE->pVLeftChild();
	Vert *pVRChild = pAFE->pVRightChild();
	if (pVLChild->qValid() && !pVLChild->qDeleted() &&
	    pVRChild->qValid() && !pVRChild->qDeleted() &&
	    pVLChild != pVRChild) {
	  if (pM2D->qCreateQuadFromVerts(pVL, pVR, pVRChild, pVLChild))
	    iSuccess++;
	}
      }
    }
  }
  pM2D->vPurge();
  pM2D->vSetAllHintFaces();
  vMessage(2, "Attempted to create %d quads, succeeded %d times.\n",
	   iAttempt, iSuccess);
}

void AnisoFront::vCreateChildEntry(AnisoFrontEntry * const pAFE)
{
  assert(pAFE->qValid());
  assert(pAFE->pVRightChild()->qValid());
  assert(pAFE->pVLeftChild()->qValid());
  AnisoFrontEntry *pAFENew = pAFENewEntry();
  Vert *pVLeft = pAFE->pVLeftChild();
  Vert *pVRight = pAFE->pVRightChild();
  pAFENew->vAssign(pVLeft, pVRight, pAFE);
  double dLen = dDIST2D(pVLeft->adCoords(), pVRight->adCoords());
  pAFENew->vSetSize(std::max(pAFE->dSize() * dStretch,
			     dLen * dDefaultThick));
  pVLeft->vSetLS(dLen);
  pVRight->vSetLS(dLen);
  assert(pAFE->qAdvanceFromHere());
  if (pAFE->qAddedInConvexCorner()) pAFENew->vMarkAddedConvex();
  vMessage(3, "Created new front entry using verts at:\n");
  vMessage(3, "  (%10.6f, %10.6f) (left)\n,",
	   pAFENew->pVLeft()->dX(), pAFENew->pVLeft()->dY());
  vMessage(3, "  (%10.6f, %10.6f) (right)\n,",
	   pAFENew->pVRight()->dX(), pAFENew->pVRight()->dY());
}
