#include "GR_config.h"
#include "GR_assert.h"
#include "GR_events.h"
#include "GR_InsertionQueue.h"
#include "GR_Mesh.h"
#include "GR_Quality.h"
#include "GR_WatsonInfo.h"

#undef EXPERIMENTAL

// Major items:
//  o InsertionQueue creation and updates (this class is expected to be a
//    thin wrapper around the STL multimap container)
//  o InsertionQueueEntry stuff, most notably insertion for entries that
//    are legally allowed to insert their circumcenter
//  o A new driver for Shewchuk insertion based on the InsertionQueue
//    model.

// Set up an initial insertion queue depending on what type the mesh
// is.  This approach has the strong advantage that the initialization
// can be mesh-type aware, but the queue processing for insertion can be
// type-blind.
InsertionQueue::InsertionQueue(Mesh * const pMIn,
			       const double dQualTarget) :
  MMap(), dWorstAllowable(0), iMaxCellCount(INT_MAX),
  qIsInitialEncroachmentFixed(false), pM(NULL)
{
  SUMAA_LOG_EVENT_BEGIN(INSERT_QUEUE);
  vMessage(1, "Initializing insertion priority queue...\n");
#ifdef EXPERIMENTAL
  vWarning("  Experimental insertion priority code in use!\n");
#endif
  assert(pMIn != NULL);
  pM = pMIn;

  if (iFuzzyComp(dQualTarget, InsertionQueueEntry::dInvalidPriority) == 0) {
    dWorstAllowable = pM->dWorstAllowedCellShape();
  }
  else {
    dWorstAllowable = dQualTarget;
  }
  vMessage(2, "Worst allowed cell shape measure is %f.\n",
	   dWorstAllowable);

  vBuildQueue();
  vMessage(2, "Initial insertion queue length is %d.\n", iQueueLength());
  SUMAA_LOG_EVENT_END(INSERT_QUEUE);
}

void InsertionQueue::vBuildQueue(const bool qBdryGuaranteedOK)
{
  // First, add all cells to the queue.  (Any that are already good
  // enough will be filtered out automatically.)
  SUMAA_LOG_EVENT_BEGIN(INSERT_QUEUE_BUILD);
  MMap.clear();
  vMessage(2, "Queueing simplex cells that don't meet quality criterion...");
  for (int iCell = 0; iCell < pM->iNumCells(); iCell++) {
    Cell *pC = pM->pCCell(iCell);
    if (!pC->qDeleted() && !pC->qIsQuasiStructured() &&
	dynamic_cast<SimplexCell*>(pC) != NULL) {
      qAddEntry(pC);
    }
  }
  vMessage(2, "queued %d.\n", int(MMap.size()));

  // Now, identify all encroached bdry faces and add them to the queue.
  if (!qBdryGuaranteedOK && pM->qBdryChangesAllowed())
    pM->iQueueEncroachedBdryEntities(*this);
  SUMAA_LOG_EVENT_END(INSERT_QUEUE_BUILD);
}

InsertionQueueEntry::InsertionQueueEntry(Cell * const pCIn) :
  dPri(dInvalidPriority), pC(pCIn), eET(eInvalid)
{
  SUMAA_LOG_EVENT_BEGIN(IQE_BUILD);
  assert(pC->qValid());
  switch (pC->eType()) {
  case Cell::eTet:
    eET = eTetCell;
    break;
  case Cell::eTriCell:
    eET = eTriCell;
    break;
  default:
    // Shouldn't get here, but we might for boundary faces.
    assert(0);
    break;
  }
  if (pC->qDeleted()) {
    SUMAA_LOG_EVENT_END(IQE_BUILD);
    return;
    // FIX ME: New bdry curve insertion will make this unnecessary,
    // because no deleted cells can ever be queued.
  }
  for (int i = 0; i < pC->iNumVerts(); i++) {
    apV[i] = pC->pVVert(i);
  }
  SUMAA_LOG_EVENT_END(IQE_BUILD);
}

