#include <algorithm>
#include "GR_ADT.h"
#undef VERIFY_QUERY

ADT::ADT(const int iNumObj, const int iNumDim, const eDataT eDType,
	 const double* const pdDataIn) :
  pADT_Left(NULL), pADT_Right(NULL), pADT_Parent(NULL),
  iLevel(0), iDataInd(-1), iNDim(iNumDim), eDT(eDType), pdData(NULL)
{
  sADTSort *asADTS = new sADTSort[iNumObj];
  int iNV = iNumVars();
  for (int i = 0; i < iNumObj; i++) {
    asADTS[i].vSetData(iNV, pdDataIn[i*iNV], &pdDataIn[i*iNV], i);
  }
  vBuildADT(asADTS, iNumObj);
  delete [] asADTS;
}

ADT::ADT(const VAVertex* const pVAVerts) :
  pADT_Left(NULL), pADT_Right(NULL), pADT_Parent(NULL),
  iLevel(0), iDataInd(-1), iNDim(pVAVerts->pTEntry(0)->iSpaceDimen()),
  eDT(ePoints), pdData(NULL)
{
  int iNumObj = pVAVerts->iSize();
  sADTSort *asADTS = new sADTSort[iNumObj];
  double *adDataIn = new double[iNumObj*iNDim];
  int i;
  for (i = 0; i < iNumObj; i++) {
    adDataIn[i*iNDim    ] = pVAVerts->pTEntry(i)->dX();
    adDataIn[i*iNDim + 1] = pVAVerts->pTEntry(i)->dY();
    if (iNDim == 3) {
      adDataIn[i*iNDim + 2] = pVAVerts->pTEntry(i)->dZ();
    }
  } 
  for (i = 0; i < iNumObj; i++) {
    asADTS[i].vSetData(iNDim, adDataIn[i*iNDim],
		       &adDataIn[i*iNDim], i);
  }
  vBuildADT(asADTS, iNumObj);
  delete [] adDataIn;
  delete [] asADTS;
}

void ADT::vBuildADT(sADTSort asADTS[], const int iNumObj)
{
  assert(iNumObj > 0);
  std::sort(asADTS, asADTS+iNumObj);
  // A balanced split, with an extra element into the left subtree if needed.
  int iMedian = iNumObj/2;

  if (pdData != NULL) delete pdData;
  pdData = new double[iNumVars()];

  int i;
  iDataInd = asADTS[iMedian].iInd;
  for (i = iNumVars() - 1; i >= 0; i--) {
    pdData[i] = asADTS[iMedian].pdData[i];
  }

  // Update the keys so that we can sort again at the next level.
  int iNextKey = iDataKey(iLevel+1);
  for (i = 0; i < iNumObj; i++) {
    asADTS[i].dKey = asADTS[i].pdData[iNextKey];
  }

  int iLeftSize = iMedian;
  int iRightSize = iNumObj - iMedian - 1;
  if (iLeftSize > 0) {
    pADT_Left = new ADT();
    pADT_Left->pADT_Parent = this;
    pADT_Left->iNDim = iNDim;
    pADT_Left->iLevel = iLevel + 1;
    pADT_Left->eDT = eDT;
    pADT_Left->vBuildADT(asADTS, iLeftSize);
  }
  if (iRightSize > 0) {
    pADT_Right = new ADT();
    pADT_Right->pADT_Parent = this;
    pADT_Right->iNDim = iNDim;
    pADT_Right->iLevel = iLevel + 1;
    pADT_Right->eDT = eDT;
    pADT_Right->vBuildADT(asADTS+iMedian+1, iRightSize);
  }
}

ADT::ADT(const ADT& ADTIn) :
  pADT_Left(NULL), pADT_Right(NULL), pADT_Parent(NULL),
  iLevel(0), iDataInd(ADTIn.iDataInd), iNDim(ADTIn.iNDim),
  eDT(ADTIn.eDT), pdData(NULL)
{
  if (ADTIn.iLevel == -1) {
    iLevel = -1;
    assert(ADTIn.pADT_Parent == NULL);
    assert(ADTIn.pADT_Left == NULL);
    assert(ADTIn.pADT_Right == NULL);
    assert(ADTIn.pdData == NULL);
    return;
  }
  pdData = new double[iNumVars()];
  for (int i = iNumVars() - 1; i >= 0; i--) {
    pdData[i] = ADTIn.pdData[i];
  }
  vCloneTree(&ADTIn);
}

