#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.0005;
static const double dStretch = 1.005;

static void vInvert2x2(double a2dMat[2][2])
{
  double dDetInv = 1./(a2dMat[0][0]*a2dMat[1][1] - a2dMat[0][1]*a2dMat[1][0]);
  double dTmp = a2dMat[0][0];
  a2dMat[0][0] = a2dMat[1][1];
  a2dMat[1][1] = dTmp;
  a2dMat[0][1] *= -1;
  a2dMat[1][0] *= -1;

  a2dMat[0][0] *= dDetInv;
  a2dMat[0][1] *= dDetInv;
  a2dMat[1][0] *= dDetInv;
  a2dMat[1][1] *= dDetInv;
}

static void vScaleMat(double a2dA[2][2], double a2dB[2][2])
// Replace A with B.A
{
  double a2dTmp[2][2];

  a2dTmp[0][0] = a2dB[0][0]*a2dA[0][0] + a2dB[0][1]*a2dA[1][0];
  a2dTmp[0][1] = a2dB[0][0]*a2dA[0][1] + a2dB[0][1]*a2dA[1][1];
  a2dTmp[1][0] = a2dB[1][0]*a2dA[0][0] + a2dB[1][1]*a2dA[1][0];
  a2dTmp[1][1] = a2dB[1][0]*a2dA[0][1] + a2dB[1][1]*a2dA[1][1];

  a2dA[0][0] = a2dTmp[0][0];
  a2dA[0][1] = a2dTmp[0][1];
  a2dA[1][0] = a2dTmp[1][0];
  a2dA[1][1] = a2dTmp[1][1];
}

static void vMassageEMatrix(double a2dA[2][2], double a2dB[2][2])
// Replace B with -B.A
{
  double a2dTmp[2][2];

  a2dTmp[0][0] = a2dB[0][0]*a2dA[0][0] + a2dB[0][1]*a2dA[1][0];
  a2dTmp[0][1] = a2dB[0][0]*a2dA[0][1] + a2dB[0][1]*a2dA[1][1];
  a2dTmp[1][0] = a2dB[1][0]*a2dA[0][0] + a2dB[1][1]*a2dA[1][0];
  a2dTmp[1][1] = a2dB[1][0]*a2dA[0][1] + a2dB[1][1]*a2dA[1][1];

  a2dB[0][0] = -a2dTmp[0][0];
  a2dB[0][1] = -a2dTmp[0][1];
  a2dB[1][0] = -a2dTmp[1][0];
  a2dB[1][1] = -a2dTmp[1][1];
}

static void vScaleVec(double adVec[2], double a2dA[2][2])
// Replace Vec with A.Vec
{
  double adTmp[] = {a2dA[0][0]*adVec[0] + a2dA[0][1]*adVec[1],
		    a2dA[1][0]*adVec[0] + a2dA[1][1]*adVec[1]};

  adVec[0] = adTmp[0];
  adVec[1] = adTmp[1];
}

static void vEliminateMat(double a2dP[2][2], double a2dQ[2][2],
			  double a2dR[2][2])
// Replace P with P - Q.R.  Can be called with Q or R the same as P
// without any danger, as implemented.
{
  double a2dTmp[2][2];

  a2dTmp[0][0] = a2dQ[0][0]*a2dR[0][0] + a2dQ[0][1]*a2dR[1][0];
  a2dTmp[0][1] = a2dQ[0][0]*a2dR[0][1] + a2dQ[0][1]*a2dR[1][1];
  a2dTmp[1][0] = a2dQ[1][0]*a2dR[0][0] + a2dQ[1][1]*a2dR[1][0];
  a2dTmp[1][1] = a2dQ[1][0]*a2dR[0][1] + a2dQ[1][1]*a2dR[1][1];

  a2dP[0][0] -= a2dTmp[0][0];
  a2dP[0][1] -= a2dTmp[0][1];
  a2dP[1][0] -= a2dTmp[1][0];
  a2dP[1][1] -= a2dTmp[1][1];
}

static void vEliminateVec(double adP[2], double a2dQ[2][2],
			  double adR[2])
// Replace P with P - Q.R
{
  double adTmp[2];

  adTmp[0] = a2dQ[0][0]*adR[0] + a2dQ[0][1]*adR[1];
  adTmp[1] = a2dQ[1][0]*adR[0] + a2dQ[1][1]*adR[1];

  adP[0] -= adTmp[0];
  adP[1] -= adTmp[1];
}

