#include <vector>
#include <typeinfo>

#include "GR_misc.h"
#include "GR_Face.h"
#include "GR_Periodic.h"
#include "GR_Bdry.h"

Periodic::Periodic() : pBP1(NULL), pBP2(NULL), dScaling(1), iDim(-1),
		       mFaceMap1to2(), mFaceMap2to1()
{
  a2dRotation[0][0] = a2dRotation[1][1] = a2dRotation[2][2] = 1;
  a2dRotation[1][0] = a2dRotation[2][0] =
    a2dRotation[0][1] = a2dRotation[2][1] =
    a2dRotation[0][2] = a2dRotation[1][2] = 0;
  adTranslation[0] = adTranslation[1] = adTranslation[2] = 0;
}

Periodic::Periodic(BdryPatch* const pPB1_In,
		   BdryPatch* const pBP2_In)
  : pBP1(NULL), pBP2(NULL), dScaling(1), iDim(-1),
		       mFaceMap1to2(), mFaceMap2to1()
{
  vSetPatches(pPB1_In, pBP2_In);
}

void Periodic::vSetPatches(BdryPatch* const pBP1_In,
			   BdryPatch* const pBP2_In)
{
  pBP1 = pBP1_In;
  pBP2 = pBP2_In;  
  assert(pBP1);
  assert(pBP2);
  assert(pBP1 != pBP2);
  if (typeid(*pBP1) != typeid(*pBP2)) {
    vFatalError("mismatched boundary patch types", "periodic boundary setup");
  }
  a2dRotation[0][0] = a2dRotation[1][1] = a2dRotation[2][2] = 1;
  a2dRotation[1][0] = a2dRotation[2][0] =
    a2dRotation[0][1] = a2dRotation[2][1] =
    a2dRotation[0][2] = a2dRotation[1][2] = 0;
  adTranslation[0] = adTranslation[1] = adTranslation[2] = 0;
  pBP1->vComputeMapping(pBP2, iDim, dScaling, adTranslation, a2dRotation);
  pBP1->vSetPeriodicData(this);
  pBP2->vSetPeriodicData(this);
}

const BdryPatch* Periodic::pBPOtherPatch(const BdryPatch* pBP) const
{
  assert(pBP == pBP1 || pBP == pBP2);
  return (pBP == pBP1 ? pBP2 : pBP1);
}

void Periodic::vPeriodicLocation(const double adLocSrc[],
				 const BdryPatch* const pBP,
				 double adLocComp[]) const
{
  assert(pBP == pBP1 || pBP == pBP2);
  if (pBP == pBP1) {
    // This is the forward direction, so we rotate, scale, and
    // translate, in that order.
    if (iDim == 2) {
      adLocComp[0] = (adLocSrc[0]*dScaling*a2dRotation[0][0] +
		      adLocSrc[1]*dScaling*a2dRotation[0][1] +
		      adTranslation[0]);
      adLocComp[1] = (adLocSrc[0]*dScaling*a2dRotation[1][0] +
		      adLocSrc[1]*dScaling*a2dRotation[1][1] +
		      adTranslation[1]);
    }
    else {
      assert(iDim == 3);
      adLocComp[0] = (adLocSrc[0]*dScaling*a2dRotation[0][0] +
		      adLocSrc[1]*dScaling*a2dRotation[0][1] +
		      adLocSrc[2]*dScaling*a2dRotation[0][2] +
		      adTranslation[0]);
      adLocComp[1] = (adLocSrc[0]*dScaling*a2dRotation[1][0] +
		      adLocSrc[1]*dScaling*a2dRotation[1][1] +
		      adLocSrc[2]*dScaling*a2dRotation[1][2] +
		      adTranslation[1]);
      adLocComp[2] = (adLocSrc[0]*dScaling*a2dRotation[2][0] +
		      adLocSrc[1]*dScaling*a2dRotation[2][1] +
		      adLocSrc[2]*dScaling*a2dRotation[2][2] +
		      adTranslation[2]);
    }
  }
  else {
    // This is the reverse direction, so we negative translate, scale
    // down, and reverse (transpose) rotate, in that order.
    double adTmp[3];
    if (iDim == 2) {
      adTmp[0] = (adLocSrc[0] - adTranslation[0]) / dScaling;
      adTmp[1] = (adLocSrc[1] - adTranslation[1]) / dScaling;
      adLocComp[0] = (adTmp[0]*a2dRotation[0][0] +
		      adTmp[1]*a2dRotation[1][0]);
      adLocComp[1] = (adTmp[0]*a2dRotation[0][1] +
		      adTmp[1]*a2dRotation[1][1]);
    }
    else {
      assert(iDim == 3);
      adTmp[0] = (adLocSrc[0] - adTranslation[0]) / dScaling;
      adTmp[1] = (adLocSrc[1] - adTranslation[1]) / dScaling;
      adTmp[2] = (adLocSrc[2] - adTranslation[2]) / dScaling;
      adLocComp[0] = (adTmp[0]*a2dRotation[0][0] +
		      adTmp[1]*a2dRotation[1][0] +
		      adTmp[2]*a2dRotation[2][0]);
      adLocComp[1] = (adTmp[0]*a2dRotation[0][1] +
		      adTmp[1]*a2dRotation[1][1] +
		      adTmp[2]*a2dRotation[2][1]);
      adLocComp[2] = (adTmp[0]*a2dRotation[0][2] +
		      adTmp[1]*a2dRotation[1][2] +
		      adTmp[2]*a2dRotation[2][2]);
    }
  }
}