const ADT& ADT::operator=(const ADT& ADTIn)
{
  if (this != &ADTIn) {
    if (ADTIn.iLevel == -1) {
      iLevel = -1;
      assert(ADTIn.pADT_Parent == NULL);
      assert(ADTIn.pADT_Left == NULL);
      assert(ADTIn.pADT_Right == NULL);
      assert(ADTIn.pdData == NULL);
      pADT_Parent = NULL;
      pADT_Left = NULL;
      pADT_Right = NULL;
      pdData = NULL;
      eDT = ADTIn.eDT;
      iNDim = ADTIn.iNDim;
    }
    else {
      if (pADT_Left) delete pADT_Left;
      if (pADT_Right) delete pADT_Right;
      pADT_Left = pADT_Right = NULL;
      if (eDT != ADTIn.eDT || iNDim != ADTIn.iNDim) {
	eDT = ADTIn.eDT;
	iNDim = ADTIn.iNDim;
	if (pdData) {
	  delete [] pdData;
	  pdData = new double[iNumVars()];
	}
      }
      for (int i = iNumVars() - 1; i >= 0; i--) {
	pdData[i] = ADTIn.pdData[i];
      }
      iDataInd = ADTIn.iDataInd;
      vCloneTree(&ADTIn);
    }
  }
  return *this;
}

void ADT::vCloneTree(const ADT* const pADTIn)
{
  assert(pADT_Left == NULL);
  assert(pADT_Right == NULL);
  if (pADTIn->pADT_Left != NULL) {
    pADT_Left = new ADT(this, iLevel+1,
			pADTIn->pADT_Left->iDataInd,
			iNDim, eDT, pADTIn->pADT_Left->pdData);
    pADT_Left->vCloneTree(pADTIn->pADT_Left);
  }
  if (pADTIn->pADT_Right != NULL) {
    pADT_Right = new ADT(this, iLevel+1,
			 pADTIn->pADT_Right->iDataInd,
			 iNDim, eDT, pADTIn->pADT_Right->pdData);
    pADT_Right->vCloneTree(pADTIn->pADT_Right);
  }
}

const ADT* ADT::pADTFindNode(const double adData[], const int iIndSearch) const
{
  int iNVars = iNumVars();
  bool qIsThisIt = (iDataInd == iIndSearch);
  for (int i = 0; i < iNVars && qIsThisIt; i++) {
    if (iFuzzyComp(adData[i], pdData[i]) != 0) {
      qIsThisIt = false;
    }
  }

  if (qIsThisIt)
    return this;
  else {
    int iKey = iDataKey();
    if (adData[iKey] <= pdData[iKey]) {
      // Search left subtree
      if (pADTLeftChild() != NULL) {
	const ADT* pADT = pADTLeftChild()->pADTFindNode(adData, iIndSearch);
	if (pADT != NULL) return pADT;
      }
    }
    if (adData[iKey] >= pdData[iKey]) {
      // Search right subtree
      if (pADTRightChild() != NULL) {
	const ADT* pADT = pADTRightChild()->pADTFindNode(adData, iIndSearch);
	if (pADT != NULL) return pADT;
      }
    }
    return NULL;
  }
}

const ADT* ADT::pADTFindNodeWithMaxValue(const int iKeyIn) const
{
  const ADT* pADTResult = this;
  int iKey = iDataKey();

  if (iKeyIn != iKey) {
    // In this case, start by searching the left subtree, if it exists.
    if (pADTLeftChild() != NULL) {
      const ADT* pADTRes2 = pADTLeftChild()->pADTFindNodeWithMaxValue(iKeyIn);

      // Now compare to this node.
      if (pADTResult->pdData[iKeyIn] < pADTRes2->pdData[iKeyIn]) {
	pADTResult = pADTRes2;
      }
    }
  }
  // Always check the right subtree, if it exists.
  if (pADTRightChild() != NULL) {
    const ADT* pADTRes2 = pADTRightChild()->pADTFindNodeWithMaxValue(iKeyIn);
    
    // Now compare to this node.
    if (pADTResult->pdData[iKeyIn] < pADTRes2->pdData[iKeyIn]) 
      pADTResult = pADTRes2;
  }
  return pADTResult;
}