InsertionQueueEntry::InsertionQueueEntry(BFace * const pBFIn) :
  dPri(dInvalidPriority), pBF(pBFIn), eET(eInvalid)
{
  SUMAA_LOG_EVENT_BEGIN(IQE_BUILD);
  assert(pBF->qValid());
  switch (pBF->eType()) {
  case Cell::eTriBFace:
  case Cell::eIntTriBFace:
    eET = eTriBFace;
    break;
  case Cell::eBdryEdge:
  case Cell::eIntBdryEdge:
    eET = eBdryEdge;
    break;
  default:
    // Shouldn't get here
    assert(0);
    break;
  }
  if (pBF->qDeleted()) {
    SUMAA_LOG_EVENT_END(IQE_BUILD);
    return;
    // FIX ME: New bdry curve insertion will make this unnecessary,
    // because no deleted BFaces can ever be queued.
  }
  for (int i = 0; i < pBF->iNumVerts(); i++) {
    apV[i] = pBF->pVVert(i);
  }
  SUMAA_LOG_EVENT_END(IQE_BUILD);
}

InsertionQueueEntry::InsertionQueueEntry(Vert * const pV0,
					 Vert * const pV1) :
  dPri(dInvalidPriority), pC(pCInvalidCell), eET(eSubSeg)
{
  SUMAA_LOG_EVENT_BEGIN(IQE_BUILD);
  assert(pV0->qValid() && pV1->qValid());
  apV[0] = pV0;
  apV[1] = pV1;
  SUMAA_LOG_EVENT_END(IQE_BUILD);
}

bool InsertionQueueEntry::qStillInMesh() const
  // Determine whether the entity described by this entry is still in
  // the mesh in its original form.
{
  SUMAA_LOG_EVENT_BEGIN(IQE_QUERY);
  switch (eET) {
  case eTriCell:
  case eTetCell:
    {
      // If it's been deleted from the mesh, check no farther.
      if (pC->qDeleted()) {
	SUMAA_LOG_EVENT_END(IQE_QUERY);
	return false;
      }
      bool qRetVal = true;
      for (int i = 0; i < pC->iNumVerts() && qRetVal; i++) {
	qRetVal = qRetVal && pC->qHasVert(apV[i]);
      }
      SUMAA_LOG_EVENT_END(IQE_QUERY);
      return qRetVal;
    }
  case eTriBFace:
  case eBdryEdge:
    // Like the cell case.
    {
      // If it's been deleted from the mesh, check no farther.
      if (pBF->qDeleted()) {
	SUMAA_LOG_EVENT_END(IQE_QUERY);
	return false;
      }
      bool qRetVal = true;
      for (int i = 0; i < pBF->iNumVerts() && qRetVal; i++) {
	qRetVal = qRetVal && pBF->qHasVert(apV[i]);
      }
      SUMAA_LOG_EVENT_END(IQE_QUERY);
      return qRetVal;
    }
  case eSubSeg:
    // A subsegment can only be removed by splitting that
    // subsegment, not some other one, so this is always false unless
    // the subseg got into the queue twice (possible).
    {
      std::set<Cell*> spCJunk;
      std::set<Vert*> spVNeigh;
      vNeighborhood(apV[0], spCJunk, spVNeigh);
      if (spVNeigh.find(apV[1]) != spVNeigh.end()) {
	SUMAA_LOG_EVENT_END(IQE_QUERY);
	return true;
      }
      else {
	SUMAA_LOG_EVENT_END(IQE_QUERY);
	return false;
      }
    }
  default:
    // Should never get here.
    assert(0);
    SUMAA_LOG_EVENT_END(IQE_QUERY);
    return false;
  }
}