static bool qFuzzyVectorComp(const double aA[], const double aB[],
			     const int iN)
{
  assert(iN == 2 || iN == 3);
  if (iN == 2) {
    return (iFuzzyComp(aA[0], aB[0]) == 0 &&
	    iFuzzyComp(aA[1], aB[1]) == 0);
  }
  else {
    return (iFuzzyComp(aA[0], aB[0]) == 0 &&
	    iFuzzyComp(aA[1], aB[1]) == 0 &&
	    iFuzzyComp(aA[2], aB[2]) == 0);
  }
}

void Periodic::vAddFace(const BdryPatch* pBP, Face* pF)
{
  // Need to know whether there's a match to this face, so first find
  // all unmatched faces associated with the -other- patch.
  std::map<Face*, Face*> *pmMapToSearch;
  bool qIsFirstPatch = (pBP == pBP1);
  if (qIsFirstPatch) {
    pmMapToSearch = &mFaceMap2to1;
  }
  else {
    pmMapToSearch = &mFaceMap1to2;
  }

  std::vector<Face*> vpFCand;
  std::map<Face*, Face*>::iterator iter;
  for (iter = pmMapToSearch->begin(); iter != pmMapToSearch->end(); iter++) {
    if (iter->second == pFInvalidFace) {
      vpFCand.push_back(iter->first);
    }
  }

  // Now identify which one, if any, is a match for the current face.
  int iVecSize = vpFCand.size();
  Face *pFMatch = pFInvalidFace;
  if (iVecSize > 0) {
    // This array will always be big enough (it'll handle quads in 3D).
    double a2dMappedCoords[4][3];
    {
      for (int ii = 0; ii < 4; ii++) {
	for (int jj = 0; jj < 4; jj++) {
	  a2dMappedCoords[ii][jj] = 0.;
	}
      }
    }
    int iV, iNV = pF->iNumVerts();
    for (iV = 0; iV < pF->iNumVerts(); iV++) {
      Vert *pV = pF->pVVert(iV);
      vPeriodicLocation(pV->adCoords(), pBP, a2dMappedCoords[iV]);
      vMessage(4, "Periodic vert at (%f, %f); match sought at (%f, %f)\n",
	       pV->dX(), pV->dY(),
	       a2dMappedCoords[iV][0], a2dMappedCoords[iV][1]);
    }
    for (int i = 0; i < iVecSize; i++) {
      // Match by checking whether the verts you just mapped match the
      // ones for this face.
      Face *pFCand = vpFCand[i];
      if (pFCand->iNumVerts() != iNV) continue;
      bool qMatching = true;
      for (iV = 0; iV < iNV && qMatching; iV++) {
	// Reset to false every time through
	qMatching = false;
	// This test method could fail for verts with identical
	// coordinates.
	const double * const adCand = pFCand->pVVert(iV)->adCoords();
	for (int iC = 0; iC < iNV; iC++) {
	  qMatching = qMatching ||
	    qFuzzyVectorComp(adCand, a2dMappedCoords[iC], iDim);
	}
      }
      if (qMatching) pFMatch = pFCand;
    }
  }

  // If none match, just add this one.
  if (pFMatch == pFInvalidFace) {
    if (qIsFirstPatch) {
      vMessage(4, "  Adding an unmatched face to patch 1!\n");
      mFaceMap1to2.insert(std::pair<Face*, Face*>(pF, pFInvalidFace));
    }
    else {
      vMessage(4, "  Adding an unmatched face to patch 2!\n");
      mFaceMap2to1.insert(std::pair<Face*, Face*>(pF, pFInvalidFace));
    }
  }
  
  // If one -does- match, then update the reverse entry and add one
  // here.
  else {
    vMessage(4, "  Found a matching periodic face!\n");
    vMessage(4, "  From (%f, %f) to (%f, %f)\n",
	     pFMatch->pVVert(0)->dX(), pFMatch->pVVert(0)->dY(), 
	     pFMatch->pVVert(1)->dX(), pFMatch->pVVert(1)->dY());
    if (qIsFirstPatch) {
      std::map<Face*, Face*>::iterator iDel = mFaceMap2to1.find(pFMatch);
      assert(iDel != mFaceMap2to1.end());
      assert(iDel->first == pFMatch);
      assert(iDel->second == pFInvalidFace);
      mFaceMap2to1.erase(iDel);
      mFaceMap2to1.insert(std::pair<Face*, Face*>(pFMatch, pF));
      mFaceMap1to2.insert(std::pair<Face*, Face*>(pF, pFMatch));
    }
    else {
      std::map<Face*, Face*>::iterator iDel = mFaceMap1to2.find(pFMatch);
      assert(iDel != mFaceMap1to2.end());
      assert(iDel->first == pFMatch);
      assert(iDel->second == pFInvalidFace);
      mFaceMap1to2.erase(iDel);
      mFaceMap1to2.insert(std::pair<Face*, Face*>(pFMatch, pF));
      mFaceMap2to1.insert(std::pair<Face*, Face*>(pF, pFMatch));
    }
  }
}