const ADT* ADT::pADTFindNodeWithMinValue(const int iKeyIn) const
{
  const ADT* pADTResult = this;
  int iKey = iDataKey();

  if (iKeyIn != iKey) {
    // In this case, start by searching the right subtree, if it exists.
    if (pADTRightChild() != NULL) {
      const ADT* pADTRes2 = pADTRightChild()->pADTFindNodeWithMinValue(iKeyIn);

      // Now compare to this node.
      if (pADTResult->pdData[iKeyIn] > pADTRes2->pdData[iKeyIn]) {
	pADTResult = pADTRes2;
      }
    }
  }
  // Always check the left subtree, if it exists.
  if (pADTLeftChild() != NULL) {
    const ADT* pADTRes2 = pADTLeftChild()->pADTFindNodeWithMinValue(iKeyIn);
    
    // Now compare to this node.
    if (pADTResult->pdData[iKeyIn] > pADTRes2->pdData[iKeyIn]) 
      pADTResult = pADTRes2;
  }
  return pADTResult;
}

void ADT::vRemoveNode(const double adData[], const int iInd)
{
  ADT *pADTNodeToRemove = const_cast<ADT*>(pADTFindNode(adData, iInd));
  assert(pADTNodeToRemove != NULL);
  pADTNodeToRemove->vBalancingPromotion();
}

void ADT::vBalancingPromotion()
{
  if (pADT_Left == NULL && pADT_Right == NULL) {
    // This node has no descendants.  It needs to be unlinked from the
    // tree.
    if (pADT_Parent == NULL) {
      // The whole tree is gone!
      iLevel = -1;
      if (pdData) delete [] pdData;
      pdData = NULL;
    }
    else {
      // Unlink from parent.
      if (pADT_Parent->pADT_Left == this) {
	pADT_Parent->pADT_Left = NULL;
      }
      else {
	assert(pADT_Parent->pADT_Right == this);
	pADT_Parent->pADT_Right = NULL;
      }
      delete this;
    }
    return;
  }
  
  // Promote from the side that's larger.
  int iLeftSize = pADT_Left->iTreeSize();
  int iRightSize = pADT_Right->iTreeSize();

  int iKey = iDataKey();
  ADT *pADTPromote;
  if (iLeftSize >= iRightSize) {
    // Find the node to promote from the left side.
    assert(iLeftSize > 0);
    pADTPromote = const_cast<ADT*>(pADT_Left->pADTFindNodeWithMaxValue(iKey));
  }
  else {
    // Find the node to promote from the right side.
    assert(iRightSize > 0);
    pADTPromote = const_cast<ADT*>(pADT_Right->pADTFindNodeWithMinValue(iKey));
  }

  iDataInd = pADTPromote->iDataInd;
  assert(iNDim == pADTPromote->iNDim);
  assert(eDT == pADTPromote->eDT);
  for (int i = iNumVars() - 1; i >= 0; i--) {
    pdData[i] = pADTPromote->pdData[i];
  }
  pADTPromote->vBalancingPromotion();
}

int ADT::iTreeSize() const
{
  if (this == NULL) return 0;
  int iLeftSize = 0, iRightSize = 0;
  iLeftSize = pADTLeftChild()->iTreeSize();
  iRightSize = pADTRightChild()->iTreeSize();
  return 1 + iLeftSize + iRightSize;
}

int ADT::iMaxTreeDepth() const
{
  if (this == NULL) return 0;
  int iLeftDepth = 0, iRightDepth = 0;
  iLeftDepth = pADTLeftChild()->iMaxTreeDepth();
  iRightDepth = pADTRightChild()->iMaxTreeDepth();
  return 1 + max(iLeftDepth, iRightDepth);
}

int ADT::iMinTreeDepth() const
{
  if (this == NULL) return 0;
  int iLeftDepth = pADTLeftChild()->iMinTreeDepth();
  int iRightDepth = pADTRightChild()->iMinTreeDepth();
  if (pADT_Left && pADT_Right) {
    return 1 + min(iLeftDepth, iRightDepth);
  } 
  else if (pADT_Left) {
    assert(pADT_Right == NULL);
    return 1 + iLeftDepth;
  }
  else if (pADT_Right) {
    assert(pADT_Left == NULL);
    return 1 + iRightDepth;
  }
  else return 1;
}

// The following #defines can be used as statements.
#define RECUR_LEFT(data, ind) \
      if (pADT_Left) \
	pADT_Left->vAddNode(data, ind); \
      else \
	pADT_Left = new ADT(this, iLevel+1, ind, iNDim, eDT, data)