#define TEST_TRI_DIAG
static void vSolveTriDiag(double a4dLHS[][3][2][2],
			  double a2dRHS[][2],
			  const int iN,
			  bool qPeriodic)
{
#define A(i) a4dLHS[i][0]
#define B(i) a4dLHS[i][1]
#define C(i) a4dLHS[i][2]
#define R(i) a2dRHS[i]

  int i;
  if (qPeriodic) {
#ifdef TEST_TRI_DIAG
    // Make a copy for testing.
    double a4dLHSIn[iN][3][2][2];
    double a2dRHSIn[iN][2];

    for (i = 0; i < iN; i++) {
      for (int j1 = 0; j1 < 2; j1++) {
	for (int j2 = 0; j2 < 2; j2++) {
	  a4dLHSIn[i][0][j1][j2] = a4dLHS[i][0][j1][j2];
	  a4dLHSIn[i][1][j1][j2] = a4dLHS[i][1][j1][j2];
	  a4dLHSIn[i][2][j1][j2] = a4dLHS[i][2][j1][j2];
	}
	a2dRHSIn[i][j1] = a2dRHS[i][j1];
      }
    }
    for (i = 0; i < iN; i++) {
      vMessage(4, "Residual for %3d: %20.10G, %20.10G\n",
	       i, a2dRHSIn[i][0], a2dRHS[i][1]);
    }
#endif
#define D(i) a3dRightColumn[i]
#define E(i) a2dBottomBlock

    // Store the data for the right-most column here; D in the alg below.
    double (*a3dRightColumn)[2][2] = new double[iN][2][2];
    for (i = 0; i < iN; i++) {
      a3dRightColumn[i][0][0] = a3dRightColumn[i][0][1] =
	a3dRightColumn[i][1][0] = a3dRightColumn[i][1][1] = 0;
    }
    a3dRightColumn[0][0][0] = a4dLHS[0][0][0][0];
    a3dRightColumn[0][0][1] = a4dLHS[0][0][0][1];
    a3dRightColumn[0][1][0] = a4dLHS[0][0][1][0];
    a3dRightColumn[0][1][1] = a4dLHS[0][0][1][1];

    // Store the data for the bottom-most row here; E in the alg below.
    // Yes, this could be done in a single block, but memory's cheap.
    double a2dBottomBlock[2][2];
    a2dBottomBlock[0][0] = a4dLHS[iN-1][2][0][0];
    a2dBottomBlock[0][1] = a4dLHS[iN-1][2][0][1];
    a2dBottomBlock[1][0] = a4dLHS[iN-1][2][1][0];
    a2dBottomBlock[1][1] = a4dLHS[iN-1][2][1][1];

    // Forward elimination.
    for (i = 0; i < iN - 3; i++) {
      vInvert2x2(B(i));
      // C_i <- B_i^-1 . C_i
      vScaleMat(C(i), B(i));
      // D_i <- B_i^-1 . D_i
      vScaleMat(D(i), B(i));
      // R_i <- B_i^-1 . R_i
      vScaleVec(R(i), B(i));
      // B_i+1 -= A_i+1 C_i
      vEliminateMat(B(i+1), A(i+1), C(i));
      // D_i+1 = - A_i+1 D_i
      vEliminateMat(D(i+1), A(i+1), D(i));
      // R_i+1 -= A_i+1 R_i
      vEliminateVec(R(i+1), A(i+1), R(i));
      // R_(iN-1) -= E_i R_i
      vEliminateVec(R(iN-1), E(i), R(i));
      // B_(iN-1) -= E_i D_i
      vEliminateMat(B(iN-1), E(i), D(i));
      // E_i+1 = -E_i C_i
      vMassageEMatrix(C(i), E(i));
    }

    // Now finish the elimination (last three rows)
    // Scale row iN-3
    vInvert2x2(B(iN-3));
    vScaleMat(C(iN-3), B(iN-3));
    vScaleMat(D(iN-3), B(iN-3));
    vScaleVec(R(iN-3), B(iN-3));

    // Eliminate in row iN-2
    vEliminateMat(B(iN-2), A(iN-2), C(iN-3));
    vEliminateMat(C(iN-2), A(iN-2), D(iN-3));
    vEliminateVec(R(iN-2), A(iN-2), R(iN-3));

    // Eliminate in row iN-1
    vEliminateMat(A(iN-1), E(iN-3), C(iN-3));
    vEliminateMat(B(iN-1), E(iN-3), D(iN-3));
    vEliminateVec(R(iN-1), E(iN-3), R(iN-3));

    // Scale row iN-2
    vInvert2x2(B(iN-2));
    vScaleMat(C(iN-2), B(iN-2));
    vScaleVec(R(iN-2), B(iN-2));

    // Eliminate in row iN-1
    vEliminateMat(B(iN-1), A(iN-1), C(iN-2));
    vEliminateVec(R(iN-1), A(iN-1), R(iN-2));

    // Scale row iN-1
    vInvert2x2(B(iN-1));
    vScaleVec(R(iN-1), B(iN-1));

    // Backward substitution
    // First row is special, because there's only one block to consider.
    vEliminateVec(R(iN-2), C(iN-2), R(iN-1));

    for (i = iN - 3; i >= 0; i--) {
      // R_i -= C_i R_i+1 + D_i R_(iN-1)
      vEliminateVec(R(i), C(i), R(i+1));
      vEliminateVec(R(i), D(i), R(iN-1));
    }

#ifdef TEST_TRI_DIAG
    // Testing
    for (i = 0; i < iN; i++) {
      vMessage(4, "Data for point %d:\n", i);
      vMessage(4, "LHS: [%15.8G %15.8G] [%15.8G %15.8G] [%15.8G %15.8G]\n",
	       a4dLHSIn[i][0][0][0], a4dLHSIn[i][0][0][1],
	       a4dLHSIn[i][1][0][0], a4dLHSIn[i][1][0][1],
	       a4dLHSIn[i][2][0][0], a4dLHSIn[i][2][0][1]);
      vMessage(4, "LHS: [%15.8G %15.8G] [%15.8G %15.8G] [%15.8G %15.8G]\n",
	       a4dLHSIn[i][0][1][0], a4dLHSIn[i][0][1][1],
	       a4dLHSIn[i][1][1][0], a4dLHSIn[i][1][1][1],
	       a4dLHSIn[i][2][1][0], a4dLHSIn[i][2][1][1]);
      vMessage(4, "RHS: [%15.8G %15.8G]   Solution:  [%15.8G %15.8G]\n",
	       a2dRHSIn[i][0], a2dRHSIn[i][1],
	       a2dRHS[i][0], a2dRHS[i][1]);
    }

    a2dRHSIn[0][0] -= (a4dLHSIn[0][0][0][0]*a2dRHS[iN-1][0] +
		       a4dLHSIn[0][0][0][1]*a2dRHS[iN-1][1] +
		       a4dLHSIn[0][1][0][0]*a2dRHS[0][0] +
		       a4dLHSIn[0][1][0][1]*a2dRHS[0][1] +
		       a4dLHSIn[0][2][0][0]*a2dRHS[1][0] +
		       a4dLHSIn[0][2][0][1]*a2dRHS[1][1]);
    a2dRHSIn[0][1] -= (a4dLHSIn[0][0][1][0]*a2dRHS[iN-1][0] +
		       a4dLHSIn[0][0][1][1]*a2dRHS[iN-1][1] +
		       a4dLHSIn[0][1][1][0]*a2dRHS[0][0] +
		       a4dLHSIn[0][1][1][1]*a2dRHS[0][1] +
		       a4dLHSIn[0][2][1][0]*a2dRHS[1][0] +
		       a4dLHSIn[0][2][1][1]*a2dRHS[1][1]);
    for (i = 1; i < iN - 1; i++) {
      a2dRHSIn[i][0] -= (a4dLHSIn[i][0][0][0]*a2dRHS[i-1][0] +
			 a4dLHSIn[i][0][0][1]*a2dRHS[i-1][1] +
			 a4dLHSIn[i][1][0][0]*a2dRHS[i][0] +
			 a4dLHSIn[i][1][0][1]*a2dRHS[i][1] +
			 a4dLHSIn[i][2][0][0]*a2dRHS[i+1][0] +
			 a4dLHSIn[i][2][0][1]*a2dRHS[i+1][1]);
      a2dRHSIn[i][1] -= (a4dLHSIn[i][0][1][0]*a2dRHS[i-1][0] +
			 a4dLHSIn[i][0][1][1]*a2dRHS[i-1][1] +
			 a4dLHSIn[i][1][1][0]*a2dRHS[i][0] +
			 a4dLHSIn[i][1][1][1]*a2dRHS[i][1] +
			 a4dLHSIn[i][2][1][0]*a2dRHS[i+1][0] +
			 a4dLHSIn[i][2][1][1]*a2dRHS[i+1][1]);
    }
    a2dRHSIn[iN-1][0] -= (a4dLHSIn[iN-1][0][0][0]*a2dRHS[iN-2][0] +
		       a4dLHSIn[iN-1][0][0][1]*a2dRHS[iN-2][1] +
		       a4dLHSIn[iN-1][1][0][0]*a2dRHS[iN-1][0] +
		       a4dLHSIn[iN-1][1][0][1]*a2dRHS[iN-1][1] +
		       a4dLHSIn[iN-1][2][0][0]*a2dRHS[0][0] +
		       a4dLHSIn[iN-1][2][0][1]*a2dRHS[0][1]);
    a2dRHSIn[iN-1][1] -= (a4dLHSIn[iN-1][0][1][0]*a2dRHS[iN-2][0] +
		       a4dLHSIn[iN-1][0][1][1]*a2dRHS[iN-2][1] +
		       a4dLHSIn[iN-1][1][1][0]*a2dRHS[iN-1][0] +
		       a4dLHSIn[iN-1][1][1][1]*a2dRHS[iN-1][1] +
		       a4dLHSIn[iN-1][2][1][0]*a2dRHS[0][0] +
		       a4dLHSIn[iN-1][2][1][1]*a2dRHS[0][1]);
    for (i = 0; i < iN; i++) {
      vMessage(4, "Residual for %3d: %20.10G, %20.10G\n",
	       i, a2dRHSIn[i][0], a2dRHSIn[i][1]);
    }
#endif

#undef D
#undef E
  }
  else {
#ifdef TEST_TRI_DIAG
    // Make a copy for testing.
    double a4dLHSIn[iN][3][2][2];
    double a2dRHSIn[iN][2];

    for (i = 0; i < iN; i++) {
      for (int j1 = 0; j1 < 2; j1++) {
	for (int j2 = 0; j2 < 2; j2++) {
	  a4dLHSIn[i][0][j1][j2] = a4dLHS[i][0][j1][j2];
	  a4dLHSIn[i][1][j1][j2] = a4dLHS[i][1][j1][j2];
	  a4dLHSIn[i][2][j1][j2] = a4dLHS[i][2][j1][j2];
	}
	a2dRHSIn[i][j1] = a2dRHS[i][j1];
      }
    }
    for (i = 0; i < iN; i++) {
      vMessage(4, "Residual for %3d: %20.10G, %20.10G\n",
	       i, a2dRHSIn[i][0], a2dRHS[i][1]);
    }
#endif

    // Forward elimination.
    for (i = 0; i < iN - 1; i++) {
      vInvert2x2(B(i));
      // C_i <- B_i^-1 . C_i
      vScaleMat(C(i), B(i));
      // R_i <- B_i^-1 . R_i
      vScaleVec(R(i), B(i));
      // B_i+1 -= A_i+1 C_i
      vEliminateMat(B(i+1), A(i+1), C(i));
      // R_i+1 -= A_i+1 R_i
      vEliminateVec(R(i+1), A(i+1), R(i));
    }

    assert(i == iN-1);
    vInvert2x2(B(i));
    // R_iN <- Binv_iN R_iN
    vScaleVec(R(i), B(i));

    // Backward substitution
    for (i = iN - 1; i > 0; i--) {
      // R_i-1 -= C_i R_i
      vEliminateVec(R(i-1), C(i-1), R(i));
    }

#ifdef TEST_TRI_DIAG
    // Testing
    for (i = 0; i < iN; i++) {
      vMessage(4, "Data for point %d:\n", i);
      vMessage(4, "LHS: [%15.8G %15.8G] [%15.8G %15.8G] [%15.8G %15.8G]\n",
	       a4dLHSIn[i][0][0][0], a4dLHSIn[i][0][0][1],
	       a4dLHSIn[i][1][0][0], a4dLHSIn[i][1][0][1],
	       a4dLHSIn[i][2][0][0], a4dLHSIn[i][2][0][1]);
      vMessage(4, "LHS: [%15.8G %15.8G] [%15.8G %15.8G] [%15.8G %15.8G]\n",
	       a4dLHSIn[i][0][1][0], a4dLHSIn[i][0][1][1],
	       a4dLHSIn[i][1][1][0], a4dLHSIn[i][1][1][1],
	       a4dLHSIn[i][2][1][0], a4dLHSIn[i][2][1][1]);
      vMessage(4, "RHS: [%15.8G %15.8G]   Solution:  [%15.8G %15.8G]\n",
	       a2dRHSIn[i][0], a2dRHSIn[i][1],
	       a2dRHS[i][0], a2dRHS[i][1]);
    }

    a2dRHSIn[0][0] -= (a4dLHSIn[0][1][0][0]*a2dRHS[0][0] +
		       a4dLHSIn[0][1][0][1]*a2dRHS[0][1] +
		       a4dLHSIn[0][2][0][0]*a2dRHS[1][0] +
		       a4dLHSIn[0][2][0][1]*a2dRHS[1][1]);
    a2dRHSIn[0][1] -= (a4dLHSIn[0][1][1][0]*a2dRHS[0][0] +
		       a4dLHSIn[0][1][1][1]*a2dRHS[0][1] +
		       a4dLHSIn[0][2][1][0]*a2dRHS[1][0] +
		       a4dLHSIn[0][2][1][1]*a2dRHS[1][1]);
    for (i = 1; i < iN - 1; i++) {
      a2dRHSIn[i][0] -= (a4dLHSIn[i][0][0][0]*a2dRHS[i-1][0] +
			 a4dLHSIn[i][0][0][1]*a2dRHS[i-1][1] +
			 a4dLHSIn[i][1][0][0]*a2dRHS[i][0] +
			 a4dLHSIn[i][1][0][1]*a2dRHS[i][1] +
			 a4dLHSIn[i][2][0][0]*a2dRHS[i+1][0] +
			 a4dLHSIn[i][2][0][1]*a2dRHS[i+1][1]);
      a2dRHSIn[i][1] -= (a4dLHSIn[i][0][1][0]*a2dRHS[i-1][0] +
			 a4dLHSIn[i][0][1][1]*a2dRHS[i-1][1] +
			 a4dLHSIn[i][1][1][0]*a2dRHS[i][0] +
			 a4dLHSIn[i][1][1][1]*a2dRHS[i][1] +
			 a4dLHSIn[i][2][1][0]*a2dRHS[i+1][0] +
			 a4dLHSIn[i][2][1][1]*a2dRHS[i+1][1]);
    }
    a2dRHSIn[iN-1][0] -= (a4dLHSIn[iN-1][0][0][0]*a2dRHS[iN-2][0] +
			  a4dLHSIn[iN-1][0][0][1]*a2dRHS[iN-2][1] +
			  a4dLHSIn[iN-1][1][0][0]*a2dRHS[iN-1][0] +
			  a4dLHSIn[iN-1][1][0][1]*a2dRHS[iN-1][1]);
    a2dRHSIn[iN-1][1] -= (a4dLHSIn[iN-1][0][1][0]*a2dRHS[iN-2][0] +
			  a4dLHSIn[iN-1][0][1][1]*a2dRHS[iN-2][1] +
			  a4dLHSIn[iN-1][1][1][0]*a2dRHS[iN-1][0] +
			  a4dLHSIn[iN-1][1][1][1]*a2dRHS[iN-1][1]);
    for (i = 0; i < iN; i++) {
      vMessage(4, "Residual for %3d: %20.10G, %20.10G\n",
	       i, a2dRHSIn[i][0], a2dRHSIn[i][1]);
    }
#endif
  }
#undef A
#undef B
#undef C
#undef R
}