void Periodic::vRemoveFace(const BdryPatch* pBP,
			   Face *pF)
{
  std::map<Face*, Face*>::iterator iter;
  std::map<Face*, Face*> *pMapForward, *pMapBackward;
  if (pBP == pBP1) {
    pMapForward = &mFaceMap1to2;
    pMapBackward = &mFaceMap2to1;
  }
  else {
    pMapForward = &mFaceMap2to1;
    pMapBackward = &mFaceMap1to2;
  }
  iter = pMapForward->find(pF);

  if (iter == pMapForward->end())
    return;

  Face *pFOther = iter->second;
  pMapForward->erase(iter);
  if (pFOther != NULL) {
    iter = pMapBackward->find(pFOther);
    assert(iter != pMapBackward->end());
    pMapBackward->erase(iter);
    pMapBackward->insert(std::pair<Face*, Face*>(pFOther, NULL));
  }
}

const Face* Periodic::pFFaceOpposite(const BdryPatch* pBP,
				     Face* pF) const
{
  if (pBP == pBP1) {
    std::map<Face*, Face*>::const_iterator iter =
      mFaceMap1to2.find(pF);
    if (iter == mFaceMap1to2.end()) return pFInvalidFace;
    else return iter->second;
  }
  else {
    std::map<Face*, Face*>::const_iterator iter =
      mFaceMap2to1.find(pF);
    if (iter == mFaceMap2to1.end()) return pFInvalidFace;
    else return iter->second;
  }
}