#define RECUR_RIGHT(data, ind) \
      if (pADT_Right) \
	pADT_Right->vAddNode(data, ind); \
      else \
	pADT_Right = new ADT(this, iLevel+1, ind, iNDim, eDT, data)

void ADT::vAddNode(const double pdNewData[], const int iNewInd)
{
  if (iLevel == -1) {
    // This should be a completely empty tree.
    assert(pADT_Left == NULL);
    assert(pADT_Right == NULL);
    assert(pADT_Parent == NULL);
    iLevel = 0;
    iDataInd = iNewInd;
    assert(iNDim != 0);
    assert(eDT == ePoints || eDT == eBBoxes);
    assert(pdData == NULL);
    pdData = new double[iNumVars()];
    for (int i = 0; i < iNumVars(); i++) {
      pdData[i] = pdNewData[i];
    }
    return;
  }

  int iLeftSize = pADT_Left->iTreeSize();
  int iRightSize = pADT_Right->iTreeSize();
  int iKey = iDataKey();
  double dDiv = pdData[iKey];
  if (iLeftSize == iRightSize) {
    // The tree is currently balanced, so who cares where we insert?
    if (pdNewData[iKey] < dDiv) {
      RECUR_LEFT(pdNewData, iNewInd);
    }
    else {
      RECUR_RIGHT(pdNewData, iNewInd);
    }
  }
  else if (iLeftSize > iRightSize) {
    assert(iLeftSize >= 1 && pADT_Left);
    // Need to add a node to the right subtree.
    if (pdNewData[iKey] > dDiv) {
      // New data belongs on right (the easy case)
      RECUR_RIGHT(pdNewData, iNewInd);
    }
    else {
      // First, take the data from the current node and put it into the
      // right subtree.
      RECUR_RIGHT(pdData, iDataInd);

      // Grab the biggest entry in the left subtree.
      ADT *pADT_Cand = const_cast<ADT*>
	(pADT_Left->pADTFindNodeWithMaxValue(iKey));
      if (pADT_Cand->pdData[iKey] < pdNewData[iKey]) {
	// The new data will now divide the two subtrees.
	iDataInd = iNewInd;
	for (int ii = 0; ii < iNumVars(); ii++) {
	  pdData[ii] = pdNewData[ii];
	}
      }
      else {
	// Promote the candidate you just found, then insert the new
	// data in the left subtree.
	iDataInd = pADT_Cand->iDataInd;
	for (int ii = 0; ii < iNumVars(); ii++) {
	  pdData[ii] = pADT_Cand->pdData[ii];
	}
	pADT_Cand->vBalancingPromotion();
	RECUR_LEFT(pdNewData, iNewInd);
      }
    }
  } // Done with forcing a new node into the right subtree
  else {
    assert(iRightSize >= 1 && pADT_Right);
    // Need to add a node to the left subtree.
    if (pdNewData[iKey] < dDiv) {
      // New data belongs on left (the easy case)
      RECUR_LEFT(pdNewData, iNewInd);
    }
    else {
      // First, take the data from the current node and put it into the
      // left subtree.
      RECUR_LEFT(pdData, iDataInd);

      // Grab the smallest entry in the right subtree.
      ADT *pADT_Cand = const_cast<ADT*>
	(pADT_Right->pADTFindNodeWithMinValue(iKey));
      if (pADT_Cand->pdData[iKey] > pdNewData[iKey]) {
	// The new data will now divide the two subtrees.
	iDataInd = iNewInd;
	for (int ii = 0; ii < iNumVars(); ii++) {
	  pdData[ii] = pdNewData[ii];
	}
      }
      else {
	// Promote the candidate you just found, then insert the new
	// data in the right subtree.
	iDataInd = pADT_Cand->iDataInd;
	for (int ii = 0; ii < iNumVars(); ii++) {
	  pdData[ii] = pADT_Cand->pdData[ii];
	}
	pADT_Cand->vBalancingPromotion();
	RECUR_RIGHT(pdNewData, iNewInd);
      }
    }
  } // Done with forcing a new node into the left subtree
}