void vMergeFE_2for1(AnisoFrontEntry* const pAFELeft,
		    AnisoFrontEntry* const pAFERight,
		    AnisoFrontEntry* const pAFEMerged)
{
  pAFEMerged->pVL = pAFELeft->pVL;
  pAFEMerged->pVR = pAFERight->pVR;

  pAFEMerged->pAFELeft = pAFELeft->pAFELeft;
  pAFEMerged->pAFELeft->pAFERight = pAFEMerged;

  pAFEMerged->pAFERight = pAFERight->pAFERight;
  pAFEMerged->pAFERight->pAFELeft = pAFEMerged;

  pAFELeft->qNeverAdvance = true;
  pAFERight->qNeverAdvance = true;

  pAFELeft->pAFELeft = pAFEInvalid;
  pAFERight->pAFERight = pAFEInvalid;

  assert(pAFEMerged->qValid());
  assert(pAFEMerged->pAFELeft->qValid());
  assert(pAFEMerged->pAFERight->qValid());
}

void vMergeFE_3for2(AnisoFrontEntry* const pAFELeft,
		    AnisoFrontEntry* const pAFEMiddle,
		    AnisoFrontEntry* const pAFERight,
		    Vert* const pVNew,
		    AnisoFrontEntry* const pAFEMergedLeft,
		    AnisoFrontEntry* const pAFEMergedRight)
{
  // Need to merge 3 faces for 2.  In the process, pAFEMiddle will get
  // squeezed out, with pAFELeft and pAFERight now meeting at pVNew.

  // Adjust verts in the new entries.
  pAFEMergedLeft->pVL = pAFELeft->pVL;
  pAFEMergedLeft->pVR = pVNew;

  pAFEMergedRight->pVL = pVNew;
  pAFEMergedRight->pVR = pAFERight->pVR;

  // Adjust connectivity along the front.
  pAFEMergedLeft->pAFELeft = pAFELeft->pAFELeft;
  if (pAFEMergedLeft->pAFELeft != pAFEInvalid) {
    pAFEMergedLeft->pAFELeft->pAFERight = pAFEMergedLeft;
  }
  pAFEMergedLeft->pAFERight = pAFEMergedRight;

  pAFEMergedRight->pAFELeft = pAFEMergedLeft;
  pAFEMergedRight->pAFERight = pAFERight->pAFERight;
  if (pAFEMergedRight->pAFERight != pAFEInvalid) {
    pAFEMergedRight->pAFERight->pAFELeft = pAFEMergedRight;
  }

  // Reset children from the previous row.
  if (pAFELeft->pAFEPrev != pAFEInvalid) {
    pAFELeft->pAFEPrev->pAFENext = pAFEMergedLeft;
    pAFELeft->pAFEPrev->pVRChild = pVNew;
  }
  if (pAFEMiddle->pAFEPrev != pAFEInvalid) {
    pAFEMiddle->pAFEPrev->pAFENext = pAFEInvalid;
  }
  if (pAFERight->pAFEPrev != pAFEInvalid) {
    pAFERight->pAFEPrev->pAFENext = pAFEMergedRight;
    pAFERight->pAFEPrev->pVLChild = pVNew;
  }

  // Set parents for this row.
  pAFEMergedLeft->pAFEPrev = pAFELeft->pAFEPrev;
  pAFEMergedRight->pAFEPrev = pAFERight->pAFEPrev;

  pAFELeft->qNeverAdvance = true;
  pAFERight->qNeverAdvance = true;

  pAFELeft->pAFELeft = pAFEInvalid;
  pAFERight->pAFERight = pAFEInvalid;

  assert(pAFEMergedLeft->qValid());
  assert(pAFEMergedRight->qValid());
  assert(pAFEMergedLeft->pAFELeft->qValid());
  assert(pAFEMergedRight->pAFERight->qValid());
}

