// There are still some holes in this new length scale code (as of
// 1/01).  For example:
//   1. Two-sided patches may return misleading distance information.
//   2. Unless bdry topology may be a problem (such as a circle/sphere,
//      or the 2D smile case (two circular arcs with common endpoints and a
//      small gap between).

// Fix Vertex copy con / operator= so that they work properly.  Also,
// what about updating length scale after a purge?  The influence data
// will be hosed, so what to do?...

#include "GR_config.h"
#include <set>
#include <map>
#include <utility>  // Needed to get definition of pair.
using std::pair;
#include "GR_events.h"
#include "GR_Bdry.h"
#include "GR_Geometry.h"
#include "GR_Mesh.h"
#include "GR_Vec.h"
#include "GR_Vertex.h"

// static void vFirstSecondNearest(const double adLengths[],
// 				const int iN,
// 				int &iFirst, double &dFirstLen,
// 				int &iSecond, double &dSecondLen)
// {
//   iFirst = iSecond = -1;
//   dFirstLen = dSecondLen = LARGE_DBL;
//   for (int i = 0; i < iN; i++) {
//     if (adLengths[i] < dFirstLen) {
//       dSecondLen = dFirstLen;
//       iSecond = iFirst;
//       dFirstLen = adLengths[i];
//       iFirst = i;
//     }
//     else if (adLengths[i] < dSecondLen) {
//       dSecondLen = adLengths[i];
//       iSecond = i;
//     }
//   }
//   assert(dSecondLen >= dFirstLen);
//   assert(iFirst != -1 || iN == 0);
//   assert(iSecond != -1 || iN <= 1);
// }

// After coarsening and/or adapt-to-length-scale, the length scale for
// all vertices is okay, but the identity of the nearest and
// second-nearest entities is hopelessly muddled.  This is true in part
// because of the deletion of entities, and in part because purging
// remaps which entities are stored where.  The following routine
// re-sanifies the influence data by using the nearest and second
// nearest vertices.  (In principle, this could be off because of a
// nearby bdry patch, but in practice, with well-shaped cells, that
// patch can't be much closer than the nearest vert anyway.
//
// Note: This function appears to buggy, in that it continually reduces
// length scale after purging, at least in 2D.  I'm not going to try to
// fix this presently, because there's a new length scale calculation in
// a development branch that will hopefully sidestep this problem
// entirely.  CFO-G, Feb 2010. 
void Mesh::vUpdateInfluence()
{
  SUMAA_LOG_EVENT_BEGIN(UPDATE_LENGTH);
  int iV;
  for (iV = 0; iV < iNumVerts(); iV++) {
    Vert *pV = pVVert(iV);
    std::set<Cell*> spCTmp;
    std::set<Vert*> spVNeigh;
    vNeighborhood(pV, spCTmp, spVNeigh);

    std::set<Vert*>::iterator iterV;
    std::map<double, Vert*> mNeighDist;
    for (iterV = spVNeigh.begin(); iterV != spVNeigh.end(); iterV++) {
      mNeighDist.insert(std::pair<double, Vert*>
			(dDistanceBetween(pV, *iterV), *iterV));
    }

    assert(mNeighDist.size() >= 1);
    // Ignore the distance data; we're -not- resetting the length scale
    // here! 
    std::map<double, Vert*>::iterator iter_map = mNeighDist.begin();
    Vert *pVFirst = iter_map->second;
    if (mNeighDist.size() >= 2) iter_map++;
    Vert *pVSecond = iter_map->second;

    pV->vSetClosest(pVFirst);
    pV->vSetSecond (pVSecond);
  } 
  SUMAA_LOG_EVENT_END(UPDATE_LENGTH);
}