//  DANGER!  The following insertion code is commented out because it
//  happens to be WRONG!  Don't uncomment without fixing it.
//   ADT* pCTree = this;
//   while ((pCTree->pADTLeftChild() != NULL) &&
// 	 (pCTree->pADTRightChild() != NULL)) {
//     int iKey = pCTree->iDataKey();
//     double dThisDiv = pCTree->pdData[iKey];
//     // Run down through one subtree or the other...
//     if (dThisDiv >= pdNewData[iKey]) pCTree = pCTree->pADTLeftChild();
//     else                             pCTree = pCTree->pADTRightChild();
//     assert(pCTree != NULL);
//   }

//   // Found a node with only one child.
//   int iKey = pCTree->iDataKey();
//   double dThisDiv = pCTree->pdData[iKey];

//   // If no children exist, add the new node on the appropriate side.
//   if (pCTree->pADTLeftChild() == NULL &&
//       pCTree->pADTRightChild() == NULL) {
//     if (dThisDiv < pdNewData[iKey]) {
//       pCTree->pADT_Right = new ADT(pCTree, pCTree->iLevel+1,
// 				   iNewInd, iNDim, eDT, pdNewData);
//     }
//     else {
//       pCTree->pADT_Left = new ADT(pCTree, pCTree->iLevel+1,
// 				  iNewInd, iNDim, eDT, pdNewData);
//     }
//   }
//   else if (pCTree->pADTLeftChild() != NULL) {
//     assert(pCTree->pADT_Right == NULL);
//     // Smallest element becomes left child, middle goes to pCTree,
//     // largest goes on the right.  Exception:  If the left child has
//     // descendants, it doesn't get moved; instead, the new node is
//     // inserted in the appropriate place in that tree.

//     if (pdNewData[iKey] >= dThisDiv) {
//       // Add new node to the right
//       pCTree->pADT_Right = new ADT(pCTree, pCTree->iLevel+1,
// 				   iNewInd, iNDim, eDT, pdNewData);
//     }
//     else if (pdNewData[iKey] >= pCTree->pADT_Left->pdData[iKey]) {
//       // New data does in the middle, and that data gets pushed to the
//       // right child.
//       ADT ADTTemp(NULL, NULL, pCTree, pCTree->iLevel,
// 		  iNewInd, iNDim, eDT, pdNewData);
//       pCTree->pADT_Right = new ADT(pCTree, pCTree->iLevel+1,
// 				   pCTree->iDataInd, iNDim, eDT,
// 				   pCTree->pdData);
//       pCTree->vCopyData(ADTTemp);
//     }
//     else {
//       // Left child exists and new data belongs on the left; check to
//       // see how many descendants the left child has. 
//       int iLeftSize = pCTree->pADTLeftChild()->iTreeSize();
//       assert(iLeftSize >= 1);
//       if (iLeftSize == 1) {
// 	// Left child is a childless leaf. New data goes on the left.
// 	pCTree->pADT_Right = new ADT(pCTree, pCTree->iLevel+1,
// 				     pCTree->iDataInd, iNDim, eDT,
// 				     pCTree->pdData);
// 	pCTree->vCopyData(*(pCTree->pADT_Left));
// 	pCTree->iLevel--;
// 	ADT ADTTemp(NULL, NULL, pCTree, pCTree->iLevel+1,
// 		    iNewInd, iNDim, eDT, pdNewData);
// 	pCTree->pADT_Left->vCopyData(ADTTemp);
//       }
//       else {
// 	// Insert recursively.
// 	pCTree->pADT_Left->vAddNode(pdNewData, iNewInd);
//       }
//     } // New data belongs on the left.
//   } // Done with cases where the left child exists.
//   else {
//     assert(pCTree->pADT_Right != NULL);
//     assert(pCTree->pADT_Left == NULL);
//     // Smallest element becomes left child, middle goes to pCTree,
//     // largest goes on the right.  Exception:  If the left child has
//     // descendants, it doesn't get moved; instead, the new node is
//     // inserted in the appropriate place in that tree.