double InsertionQueueEntry::dEvaluatePriority() const
{
  SUMAA_LOG_EVENT_BEGIN(IQE_QUERY);
  // Compute the priority the first time.  After that, return a stored
  // value.
  if (iFuzzyComp(dPri, dInvalidPriority) == 0) {
    switch (eET) {
    case eSubSeg:
      dPri = dSubSegPriority;
      break;
    case eTriBFace:
    case eBdryEdge:
      dPri = pBF->dInsertionPriority();
      break;
    case eTetCell:
    case eTriCell:
      if (pC->qIsQuasiStructured())
	dPri = dNeverQueuedPriority;
      else {
	// -D/LS (if bigger than 1) + min(angle, shape), where shape is
	// what's shown below and angle is min dihed (if min dihed is
	// small enough for us to care; otherwise, angle = 1)
	double dSizeQual = 0, dShapeQual = 0;
	int iNVerts = pC->iNumVerts();
	int iDim = pC->pVVert(0)->iSpaceDimen();

	SUMAA_LOG_EVENT_BEGIN(PROBE1);
	double dRadius = (dynamic_cast<SimplexCell*>(pC))->dCircumradius();
	SUMAA_LOG_EVENT_END(PROBE1);
	double dSize = 2/sqrt(double(iDim)) * dRadius;

	// Find the correct size based on length scale.  Also, if the
	// vertex is at a small angle in the input, make sure that you
	// don't try to split this cell.
	double dCorrectSize = 0;
//	double dCorrectSize = 1.e300;
	for (int ii = 0; ii < iNVerts; ii++) {
	  Vert *pV = pC->pVVert(ii);
	  dCorrectSize += pV->dLS();
//	  double dThisSize = pV->dLS();
//	  dCorrectSize = MIN(dCorrectSize, dThisSize);
	  if (pV->qIsSmallAngleCorner()) {
	    dShapeQual = dNeverQueuedPriority;
	  }
	}
	dCorrectSize /= iNVerts;
	vMessage(4, "Correct size: %10.6f  Actual size: %10.6f  Ratio:  %10.7f\n",
		 dCorrectSize, dSize, dSize / dCorrectSize);
	SUMAA_LOG_EVENT_BEGIN(PROBE2);
	if (dSize > dCorrectSize) {
 	  dSizeQual = -dSize / dCorrectSize;
	  //	  dSizeQual = 0;
	}
	else if (dSize < dCorrectSize / 150000) {
	  // This provides a hard floor on cell size, even for small
	  // angles in the input, where no guarantees apply.
	  dSizeQual = dNeverQueuedPriority;
	}
	else {
#ifndef EXPERIMENTAL
	  // This call works properly in both 2D and 3D.
	  double dShort = pC->dShortestEdgeLength();
	  // The following scale factor is used to normalize the
	  // shortest-edge-to-circumradius ratio to the range 0 to 1.
	  double dShortestToRadius;
	  if (iDim == 2) {
	    dShortestToRadius = dShort / (sqrt(3.)*dRadius);
	  }
	  else {
	    dShortestToRadius = dShort*sqrt(.375) / dRadius;
	  }
	  dShapeQual += dShortestToRadius;
#else
	  // 2D same as ever.
	  if (iDim == 2) {
	    double dShort = pC->dShortestEdgeLength();
	    dShapeQual += dShort / (sqrt(3.)*dRadius);
	  }
	  else {
	    double dSliverQual;
	    int ii;
	    vInToCircumSphere(pC, &ii, &dSliverQual);
	    vMessage(4, "This tet has in- to circum- ratio of %f\n",
		     dSliverQual);
	    dShapeQual += dSliverQual;
	  }
#endif
	}
	SUMAA_LOG_EVENT_END(PROBE2);
	// The following code for including dihedral angle in choosing
	// cells to split turns out not to be very effective in practice
	// in eliminating bad tetrahedra.
//	if (dPri > 0.2 && eET == eTetCell) {
//	  // If the cell is a tet and the aspect ratio is reasonably
//	  // good, consider dihedral angle as well.
//	  int iN;
//	  double adRes[1];
//	  vMinDihed(pC, &iN, adRes);
//	  assert(iN == 1);
//	  // Convert from degrees back to radians.
//	  double dAngleQual = adRes[0] * M_PI/180;
//	  dShapeQual = MIN(dShapeQual, dAngleQual*2);
//	}
	dPri = dSizeQual + dShapeQual;
	if (dPri < dWorstCellPriority) dPri = dWorstCellPriority;
	vMessage(4, "Priority of a cell is %f (%f + %f)\n", dPri,
		 dSizeQual, dShapeQual);
      }
      break;
    case eInvalid:
      assert(0);
    }
  }
  SUMAA_LOG_EVENT_END(IQE_QUERY);
  return (dPri);
}