void Mesh::vInitLengthScale(const double dRes, const double dGrade)
{
  SUMAA_LOG_EVENT_BEGIN(INIT_LENGTH);
  dLipschitzAlpha = dGrade;
  dMeshResolution = dRes;
  qLengthScaleInitialized = true;
  std::set<Cell*> spCInc;
  std::set<Vert*> spVNeigh;
  std::set<BFace*> spBFInc;
  bool qBdryVert;
  bool qSetFirst = false;
  bool qSetSecond = false;

  vMessage(1, "Setting up initial geometric length scale for mesh...\n");
  if (qLengthScaleFromCellSizes) {
    // Calculate the length scale based on cell sizes around the vertex
    double *adNetAngle = new double[iNumVerts()];
    double *adVertSize = new double[iNumVerts()];
//      bool qAnyInteriorVerts = false;
    int i;
    for (i = 0; i < iNumVerts(); i++) {
      adNetAngle[i] = adVertSize[i] = 0;
//        if (!pVVert(i)->qIsBdryVert()) qAnyInteriorVerts = true;
    }
    for (i = 0; i < iNumCells(); i++) {
      Cell *pC = pCCell(i);
      double dPartialCellSize = pC->dSize() / pC->iNumVerts();
      double adAngles[4];
      int iN;
      pC->vAllSolid(adAngles, &iN);
      assert(iN == pC->iNumVerts());
      for (int ii = 0; ii < pC->iNumVerts(); ii++) {
	int iV = iVertIndex(pC->pVVert(ii));
	adNetAngle[iV] += adAngles[ii];
	adVertSize[iV] += dPartialCellSize;
      }
    }
    
    int iDim = pVVert(0)->iSpaceDimen();
    const double dPower = 1./iDim;
    for (i = 0; i < iNumVerts(); i++) {
      // The net angle will be the same for all interior verts.  The
      // following expression is a convenient way to handle boundary
      // verts.  Some tweaking will probably be needed to keep meshopt
      // from going nuts on the length scale.  -After- that tweaking is
      // done, then we'll work on the issue of coarsening, and tweak a
      // constant there.
      adVertSize[i] = pow((2*iDim*adVertSize[i] / (M_PI*adNetAngle[i]/180)),
			  dPower);
    }
    delete [] adNetAngle;
  
    for (i = 0; i < iNumVerts(); i++) {
      pVVert(i)->vSetLS(adVertSize[i] * dMeshResolution);
    }
    delete [] adVertSize;
    dMeshResolution = 1;
  }
  
  else {
    // Calculate length scale based on boundary data.
    for (int iVert = iNumVerts() - 1; iVert >= 0; iVert --) {
      qSetFirst = false;
      qSetSecond = false;
      
      Vert *pV = pVVert(iVert);
      if (pV->qDeleted()) continue;
      if (pV->iSpaceDimen() == 2) {
	vMessage(3, " Vertex %d, located at (%g, %g)\n",
		 iVert, pV->dX(), pV->dY());
      }
      else {
	vMessage(3, " Vertex %d, located at (%g, %g, %g)\n",
		 iVert, pV->dX(), pV->dY(), pV->dZ());
      }
      // For all points, begin by finding the neighboring verts, all
      // incident cells, and all incident bdry faces, if any exist.
      vNeighborhood(pV, spCInc, spVNeigh, &spBFInc, &qBdryVert);
      vMessage(3, "  Vertex has %zd inc cells, %zd inc bfaces, %zd neigh verts\n",
	       spCInc.size(), spBFInc.size(), spVNeigh.size());
      pV->vSetClosest(pV);
      qSetFirst = true;
      double dLocalFeatureSize = LARGE_DBL;
      
      // For verts on the bdry, we need to get a set of all bdry patches
      // incident on neighbors but not of pV, so that we can find out
      // how far it is to each of them.  Here, we're interested in the
      // closest one.
      
      if (qBdryVert) {
	vMessage(3, "  Bdry vert!  Making a list of patches incident on neighbors...\n");
	// Begin by making a set of all bdry patches incident on pV.
	assert(spBFInc.size() >= 2);
	int iBF = 0;
	std::set<BFace*>::iterator iterBF;
	for (iterBF = spBFInc.begin(); iterBF != spBFInc.end();
	     iterBF++, iBF++) {
	  BFace *pBF = *iterBF;
	  BdryPatch *pBP = pBF->pPatchPointer();
	  double dLocalCurve = pBP->dCurvature(pV->adCoords());
	  double dLocalRadius;
	  if (iFuzzyComp(dLocalCurve, 0) == 0) {
	    dLocalRadius = LARGE_DBL;
	  }
	  else {
	    dLocalRadius = 1./dLocalCurve;
	  }
	  dLocalFeatureSize = min(dLocalFeatureSize, dLocalRadius);
	  vMessage(3, "Local radius of curvature: %10.6g  LFS: %10.6g\n",
		   dLocalRadius, dLocalFeatureSize);
	}
	
	std::set<BdryPatch*> spBPIncOnNeigh;
	std::set<Vert*>::iterator iterV;
	for (iterV = spVNeigh.begin(); iterV != spVNeigh.end(); iterV++) {
	  Vert *pVNeigh = *iterV;
	  if (pVNeigh->qIsBdryVert()) {
	    std::set<Cell*> spCIncNeigh;
	    std::set<Vert*> spVNeighNeigh;
	    std::set<BFace*> spBFIncNeigh;
	    bool qBdryVertNeigh;
	    vNeighborhood(pVNeigh, spCIncNeigh, spVNeighNeigh,
			  &spBFIncNeigh, &qBdryVertNeigh);
	    assert(qBdryVertNeigh);
	    assert(spBFIncNeigh.size() >= 2);
	    assert(spVNeighNeigh.count(pV) != 0);
	    
	    for (iterBF = spBFIncNeigh.begin();
		 iterBF != spBFIncNeigh.end();
		 iterBF++) {
	      spBPIncOnNeigh.insert((*iterBF)->pPatchPointer());
	    }
	  } // Done adding patches for this bdry vert
	} // Done checking all neighbors
	
	// Now remove bdry patches that are incident on pV
	for (iterBF = spBFInc.begin(); iterBF != spBFInc.end();
	     iterBF++) {
	  BFace *pBF = *iterBF;
	  spBPIncOnNeigh.erase(pBF->pPatchPointer());
	}

	// How many are left?
	int iNumNeighBPatches = spBPIncOnNeigh.size();
	vMessage(3, "  %d such patches exist\n", iNumNeighBPatches);
	vMessage(3, "  Local feature size currently %g\n",
		 dLocalFeatureSize); 
	if (iNumNeighBPatches >= 1) {
	  double dMinDist = LARGE_DBL;
	  std::set<BdryPatch*>::iterator iterBP;
	  std::set<BdryPatch*>::iterator iterClosest = spBPIncOnNeigh.end();
	  for (iterBP = spBPIncOnNeigh.begin();
	       iterBP != spBPIncOnNeigh.end();
	       iterBP++) {
	    BdryPatch *pBP = *iterBP;

	    // This is a tricky spot, as demonstrated by the original
	    // tire.smesh input file, which has its length scale
	    // collapse because of patches that fold back on each other.
	    // Other cases seem to suffer from this bug, too, just not
	    // as incredibly severely.
	    // FIX ME: 0.3.0
	    int iSide = 0;
	    double dDistToBdry =
	      fabs(pBP->dDistFromBdry(pV->adCoords(), &iSide));
	    // For double-sided patches, the absolute value is fine.
	    // For single-sided, adjust so that distance behind the
	    // patch is considered negative.
 	    if (iSide == -1 && pBP->qIsBoundary()) dDistToBdry *= -1;

	    if (dDistToBdry > 0 && dDistToBdry < dMinDist) {
	      dMinDist = dDistToBdry;
	      vMessage(3, "  Local feature size currently %g\n",
		       dMinDist); 
	      iterClosest = iterBP;
	    }
	  }
	  if (iterClosest != spBPIncOnNeigh.end()) {
	    dLocalFeatureSize = dMinDist;
	    vMessage(3, "  Local feature size currently %g\n",
		     dLocalFeatureSize); 
	    pV->vSetSecond(*iterClosest);
	    qSetSecond = true;
	  }
	}
      } 
      
      // Find the distance to each neighboring vertex.  Also, find the
      // first and second nearest.
      std::set<Vert*>::iterator iterV;
      std::map<double, Vert*> mNeighDist;
      for (iterV = spVNeigh.begin(); iterV != spVNeigh.end();
	   iterV++) {
	mNeighDist.insert(std::pair<double, Vert*>
			  (dDistanceBetween(pV, *iterV), *iterV));
      }
      // Now we're ready to find the real length scale here.

      assert(mNeighDist.size() >= 1);
      std::map<double, Vert*>::iterator iter_map = mNeighDist.begin();
      double dProposedLen;
      Vert *pVFirst = iter_map->second;
      if (qBdryVert && !pVFirst->qIsBdryVert()) {
	dProposedLen = iter_map->first;
	if (mNeighDist.size() != 1) iter_map++;
	// Don't need the second length in this case.
      }
      else {
	// Don't need the distance to the nearest vert in this case.
	if (mNeighDist.size() != 1) iter_map++;
	dProposedLen = iter_map->first;
      }
      Vert *pVSecond = iter_map->second;

      // This quantity will be huge for interior verts
      double dLS = dLocalFeatureSize * dMeshResolution;  
      vMessage(3, "  Based on bdry data, estimate length scale to be %g\n",
	       dLS);
      
      if (dLS > dProposedLen) {
	dLS = dProposedLen;
	pV->vSetClosest(pVFirst);
	qSetFirst = true;
	pV->vSetSecond(pVSecond);
	qSetSecond = true;
      }
      
      // Hack by Charles. Not sure this is correct.
      if (!qSetFirst) {
	pV->vSetClosest(pVFirst);
	vMessage(2, "WARNING: Using the hack for the closest vert!\n");
	qSetFirst = true;
      }
      
      if (!qSetSecond) {
	pV->vSetSecond(pVSecond);
	vMessage(2, "WARNING: Using the hack for the second vert!\n");
	qSetSecond = true;			
      }
      
      assert(qSetFirst);
      assert(qSetSecond);
    
      pV->vSetLS(dLS);
      spVUpdateLS.insert(pV);
      vMessage(3, " Ungraded LS for vertex %d is %g\n", iVert,
	       dLS);
    }
  } // Done with this vert

  // Now make sure that grading, etc, is okay...
  vMessage(2, "Updating length scale to take grading into account...\n");
  vUpdateLengthScale();
  vMessage(2, "Done with grading check.\n");
  vMessage(2, "Done with length scale initialization.\n");
  SUMAA_LOG_EVENT_END(INIT_LENGTH);
}