static int iNextI(const int i, const int iMax, const bool qPeriodic) {
  if (i < iMax-1) return i+1;
  else if (qPeriodic) return 0;
  else return -1;
}

static int i2ndNextI(const int i, const int iMax, const bool qPeriodic) {
  if (i < iMax-2) return i+2;
  else if (qPeriodic) return i - (iMax-2);
  else return -1;
}

void AnisoFront::vMarchChanSteger(const int iMaxLayers, int& iVertsAdded)
// This routine is intended to march the current layer out no more than
// iLayers steps farther from the surface.  Fewer steps may be taken if
// every AnisoFrontEntry is terminated before that.
{
  int iLayer = 0;
  iVertsAdded = 0;
  while (iLayer < iMaxLayers && !queueFrag.empty()) {
    iLayer++;
    // For a full layer, you must process every mesh fragment in the
    // queue.
    for (int iFrag = queueFrag.size() - 1; iFrag >= 0; iFrag--) {
      AnisoFrag AFrag = queueFrag.front();
      queueFrag.pop();

      // Extract the current coordinates of all the front vertices.
      int iNumCells = AFrag.size();
      int iNumVerts = iNumCells + 1;
      double (*a2dOldCoords)[2] = new double[iNumVerts][2];
      double *adCellSize = new double[iNumVerts];
      Vert *(*apVOldVerts) = new Vert*[iNumVerts];
      double (*a2dNorm)[2] = new double[iNumVerts][2];

      AnisoFrag::iterator iter;
      int i = 0;
      for (iter = AFrag.begin(); iter != AFrag.end(); iter++, i++) {
	AnisoFrontEntry* pAFE = *iter;
	Vert *pVL = pAFE->pVLeft();
	Vert *pVR = pAFE->pVRight();
	apVOldVerts[i] = pVL;
	// This next line gets overwritten every time except the last....
	apVOldVerts[i+1] = pVR;

	pAFE->vUnitNormal(a2dNorm[i]);

	a2dOldCoords[i][0] = pVL->dX();
	a2dOldCoords[i][1] = pVL->dY();
	a2dOldCoords[i+1][0] = pVR->dX();
	a2dOldCoords[i+1][1] = pVR->dY();

	double dThisThickness;
	if (pAFE->pAFEParent() == pAFEInvalid) {
	  // This complicated business helps get a good thickness after
	  // a 2->1 merge.
	  if (pAFE->pAFERightNeighbor() != pAFEInvalid &&
	      pAFE->pAFERightNeighbor()->pAFEParent() != pAFEInvalid) {
	    AnisoFrontEntry *pAFERightUncle =
	      pAFE->pAFERightNeighbor()->pAFEParent();
	    double adRightVec[] =
	      {pVR->dX() - pAFERightUncle->pVLeft()->dX(),
	       pVR->dY() - pAFERightUncle->pVLeft()->dY()};
	    dThisThickness = dDOT2D(a2dNorm[i], adRightVec);
	  }
	  else if (pAFE->pAFELeftNeighbor() != pAFEInvalid &&
	      pAFE->pAFELeftNeighbor()->pAFEParent() != pAFEInvalid) {
	    AnisoFrontEntry *pAFELeftUncle =
	      pAFE->pAFELeftNeighbor()->pAFEParent();
	    double adLeftVec[] =
	      {pVR->dX() - pAFELeftUncle->pVRight()->dX(),
	       pVR->dY() - pAFELeftUncle->pVRight()->dY()};
	    dThisThickness = dDOT2D(a2dNorm[i], adLeftVec);
	  }
	  else {
	    dThisThickness = dDefaultThick;
	  }
	}
	else {
	  double adLeftVec[] =
	    {pVL->dX() - pAFE->pAFEParent()->pVLeft()->dX(),
	     pVL->dY() - pAFE->pAFEParent()->pVLeft()->dY()};
	  double adRightVec[] =
	    {pVR->dX() - pAFE->pAFEParent()->pVRight()->dX(),
	     pVR->dY() - pAFE->pAFEParent()->pVRight()->dY()};
	  double dLeftDot = dDOT2D(a2dNorm[i], adLeftVec);
	  double dRightDot = dDOT2D(a2dNorm[i], adRightVec);
	  dThisThickness = 0.5*(dLeftDot + dRightDot);
	}
	// Yes, it's true that for skewed cells this isn't, strictly
	// speaking, an accurate representation of cell size.
	adCellSize[i] = dThisThickness * dStretch *
	  dDistanceBetween(pVL, pVR);
      }
//       bool qIsPeriodic = false;
//       if (apVOldVerts[0] == apVOldVerts[iNumVerts - 1]) {
// 	// Make sure that this isn't a sharp outside corner, where
// 	// periodicity will fail....
// 	double adVecA[] = {apVOldVerts[1]->dX() - apVOldVerts[0]->dX(),
// 			   apVOldVerts[1]->dY() - apVOldVerts[0]->dY()};
// 	double adVecB[] = {apVOldVerts[iNumVerts - 2]->dX()
// 			   - apVOldVerts[iNumVerts - 1]->dX(),
// 			   apVOldVerts[iNumVerts - 2]->dY()
// 			   - apVOldVerts[iNumVerts - 1]->dY()};
// 	vNORMALIZE2D(adVecA);
// 	vNORMALIZE2D(adVecB);
// 	double dDot = dDOT2D(adVecA, adVecB);
// 	vMessage(2, "Possible periodic: dot product = %f\n",
// 		 dDot);
// 	if (dDot < M_SQRT1_2) qIsPeriodic = true;
//       }
      bool qIsPeriodic = false;
      if (apVOldVerts[0] == apVOldVerts[iNumVerts - 1]) {
	qIsPeriodic = true;
      }

      // Smooth the cell sizes.
      for (int iPass = 0; iPass < 3; iPass++) {
	double dEpsSize = 0.1;
	for (i = 1; i < iNumCells - 1; i++) {
	  adCellSize[i] +=
	    dEpsSize * (adCellSize[i-1] - 2*adCellSize[i] + adCellSize[i+1]);
	}
	for (i = iNumCells - 2; i >= 1; i--) {
	  adCellSize[i] +=
	    dEpsSize * (adCellSize[i-1] - 2*adCellSize[i] + adCellSize[i+1]);
	}
      }

      // Set the smoothing coefficient for the marching scheme.
      double dEps = 0.05;

      // Set up the RHS and LHS for the tri-diagonal solver.
      double (*a4dLHS)[3][2][2] = new double[iNumVerts][3][2][2];
      double (*a2dDeltaR)[2] = new double[iNumVerts][2];
      for (i = 0; i < iNumVerts; i++) {
	double dXdXi, dYdXi, dDetB, dXdEta, dYdEta, dThisSize;
	if (i == 0) {
	  if (qIsPeriodic) {
	    dXdXi = (a2dOldCoords[1][0] - a2dOldCoords[iNumVerts-2][0]) * 0.5;
	    dYdXi = (a2dOldCoords[1][1] - a2dOldCoords[iNumVerts-2][1]) * 0.5;
	    dThisSize = (adCellSize[0] + adCellSize[iNumCells-1]) * 0.5;
	  }
	  else {
	    dXdXi = (a2dOldCoords[i+1][0] - a2dOldCoords[i][0]);
	    dYdXi = (a2dOldCoords[i+1][1] - a2dOldCoords[i][1]);
	    dThisSize = adCellSize[0];
	  }
	}
	else if (i == iNumVerts - 1) {
	  dXdXi = (a2dOldCoords[i][0] - a2dOldCoords[i-1][0]);
	  dYdXi = (a2dOldCoords[i][1] - a2dOldCoords[i-1][1]);
	  dThisSize = adCellSize[iNumCells-1];
	}
	else {
	  dXdXi = (a2dOldCoords[i+1][0] - a2dOldCoords[i-1][0]) * 0.5;
	  dYdXi = (a2dOldCoords[i+1][1] - a2dOldCoords[i-1][1]) * 0.5;
	  dThisSize = (adCellSize[i] + adCellSize[i-1]) * 0.5;
	}
	dDetB = dXdXi*dXdXi + dYdXi*dYdXi;
	dXdEta = -dYdXi * dThisSize / dDetB;
	dYdEta =  dXdXi * dThisSize / dDetB;

	if (qIsPeriodic ||
	    (i > 0 && i < iNumVerts - 1)) {
	  double a2dBInvA[2][2];
	  a2dBInvA[0][0] = (dXdXi*dXdEta - dYdXi*dYdEta) / dDetB;
	  a2dBInvA[0][1] = (dXdXi*dYdEta + dXdEta*dYdXi) / dDetB;
	  a2dBInvA[1][0] = a2dBInvA[0][1];
	  a2dBInvA[1][1] = - a2dBInvA[0][0];

	  a4dLHS[i][0][0][0] = - a2dBInvA[0][0]*0.5 - 2*dEps;
	  a4dLHS[i][0][0][1] = - a2dBInvA[0][1]*0.5;
	  a4dLHS[i][0][1][0] = - a2dBInvA[1][0]*0.5;
	  a4dLHS[i][0][1][1] = - a2dBInvA[1][1]*0.5 - 2*dEps;

	  a4dLHS[i][1][0][0] = 1 + 4*dEps;
	  a4dLHS[i][1][0][1] = 0;
	  a4dLHS[i][1][1][0] = 0;
	  a4dLHS[i][1][1][1] = 1 + 4*dEps;

	  a4dLHS[i][2][0][0] = a2dBInvA[0][0]*0.5 - 2*dEps;
	  a4dLHS[i][2][0][1] = a2dBInvA[0][1]*0.5;
	  a4dLHS[i][2][1][0] = a2dBInvA[1][0]*0.5;
	  a4dLHS[i][2][1][1] = a2dBInvA[1][1]*0.5 - 2*dEps;

	  if (i == 0) {
	    // The periodic wrap-around case
	    a2dDeltaR[i][0] = dXdEta + dEps *
	      (a2dOldCoords[i+1][0] - 2*a2dOldCoords[i][0]
	       + a2dOldCoords[iNumVerts-2][0]);
	    a2dDeltaR[i][1] = dYdEta + dEps *
	      (a2dOldCoords[i+1][1] - 2*a2dOldCoords[i][1]
	       + a2dOldCoords[iNumVerts-2][1]);
	  }
	  else {
	    a2dDeltaR[i][0] = dXdEta + dEps *
	      (a2dOldCoords[i+1][0] - 2*a2dOldCoords[i][0]
	       + a2dOldCoords[i-1][0]);
	    a2dDeltaR[i][1] = dYdEta + dEps *
	      (a2dOldCoords[i+1][1] - 2*a2dOldCoords[i][1]
	       + a2dOldCoords[i-1][1]);
	  }
	}
	else {
	  // End conditions for aperiodic problems.
	  a4dLHS[i][0][0][0] = a4dLHS[i][0][0][1] = a4dLHS[i][0][1][0] =
	    a4dLHS[i][0][1][1] = 0;

	  a4dLHS[i][1][0][0] = 1;
	  a4dLHS[i][1][0][1] = 0;
	  a4dLHS[i][1][1][0] = 0;
	  a4dLHS[i][1][1][1] = 1;

	  a4dLHS[i][2][0][0] = a4dLHS[i][2][0][1] = a4dLHS[i][2][1][0] =
	    a4dLHS[i][2][1][1] = 0;

	  a2dDeltaR[i][0] = dXdEta;
	  a2dDeltaR[i][1] = dYdEta;
	}
      }

      // Replace the equation at trailing edges, a la Chan and Steger,
      // to keep wonky things from happening there.
      for (i = 0; i < iNumCells-1; i++) {
	int iLeft = i-1;
	if (iLeft == -1) {
	  if (qIsPeriodic) iLeft = iNumCells-1;
	  else continue;
	}
	double dDot = dDOT2D(a2dNorm[iLeft],
			     a2dNorm[i]);
	double dCross = dCROSS2D(a2dNorm[iLeft],
				 a2dNorm[i]);
	if ((dDot < -0.5) && (dCross < 0)) {
	  // Replace this equation with -dR_{i-1} + 2*dR_{i} - dR_{i+1}
	  // = 0.
	  double dFactor = 0.5;
	  a4dLHS[i][0][0][0] = -dFactor;
	  a4dLHS[i][0][0][1] = 0;
	  a4dLHS[i][0][1][0] = 0;
	  a4dLHS[i][0][1][1] = -dFactor;

	  a4dLHS[i][1][0][0] = 1;
	  a4dLHS[i][1][0][1] = 0;
	  a4dLHS[i][1][1][0] = 0;
	  a4dLHS[i][1][1][1] = 1;

	  a4dLHS[i][2][0][0] = -dFactor;
	  a4dLHS[i][2][0][1] = 0;
	  a4dLHS[i][2][1][0] = 0;
	  a4dLHS[i][2][1][1] = -dFactor;

	  a2dDeltaR[i][0] = 0;
	  a2dDeltaR[i][1] = 0;
	}
      }

      Vert *(*apVNewVerts) = new Vert*[iNumVerts];
      double (*a2dNewCoords)[2] = new double[iNumVerts][2];
      if (qIsPeriodic) {
	// Actually solve the equations.
	vSolveTriDiag(a4dLHS, a2dDeltaR, iNumVerts-1, true);

	// Compute new locations for the vertices.
	for (i = 0; i < iNumVerts-1; i++) {
	  a2dNewCoords[i][0] = a2dOldCoords[i][0] + a2dDeltaR[i][0];
	  a2dNewCoords[i][1] = a2dOldCoords[i][1] + a2dDeltaR[i][1];
	}

	// Create new verts.
	for (i = 0; i < iNumVerts-1; i++) {
	  apVNewVerts[i] = pM2D->pVInsertNearVert(apVOldVerts[i], a2dNewCoords[i]);
	  vMessage(4, "%d: %p\n",
		   i, apVNewVerts[i]);
	  apVNewVerts[i]->vSetLS(dMAG2D(a2dDeltaR[i])*dFinishAR);
	  assert(apVNewVerts[i]->qValid());
	}
	apVNewVerts[iNumVerts-1] = apVNewVerts[0];
	iVertsAdded += iNumVerts - 1;
      }
      else {
	// Actually solve the equations.
	vSolveTriDiag(a4dLHS, a2dDeltaR, iNumVerts, qIsPeriodic);

	// Compute new locations for the vertices.
	for (i = 0; i < iNumVerts; i++) {
	  a2dNewCoords[i][0] = a2dOldCoords[i][0] + a2dDeltaR[i][0];
	  a2dNewCoords[i][1] = a2dOldCoords[i][1] + a2dDeltaR[i][1];
	}

	// Create new verts.
	for (i = 0; i < iNumVerts; i++) {
	  apVNewVerts[i] = pM2D->pVInsertNearVert(apVOldVerts[i], a2dNewCoords[i]);
	  vMessage(4, "%d: %p\n",
		   i, apVNewVerts[i]);
	  assert(apVNewVerts[i]->qValid());
	}
	iVertsAdded += iNumVerts;
      }

      // Now create the new front fragment or fragments.
      //
      // Key decisions:
      //   Which new faces won't be marched any further? (These will be
      //     marked as non-advancing, and not put into a fragment.)
      //   Which new faces should be merged together in inside corners?
      i = 0;
      AnisoFrag AFragNew;

      // Pass 1:  Create new AFE's, and compute aspect ratio for each
      // new quad, marching direction for each line.
      double *adAspect = new double[iNumCells];
      int *aiAspectState = new int[iNumCells];
      AnisoFrontEntry **apAFE = new AnisoFrontEntry*[iNumCells];
      for (iter = AFrag.begin(); iter != AFrag.end(); iter++, i++) {
	AnisoFrontEntry* pAFEOld = *iter;
	AnisoFrontEntry* pAFE = apAFE[i] = pAFENewEntry();
	pAFE->vAssign(apVNewVerts[i], apVNewVerts[i+1], pAFEOld);
	assert(pAFE->qValid());
	aiAspectState[i] = 0;

	double dLen = pAFE->dLength();
	double dAveHeight =
	  (dDistanceBetween(pAFE->pVLeft(), pAFEOld->pVLeft()) +
	   dDistanceBetween(pAFE->pVRight(), pAFEOld->pVRight())) * 0.5;
	adAspect[i] = dLen / dAveHeight;

      }
      assert(i == iNumCells);

      // Pass 1.5:  Identify, for each edge, whether it's short and
      // whether it's neighbors are short.
      for (i = 0; i < iNumCells; i++) {
	if (adAspect[i] < dFinishAR) {
	  aiAspectState[i] += 4;
	  if (i == iNumCells - 1) {
	    if (qIsPeriodic) aiAspectState[0] += 2;
	  }
	  else {
	    aiAspectState[i+1] += 2;
	  }
	  if (i == 0) {
	    if (qIsPeriodic) aiAspectState[iNumCells-1] += 1;
	  }
	  else {
	    aiAspectState[i-1] += 1;
	  }
	}
      }

      // Possible aspect states run from 0 to 7.  States smaller than 4
      // require no action whatsoever, at least for the current cell.

      // For straight-ish parts of the fragment, states 4-7 simply
      // terminate.

      // For inside corners, the canonical 2->1 reduction involves
      // consecutive states of 5 and 6.  The 3->2 reduction has
      // consecutive states of 5, 7, 6 or 1, 4, 2.  In both cases, the
      // normals at the ends of these runs are converging
      // significantly.

      // Even for inside corners, at this point I'm doing nothing about
      // longer runs of stuff to terminate (like 5, 7, 7, 6 and 5, 7, 7,
      // 7, 7, 7, 6 and so on).  I can wave my hands and claim that for
      // smooth bdry edge length variation, the next stuff out should
      // terminate before it has a chance to cause problems.  We'll see
      // if that's actually true.

      // Pass 1.75: Decide the fate of every new front face: terminate (10),
      // continue(11), merge 2->1 (12), merge 3->2 (13).
//       for (i = 0; i < iNumCells; i++) {
//	vMessage(2, "i: %3d.  Aspect ratio: %10.6f.  Left normal: (%10.6f, %10.6f).  State: %d. Loc: (%10.6f, %10.6f)\n",  i, adAspect[i], a2dNorm[i][0], a2dNorm[i][1], aiAspectState[i], a2dOldCoords[i][0], a2dOldCoords[i][1]);
//       }

      for (i = 0; i < iNumCells; i++) {
	int iState = aiAspectState[i];
	int iNext = iNextI(i, iNumCells, qIsPeriodic);
	int i2nd = i2ndNextI(i, iNumCells, qIsPeriodic);
	if (iState == 5 &&
	    iNext != -1 && aiAspectState[iNext] == 6) {
	  // Check the normals
	  if (dDOT2D(a2dNorm[i], a2dNorm[iNext]) < 0.85
	      ) {
	    // Converging enough.  Merge these two into one.
	    aiAspectState[i] = aiAspectState[iNext] = 12;
	  }
	}
	else if ((iState == 5 &&
		  iNext != -1 && aiAspectState[iNext] == 7 &&
		  i2nd != -1 && aiAspectState[i2nd] == 6) ||
		 (iState == 1 &&
		  iNext != -1 && aiAspectState[iNext] == 4 &&
		  i2nd != -1 && aiAspectState[i2nd] == 2)) {
	  // Check the normals
	  if (dDOT2D(a2dNorm[i], a2dNorm[i2nd]) < 0.9) {
	    // Converging enough.  Merge these three into two.
	    aiAspectState[i] = aiAspectState[iNext] =
	      aiAspectState[i2nd] = 13;
	  }
	}
      }
      // Do the following in a separate loop so there won't be any
      // problem with wrap-around on periodic fragments that happen to
      // have an inside corner at the "beginning" in the order the
      // fragment is transcribed.
      for (i = 0; i < iNumCells; i++) {
	int iState = aiAspectState[i];
	if (iState < 4) aiAspectState[i] = 11; // continue marching
	else if (iState < 10) aiAspectState[i] = 10; // terminate
      }

      // Pass 2:  Actually update the fragments.
      int iStart = 0;
      if (qIsPeriodic) {
	for ( ; aiAspectState[iStart] == 12; iStart++)
	  {}
      }
      i = iStart;
      do {
//	if (aiAspectState[i] != 11) {
//	  vMessage(2, "Face %d, Aspect state = %d\n", i, aiAspectState[i]);
//	}
	switch (aiAspectState[i]) {
	case 10: // Terminate
	  if (i == 0 || i == iNumCells-1) qIsPeriodic = false;
	  apAFE[i]->vMarkNonAdvancing();
	  vMessage(3, "No advance from front entry %p\n", apAFE[i]);
	  if (AFragNew.size() > 0) {
	    // If there's currently a fragment on the go, add it to the
	    // queue and begin again.
	    vMessage(2, "Adding a fragment of size %zd\n", AFragNew.size());
	    queueFrag.push(AFragNew);
	    AFragNew.clear();
	  }
	  break;
	case 11: // Continue marching
	  AFragNew.push_back(apAFE[i]);
	  vMessage(3, "Advancing from front entry %p\n", apAFE[i]);
	  break;
	case 12: // Merge 2->1
	  {
	    int iNext = iNextI(i,iNumCells,qIsPeriodic);
	    assert(iNext != -1);
	    AnisoFrontEntry *pAFEMerge = pAFENewEntry();
	    vMergeFE_2for1(apAFE[i], apAFE[iNext],
			   pAFEMerge);
	    AFragNew.push_back(pAFEMerge);
	    vMessage(2, "Merged two front entries for one. %p %p\n",
		     apAFE[i], apAFE[iNext]);
	    i = iNext;
	  }
	  break;
	case 13: // Merge 3->2
	  {
	    int iNext = iNextI(i, iNumCells, qIsPeriodic);
	    assert(iNext != -1);
	    int i2nd = i2ndNextI(i, iNumCells, qIsPeriodic);
	    assert(i2nd != -1);
	    AnisoFrontEntry *pAFEMergeLeft = pAFENewEntry();
	    AnisoFrontEntry *pAFEMergeRight = pAFENewEntry();
	    // Create a new vertex at the midpoint of the edge
	    // apAFE[iNext].
	    AnisoFrontEntry *pAFEMid = apAFE[iNext];
	    double adNewLoc[] =
	      {(pAFEMid->pVLeft()->dX() + pAFEMid->pVRight()->dX()) / 2,
	       (pAFEMid->pVLeft()->dY() + pAFEMid->pVRight()->dY()) / 2};
	    Vert* pVNew = pM2D->pVInsertNearVert(pAFEMid->pVLeft(), adNewLoc);
	    assert(pVNew->qValid());

	    vMergeFE_3for2(apAFE[i], apAFE[iNext], apAFE[i2nd], pVNew,
			   pAFEMergeLeft, pAFEMergeRight);

	    // Now queue the two old verts for deletion.
	    spVToDelete.insert(pAFEMid->pVLeft());
	    spVToDelete.insert(pAFEMid->pVRight());
	    pAFEMid->pVLeft()->vMarkToDelete();
	    pAFEMid->pVRight()->vMarkToDelete();

	    AFragNew.push_back(pAFEMergeLeft);
	    AFragNew.push_back(pAFEMergeRight);
	    vMessage(2, "Merged three front entries for two. %p %p %p\n",
		     apAFE[i], apAFE[iNext], apAFE[i2nd]);
	    i = i2nd;
	  }
	  break;
	default:
	  assert(0);
	  break;
	}
	i = iNextI(i, iNumCells, qIsPeriodic);
      } while (i != iStart && i != -1);
      // Done creating new fragments from the old one.
      if (AFragNew.size() > 0) {
	// If you were originally periodic, and there's now more than one
	// fragment, and you didn't terminate at either end of the loop,
	// then the first and last fragments need to be spliced.
	if (qIsPeriodic && queueFrag.size() > 1) {
	  AnisoFrag AFragTemp = queueFrag.front();
	  queueFrag.pop();
	  AFragNew.insert(AFragNew.end(), AFragTemp.begin(),
			  AFragTemp.end());
	}
	vMessage(2 , "Adding a fragment of size %zd\n", AFragNew.size());
	queueFrag.push(AFragNew);
	// No need to clear; it's about to go out of scope anyway.
      }

      // Clean up.
      delete [] a2dOldCoords;
      delete [] a2dNewCoords;
      delete [] adCellSize;
      delete [] apVNewVerts;
      delete [] apVOldVerts;
      delete [] a4dLHS;
      delete [] a2dDeltaR;
      delete [] adAspect;
      delete [] aiAspectState;
      delete [] a2dNorm;
      delete [] apAFE;
    }
    vMessage(2, "Done with layer %d; Fragment queue now has %zd entries.\n",
	     iLayer, queueFrag.size());
  }
}