//     if (pdNewData[iKey] <= dThisDiv) {
//       // Add new node to the left
//       pCTree->pADT_Left = new ADT(pCTree, pCTree->iLevel+1,
// 				   iNewInd, iNDim, eDT, pdNewData);
//     }
//     else if (pdNewData[iKey] <= pCTree->pADT_Right->pdData[iKey]) {
//       // New data does in the middle, and that data gets pushed to the
//       // left child.
//       ADT ADTTemp(NULL, NULL, pCTree, pCTree->iLevel,
// 		  iNewInd, iNDim, eDT, pdNewData);
//       pCTree->pADT_Left = new ADT(pCTree, pCTree->iLevel+1,
// 				  pCTree->iDataInd, iNDim, eDT,
// 				  pCTree->pdData);
//       pCTree->vCopyData(ADTTemp);
//     }
//     else {
//       // Check to see how many descendants the right child has.
//       int iRightSize = pCTree->pADTRightChild()->iTreeSize();
//       assert(iRightSize >= 1);
//       if (iRightSize == 1) {
// 	// Right child is a childless leaf. New data goes on the right.
// 	pCTree->pADT_Left = new ADT(pCTree, pCTree->iLevel+1,
// 				    pCTree->iDataInd, iNDim, eDT,
// 				    pCTree->pdData);
// 	pCTree->vCopyData(*(pCTree->pADT_Right));
// 	pCTree->iLevel--;
// 	ADT ADTTemp(NULL, NULL, pCTree, pCTree->iLevel+1,
// 		    iNewInd, iNDim, eDT, pdNewData);
// 	pCTree->pADT_Right->vCopyData(ADTTemp);
//       }
//       else {
// 	// Insert recursively.
// 	pCTree->pADT_Right->vAddNode(pdNewData, iNewInd);
//       }
//     } 
//   } // Done with cases where the right child exists.
// }

void ADT::vPointQuery(const double adRange[], std::vector<int>& veciResult)
  const
{
  int iKey = iDataKey();
  double dThisDiv = pdData[iKey];
  
  // Check the point for membership in the test range.
  bool qAccept = true;
  for (int i = 0; i < iNDim && qAccept; i++)
    qAccept =  ((pdData[i] <= adRange[2*i+1]) && 
		(pdData[i] >= adRange[2*i  ]));

  if (qAccept)
    // Add this point.
    veciResult.push_back( iDataInd );

  // Point falls above the bottom of the range.  Check left subtree.
  if ( (dThisDiv >= adRange[iKey*2    ]) && pADTLeftChild() ) 
    pADTLeftChild()->vPointQuery(adRange, veciResult);

  // Point falls below the top of the range.  Check right subtree.
  if ( (dThisDiv <= adRange[iKey*2 + 1]) && pADTRightChild() ) 
    pADTRightChild()->vPointQuery(adRange, veciResult);
}

void ADT::vBBoxQuery(const double adRange[], std::vector<int>& veciResult) const
{
  int iKey = iDataKey();
  double dThisDiv = pdData[iKey];
  
  // Check the current node for overlap with the test bbox.
  bool qAccept = true;
  for (int i = 0; i < iNDim && qAccept; i++)
    qAccept = ((pdData[2*i  ] <= adRange[2*i+1]) && 
	       (pdData[2*i+1] >= adRange[2*i  ]));

  // The node overlaps; add it to the result list.
  if (qAccept)
    // Add this BBox.
    veciResult.push_back( iDataInd );

  ADT *pADTLeft  = pADTLeftChild();
  ADT *pADTRight = pADTRightChild();

  // Left subtree must be checked if it exists and either
  //   (a) the last divider was a minimum or
  //   (b) the low end of the range is below the divider
  if (pADTLeft &&
      (iKey%2 == 0  ||  dThisDiv >= adRange[(iKey/2)*2    ]) )
    pADTLeft->vBBoxQuery(adRange, veciResult);

  // Right subtree must be checked if it exists and either
  //   (a) the last divider was a maximum or
  //   (b) the high end of the range is above the divider
  if ( (pADTRight) &&
      (iKey%2 == 1  ||  dThisDiv <= adRange[(iKey/2)*2 + 1]) )
    pADTRight->vBBoxQuery(adRange, veciResult);
}

ADT::iterator ADT::begin()
{
  ADT *pADT = this;
  while (pADT->pADT_Left) {
    pADT = pADT->pADT_Left;
  }
  return iterator(pADT);
}

ADT::iterator ADT::end() {return iterator(NULL);}

const ADT::iterator ADT::begin() const
{
  const ADT *pADT = this;
  while (pADT->pADT_Left) {
    pADT = pADT->pADT_Left;
  }
  return iterator(const_cast<ADT*>(pADT));
}

const ADT::iterator ADT::end() const {return iterator(NULL);}