void Mesh::vUpdateLengthScale(std::set<Vert*>& spV)
{
  assert(spVUpdateLS.empty());
  spVUpdateLS.insert(spV.begin(), spV.end());
  vUpdateLengthScale();
}

void Mesh::vUpdateLengthScale(Vert * const pVIn)
{
  // Checking for correct grading and boundary influence.  Each vert
  // gets checked against its neighbors.  If the vert's length scale
  // changes, then its neighbors get added to the set of verts to check
  // (if they're already in the set, they don't get duplicated.

  SUMAA_LOG_EVENT_BEGIN(LENGTH_SCALE);
  SUMAA_LOG_EVENT_BEGIN(UPDATE_LENGTH);
  if (pVIn->qValid()) spVUpdateLS.insert(pVIn);

  while (!spVUpdateLS.empty()) {
    std::set<Vert*>::iterator iter = spVUpdateLS.begin();
    Vert *pV = *iter;
    spVUpdateLS.erase(iter);
    vSetLengthScale(pV);
  }
  assert(spVUpdateLS.empty());
  SUMAA_LOG_EVENT_END(UPDATE_LENGTH);
  SUMAA_LOG_EVENT_END(LENGTH_SCALE);
}

void Mesh::vSetLengthScale(Vert * const pV)
  // For the given vertex, find its correct length scale, then tag its
  // neighbors for subsequent checking.
{
  // First ID all the entities that contribute to length scale estimates
  // for the neighbors of this vert.
  if (pV->qDeleted()) return;

  if (pV->iSpaceDimen() == 2) {
    vMessage(3, "Setting length scale for vertex %d at (%f, %f); was %f\n",
	     iVertIndex(pV), pV->dX(), pV->dY(), pV->dLS());
  }
  else {
    vMessage(4, "Setting length scale for vertex %d at (%f, %f, %f); was %f\n",
	     iVertIndex(pV), pV->dX(), pV->dY(), pV->dZ(), pV->dLS());
  }
  std::set<Vert*> spVInfluence;
  std::set<BdryPatch*> spBPInfluence;

  std::set<Vert*> spVNeigh;
  
  std::set<Face*> spFNearby;
  {
    std::set<Cell*> spCInc;
    std::set<BFace*> spBFInc;
    bool qBdryVert;
    vNeighborhood(pV, spCInc, spVNeigh, &spBFInc, &qBdryVert, &spFNearby);
  }

  bool qChangedLS = false;
  double dLenScale;
  if (pV->dLS() > 0) 
    dLenScale = pV->dLS();
  else
    dLenScale = LARGE_DBL;
  
  // First, do a check for grading.
  std::set<Vert*>::iterator iterV;
  for (iterV = spVNeigh.begin(); iterV != spVNeigh.end(); iterV++) {
    Vert *pVNeigh = *iterV;
    double dNeighLen = pVNeigh->dLS();
    // If multiple verts are inserted between checks, and two of them
    // are adjacent, then we've got terrible trouble if we try to use
    // the neighbor's (non-existent) length scale data.
    if (dNeighLen > 0) {
      // If the neighbor has a length scale, then this code is safe to
      // execute.  
      double dDistance = dDist(pV, pVNeigh);
      double dLipschitzLen = dNeighLen + dDistance * dAlpha();
      if (iFuzzyComp(dLipschitzLen, dLenScale) == -1) {
	dLenScale = dLipschitzLen;
	pV->vCopyInfluence(pVNeigh);
	pV->vSetLS(dLenScale);
	vMessage(3, "Vertex %d has length scale %f.\n",
		 iVertIndex(pV), dLenScale);
	qChangedLS = true;
      }
    } // The neighbor has a length scale
  } // Done checking all neighbors for grading

  // There are no more checks to be done for a mesh that has been
  // generated elsewhere and therefore has its length scale determined
  // by cell sizes rather than distance to boundaries.
  if (qLengthScaleFromCellSizes) return;

  // Otherwise, check boundary distances carefully to determine length
  // scale. 
  
  // For all BFace's incident on Faces on the perimeter of the star of
  // cells incident on pV, add their BdryPatches to the list of those
  // to be checked.  This fixes some cases where coincidence robs us
  // of the nearby patch we need to really set the length scale
  // properly.
  std::set<Face*>::iterator iterF;
  for (iterF = spFNearby.begin(); iterF != spFNearby.end(); iterF++) {
    Face *pF = *iterF;
    Cell *pC = pF->pCCellLeft();
    if (pC->qIsBdryCell()) {
      BFace *pBF = dynamic_cast<BFace*>(pC);
      spBPInfluence.insert(pBF->pPatchPointer());
    }
    else {
      pC = pF->pCCellRight();
      if (pC->qIsBdryCell()) {
	BFace *pBF = dynamic_cast<BFace*>(pC);
	spBPInfluence.insert(pBF->pPatchPointer());
      }
    }
  }

  if (pV->dLS() > 0) {
    dLenScale = pV->dLS();
    Vert *pVTmp;
    BdryPatch *pBPTmp;
    pV->vGetClosest(&pVTmp, &pBPTmp);
    if (pVTmp->qValid()) {
      assert(pBPTmp == NULL);
      spVInfluence.insert(pVTmp);
    }
    else {
      spBPInfluence.insert(pBPTmp);
    }

    pV->vGetSecond(&pVTmp, &pBPTmp);
    if (pVTmp->qValid()) {
      assert(pBPTmp == NULL);
      spVInfluence.insert(pVTmp);
    }
    else {
      spBPInfluence.insert(pBPTmp);
    }
  }
  
  for (iterV = spVNeigh.begin(); iterV != spVNeigh.end(); iterV++) {
    Vert *pVNeigh = *iterV;
    double dNeighLen = pVNeigh->dLS();
    // If multiple verts are inserted between checks, and two of them
    // are adjacent, then we've got terrible trouble if we try to use
    // the neighbor's (non-existent) length scale data.
    if (dNeighLen > 0) {
      Vert *pVClosest, *pVSecond;
      BdryPatch *pBPClosest, *pBPSecond;
      
      pVNeigh->vGetClosest(&pVClosest, &pBPClosest);
      if (pVClosest->qValid()) {
	assert(!pBPClosest->qValid());
	// The following kludge courtesy of length scale issues with
	// vertex deletion.
// 	if (pVClosest->qDeleted()) {
// 	  spVUpdateLS.insert(pVNeigh);
// 	}
// 	else {
	  spVInfluence.insert(pVClosest);
// 	}
      }
      else {
	assert(pBPClosest->qValid());
	spBPInfluence.insert(pBPClosest);
      }
      
      pVNeigh->vGetSecond(&pVSecond, &pBPSecond);
      if (pVSecond->qValid()) {
	assert(!pBPSecond->qValid());
	// The following kludge courtesy of length scale issues with
	// vertex deletion.
// 	if (pVSecond->qDeleted()) {
// 	  spVUpdateLS.insert(pVNeigh);
// 	}
// 	else {
	  spVInfluence.insert(pVSecond);
// 	}
      }
      else {
	assert(pBPSecond->qValid());
	spBPInfluence.insert(pBPSecond);
      }
    } // Done with work for neighbors with length scales.
  } // Done getting all the potential influencing entities
    
  // Order all the verts and bdry patches
  std::multimap<double, pair<Vert*, BdryPatch*> > mOrdering;

  for (iterV = spVInfluence.begin(); iterV != spVInfluence.end(); iterV++) {
    Vert *pVI = *iterV;
    pair<Vert*, BdryPatch*> pPart(pVI, static_cast<BdryPatch*>(NULL));
    pair<const double, pair<Vert*, BdryPatch*> > pNew(dDist(pV, pVI), pPart); 
    mOrdering.insert(pNew);
  }

  for (std::set<BdryPatch*>::iterator iterBP = spBPInfluence.begin();
       iterBP != spBPInfluence.end(); iterBP++) {
    BdryPatch *pBPI = *iterBP;
    int iSide = 0;
    double dDistance = fabs(pBPI->dDistFromBdry(pV->adCoords(), &iSide));
    // For double-sided patches, the absolute value is fine.
    // For single-sided, adjust so that distance behind the
    // patch is considered negative.
    if (iSide == -1 && pBPI->qIsBoundary()) dDistance *= -1;
    // Don't include bdry patches with the vert on the wrong side
    if (dDistance >= 0) {
      pair<Vert*, BdryPatch*> pPart(pVInvalidVert, pBPI);
      pair<const double, pair<Vert*, BdryPatch*> > pNew(dDistance, pPart);
      mOrdering.insert(pNew);
    }
  }

  switch (mOrdering.size()) {
  case 0:
    // This case should never occur, but sometimes we can salvage it
    // from the grading stuff.
    if (pV->dLS() > 0) break;
    assert(0);
    break;
  case 1:
    {
      // In this case, use the distance to the one entry in the list ---
      // there's not a lot of choice...
      std::multimap<double, pair<Vert*, BdryPatch*> >::iterator itOnly;
      itOnly = mOrdering.begin();
      double dLocalFeatureSize = itOnly->first;
      if (iFuzzyComp(dLocalFeatureSize, 0) == 0 && pV->dLS() > 0) break;
      if (dLocalFeatureSize * dMeshResolution < dLenScale) {
	dLenScale = dLocalFeatureSize * dMeshResolution;
	assert(itOnly->second.first != pV);
	pV->vSetSecond(itOnly->second.first, itOnly->second.second);
	pV->vSetClosest(pV, static_cast<BdryPatch*>(NULL));
	qChangedLS = true;
      }
    }
    break;
  default:
    // Each time a new entity comes up (iter), it must be checked against
    // all previous entities (iter1).  If a disjoint pair (entities that
    // don't share at least one point) is found, then we've hit the
    // jackpot.  Otherwise, keep looking.
    std::multimap<double, pair<Vert*, BdryPatch*> >::iterator itSec, itClose;
    itSec = mOrdering.begin();
    bool qDisjoint = false;
    
    for (itSec = mOrdering.begin();
	 !qDisjoint && (itSec != mOrdering.end());
	 itSec++) {
      Vert *pVTry = itSec->second.first;
      BdryPatch *pBPTry = itSec->second.second;
      for (itClose = mOrdering.begin();
	   !qDisjoint && (itClose != itSec);
	   itClose++) {
	Vert *pVCand = itClose->second.first;
	BdryPatch *pBPCand = itClose->second.second;
	if (pVCand->qValid()) {
	  assert(!pBPCand->qValid());
	  if (pVTry->qValid()) {
	    assert(!pBPTry->qValid());
	    qDisjoint = (pVCand != pVTry);
	  }
	  else {
	    assert(pBPTry->qValid());
	    qDisjoint = pBPTry->qDisjointFrom(pVCand);
	  }
	} // Outer if
	else {
	  assert(pBPCand->qValid());
	  if (pVTry->qValid()) {
	    assert(!pBPTry->qValid());
	    qDisjoint = pBPCand->qDisjointFrom(pVTry);
	  }
	  else {
	    assert(pBPTry->qValid());
	    qDisjoint = pBPCand->qDisjointFrom(pBPTry);
	  }
	} // Outer else
      } // Done checking this pair of iterators
    } // Done checking this next-lowest distance entity.

    // Not sure I really understand how this problem comes up.  Deserves
    // future study and possible fixing. CFO-G 6/05
    // FIX ME
    if (qDisjoint) {
      // Each iterator got incremented as we were bailing out of the
      // loops, so decrement them to get the appropriate data.
      itSec --;
      itClose --;
    }
    else {
      if (pV->dLS() > 0) break;
      // Try to work with this anyway, using the two closest entities.
      itClose = mOrdering.begin();
      itSec = itClose; itSec++;
    }
    
    double dLocalFeatureSize = itSec->first;
    if (dLocalFeatureSize > 0) {
      if (iFuzzyComp(dLocalFeatureSize * dMeshResolution, dLenScale) ==
	  -1) {
	dLenScale = dLocalFeatureSize * dMeshResolution;
	assert(itSec != itClose);
	assert((itSec->second.first != itClose->second.first) ||
	       (!itSec->second.first->qValid() &&
		!itClose->second.first->qValid()));
	assert((itSec->second.second != itClose->second.second) ||
	       (!itSec->second.second->qValid() &&
		!itClose->second.second->qValid()));
	pV->vSetSecond(itSec->second.first, itSec->second.second);
	pV->vSetClosest(itClose->second.first, itClose->second.second);
	qChangedLS = true;
      }
    }
  }

  assert(dLenScale < LARGE_DBL*0.9999);
  
  if (qChangedLS) {
    pV->vSetLS(dLenScale);
    vMessage(3, "Set length scale for vertex %d to %g.\n",
	     iVertIndex(pV), dLenScale); 
    // Mark all neighbors whose length scale would change as a result of
    // grading so that they will be checked...
    for (iterV = spVNeigh.begin(); iterV != spVNeigh.end(); iterV++) {
      Vert *pVNeigh = *iterV;
      if (iFuzzyComp(pVNeigh->dLS(),
		     dLenScale + dAlpha()*dDist(pV, pVNeigh)) == 1) {
	spVUpdateLS.insert(pVNeigh);
      }
    }
  }
}

void Mesh::vRecomputeLengthScale()
{
  qLengthScaleFromCellSizes = true;
  vInitLengthScale(sqrt(2.), dLipschitzAlpha);
}