void AnisoFront::vInitFrontQueue(const double dFinishAR_In)
  // Collect all of the current front entries into fragments, and queue
  // them.
{
  dFinishAR = dFinishAR_In;
  // Can't clear a queue, so hope you didn't need to...
  assert(queueFrag.empty());

  // This data structure will always hold all the entries that haven't
  // been processed.
  std::set<AnisoFrontEntry*> sAFERemaining;
  vMessage(2, "Initial front size: %d\n", VAAF.iSize());
  for (int i = VAAF.iSize()-1; i >= 0; i--) {
    AnisoFrontEntry* pAFE = &(VAAF[i]);
    if (!pAFE->qNullEntry() && pAFE->qAdvanceFromHere()) {
      sAFERemaining.insert(pAFE);
    }
  }

  do {
    // Grab an entry.
    AnisoFrontEntry *pAFE = (*sAFERemaining.begin());

    // First, move to the left until you reach the last one in this
    // fragment, or discover that this fragment is a loop.
    AnisoFrontEntry *pAFEStart = pAFE;
    while (pAFE->pAFELeftNeighbor()->qValid()) {
      pAFE = pAFE->pAFELeftNeighbor();
      if (pAFE == pAFEStart) break;
    }

    // This next line makes it so that "normal" cases will work
    // properly.
    if (pAFE != pAFEStart) pAFEStart = NULL;

    // Walk to the right until you hit the end or return to the
    // beginning.  Yes, both the periodic and non-periodic cases can be
    // handled together.
    AnisoFrag AFrag;
    do {
      AFrag.push_back(pAFE);
      sAFERemaining.erase(pAFE);
      vMessage(3, "Adding an anisofront entry: (%7f, %7f) to (%7f, %7f)\n",
	       pAFE->pVRight()->dX(), pAFE->pVRight()->dY(),
	       pAFE->pVLeft()->dX(), pAFE->pVLeft()->dY());
      pAFE = pAFE->pAFERightNeighbor();
    } while (pAFE->qValid() && (pAFE != pAFEStart));
    vMessage(2, "Fragment with size %3zd; # remaining %3zd\n",
	     AFrag.size(), sAFERemaining.size());
    queueFrag.push(AFrag);
  } while (!sAFERemaining.empty());
  vMessage(2, "Total fragments in queue: %zd\n", queueFrag.size());
}