void ADT::vPointExhaust(const double adRange[],
			std::vector<int>& veciResult) const
{
  for (ADT::iterator iter = begin(); iter != end(); iter++) {
    int iIndex = iter->iDataIndex();
    bool qAccept = true;
    for (int i = 0; i < iNDim && qAccept ; i++)
      qAccept = ((iter->pdData[i] <= adRange[2*i+1]) &&
		 (iter->pdData[i] >= adRange[2*i  ]));
    
    // The node overlaps; add it to the result list.
    if (qAccept)
      veciResult.push_back( iIndex );
  }
}

void ADT::vBBoxExhaust(const double adRange[],
		       std::vector<int>& veciResult) const
{
  for (ADT::iterator iter = begin(); iter != end(); iter++) {
    int iIndex = iter->iDataIndex();
    bool qAccept = true;
    for (int i = 0; i < iNDim && qAccept ; i++)
      qAccept = ((iter->pdData[2*i  ] <= adRange[2*i+1]) &&
		 (iter->pdData[2*i+1] >= adRange[2*i  ]));
    if (qAccept)
      veciResult.push_back( iIndex );
  }
}

std::vector<int> ADT::veciRangeQuery(double adRange[]) const
{
  int iDimRange = 2*iNDim;
  int iCheck;
  for (iCheck = 0; iCheck < iDimRange ; iCheck += 2) {
    assert(adRange[iCheck] <= adRange[iCheck+1]);
    if (iFuzzyComp(adRange[iCheck], adRange[iCheck+1]) == 0) {
      adRange[iCheck] -= 1.e-8;
      adRange[iCheck+1] += 1.e-8;
    }
  }
  std::vector<int> veciResult;
#ifdef VERIFY_QUERY
  std::vector<int> veciCheck;
#endif

  switch (eDT) {
  case ePoints:
#ifdef VERIFY_QUERY
    vPointExhaust(adRange, veciCheck);
#endif
    vPointQuery(adRange, veciResult);
    break;
  case eBBoxes:
#ifdef VERIFY_QUERY
    vBBoxExhaust(adRange, veciCheck);
#endif
    vBBoxQuery(adRange, veciResult);
    break;
#ifndef NDEBUG
  default:
    assert(0);
    break;
#endif
  }
#ifdef VERIFY_QUERY
  vMessage(2, "Exhaustive search gave %d items\n", veciCheck.size());
  int ii;
  std::sort(veciCheck.begin(), veciCheck.end());
  for (ii = 0; ii < veciCheck.size(); ii++) {
    vMessage(2, "  %d\n", veciCheck[ii]);
  }
  vMessage(2, "Tree search gave %d items\n", veciResult.size());
  std::sort(veciResult.begin(), veciResult.end());
  for (ii = 0; ii < veciResult.size(); ii++) {
    vMessage(2, "  %d\n", veciResult[ii]);
  }
  return(veciCheck);
#else
  return(veciResult);
#endif
}

void ADT::vSpew() const
{
  ADT::iterator iter = begin();
  for ( ; iter != end() ; iter++) {
    vMessage(3, "  Level: %2d  Index:  %3d\n",
	      iter->iTreeLevel(), iter->iDataIndex());
  }
}

bool ADT::qValid() const
{
  if (iLevel == -1) {
    // This is the special case of an empty tree.
    assert(pADT_Parent == NULL);
    return true;
  }
  
  if (pADT_Parent) {
    if ((pADT_Parent->pADT_Left != this &&
	 pADT_Parent->pADT_Right != this) ||
	(pADT_Parent->iLevel != iLevel-1) ||
	(pADT_Parent->eDT != eDT) ||
	(pADT_Parent->iNDim != iNDim))
      return false;
  }

  // For a tree to be valid, it must correctly divide its children, and
  // its children must also be valid.
  int iKey = iDataKey();
  double dDiv = pdData[iKey];

  ADT::iterator iter;
  // Check the left subtree
  if (pADT_Left) {
    iter = pADT_Left->begin();
    for ( ; &(*iter) != this ; iter++ ) {
      if (iter->pdData[iKey] > dDiv)
	return false;
    }
    if (!pADT_Left->qValid()) return false;
  }

  // Check the right subtree
  if (pADT_Right) {
    iter = pADT_Right->begin();
    for ( ; iter->iLevel < iLevel ; iter++ ) {
      if (iter->pdData[iKey] < dDiv)
	return false;
    }
    if (!pADT_Right->qValid()) return false;
  }

  return true;
}
  