void InsertionQueue::vQualityRefine(const int iMaxVertsAdded)
{
  if (qIsQueueEmpty()) return;
  InsertionQueueEntry IQETop;
  WatsonInfo *pWI = NULL;
  int iVertsAdded = 0, iVertsAddedSincePurge = 0;

  SUMAA_LOG_EVENT_BEGIN(INSERT_QUEUE_REFINE);
  int iSpaceDimen = pM->pVVert(0)->iSpaceDimen();
  int iTopoDimen = pM->eType() == Mesh::eVolMesh ? 3 : 2;

  vMessage(1, "Refining mesh to reach quality target...\n");

  while (!qIsQueueEmpty()// dTopPriority() < dWorstAllowable
	 && iVertsAdded < iMaxVertsAdded) {
//     vMessage(2, "Num verts: %6d  Worst cell: %6f\n",
//	     pM->iNumVerts(), dTopPriority());
    int iNumAddedToQueue;
    do {
      // Get a new queue entry, and we'll go again.
      IQETop = IQETopValidEntry();
      // Find out about the neighborhood of this entity.
      if (pWI) delete pWI; // Need this to avoid hemorraging memory.
      pWI = new WatsonInfo(IQETop, pM);

      // Add any bdry entities encroached by splitting this entity to
      // the queue. (May not be any, may be quite a few.)  The offset
      // makes it so that bdry entities that are split as a result of
      // encroachment while splitting another are processed -after- the
      // original entity.
      double dOffset;
      switch (IQETop.eType()) {
      case InsertionQueueEntry::eSubSeg:
	vMessage(4, "Splitting an encroached subseg between verts %d and %d\n",
		 pM->iVertIndex(IQETop.pVVert(0)),
		 pM->iVertIndex(IQETop.pVVert(1)));
	vMessage(4, "  located at (%f, %f, %f) \n  and (%f, %f, %f)\n",
		 IQETop.pVVert(0)->dX(), IQETop.pVVert(0)->dY(),
		 IQETop.pVVert(0)->dZ(), IQETop.pVVert(1)->dX(),
		 IQETop.pVVert(1)->dY(), IQETop.pVVert(1)->dZ());
	dOffset = IQETop.dEvaluatePriority() -
	  InsertionQueueEntry::dSubSegPriority + 1;
	break;
      case InsertionQueueEntry::eTriBFace:
	vMessage(4, "Splitting an encroached subfacet; verts (%d, %d, %d)\n",
		 pM->iVertIndex(IQETop.pVVert(0)),
		 pM->iVertIndex(IQETop.pVVert(1)),
		 pM->iVertIndex(IQETop.pVVert(2)));
	vMessage(4, "  located at (%f, %f, %f),\n  (%f %f %f),\n  and (%f, %f, %f)\n",
		 IQETop.pVVert(0)->dX(), IQETop.pVVert(0)->dY(),
		 IQETop.pVVert(0)->dZ(), IQETop.pVVert(1)->dX(),
		 IQETop.pVVert(1)->dY(), IQETop.pVVert(1)->dZ(),
		 IQETop.pVVert(2)->dX(), IQETop.pVVert(2)->dY(),
		 IQETop.pVVert(2)->dZ());
	dOffset = IQETop.dEvaluatePriority() -
	  InsertionQueueEntry::dBdryFacePriority + 1;
	break;
      case InsertionQueueEntry::eBdryEdge:
	vMessage(4, "Splitting an encroached subsegment; verts (%d, %d)\n",
		 pM->iVertIndex(IQETop.pVVert(0)),
		 pM->iVertIndex(IQETop.pVVert(1)));
	if (iSpaceDimen == 2) {
	  // 2D mesh
	  vMessage(4, "  located at (%f, %f),\n  and (%f, %f)\n",
		   IQETop.pVVert(0)->dX(), IQETop.pVVert(0)->dY(),
		   IQETop.pVVert(1)->dX(), IQETop.pVVert(1)->dY());
	}
	else {
	  // Surface mesh
	  vMessage(4, "  located at (%f, %f, %f),\n  and (%f, %f, %f)\n",
		   IQETop.pVVert(0)->dX(), IQETop.pVVert(0)->dY(),
		   IQETop.pVVert(0)->dZ(), IQETop.pVVert(1)->dX(),
		   IQETop.pVVert(1)->dY(), IQETop.pVVert(1)->dZ());
	}
	dOffset = IQETop.dEvaluatePriority() -
	  InsertionQueueEntry::dBdryFacePriority + 1;
	break;
      default:
	if (iSpaceDimen == 2) {
	  // 2D mesh
	  vMessage(3, "Splitting a bad tri; verts (%d, %d, %d)\n",
		   pM->iVertIndex(IQETop.pVVert(0)),
		   pM->iVertIndex(IQETop.pVVert(1)),
		   pM->iVertIndex(IQETop.pVVert(2)));
	  vMessage(3, "  located at (%f, %f),\n  (%f, %f),\n    (%f, %f)\n",
		   IQETop.pVVert(0)->dX(), IQETop.pVVert(0)->dY(),
		   IQETop.pVVert(1)->dX(), IQETop.pVVert(1)->dY(),
		   IQETop.pVVert(2)->dX(), IQETop.pVVert(2)->dY());
	}
	else if (iTopoDimen == 2) {
	  // Surface mesh
	  vMessage(4, "Splitting a bad tri; verts (%d, %d, %d)\n",
		   pM->iVertIndex(IQETop.pVVert(0)),
		   pM->iVertIndex(IQETop.pVVert(1)),
		   pM->iVertIndex(IQETop.pVVert(2)));
	  vMessage(4, "  located at (%f, %f, %f),\n  (%f, %f, %f),\n",
		   IQETop.pVVert(0)->dX(), IQETop.pVVert(0)->dY(),
		   IQETop.pVVert(0)->dZ(), IQETop.pVVert(1)->dX(),
		   IQETop.pVVert(1)->dY(), IQETop.pVVert(1)->dZ());
	  vMessage(4, "  (%f, %f, %f)\n",
		   IQETop.pVVert(2)->dX(), IQETop.pVVert(2)->dY(),
		   IQETop.pVVert(2)->dZ());
	}
	else {
	  // Volume mesh
	  vMessage(4, "Splitting a bad tet; verts (%d, %d, %d, %d)\n",
		   pM->iVertIndex(IQETop.pVVert(0)),
		   pM->iVertIndex(IQETop.pVVert(1)),
		   pM->iVertIndex(IQETop.pVVert(2)),
		   pM->iVertIndex(IQETop.pVVert(3)));
	  vMessage(4, "  located at (%f, %f, %f),\n  (%f, %f, %f),\n",
		   IQETop.pVVert(0)->dX(), IQETop.pVVert(0)->dY(),
		   IQETop.pVVert(0)->dZ(), IQETop.pVVert(1)->dX(),
		   IQETop.pVVert(1)->dY(), IQETop.pVVert(1)->dZ());
	  vMessage(4, "  (%f, %f, %f), and\n  (%f, %f, %f),\n",
		   IQETop.pVVert(2)->dX(), IQETop.pVVert(2)->dY(),
		   IQETop.pVVert(2)->dZ(), IQETop.pVVert(3)->dX(),
		   IQETop.pVVert(3)->dY(), IQETop.pVVert(3)->dZ());
	}
	dOffset = 0;
      }
      if (iSpaceDimen == 2) {
	vMessage(3, "New vertex (if added) will be #%d at (%f, %f)\n",
		 pM->iNumVerts(), (pWI->adCoords())[0],
		 (pWI->adCoords())[1]);
      }
      else {
	vMessage(3, "New vertex (if added) will be #%d at (%f, %f, %f)\n",
		 pM->iNumVerts(), (pWI->adCoords())[0],
		 (pWI->adCoords())[1], (pWI->adCoords())[2]);
      }

      iNumAddedToQueue = pWI->iQueueEncroachedBdryEntities(*this, dOffset);
      vMessage(4, "Added %d entries to the queue; now have %d entries.\n",
	       iNumAddedToQueue, iQueueLength());

      // Did we add something to the queue that has a higher priority
      // than the entity we were just looking at?  Well, if we added
      // nothing, then obviously not.  Otherwise, check!
    } while (iNumAddedToQueue > 0 &&
	     IQETop != IQETopValidEntry());
    vMessage(4, "Inserting in mesh...\n");

    // Add new vertex.  Use a Mesh member function, because this way the
    // Mesh can provide new cells, faces, bfaces, and a vertex as needed.
    switch (iTopoDimen) {
    case 3:
      pM->vCleanupWatson(*pWI);
      // Cleanup of Watson insertion data has left a region to insert
      // in, so fall through
    case 2:
      if (pWI->iNumCellsToRemove() > 0) {
	int iRes = pM->iInsertPointWatson(*pWI);
	iVertsAdded += iRes;
	iVertsAddedSincePurge += iRes;
	if (pM->iNumVerts() % 1000 == 0) {
	  vMessage(2, "Mesh has %d verts so far; queue length is %d\n",
		   pM->iNumVerts(), iQueueLength());
	}
      }
      break;
    default:
      assert(0);
    }

    // We just split this entity, so it will no longer exist.
    vPopTopEntry();
    vMessage(4, "Removed one entry from the queue; now have %d entries.\n",
	     iQueueLength());

    // Add newly created cells at their proper places in the priority
    // queue.  Also adds encroached faces and subsegments, so that
    // dis-encroaching an initial constrained mesh works.
    iNumAddedToQueue = pWI->iAddNewEntitiesToQueue(*this);
    vMessage(4, "Added %d entries to the queue; now have %d entries.\n",
	     iNumAddedToQueue, iQueueLength());

    vMessage(3, "Queue depth:  %d.  Number of verts: %d.\n",
	     iQueueLength(), pM->iNumVerts());

    // If there's nothing left in the queue, we're done.
    if (qIsQueueEmpty()) break;

    if (iVertsAddedSincePurge >= 20000
	&&
	(IQETopValidEntry().eType() == InsertionQueueEntry::eTetCell ||
	 IQETopValidEntry().eType() == InsertionQueueEntry::eTriCell)) {
      vMessage(1, "Purging mesh and rebuilding queue...\n");
      vMessage(1, "Mesh size before: %d cells, %d faces, %d bdry faces, %d verts\n",
	       pM->iNumCells(), pM->iNumFaces(), pM->iNumTotalBdryFaces(),
	       pM->iNumVerts());
      vMessage(1, "Queue length before: %d\n", iQueueLength());
      pM->vPurge();
      vMessage(1, "Mesh size after: %d cells, %d faces, %d bdry faces, %d verts\n",
	       pM->iNumCells(), pM->iNumFaces(), pM->iNumTotalBdryFaces(),
	       pM->iNumVerts());
      vBuildQueue(true);
      vMessage(1, "Queue length after: %d\n", iQueueLength());
      vMessage(1, "First queue entry has priority %f (target is %f)\n",
	       dTopPriority(), dWorstAllowable);
      iVertsAddedSincePurge = 0;
    }
  } // End of insertion driver loop
  SUMAA_LOG_EVENT_END(INSERT_QUEUE_REFINE);
  pM->vPurge();
  if (pWI) delete pWI;
}

bool InsertionQueueEntry::operator==(const InsertionQueueEntry& IQE) const
{
  if (eType() != IQE.eType()) return false;
  switch (eType()) {
    case InsertionQueueEntry::eTriCell:
    case InsertionQueueEntry::eTetCell:
    {
      if (pCCell() == IQE.pCCell()) return true;
    }
    case InsertionQueueEntry::eTriBFace:
    case InsertionQueueEntry::eBdryEdge:
    {
      if (pBFBFace() == IQE.pBFBFace()) return true;
    }
    case InsertionQueueEntry::eSubSeg:
    {
      if ((pVVert(0) == IQE.pVVert(0) &&
	   pVVert(1) == IQE.pVVert(1))
	  ||
	  (pVVert(0) == IQE.pVVert(1) &&
	   pVVert(1) == IQE.pVVert(0)))
      return true;
    }
    default:
    {
      // Should never get here.
      assert(0);
    }
  }
  return false;
}
